#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 <adept\collisionvolume.hpp>
#include "path.hpp"
#include "bridge.hpp"

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

using namespace MW4AI;
using namespace MechWarrior4;
using namespace ElementRenderer;


void CRailLink::DestroyBridge (void)
{
	m_UsageFlags[0] = JUMPFLAG;
	m_UsageFlags[1] = JUMPFLAG;
}

bool CRailGraph::LinkWeight (CRailLink *link)
{
	Stuff::Scalar dist;
	float weight[NUM_MOVE_TYPES][2];
	int k;
	CRailNode *src,*dest;

	src = link->Location (0);
	dest = link->Location (1);
	dist = src->LinkLength (link);
	if (dist<0)
	{
		dist = dist * -1.0f;
		weight[0][0] = -1.0f;							// turn off jumping for these types of nodes
		weight[0][1] = -1.0f;
		for (k=1;k<NUM_MOVE_TYPES;k++)
		{
			weight[k][0] = dist;
			weight[k][1] = dist;
		}
	}
	else if (dist > GRIDMOVE_FAIL_THRESHOLD)
	{
		DeleteLink (link->ID ());
		return false;
	}
	else
	{
		for (k=0;k<NUM_MOVE_TYPES;k++)
		{
			weight[k][0] = m_WorkGrid->CalcWeight (src->SubNode (link).location,dest->SubNode (link).location,k);
			if (weight[k][0] != -1.0f)
			{
				weight[k][0] += src->Radius ();
				weight[k][0] += dest->Radius ();
			}
			weight[k][1] = m_WorkGrid->CalcWeight (dest->SubNode (link).location,src->SubNode (link).location,k);
			if (weight[k][1] != -1.0f)
			{
				weight[k][1] += src->Radius ();
				weight[k][1] += dest->Radius ();
			}
		}
	}

	for (k=0;k<NUM_MOVE_TYPES;k++)			
		if ((weight[k][0] != -1.0f) || (weight[k][1] != -1.0f))
			break;
	if (k == NUM_MOVE_TYPES)					// path is impassable for all move types
	{
		DeleteLink (link->ID ());
		return false;
	}
	for (k=0;k<NUM_MOVE_TYPES;k++)
	{
		link->Weight (weight[k][0],k,link->Location (0),true);
		link->Weight (weight[k][1],k,link->Location (1),true);
	}
	return true;
}

void CRailGraph::CalcLinks (int *start,int numnodes,int min,int max,int range,WorkCallBack callback)
{
	int i,j,k,size,totallinks;
	Stuff::Point3D loc;
	float curpercent,dpercent;

	CreateGrid (callback,80.0f);
	Check_Pointer (m_WorkGrid);
	CalcEditorDistances ();

	UpdateNodes (callback,80);

	CreateRectData (callback);

	my_heap<NodeDist,true,raildistcmp> distheap;

	totallinks = 0;
	int count=0;
	size = numnodes;
	curpercent = 90.0f;												
	dpercent = 10.0f / size;
	totallinks += m_Links.size ();

	for (i=0;i<(size-1);i++)
	{
		curpercent += dpercent;
		if (callback)
			callback (curpercent);

		if (start[i] == -1)
			continue;

		if (!m_Nodes[start[i]])
			continue;
		distheap.clear ();
		for (j=i+1;j<size;j++)
		{
			if (start[j] == -1)
				continue;
			if (m_Nodes[start[j]])
				distheap.Insert (NodeDist (m_Nodes[start[i]],m_Nodes[start[j]]));
		}
		count = m_Nodes[start[i]]->NumLinks ();
		while ((count < max) && (totallinks <MAXLINKS))
		{
			if (!distheap.size ())
				break;
			NodeDist tempdist = distheap.ExtractMin ();
			float dist;

			Verify (tempdist.src == m_Nodes[start[i]]);
			dist = tempdist.src->EditorDistance (tempdist.dest);
			if (dist > range)
			{
				distheap.Insert (tempdist);
				break;
			}

			float weight[NUM_MOVE_TYPES][2];
			for (k=0;k<NUM_MOVE_TYPES;k++)
			{
				weight[k][0] = 0.0f;
				weight[k][1] = 0.0f;
			}

			int linkid = AddLink (tempdist.src,tempdist.dest,weight);
			CRailLink *link;
			link = Link (linkid);

			LinkWeight (link);

			count++;
			totallinks++;
		}
		count = m_Nodes[start[i]]->NumLinks ();
		while ((count < min) && (totallinks <MAXLINKS))
		{
			if (!distheap.size ())
				break;

			NodeDist tempdist = distheap.ExtractMin ();

			Verify (tempdist.src == m_Nodes[start[i]]);

			float weight[NUM_MOVE_TYPES][2];
			for (k=0;k<NUM_MOVE_TYPES;k++)
			{
				weight[k][0] = 0.0f;
				weight[k][1] = 0.0f;
			}
			
			int linkid = AddLink (tempdist.src,tempdist.dest,weight);
			CRailLink *link;
			link = Link (linkid);

			LinkWeight (link);

			count++;
			totallinks++;
		}
	}



	size = m_BlockedLinks.size ();
	int numlink;
	numlink = m_Links.size ();
	for (i=0;i<size;i++)
	{
		if (m_BlockedLinks[i] != Stuff::Point3D (-1,-1,-1))
		{
			for (j=0;j<numlink;j++)
			{
				if (!m_Links[j])
					continue;
				if ((m_Links[j]->Location(0)->ID () == m_BlockedLinks[i].x) && (m_Links[j]->Location(1)->ID () == m_BlockedLinks[i].y))
				{
					DeleteLink (j);
					continue;
				}
				if ((m_Links[j]->Location(0)->ID () == m_BlockedLinks[i].y) && (m_Links[j]->Location(1)->ID () == m_BlockedLinks[i].x))
				{
					DeleteLink (j);
					continue;
				}
			}
		}
	}

//	AddBridgeLinks ();
	AnalyzePaths ();
	KillGrid ();
}

