# Rbot Navigation Cog - Alpha 0.32
#
# Created by Raynar

flags=0x280

symbols

message		startup
message		trigger

# Pathnodes
thing		node0=-1
thing		node1=-1
thing		node2=-1
thing		node3=-1
thing		node4=-1
thing		node5=-1
thing		node6=-1
thing		node7=-1
thing		node8=-1
thing		node9=-1
thing		node10=-1
thing		node11=-1
thing		node12=-1
thing		node13=-1
thing		node14=-1
thing		node15=-1
thing		node16=-1
thing		node17=-1
thing		node18=-1
thing		node19=-1
thing		node20=-1
thing		node21=-1
thing		node22=-1
thing		node23=-1
thing		node24=-1
thing		node25=-1
thing		node26=-1
thing		node27=-1
thing		node28=-1
thing		node29=-1
thing		node30=-1
thing		node31=-1
thing		node32=-1
thing		node33=-1
thing		node34=-1
thing		node35=-1
thing		node36=-1
thing		node37=-1
thing		node38=-1
thing		node39=-1
thing		node40=-1
thing		node41=-1
thing		node42=-1
thing		node43=-1
thing		node44=-1
thing		node45=-1
thing		node46=-1
thing		node47=-1
thing		node48=-1
thing		node49=-1
thing		node50=-1
thing		node51=-1
thing		node52=-1
thing		node53=-1
thing		node54=-1
thing		node55=-1
thing		node56=-1
thing		node57=-1
thing		node58=-1
thing		node59=-1
thing		node60=-1
thing		node61=-1
thing		node62=-1
thing		node63=-1
thing		node64=-1
thing		node65=-1
thing		node66=-1
thing		node67=-1
thing		node68=-1
thing		node69=-1
thing		node70=-1
thing		node71=-1
thing		node72=-1
thing		node73=-1
thing		node74=-1
thing		node75=-1
thing		node76=-1
thing		node77=-1
thing		node78=-1
thing		node79=-1
thing		node80=-1
thing		node81=-1
thing		node82=-1
thing		node83=-1
thing		node84=-1
thing		node85=-1
thing		node86=-1
thing		node87=-1
thing		node88=-1
thing		node89=-1
thing		node90=-1
thing		node91=-1
thing		node92=-1
thing		node93=-1
thing		node94=-1
thing		node95=-1
thing		node96=-1
thing		node97=-1
thing		node98=-1
thing		node99=-1

# Path info
int		pathsize=10
int		numpaths=10
int		inf0=0x0
int		inf1=0x0
int		inf2=0x0
int		inf3=0x0
int		inf4=0x0
int		inf5=0x0
int		inf6=0x0
int		inf7=0x0
int		inf8=0x0
int		inf9=0x0

# Max nodes available per path
int		max0		local
int		max1		local
int		max2		local
int		max3		local
int		max4		local
int		max5		local
int		max6		local
int		max7		local
int		max8		local
int		max9		local

# Misc vars
flex	mindist		local
int		index		local
int		offset		local
int		index1		local
int		offset1		local
int		index2		local
int		offset2		local
int		busy=0		local
int		botid		local
thing	rbot		local
vector	botpos		local
int		botmode		local
int		lastposnum	local
int		nextposnum	local
thing	nextpos		local
thing	item		local
int		curpath		local
int		changepath	local
int		rundir		local
int		special		local
int		newnode		local
int		newpath		local
int		maxoffset	local
int		captured_red	local
int		captured_gold	local
int		redflag_taken	local
int		goldflag_taken	local
vector	tempvector1		local
vector	tempvector2		local
int		curred_priority		local
int		newred_priority		local
int		curgold_priority	local
int		newgold_priority	local
int		debug=0		local

end

# Code Section

code

