/***********************************************************************/
/** @name soccer.c 
    This program makes an Eyebot robot vehicle find a soccer ball by
    use of its camera and carry it into the goal. To find the ball the
    robot searches for pixels of the previousely defined ball-colour
    and drives to the ball. If the ball is cought in front of the
    robot it will drive to the position of the goal, as soon as the
    goal is seen the robot will steer to that position.

    Same program is used by the goalkeeper robot, distinction by
    machine id.
    
    Changes to last version: Fitted to Mk3/4.
    
    @author Birgit Graf, UWA, 2000
    @version 6
*/
/***********************************************************************/

/*@{*/


#include "global.h"

#include "general.h"
#include "imageglobal.h"
#include "image.h"
#include "sensors.h"
#include "servos.h"
#include "driveglobal.h"
#include "drive.h"



/** flag to say whether comms system is used or not */
int comms_flag = FALSE;


/***********************************************************************/
/** Say whether player is goalkeeper or defender -> use comms.
 */
/***********************************************************************/

int I_use_comms()
{
  return (player_pos < 4 && comms_flag);
}


/***********************************************************************/
/** Start program.
    The main-funktion controlls the several states of the
    program. It always shows the input of the camera, and starts the
    main menue, where you can set several parameters like the ball
    colour or the threshold for ball recognition. Before starting the
    ball search, colour
		of the ball and the goal as well as the robot's
    starting position have to be set.
*/
/***********************************************************************/

