#include "MW4Headers.hpp"
#include "adept\application.hpp"
#include "rail_move.hpp"
#include "move_rect.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;
	extern __int64 tNodeToPathTime;
};

using namespace MW4AI;
using namespace MechWarrior4;

void CRailPath::AddStartPoint (Stuff::Point3D& loc,bool& startbrid,stlport::vector<NodePathElement>::iterator& iter)
{
	Stuff::Point3D extra,destloc1,destloc2,center;
	Stuff::Point3D d1vec,d2vec,third;
	int nodeto;

	AutoHeap local_heap (g_RailHeap);
	const OBRect *brid;
	brid = m_Graph->RectList ()->Inside (m_Start);
	if (brid)
	{
		CRailNode *destnode1,*destnode2;
		Verify (m_NodePath.size () >= 2);
		destnode1 = m_NodePath[0].destnode;
		destnode2 = m_NodePath[1].destnode;
		startbrid = true;

		const OBRect *crossbrid;
		crossbrid = m_Graph->RectList ()->Collide (destnode1->Location (),destnode2->Location ());
		m_Path.push_back (PathElement (m_Start,BRIDGEFLAG,true,-1));
		if (brid == crossbrid)
		{
			iter++;
			nodeto = 1;
		}
		else
		{
			nodeto = 0;
		}

		if (brid->vertical)
		{
			destloc2 = brid->BottomPoint ();
			destloc1 = brid->TopPoint ();
			if (m_NodePath[nodeto].destnode->Location ().z < destloc1.z)
			{
				m_Path.push_back (PathElement (destloc1,BRIDGEFLAG,true,-1));
				loc = destloc1;
			}
			else
			{
				m_Path.push_back (PathElement (destloc2,BRIDGEFLAG,true,-1));
				loc = destloc2;
			}
		}
		else
		{
			destloc2 = brid->RightPoint ();
			destloc1 = brid->LeftPoint ();
			if (m_NodePath[nodeto].destnode->Location ().x < destloc1.x)
			{
				m_Path.push_back (PathElement (destloc1,BRIDGEFLAG,true,-1));
				loc = destloc1;
			}
			else
			{
				m_Path.push_back (PathElement (destloc2,BRIDGEFLAG,true,-1));
				loc = destloc2;
			}
		}
		loc.y = 0;
	}
	else
	{
//		m_Path.push_back (PathElement (m_Start,m_NodePath[0].usage,false,-1));		
		loc = m_Start;
		loc.y = 0;
	}
	iter++;
}

void CRailPath::AddBridgeToPath (stlport::vector<NodePathElement>::iterator iter,Stuff::Point3D& loc,const OBRect *bridge,bool startbrid)
{
	AutoHeap local_heap (g_RailHeap);
	Stuff::Point3D destloc1,destloc2;
	if (bridge->vertical)
	{
		if (loc.z >= bridge->bottom)
		{
			destloc2 = bridge->BottomPoint ();
			destloc1 = bridge->TopPoint ();
		}
		else
		{
			destloc2 = bridge->TopPoint ();
			destloc1 = bridge->BottomPoint ();
		}
	}
	else
	{
		if (loc.x >= bridge->right)
		{
			destloc2 = bridge->RightPoint ();
			destloc1 = bridge->LeftPoint ();
		}
		else
		{
			destloc2 = bridge->LeftPoint ();
			destloc1 = bridge->RightPoint ();
		}
	}
	Verify (!startbrid);
	if (!startbrid)
		m_Path.push_back (PathElement (destloc2,iter->usage + BRIDGEFLAG,false,iter->link->ID ()));
	m_Path.push_back (PathElement (destloc1,BRIDGEFLAG,true,iter->link->ID ()));
	loc = destloc1;
}

