/***********************************************************************/
/** @name soccerdemo.c 
    This program makes an Eyebot robot vehicle find a soccer ball by
    use of its camera, drive towards it and kick it.

		@author Birgit Graf, UWA, 2000
    @version 1
*/
/***********************************************************************/

/*@{*/


#include "global.h"

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


/***********************************************************************/
/** 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;
   
  /*  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();
  
  Init_Cam();			/* init camera */
  Init_PSD();			/* init infra-red sensors */
  Init_Drive();		/* init motors */
  Init_Servos();	/* init servos */
  
  do
  {
		/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
		LCDMenu("SET", "GO", "", "END");
		/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */

		/* print name and ID of robot */
		LCDSetPos(0, 10);
		LCDPrintf("%s\n", OSMachineName());
		LCDSetPos(1, 10);
		LCDPrintf("ID: %d\n", (int) OSMachineID());
		
		/* print current colour values for ball and goal */
		LCDSetPos(3, 10);
		LCDPrintf("B:%3d\n", get_ball_col());
		
		CAMGetColFrame(&img, FALSE); 
		IPColor2Grey(&img, &greyimg);
		LCDPutGraphic(&greyimg);  
		
		key = KEYRead();
		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 */
						
						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: information 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 - - - - - */
      LCDClear();
			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;
      
      /* initialize and start processes for ball-detection, drive to
			goal, obstacle detection and check for key-input (end?) */
      
      OSMTInit(COOP);
      
			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");
			
			drive_p = OSSpawn("drive", PPdrive_field, 10 * SSIZE, MIN_PRI, 1); 
      if(!drive_p) OSPanic("spawn for drive failed");
      
			number_tasks = 4;
			
      
			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();
			
      break;
      
    case KEY4: exit_program = TRUE; break;      /* - - - - END - - - - */
    default: break;
    }
  } while (!exit_program);
  
  End_Servos();
  End_Drive();
  End_PSD();
  return 0;
}

/*@}*/