void CRailGraph::AddBridgeLinks (void)
{
	int i,j,k,max;

	Verify (m_WorkGrid);
	max = m_Nodes.size ();
	for (i=0;i<max-1;i++)
	{
		if (!m_Nodes[i])
			continue;

		for (j=i+1;j<max;j++)
		{
			if (!m_Nodes[j])
				continue;
/*
			int numlinks;
			numlinks = m_Nodes[j]->NumLinks ();

			for (k=0;k<numlinks;k++)
			{
				CRailLink *link;
				link = m_Nodes[j]->Link (k);
				if (link)
					if (link->OtherLocation (m_Nodes[j]) == m_Nodes[i])
						continue;
			}
*/
			if (!m_Nodes[i]->LinkTo (m_Nodes[j]))
			{
				float weight[NUM_MOVE_TYPES][2];
				for (k=0;k<NUM_MOVE_TYPES;k++)
				{
					weight[k][0] = 0.0f;
					weight[k][1] = 0.0f;
				}

				CRailLink *link;
				int linkid = AddLink (i,j,weight);
				link = Link (linkid);

				int bridgeid;
				bridgeid = m_WorkGrid->CalcBridgeLink (m_Nodes[i]->SubNode (link).location,m_Nodes[j]->SubNode (link).location);
				if (bridgeid == -1)
				{
					DeleteLink (linkid);
					continue;
				}

				if (!TestBridge (bridgeid,i,j))			// test for the only two nodes that can go over bridge
				{
					DeleteLink (linkid);
					continue;
				}

				Scalar dist;
				dist = m_Nodes[i]->LinkLength (link);
				Verify (dist > 0);			// should not be any overlap in the nodes because a bridge is a building
				for (k=0;k<NUM_MOVE_TYPES;k++)
				{
					weight[k][0] = dist;
					weight[k][0] += m_Nodes[i]->Radius ();
					weight[k][0] += m_Nodes[j]->Radius ();
					weight[k][1] = weight[k][0];
				}
				for (k=0;k<NUM_MOVE_TYPES;k++)
				{
					link->Weight (weight[k][0],k,link->Location (0),true);
					link->Weight (weight[k][1],k,link->Location (1),true);
				}
				link->Bridge (bridgeid);
			}
		}
	}
}