void CRailPath::AddPointToPath (stlport::vector<NodePathElement>::iterator iter,Stuff::Point3D& loc)
{
	Stuff::Point3D extra,destloc1,destloc2,center;
	Stuff::Point3D d1vec,d2vec,third;
	CRailNode *destnode;
	CRailLink *destlink;
	Stuff::Scalar distnode;
	Stuff::Scalar slope,intercept,rad,a,b,c,inner,root,x1,x2,t1,t2,y,w;

	AutoHeap local_heap (g_RailHeap);
	destnode = iter->destnode;
	destlink = iter->link;
	Verify (destnode);
	Verify (destlink);

	destloc1 = destnode->SubNode (destlink).location;
	destloc2 = destlink->OtherLocation (destnode)->SubNode (destlink).location;
	center = destnode->Location ();
	center.y = 0;
	destloc1.y = destloc2.y = 0;
	extra.Subtract (center,loc);
	distnode = extra.GetLengthSquared ();
	rad = destnode->RadiusSquared();

	if (destloc2.x == loc.x)
		slope = 100000.0f;
	else
		slope = (destloc2.z-loc.z)/(destloc2.x-loc.x);
	intercept = destloc2.z-(slope*destloc2.x);

	w = intercept-center.z;
	a = (slope*slope)+1.0f;
	b = (2.0f*slope*w)-(2.0f*center.x);
	c = (w*w)-(rad)+(center.x*center.x);
//		b = (2.0f*slope*intercept)-(2.0f*center.x)-(2.0f*slope*center.z);
//		c = (center.x*center.x)+(intercept*intercept)-(2.0*center.z*intercept)+(center.y*center.y)-(rad);
	inner = (b*b) - (4.0f*a*c);
	if (inner < 0)
	{
		Stuff::Point3D temppt;

		temppt = m_Graph->PullPointElement (loc,destloc2,iter->usage);
		if (temppt != Stuff::Point3D (-1,-1,-1))
		{
			m_Path.push_back (PathElement (temppt,iter->usage,false,iter->link->ID ()));
			Verify (!(CheckCanPass(m_Graph->Passable (temppt.x,temppt.z) , ConvertUsagetoMovePass (iter->usage))));
		}

		temppt = m_Graph->PullPointElement (loc,destloc1,iter->usage);
		if (temppt != Stuff::Point3D (-1,-1,-1))
		{
			m_Path.push_back (PathElement (temppt,iter->usage,false,iter->link->ID ()));
			Verify (!(CheckCanPass(m_Graph->Passable (temppt.x,temppt.z) , ConvertUsagetoMovePass (iter->usage))));
		}

		loc = temppt;
	}
	else
	{
		root = (float) sqrt (inner);
		x1 = (-b+root)/(2*a);
		x2 = (-b-root)/(2*a);

		if (destloc2.x == loc.x)
		{
			t1 = (x1-loc.x)/(0.00001f);
			t2 = (x2-loc.x)/(0.00001f);
		}
		else
		{
			t1 = (x1-loc.x)/(destloc2.x-loc.x);
			t2 = (x2-loc.x)/(destloc2.x-loc.x);
		}
		if ((t1>=0) && (t1<=1.0))
		{
			y= slope*x1 + intercept;
			Stuff::Point3D temppt;

			temppt.x = x1;
			temppt.y = 0;
			temppt.z = y;
			temppt = m_Graph->PullPointElement (loc,temppt,iter->usage);
			if (temppt != Stuff::Point3D (-1,-1,-1))
			{
				m_Path.push_back (PathElement (temppt,iter->usage,false,iter->link->ID ()));
				Verify (!(CheckCanPass(m_Graph->Passable (temppt.x,temppt.z) , ConvertUsagetoMovePass (iter->usage))));
			}
			loc = temppt;
		}
		else if ((t2>=0) && (t2<=1.0))
		{
			y= slope*x2 + intercept;
			Stuff::Point3D temppt;

			temppt.x = x2;
			temppt.y = 0;
			temppt.z = y;
			temppt = m_Graph->PullPointElement (loc,temppt,iter->usage);
			if (temppt != Stuff::Point3D (-1,-1,-1))
			{
				m_Path.push_back (PathElement (temppt,iter->usage,false,iter->link->ID ()));
				Verify (!(CheckCanPass(m_Graph->Passable (temppt.x,temppt.z) , ConvertUsagetoMovePass (iter->usage))));
			}
			loc = temppt;
		}
		else
		{
			Stuff::Point3D temppt;

			temppt = m_Graph->PullPointElement (loc,destloc2,iter->usage);
			if (temppt != Stuff::Point3D (-1,-1,-1))
			{
				m_Path.push_back (PathElement (temppt,iter->usage,false,iter->link->ID ()));
				Verify (!(CheckCanPass(m_Graph->Passable (temppt.x,temppt.z) , ConvertUsagetoMovePass (iter->usage))));
			}

			temppt = m_Graph->PullPointElement (loc,destloc1,iter->usage);
			if (temppt != Stuff::Point3D (-1,-1,-1))
			{
				m_Path.push_back (PathElement (temppt,iter->usage,false,iter->link->ID ()));
				Verify (!(CheckCanPass(m_Graph->Passable (temppt.x,temppt.z) , ConvertUsagetoMovePass (iter->usage))));
			}

			loc = temppt;
		}
	}
}

