/**
 * A simple coach for the CIIPS Glory soccer team.
 *
 * Only sends and receives messages to and from the robots.
 *
 */

#include "eyebot.h"
#include <string.h>

void LCDClearLine(int i)
{
	if(i>0 && i<8)
	{
		LCDSetPos(i,0);
		LCDPrintf("               ");
	}
}

void print_menu(char* cmd,int mes_num, char* msg)
{
	LCDSetPos(0,0);
	LCDPutString("--Glory Coach--\n");
	LCDClearLine(1);
	LCDSetPos(1,0);
	LCDPrintf("%s",cmd);
	LCDClearLine(2);
	LCDSetPos(2,0);
	LCDPrintf("%d:%s\n",mes_num, msg);
	LCDSetPos(3,0);
	LCDPrintf("---------------");
}

void print_inmessage(int mes_num, int from, char* msg)
{
	LCDSetPos(6,0);
	LCDPrintf("\n%d:%d %s",mes_num, from, msg);
}

int main()
{
	BYTE myId, nextId, fromId;
	char msg_in[16]; /* message buffer */
	char msg_out[16]; /* message buffer */
	int  len, err;
	int key;
	int msg_num;
	int msg_len;
	int cursor;
	int menu_i;
	int menu_i_max;
	char command[][8] =
	{{"attack\0"},	// stop players but not goalie
	 {"defend\0"},	// go goalie and players
	};

	menu_i_max = 2;
	msg_len = 7;

	// start the radio
	myId = OSMachineID();
	LCDPrintf("I am robot %d\n", myId);

  	LCDPutString("radio ");
  	err = RADIOInit();
  	if (err)
  	{
		LCDPutString("error\nexiting\n");
		return 1;
	}
 	else LCDPutString("ok\n");

	// print the main menu
	menu_i = 0;
	msg_num = 0;
	memset(msg_in,0,16);
	memset(msg_out,0,16);
	print_menu(command[menu_i],msg_num,msg_out);
	LCDMenu("Send"," +"," -","END");

	// monitor the radio
	msg_num=0;
	while ((key=KEYRead()) != KEY4)
	{
  		if (RADIOCheck())  /* check whether message is waiting */
    		{
			msg_num++;
			memset(msg_in,0,16);
			RADIORecv(&fromId, &len, (BYTE*)msg_in);  /* wait for next message */
			print_inmessage(msg_num, fromId, msg_in);
			if (err)
			{
				LCDPutString("Error Send\n");
				return 1;
			}
			print_menu(command[menu_i],msg_num,msg_out);
		}


		if(key==KEY1)
		{
			memset(msg_out,0,16);
			strcpy(msg_out,command[menu_i]);
			RADIOSend(BROADCAST,msg_len,(BYTE*)msg_out);
			print_menu(command[menu_i],msg_num,msg_out);
			msg_num++;
		}

		if(key==KEY2)
		{
			menu_i++;
			menu_i%=menu_i_max;
			print_menu(command[menu_i],msg_num,msg_out);
		}

		if(key==KEY3)
		{
			menu_i--;
			if(menu_i<0)
				menu_i = menu_i_max-1;
			print_menu(command[menu_i],msg_num,msg_out);
		}
	}

  RADIOTerm();
  return 0;
}