bool CRailGraph::TestBridge (int id,int node1,int node2)
{
	Point3D bridgeloc;
	Point3D node1loc,node2loc;
	int max,i;
	Point3D destloc2,center,loc,extra;
	Scalar rad,slope,w,t1,t2,a,b,c;
	Scalar intercept,inner,root,x1,x2;

	node1loc = m_Nodes[node1]->Location ();
	node2loc = m_Nodes[node2]->Location ();
	Entity* ent = NameTable::GetInstance()->FindData(id);
	Verify (ent);
	bridgeloc = (Point3D) ent->GetLocalToWorld ();

	max = m_Nodes.size ();
	for (i=0;i<max;i++)
	{
		if (i == node1)
			continue;
		if (i == node2)
			continue;
		if (!m_Nodes[i])
			continue;
		
		destloc2 = bridgeloc;
		loc = m_Nodes[node1]->Location ();
		loc.y = 0;
		center = m_Nodes[i]->Location ();
		center.y = 0;
		destloc2.y = 0;
		rad = m_Nodes[i]->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);
		inner = (b*b) - (4.0f*a*c);
		if (inner < 0)
		{
			// ok because they never cross, should never happen
		}
		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))
			{
				// intersect
				return false;
			}
			else if ((t2>=0) && (t2<=1.0))
			{
				// intersect
				return false;
			}
			else
			{
				// don't intersect on line segment
			}
		}
		loc = m_Nodes[node2]->Location ();
		destloc2 = bridgeloc;
		center = m_Nodes[i]->Location ();
		center.y = 0;
		loc.y = destloc2.y = 0;
		rad = m_Nodes[i]->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);
		inner = (b*b) - (4.0f*a*c);
		if (inner < 0)
		{
			// ok because they never cross, should never happen
		}
		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))
			{
				// intersect
				return false;
			}
			else if ((t2>=0) && (t2<=1.0))
			{
				// intersect
				return false;
			}
			else
			{
				// don't intersect on line segment
			}
		}
	}
	return true;
}

int CRailWorkGrid::CalcBridgeLink (Point3D src, Point3D dest)
{
	Stuff::Point3D extra;
	Stuff::Scalar sx,sy,dx,dy;
	int ix,iy,ex,ey;
	int toret;
	Stuff::Scalar curweight;
	Stuff::Scalar lastheight,curheight,dh;
	int count;

	dest.x -= MinX;
	dest.z -= MinZ;
	src.x -= MinX;
	src.z -= MinZ;

	extra.Subtract (dest,src);

	toret = -1;
	dest.x /= 10.0f;
	dest.z /= 10.0f;
	src.x /= 10.0f;
	src.z /= 10.0f;

	dx = dest.x - src.x;
	dy = dest.z - src.z;
	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 = src.x;
	sy = src.z;
	ex = (int) dest.x;
	ey = (int) dest.z;
	ix = (int) sx;
	iy = (int) sy;
	lastheight = m_Grid[ix][iy].height;
	curweight = 0;
	while (count)
	{
		count--;
		sx += dx;
		sy += dy;
		ix = (int) sx;
		iy = (int) sy;

		if (m_Grid[ix][iy].objectid != m_Grid[ix][iy].bridge)
			return -1;

		dh = 0;
		if (m_Grid[ix][iy].bridge != -1)
		{
			if (toret == -1)
				toret = m_Grid[ix][iy].bridge;
			else if (m_Grid[ix][iy].bridge != toret)
				return -1;
		}
		else
		{
			curheight = m_Grid[ix][iy].height;
			dh = lastheight - curheight;
			dh = dh < 0 ? dh*-1.0f : dh;
			dh /= 10.0f;
			if (dh >= 0.5f)
				return -1;
		}
		lastheight = m_Grid[ix][iy].height;


		curweight += (1.0f + dh);
//		if (CheckSquare (ix,iy,WHEELINDEX) == -1)
//			return -1;
//		curweight += tempweight;
	}

	return toret;
}

void CRailGraph::CalcLinks (int min,int max,int range,WorkCallBack callback)
{
	int i,size;
	int *temp;
	bool flag;

	ClearLinks ();

	flag = false;
	gos_PushCurrentHeap (g_RailHeap);
	temp = new int[m_Nodes.size ()];
	gos_PopCurrentHeap ();
	size = m_Nodes.size ();
	for (i=0;i<size;i++)
	{
		if (m_Nodes[i])
		{
			temp[i] = m_Nodes[i]->ID ();
			flag = true;
		}
		else
			temp[i]	= -1;
	}

	if (!flag)
		return;
	CalcLinks (temp,size,min,max,range,callback);
}