startup:
	if(!IsServer()) return;
	// work out max number of positions per path
	for (index=0;index<numpaths;index=index+1)
	{
		max0[index]=pathsize-1;
		for (offset=pathsize-1;offset>=0;offset=offset-1)
		{
			if (node0[index*pathsize+offset]==-1) max0[index]=offset-1;
		}
	}
	return;

trigger:
	if(!IsServer()) return;
	if (getsourceref() == 12210) // updated CTF info
	{
		captured_red=GetParam(0);
		captured_gold=GetParam(1);
		redflag_taken=GetParam(2);
		goldflag_taken=GetParam(3);
	}
	if (getsourceref() == 10000) // bot wants to know next location
	{
		if (busy)
		{
			SendTrigger(-1, 10010, getparam(0), -1, -1, -1);
			return;
		}
		busy=1;
		botid=getparam(0);   // which bot requested the info
		rbot=getparam(1);
		botpos=getthingpos(getparam(1));  // current position of bot
		botmode=getparam(2); // botmode
		lastposnum=getparam(3);

		if (BitTest(botmode,0x1000)) rundir=0; else rundir=1; // set run direction from botmode

		if (lastposnum == -1) // bot lost - find nearest node
		{
		//print("bot lost!");
			tempvector1=VectorSet('0','0',VectorZ(GetThingPos(rbot)));

			mindist=100; // default min distance
			for (index=1;index<100;index=index+1)
			{
				tempvector2=VectorSet('0','0',VectorZ(GetThingPos(node0[index])));

				if ((VectorDist(botpos, GetThingPos(node0[index])) < mindist) && (HasLOS(rbot, node0[index]) == 1) && (node0[index] != -1) && (VectorDist(tempvector1,tempvector2) < 0.4)) // if node is closer than min distance and bot can see it and the node is not undefined and node is not too high to reach
				//if (VectorDist(botpos, GetThingPos(node0[index])) < mindist)
				{
					mindist=VectorDist(botpos, GetThingPos(node0[index]));
					nextposnum=index;
				}
			}
		}
		else
		{
			// work out current path
			for (index=numpaths;index>0;index=index-1)
			{
				if (lastposnum < (pathsize*index)) curpath=index-1;
			}

			// CTF only - work out current path priority
			//curred_priority=4; // lowest priority
			//curgold_priority=4; // lowest priority
			//if (BitTest(inf0[curpath],0x1) || BitTest(inf0[curpath],0x2)) curred_priority=1;
			//if (BitTest(inf0[curpath],0x4) || BitTest(inf0[curpath],0x8)) curgold_priority=1;
			//if (BitTest(inf0[curpath],0x10) || BitTest(inf0[curpath],0x20)) curred_priority=2;
			//if (BitTest(inf0[curpath],0x40) || BitTest(inf0[curpath],0x80)) curgold_priority=2;
			//if (BitTest(inf0[curpath],0x100) || BitTest(inf0[curpath],0x200)) curred_priority=3;
			//if (BitTest(inf0[curpath],0x400) || BitTest(inf0[curpath],0x800)) curgold_priority=3;

			// Check for intersections - add: if bot's team flag is taken or dropped -> randomly change paths to look for it
			changepath=0;
			for (newpath=0;newpath<numpaths;newpath=newpath+1)
			{
				for (offset=0;offset<=max0[newpath];offset=offset+1)
				{
					newnode=newpath*pathsize+offset;
					if (node0[lastposnum] == node0[newnode] && lastposnum != newnode) // node 'thing' is the same
					{
						if (lastposnum >= (curpath*pathsize+max0[curpath]) && rundir==1) changepath=1; // at end of path and running forward - change paths
						if (lastposnum <= (curpath*pathsize) && rundir==0) changepath=1; // at start of path and running backwards - change paths
						if (rand() > 0.5) changepath=1; // randomly change path

						// CTF only - work out new path priority
						//newred_priority=4; // lowest priority
						//newgold_priority=4; // lowest priority
						//if (BitTest(inf0[newpath],0x1) || BitTest(inf0[newpath],0x2)) newred_priority=1;
						//if (BitTest(inf0[newpath],0x4) || BitTest(inf0[newpath],0x8)) newgold_priority=1;
						//if (BitTest(inf0[newpath],0x10) || BitTest(inf0[newpath],0x20)) newred_priority=2;
						//if (BitTest(inf0[newpath],0x40) || BitTest(inf0[newpath],0x80)) newgold_priority=2;
						//if (BitTest(inf0[newpath],0x100) || BitTest(inf0[newpath],0x200)) newred_priority=3;
						//if (BitTest(inf0[newpath],0x400) || BitTest(inf0[newpath],0x800)) newgold_priority=3;
						//if (GetThingUserData(rbot) == 1) // red team
						//{
						//	if (captured_gold == 2 && rbot == goldflag_taken) // gold flag taken by this bot
						//	{
						//		if (captured_red == 0) // red flag at red base - return to red base with gold flag
						//		{
						//			if (newred_priority < curred_priority) changepath=1;
						//		}
						//	}
						//	else if (captured_gold == 0) // gold flag at gold base - move to gold base
						//	{
						//		if (newgold_priority < curgold_priority) changepath=1;
						//	}
						//	if (captured_red != 0 && rand() > 0.4) changepath=1; // our flag is missing - go look for it
						//}
						//else if (GetThingUserData(rbot) == 2) // gold team
						//{
						//	if (captured_red == 2 && rbot == redflag_taken) // red flag taken by this bot
						//	{
						//		if (captured_gold == 0) // gold flag at gold base - return to gold base with red flag
						//		{
						//			if (newgold_priority < curgold_priority) changepath=1;
						//		}
						//	}
						//	else if (captured_red == 0) // red flag at red base - move to red base
						//	{
						//		if (newred_priority < curred_priority) changepath=1;
						//	}
						//	if (captured_gold != 0 && rand() > 0.4) changepath=1; // our flag is missing - go look for it
						//}

						if (changepath)
						{
							// Intersection debug info
							//jkstringclear();
							//jkStringConcatAsciiString("changing paths - lastposnum: ");
							//jkStringConcatInt(lastposnum);  
							//jkStringConcatAsciiString(": newnode: ");
							//jkStringConcatInt(newnode);  
							//jkStringConcatAsciiString(" curpath: ");
							//jkStringConcatInt(curpath);
							//jkStringConcatAsciiString(" newpath: ");
							//jkStringConcatInt(newpath);
							//jkStringOutput();

							lastposnum=newnode; // change path
							curpath=newpath;
							offset=101; // quit checking
							newpath=numpaths; // quit checking
						}
					}
				}
			}

			// CTF only - Set run direction
			//if (GetThingUserData(rbot) == 1) // red team
			//{
			//	if (captured_gold == 2 && rbot == goldflag_taken) // gold flag taken by this bot
			//	{
			//		if (captured_red == 0) // red flag at red base - return to red base with gold flag
			//		{
			//			if (BitTest(inf0[curpath],0x1) || BitTest(inf0[curpath],0x10) || BitTest(inf0[curpath],0x100)) rundir=1; // run forward
			//			if (BitTest(inf0[curpath],0x2) || BitTest(inf0[curpath],0x20) || BitTest(inf0[curpath],0x200)) rundir=0; // run backward
			//		}
			//	}
			//	else if (captured_gold == 0) // gold flag at gold base - move to gold base
			//	{
			//		if (BitTest(inf0[curpath],0x4) || BitTest(inf0[curpath],0x40) || BitTest(inf0[curpath],0x400)) rundir=1; // run forward
			//		if (BitTest(inf0[curpath],0x8) || BitTest(inf0[curpath],0x80) || BitTest(inf0[curpath],0x800)) rundir=0; // run backward
			//	}
			//}
			//else if (GetThingUserData(rbot) == 2) // gold team
			//{
			//	if (captured_red == 2 && rbot == redflag_taken) // red flag taken by this bot
			//	{
			//		if (captured_gold == 0) // gold flag at gold base - return to gold base with red flag
			//		{
			//			if (BitTest(inf0[curpath],0x4) || BitTest(inf0[curpath],0x40) || BitTest(inf0[curpath],0x400)) rundir=1; // run forward
			//			if (BitTest(inf0[curpath],0x8) || BitTest(inf0[curpath],0x80) || BitTest(inf0[curpath],0x800)) rundir=0; // run backward
			//		}
			//	}
			//	else if (captured_red == 0) // red flag at red base - move to red base
			//	{
			//		if (BitTest(inf0[curpath],0x1) || BitTest(inf0[curpath],0x10) || BitTest(inf0[curpath],0x100)) rundir=1; // run forward
			//		if (BitTest(inf0[curpath],0x2) || BitTest(inf0[curpath],0x20) || BitTest(inf0[curpath],0x200)) rundir=0; // run backward
			//	}
			//}

			if (BitTest(inf0[curpath],0x1000)) rundir=1;

			if (lastposnum >= (curpath*pathsize+max0[curpath])) // at end of path - reverse direction
			{
				rundir=0;
			}
			if (lastposnum <= (curpath*pathsize))  // at start of path - reverse direction
			{
				rundir=1;
			}

			if (rundir) // move forward along path
			{
				nextposnum=lastposnum+1;
			}
			else // move backward along path
			{
				nextposnum=lastposnum-1;
			}
		}

		// Path debug info
		//jkstringclear();
		//jkStringConcatAsciiString("botid: ");
		//jkStringConcatInt(botid);  
		//jkStringConcatAsciiString(": last: ");
		//jkStringConcatInt(lastposnum);  
		//jkStringConcatAsciiString(" next: ");
		//jkStringConcatInt(nextposnum);
		//jkStringConcatAsciiString(" curpath: ");
		//jkStringConcatInt(curpath);
		//jkStringConcatAsciiString(" rundir: ");
		//jkStringConcatInt(rundir);
		//jkStringConcatAsciiString(" max0[curpath]: ");
		//jkStringConcatInt(max0[curpath]);
		//jkStringOutput();

		// CTF only - debug info
		//jkstringclear();
		//jkStringConcatAsciiString("pathinfo:");
		//if (BitTest(inf0[curpath],0x1)) jkStringConcatAsciiString(" red-fwd-1");
		//if (BitTest(inf0[curpath],0x2)) jkStringConcatAsciiString(" red-rev-1");
		//if (BitTest(inf0[curpath],0x4)) jkStringConcatAsciiString(" gold-fwd-1");
		//if (BitTest(inf0[curpath],0x8)) jkStringConcatAsciiString(" gold-rev-1");
		//if (BitTest(inf0[curpath],0x10)) jkStringConcatAsciiString(" red-fwd-2");
		//if (BitTest(inf0[curpath],0x20)) jkStringConcatAsciiString(" red-rev-2");
		//if (BitTest(inf0[curpath],0x40)) jkStringConcatAsciiString(" gold-fwd-2");
		//if (BitTest(inf0[curpath],0x80)) jkStringConcatAsciiString(" gold-rev-2");
		//if (BitTest(inf0[curpath],0x100)) jkStringConcatAsciiString(" red-fwd-3");
		//if (BitTest(inf0[curpath],0x200)) jkStringConcatAsciiString(" red-rev-3");
		//if (BitTest(inf0[curpath],0x400)) jkStringConcatAsciiString(" gold-fwd-3");
		//if (BitTest(inf0[curpath],0x800)) jkStringConcatAsciiString(" gold-rev-3");
		//jkStringOutput();

		// send info to bot
		nextpos=node0[nextposnum];
		SendTrigger(-1, 11000, botid, nextpos, nextposnum, rundir);
		busy=0;
	}
	return;

end
