/***********************************************************************/
/** @name main.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: program split into several modules.
    
    @author Birgit Graf, UWA, 1998
    @version 5
*/
/***********************************************************************/

/*@{*/


#include "global.h"

#include "general.h"
#include "image.h"
#include "sensors.h"
#include "servos.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 when playing as goalie */
  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;

  LCDMode(SCROLLING|NOCURSOR);  /* init lcd */
  LCDClear();
  InitPSD();			/* init infra-red sensors */
  InitCam();			/* init camera */


  if (player_pos == 0)		/* set position depending on ID */
  {  if (OSMachineID() < 6)
      player_pos = (int) OSMachineID();
    else
      player_pos = (int) OSMachineID() % 6 + 2;
  }

  InitDrive();			/* init motors */
  InitServos();

/*   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); */
/*   use_motors = TRUE; */
/*   LCDClear(); */
 
  /* test mode without motors? */
  if (!I_am_goalie() && !competition_mode)		
  {
    LCDMenu("yes","no"," "," ");
    
    LCDSetPos(2,0);
    LCDPrintf("Use motors?\n");
    use_motors = (KEYGet() == KEY1);
    LCDClear();
  }

  comms_flag = FALSE;
  competition_mode = FALSE;
/*   use_motors = TRUE; */
  attack_flag = FALSE;

  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);  
	    CAMGetColFrame(&img, FALSE); /* can't use marked picture */
	    
	    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");
      
      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");
      
/*       if (!I_am_goalie()) */
/* 	start_spline_IRQ(); */
      
      if(use_motors)
      {
	number_tasks = 4;
	
	obstacle_p = OSSpawn("obstacle", PPobstacle_test, 10*SSIZE, MIN_PRI, 1);
	if(!obstacle_p) OSPanic("spawn for obstacle avoidance failed");
      }
      else
	number_tasks = 3;
      
      if (!competition_mode)
	OSReady(end_p);
      OSReady(ball_p);	
      OSReady(drive_p);	
      
      if(use_motors)
	OSReady(obstacle_p); 

      OSReschedule();
      
      while (end_flag < number_tasks) /* all processes must have finished */
      {
	LCDSetPos(0,0);
	LCDPrintf("%d\n", end_flag);
      } 
      
/*       if (!I_am_goalie() && use_motors) */
/* 	stop_spline_IRQ(); */
      
      VWSetSpeed(vw, 0, 0);
      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(); */
  
  ReleaseServos();
  VWRelease(vw);
  PSDRelease();  
return 0;
}

/*@}*/