void CRailPath::AddEndPoint (Stuff::Point3D& loc)
{
	Stuff::Point3D extra,destloc1,destloc2,center;
	Stuff::Point3D d1vec,d2vec,third;
	const OBRect *brid;

	AutoHeap local_heap (g_RailHeap);
	brid = m_Graph->RectList ()->Inside (m_End);
	if (brid)
	{
		Stuff::Point3D temppt,where;
		where.y = 0;
		if (brid->vertical)
		{
			destloc2 = brid->BottomPoint ();
			destloc1 = brid->TopPoint ();
			if (loc.z < destloc1.z)
			{
				where.x = destloc1.x;
				where.z = destloc1.z;
			}
			else
			{
				where.x = destloc2.x;
				where.z = destloc2.z;
			}
			temppt = m_Graph->PullPointElement (loc,where,m_UsageAllowed);
			if (temppt != Stuff::Point3D (-1,-1,-1))
				m_Path.push_back (PathElement (temppt,m_UsageAllowed,false,-1));
		}
		else
		{
			destloc2 = brid->RightPoint ();
			destloc1 = brid->LeftPoint ();
			if (loc.x < destloc1.x)
			{
				where.x = destloc1.x;
				where.z = destloc1.z;
			}
			else
			{
				where.x = destloc2.x;
				where.z = destloc2.z;
			}
			temppt = m_Graph->PullPointElement (loc,where,m_UsageAllowed);
			if (temppt != Stuff::Point3D (-1,-1,-1))
				m_Path.push_back (PathElement (temppt,m_UsageAllowed,false,-1));
		}
		m_Path.push_back (PathElement (m_End,BRIDGEFLAG,true,-1));
	}
	else
	{
		Stuff::Point3D temppt;
		temppt.x = m_End.x;
		temppt.y = 0;
		temppt.z = m_End.z;
		temppt = m_Graph->PullPointElement (loc,temppt,m_UsageAllowed);
		if (temppt != Stuff::Point3D (-1,-1,-1))
			m_Path.push_back (PathElement (temppt,m_UsageAllowed,false,-1));

	}
}