void CRailGraph::BlockLink (int node1,int node2)
{
	int size,i;

	size = m_BlockedLinks.size ();
	for (i=0;i<size;i++)
	{
		if (m_BlockedLinks[i] == Stuff::Point3D (-1,-1,-1))
			break;
	}
	m_BlockedLinks[i] = Stuff::Point3D ((float) node1,(float) node2,0.0f);
}

void CRailGraph::RemoveBlock (int node)
{
	int i,size;

	size = m_BlockedLinks.size ();
	for (i=0;i<size;i++)
	{
		if (m_BlockedLinks[i].x == node)
			m_BlockedLinks[i] = Stuff::Point3D (-1,-1,-1);
		if (m_BlockedLinks[i].y == node)
			m_BlockedLinks[i] = Stuff::Point3D (-1,-1,-1);
	}
}
void CRailLink::ConstructLinkObjectStream(int id,Stuff::MemoryStream *stream,Stuff::NotationFile *model_file)
{
	char entryname[25],notename[25];
	int id1,id2;
	float weight[NUM_MOVE_TYPES][2];
	float calcweight[NUM_MOVE_TYPES][2];
	float allweight;
	bool jump;
	bool Tracked;
	bool Flyer;
	bool Wheeled;
	bool Legged;
	bool Hover;
	int bridge;
	bool water;
	int moveflags[2];


	Verify (id>=0);
	Verify (id<MAXLINKS);
	sprintf (entryname,"Link%d",id);
	id1 = -1;
	id2 = -1;

	Page *link_page = model_file->FindPage(entryname);
	if (link_page)
	{
		Check_Object(link_page);
		link_page->GetEntry("Node1",&id1,false);
		link_page->GetEntry("Node2",&id2,false);
	}
	Verify (id1>=-1);
	Verify (id2>=-1);

	if ((id1==-1) || (id2 == -1))
	{
		*stream << id1;
		*stream << id2;
		return;
	}

	Check_Object(link_page);
	int i;
	if (link_page->GetEntry ("Weight",&allweight,false))
		for (i=0;i<NUM_MOVE_TYPES;i++)
		{
			weight[i][0] = allweight;
			weight[i][1] = allweight;
			calcweight[i][0] = allweight;
			calcweight[i][1] = allweight;
		}
	else
		for (i=0;i<NUM_MOVE_TYPES;i++)
		{
			weight[i][0] = 1.0f;
			weight[i][1] = 1.0f;
			calcweight[i][0] = 1.0f;
			calcweight[i][1] = 1.0f;
		}
	for (i=0;i<NUM_MOVE_TYPES;i++)
	{
		sprintf (notename,"Weight%d",i);
		if (link_page->GetEntry(notename,&(weight[i][0]),false))
		{
			weight[i][1] = weight[i][0];
		}
		else
		{
			sprintf (notename,"Weight%dd0",i);
			link_page->GetEntry(notename,&(weight[i][0]),false);
			sprintf (notename,"Weight%dd1",i);
			link_page->GetEntry(notename,&(weight[i][1]),false);
		}
		Verify (weight[i][0] >= -1.0f);
		Verify (weight[i][1] >= -1.0f);
	}
	for (i=0;i<NUM_MOVE_TYPES;i++)
	{
		sprintf (notename,"CalcWeight%d",i);
		if (link_page->GetEntry(notename,&(calcweight[i][0]),false))
		{
			calcweight[i][1] = calcweight[i][0];
		}
		else
		{
			sprintf (notename,"CalcWeight%dd0",i);
			link_page->GetEntry(notename,&(calcweight[i][0]),false);
			sprintf (notename,"CalcWeight%dd1",i);
			link_page->GetEntry(notename,&(calcweight[i][1]),false);
		}
		Verify (calcweight[i][0] >= -1.0f);
		Verify (calcweight[i][1] >= -1.0f);
	}

	moveflags[0] = 0;
	if (link_page->GetEntry("UsageFlags",&moveflags[0],false))
	{
		moveflags[1] = moveflags[0];
	}
	else if (link_page->GetEntry ("UsageFlags0",&moveflags[0],false))
	{
		link_page->GetEntry ("UsageFlags1",&moveflags[1],false);
	}
	else
	{
		Tracked = false;
		Flyer = false;
		Wheeled = false;
		Legged = true;
		Hover = false;
		jump = false;
		water = false;
		link_page->GetEntry("Jump",&jump,false);
		link_page->GetEntry("Tracked",&Tracked,false);
		link_page->GetEntry("Flyer",&Flyer,false);
		link_page->GetEntry("Wheeled",&Wheeled,false);
		link_page->GetEntry("Legged",&Legged,false);
		link_page->GetEntry("Hover",&Hover,false);
		link_page->GetEntry("Water",&water,false);
		moveflags[0] |= jump ? JUMPFLAG : 0;
		moveflags[0] |= Tracked ? TRACKFLAG : 0;
		moveflags[0] |= Flyer ? FLYFLAG : 0;
		moveflags[0] |= Wheeled ? WHEELFLAG : 0;
		moveflags[0] |= Legged ? LEGFLAG : 0;
		moveflags[0] |= Hover ? HOVERFLAG : 0;
		moveflags[0] |= water ? WATERFLAG : 0;
		moveflags[1] = moveflags[0];
	}
	bridge = -1;
	link_page->GetEntry("Bridge",&bridge,false);

	*stream << id1;
	*stream << id2;
	for (i=0;i<NUM_MOVE_TYPES;i++)
	{
		*stream << weight[i][0];
		*stream << weight[i][1];
	}
	for (i=0;i<NUM_MOVE_TYPES;i++)
	{
		*stream << calcweight[i][0];
		*stream << calcweight[i][1];
	}
	*stream << moveflags[0];
	*stream << moveflags[1];
	*stream << bridge;
}