int main(void)
{
  /* -----------= processes =-------------- */
  /** process to check for end (keypress) */
  struct tcb *end_p;
  /** process to look for obstacles */
  struct tcb *obstacle_p;
  /** process to search ball */
  struct tcb *ball_p;
  /** process to move robot according to flags set in the other processes */
  struct tcb *drive_p;
  
  /** number of tasks running */
  int number_tasks;
  
  /** flags to exit loops around switch-statements */ 
  int exit_settings = FALSE;  
	int exit_program = FALSE;	
  int exit_settings2 = FALSE;
  int exit_colour = FALSE;
  
  int blue_goal;		/* playing on blue goal? */
  
  /*  char message[5];*/              /* message to be read from comms system */
  int key;												/* keypress */
  
  /** picture to work on */
  colimage img;
  /** picture for LCD-output */
  image greyimg;
  
  /** set flags for use */
  competition_mode = FALSE;
  attack_flag = FALSE;
  
  /** start initialisations */
  LCDMode(SCROLLING|NOCURSOR);  /* init lcd */
  LCDClear();

  end_p=NULL;
  /* initialise end_p to remove any possible warnings*/

  if (player_pos == 0)		
  {
		/* set position depending on ID, data required by Init_Drive! */
		player_pos = OSMachineID();

    if (player_pos > POS_MAX)
			player_pos = OSMachineID() % POS_MAX + 1;
	}
  
  Init_Cam();			/* init camera */
  Init_PSD();			/* init infra-red sensors */
  Init_Drive();		/* init motors */
  Init_Servos();	/* init servos */
  
  /*   if (player_pos < 4) */
  /*     { */
  /*       LCDMenu("yes","no"," "," "); */
  /*       LCDSetPos(2,0); */
  /*       LCDPrintf("Use comms?\n");	 */
  /*       comms_flag = (KEYGet() == KEY1); */
  /*       LCDClear(); */
  /*     } */
  
  /*   if (I_use_comms()) */
  /*     RadioInit(3);  */                /* init comms system for 3 robots
																						(defenders + goalie) */
  
  
  /* competition mode without picture output and stop button? */
  /*   LCDMenu("yes","no"," "," "); */
  /*   LCDSetPos(2,0); */
  /*   LCDPrintf("Competition?\n");	 */
  /*   competition_mode = (KEYGet() == KEY1); */
  /*   LCDClear(); */
  
  do
  {
    if (!attack_flag)
	/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
	LCDMenu("SET", "GO", "Aoff", "END");
	/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
    else
	if (attack_flag == 1)
	  LCDMenu("SET", "GO", "Aon1", "END");
	else
	  LCDMenu("SET", "GO", "Aon0", "END");
	
	/* print name, ID and position of robot */
	LCDSetPos(0, 10);
	LCDPrintf("%s\n", OSMachineName());
	LCDSetPos(1, 10);
	LCDPrintf("%d,", (int) OSMachineID());
	switch (player_pos)
	{
	case 1: LCDPrintf("GO\n"); break;
	case 2: LCDPrintf("LB\n"); break;
	case 3: LCDPrintf("RB\n"); break;
	case 4: LCDPrintf("LF\n"); break;
	case 5: LCDPrintf("RF\n"); break;
	}
	
	/* print current colour values for ball and goal */
	LCDSetPos(3, 10);
	LCDPrintf("B:%3d\n", get_ball_col());
	
	if (!I_am_goalie()) 
	{
	  LCDSetPos(4, 10);
	  LCDPrintf("G:%3d\n", get_goal_col(&blue_goal));
	  
	  LCDSetPos(5, 10);
	  if (blue_goal)
	    LCDPrintf("blue\n");
	  else
	    LCDPrintf("yell\n");      
	}
	
	CAMGetColFrame(&img, FALSE); 
	IPColor2Grey(&img, &greyimg);
	LCDPutGraphic(&greyimg);  
	
	key = KEYRead();
	if (I_use_comms())
	{
	  /*      message[0] = 'X'; */
	  /*       if (RadioRx(&message) != -1)  */
	  /* 	if(message[0] == 'g')  */
	  /* 	  key = KEY2 + 1; */
	}
	switch(key)
	{
	/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
	case KEY1:			/* - - - - - SET - - - - - */
	  LCDClear();
	  do
	  {
	    LCDMenu("COL", "POS", "...", "END");
	    
	    CAMGetColFrame(&img, FALSE); 
	    IPColor2Grey(&img, &greyimg);
	    LCDPutGraphic(&greyimg);  
	    
	    switch(KEYRead())
	    {
	    case KEY1:		/* COL: set colour values */
		LCDClear();
		do
		{
		  if (I_am_goalie()) 
		    LCDMenu("BALL", " ", "INFO", "END");
		  else
		    LCDMenu("BALL", "GOAL", "INFO", "END");
		  
		  CAMGetColFrame(&img, FALSE);
		  IPColor2Grey(&img,&greyimg);
		  mark_object(greyimg, imagerows / 2 - 1,  imagecolumns / 2 - 1, 0);
		  LCDPutGraphic(&greyimg);  
		  
		  LCDSetPos(1, 11);
		  LCDPrintf("hue:\n");
		  LCDSetPos(2, 11);
		  LCDPrintf("%3d\n", RGBtoHue(img[imagerows/2][imagecolumns/2][0],
		    img[imagerows/2][imagecolumns/2][1],
		    img[imagerows/2][imagecolumns/2][2]));
		  
		  switch(KEYRead())
		  {
		  case KEY1:		/* BALL: colour of ball */
		    set_ball_colour(img);
		    break; 
		  case KEY2:		/* GOAL: colour of goal */
		    if (!I_am_goalie()) 
		    {
			set_goal_colour(img);
		    }
		    break; 
		  case KEY3:		/* INFO: infromation about what to do */
		    LCDClear();
		    LCDMenu("", "", "", "OK");
		    LCDPutString("Place robot to  face BALL/GOAL, object must be  ");
		    LCDPutString("seen in middle  of screan (see  lines)\n");
		    KEYWait(KEY4);
		    LCDClear();
		    if (I_am_goalie()) 
			LCDMenu("BALL", " ", "INFO", "END");
		    else
			LCDMenu("BALL", "GOAL", "INFO", "END");
		    break;
		  case KEY4: exit_colour = TRUE; break; /* END */
		  default: break;
		  }
		} while (!exit_colour);
		
		LCDClear();
		exit_colour = FALSE;
		break;
		
	    case KEY2:		/* POS: set starting position */
		set_coordinates();
		break;
		
	    case KEY3:		/* ...: further settings */
		LCDClear();
		do
		{
		  LCDMenu("CAM", "IMG", "DRV", "END");
		  
		  CAMGetColFrame(&img, FALSE);
		  IPColor2Grey(&img, &greyimg);
		  LCDPutGraphic(&greyimg);  
		  
		  switch(KEYRead())
		  {
		  case KEY1: set_cam_parameters();
		    break;		/* CAM: set camera values */
		  case KEY2: set_img_parameters();
		    break;		/* IMG: set image parameters */
		  case KEY3: set_drv_parameters();
		    break;		/* DRV: set driving parameters */
		  case KEY4: exit_settings2 = TRUE;
		    break;		/* END */
		  default: break;
		  }
		} while (!exit_settings2);
		
		LCDClear();
		exit_settings2 = FALSE;
		break;
		
	    case KEY4: exit_settings = TRUE; break; /* END */
	    default: break;
	    }
	  } while (!exit_settings);
	  LCDClear();
	  exit_settings = FALSE; break;
	  
    /* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
    case KEY2:			/* - - - - - GO - - - - - */
	/*       if (I_use_comms()) */
	/* 	if (!RadioTx(0, "g", 1)) */
	/* 	  error("no message sent"); */
	/*     case (KEY2+1):                       */
      LCDClear();
      if(!competition_mode)
	  LCDMenu("", "", "STOP", "");
      
      end_flag = FALSE;		/* reset flags for motion control*/
      obstacle_flag = FALSE;
      see_ball_flag = FALSE;
      got_ball_flag = FALSE;
	
      start_flag = TRUE;	/* drive to home position without turning */
      start_flag1 = TRUE;	
      next_home = 0;
      
      /*       if (I_use_comms())*/	/* don't need comms any more */ 
	/* 	RadioRelease(); */
      
      /* initialize and start processes for ball-detection, drive to
	    goal, obstacle detection and check for key-input (end?) */
      
      OSMTInit(COOP);
      
      if (!competition_mode)
      {
	  end_p = OSSpawn("end", PPend_test, SSIZE, MIN_PRI, 1);
	  if(!end_p) OSPanic("spawn for end test failed");
      }
      
      ball_p = OSSpawn("ball", PPball_test, 30 * SSIZE, MIN_PRI, 1); 
      if(!ball_p) OSPanic("spawn for go for ball failed");
      
	obstacle_p = OSSpawn("obstacle", PPobstacle_test, 10*SSIZE, MIN_PRI, 1);
	if(!obstacle_p) OSPanic("spawn for obstacle avoidance failed");

      if (I_am_goalie())
	  drive_p = OSSpawn("drive", PPdrive_goal, SSIZE, MIN_PRI, 1); 
      else
	  drive_p = OSSpawn("drive", PPdrive_field, 10 * SSIZE, MIN_PRI, 1); 
      if(!drive_p) OSPanic("spawn for drive failed");
      
	number_tasks = 4;
	  
      
      if (!competition_mode)
				OSReady(end_p);
      OSReady(ball_p);	
			OSReady(obstacle_p); 
      OSReady(drive_p);	
      	
      OSReschedule();
      
      while (end_flag < number_tasks) /* all processes must have finished */
      {
				LCDSetPos(0,0);
				LCDPrintf("%d\n", end_flag);
      } 
      
      drive_stop();
      LCDClear();
	
	/*       if (I_use_comms()) */
	/* 	RadioInit(3);     */             /* ready for next start */
      break;
      
    /* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
    case KEY3: attack_flag ++;
      if (attack_flag>2) attack_flag = 0; 
      break;			/* - - - - - Aon/off - - - - - */
    case KEY4: exit_program = TRUE; break;      /* - - - - END - - - - */
    default: break;
    }
  } while (!exit_program);
  
  /*   if (I_use_comms())*/	/* don't need comms any more */ 
  /*     RadioRelease(); */
  
  End_Servos();
  End_Drive();
  End_PSD();
  return 0;
}

/*@}*/