void CRailPath::HandleOneNodePath (void)
{
	Stuff::Point3D loc,extra,destloc1,destloc2,center;
	Stuff::Point3D d1vec,d2vec,third;
	Stuff::Point3D temppt;

	AutoHeap local_heap (g_RailHeap);
	const OBRect *brid1,*brid2;
	brid1 = m_Graph->RectList ()->Inside (m_Start);
	brid2 = m_Graph->RectList ()->Inside (m_End);
	if ((brid1 == brid2) && (brid1 != NULL))					// start and stop on the same bridge
	{
		m_Path.push_back (PathElement (m_Start,BRIDGE_FLAG,true,-1));
		m_Path.push_back (PathElement (m_End,BRIDGE_FLAG,true,-1));
	}
	else if (brid1 != brid2 && (brid1 != NULL) && (brid2 != NULL))		// start and stop on two different bridges
	{
		m_Path.push_back (PathElement (m_Start,BRIDGE_FLAG,true,-1));
		if (brid1->vertical)																						// calc point on edge of bridge to get off
		{
			destloc2 = brid1->BottomPoint ();
			destloc1 = brid1->TopPoint ();
			if (m_End.z < destloc1.z)
				m_Path.push_back (PathElement (destloc1,BRIDGE_FLAG,true,-1));
			else
				m_Path.push_back (PathElement (destloc2,BRIDGE_FLAG,true,-1));
		}
		else
		{
			destloc2 = brid1->RightPoint ();
			destloc1 = brid1->LeftPoint ();
			if (m_End.x < destloc1.x)
				m_Path.push_back (PathElement (destloc1,BRIDGE_FLAG,true,-1));
			else
				m_Path.push_back (PathElement (destloc2,BRIDGE_FLAG,true,-1));
		}

		if (brid2->vertical)																						// calc point on edge of bridge to get onto
		{
			destloc2 = brid2->BottomPoint ();
			destloc1 = brid2->TopPoint ();
			if (m_Start.z < destloc1.z)
				m_Path.push_back (PathElement (destloc1,m_NodePath[0].usage,false,-1));
			else
				m_Path.push_back (PathElement (destloc2,m_NodePath[0].usage,false,-1));
		}
		else
		{
			destloc2 = brid2->RightPoint ();
			destloc1 = brid2->LeftPoint ();
			if (m_Start.x < destloc1.x)
				m_Path.push_back (PathElement (destloc1,m_NodePath[0].usage,false,-1));
			else
				m_Path.push_back (PathElement (destloc2,m_NodePath[0].usage,false,-1));
		}
		m_Path.push_back (PathElement (m_End,BRIDGE_FLAG,true,-1));
	}
	else if ((brid1) && (m_Graph->RectList ()->Collide (m_Start,m_End)))								// start on a bridge and stop off a bridge
	{
		m_Path.push_back (PathElement (m_Start,BRIDGE_FLAG,true,-1));
		if (brid1->vertical)																					// calc point on edge of bridge to get off
		{
			destloc2 = brid1->BottomPoint ();
			destloc1 = brid1->TopPoint ();
			if (m_End.z < destloc1.z)
				m_Path.push_back (PathElement (destloc1,BRIDGE_FLAG,true,-1));
			else
				m_Path.push_back (PathElement (destloc2,BRIDGE_FLAG,true,-1));
		}
		else
		{
			destloc2 = brid1->RightPoint ();
			destloc1 = brid1->LeftPoint ();
			if (m_End.x < destloc1.x)
				m_Path.push_back (PathElement (destloc1,BRIDGE_FLAG,true,-1));
			else
				m_Path.push_back (PathElement (destloc2,BRIDGE_FLAG,true,-1));
		}
		m_Path.push_back (PathElement (m_End,m_NodePath[0].usage,false,-1));
	}
	else if ((brid2) && (m_Graph->RectList ()->Collide (m_Start,m_End)))							// start off a bridge and stop on a bridge
	{
		m_Path.push_back (PathElement (m_Start,m_NodePath[0].usage,false,-1));
		if (brid2->vertical)																					// calc point on edge of bridge to get onto
		{
			destloc2 = brid2->BottomPoint ();
			destloc1 = brid2->TopPoint ();
			if (m_Start.z < destloc1.z)
				m_Path.push_back (PathElement (destloc1,m_NodePath[0].usage,false,-1));
			else
				m_Path.push_back (PathElement (destloc2,m_NodePath[0].usage,false,-1));
		}
		else
		{
			destloc2 = brid2->RightPoint ();
			destloc1 = brid2->LeftPoint ();
			if (m_Start.x < destloc1.x)
				m_Path.push_back (PathElement (destloc1,m_NodePath[0].usage,false,-1));
			else
				m_Path.push_back (PathElement (destloc2,m_NodePath[0].usage,false,-1));
		}
		m_Path.push_back (PathElement (m_End,BRIDGE_FLAG,true,-1));
	}
	else																														// normal one node path, could be under bridge
	{
		m_Path.push_back (PathElement (m_Start,m_NodePath[0].usage,false,-1));

		temppt = m_Graph->PullPointElement (m_Start,m_End,m_NodePath[0].usage);
		if (temppt == Stuff::Point3D (-1,-1,-1))
		{
			m_Calced = false;
			return;
		}
		m_Path.push_back (PathElement (temppt,m_NodePath[0].usage,false,-1));
	}
	m_InPath = m_Path.begin ();
}