void CRailLink::DumpNotationFile (Stuff::NotationFile *file)
{
	char entryname[25],notename[25];
	int i;

	sprintf (entryname,"Link%d",m_ID);
	Page *link_page = file->SetPage(entryname);
	Check_Object(link_page);
	Verify (m_Loc[0]);
	Verify (m_Loc[1]);
	link_page->SetEntry ("Node1",(int) m_Loc[0]->ID ());
	link_page->SetEntry ("Node2",(int) m_Loc[1]->ID ());
#if 0
	if (m_Loc[0] != 0)
	{
		link_page->SetEntry ("Node1",(int) m_Loc[0]->ID ());
	}
	if (m_Loc[1] != 0)
	{
		link_page->SetEntry ("Node2",(int) m_Loc[1]->ID ());
	}
#endif
	for (i=0;i<NUM_MOVE_TYPES;i++)
	{
		sprintf (notename,"Weight%dd0",i,0);
		link_page->SetEntry (notename,m_Weight[i][0]);
		sprintf (notename,"Weight%dd1",i,1);
		link_page->SetEntry (notename,m_Weight[i][1]);
	}
	for (i=0;i<NUM_MOVE_TYPES;i++)
	{
		sprintf (notename,"CalcWeight%dd0",i);
		link_page->SetEntry (notename,m_CalcedWeight[i][0]);
		sprintf (notename,"CalcWeight%dd1",i);
		link_page->SetEntry (notename,m_CalcedWeight[i][1]);
	}
	link_page->SetEntry ("UsageFlags0",m_UsageFlags[0]);
	link_page->SetEntry ("UsageFlags1",m_UsageFlags[1]);
	link_page->SetEntry ("Bridge",m_Bridge);
}

CRailLink::CRailLink (Stuff::MemoryStream *stream,CRailGraph *parent)
{
	int t1,t2;
	int i;

	Check_Pointer(this);
	Check_Object(stream);
	Check_Pointer (parent);
	*stream >> t1;
	*stream >> t2;
	m_LocID[0] = t1;
	m_LocID[1] = t2;
	if ((t1 == -1) || (t2 == -1))
		return;
	m_Loc[0] = parent->Node (t1);
	m_Loc[1] = parent->Node (t2);
	for (i=0;i<NUM_MOVE_TYPES;i++)
	{
		*stream >> m_Weight[i][0];
		*stream >> m_Weight[i][1];
	}
	for (i=0;i<NUM_MOVE_TYPES;i++)
	{
		*stream >> m_CalcedWeight[i][0];
		*stream >> m_CalcedWeight[i][1];
	}
	*stream >> m_UsageFlags[0];
	*stream >> m_UsageFlags[1];
	*stream >> m_Bridge;
}