void CRailPath::AddJumpPoint (stlport::vector<NodePathElement>::iterator iter,Stuff::Point3D& loc)
{
	Stuff::Point3D destloc;
	CRailNode *destnode,*srcnode;
	CRailLink *destlink;

	AutoHeap local_heap (g_RailHeap);
	destnode = iter->destnode;
	destlink = iter->link;
	srcnode = destlink->OtherLocation (destnode);
	Verify (destnode);
	Verify (destlink);

	destloc = srcnode->Location ();					// move to center of current node to get speed up
	m_Path.push_back (PathElement (destloc,LEGFLAG,false,iter->link->ID ()));

	destloc = srcnode->SubNode (destlink).location;	// move to edge of current node
	m_Path.push_back (PathElement (destloc,LEGFLAG,true,iter->link->ID ()));

	destloc = destnode->SubNode (destlink).location;														// jump to edge of next node
	m_Path.push_back (PathElement (destloc,iter->usage,true,iter->link->ID ()));

}

void CRailPath::ConvertNodeToPath (void)
{
	PATH_LOGIC("Convert Node");
	TIME_FUNCTION(tNodeToPathTime);

	stlport::vector<NodePathElement>::iterator iter;
	Stuff::Point3D loc,extra,destloc1,destloc2,center;
	Stuff::Point3D d1vec,d2vec,third;
	CRailNode *destnode;
	CRailLink *destlink;
	bool startbrid=false;
	Stuff::Scalar distnode;
	Stuff::Scalar rad;

	AutoHeap local_heap (g_RailHeap);
	m_Path.clear ();
	m_InPath = m_Path.begin ();
	if (m_NodePath.size () == 0)
	{
		Verify (false);
		return;
	}
	if (m_NodePath.size () == 1)
	{
		HandleOneNodePath ();
		return;
	}

	iter = m_NodePath.begin ();

	AddStartPoint (loc,startbrid,iter);

	while (iter != m_NodePath.end ())
	{
		destnode = iter->destnode;
		destlink = iter->link;
		Verify (destnode);
		Verify (destlink);

		center = destnode->Location ();
		center.y = 0;
		destloc1.y = destloc2.y = 0;
		extra.Subtract (center,loc);

		distnode = extra.GetLengthSquared ();
		rad = destnode->RadiusSquared();

		const OBRect *brid;
		brid = m_Graph->RectList ()->Collide (destnode->Location (),destlink->OtherLocation (destnode)->Location ());
		if (brid)
		{
			AddBridgeToPath (iter,loc,brid,startbrid);
		}
		else if (iter->usage & JUMPFLAG)
		{
			AddJumpPoint (iter,loc);
		}
		else if (distnode > rad)		// if not already in the next node
		{
			AddPointToPath (iter,loc);
		}

		startbrid = false;
		iter++;
	}

	AddEndPoint (loc);

	m_InPath = m_Path.begin ();
}