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

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

/*@{*/


#include "global.h"

#include "general.h"
#include "imageVis.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;
  
  /*  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();
  
  /*- Camera Initialisation -*/
  Init_Drive();		/* init motors */
  Init_PSD();			/* init infra-red sensors */
	Init_Camera();	/* init camera */
  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());
		
		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();
					Set_BallColour();
 					LCDClear();
 					break;
					
				case KEY2:		/* POS: set starting position */
					set_coordinates();
					break;
					
				case KEY3:		/* ...: further settings */
					LCDClear();
					do
					{
						LCDMenu("DRV", "CAM", "", "END");
						
						CAMGetColFrame(&img, FALSE);
						IPColor2Grey(&img, &greyimg);
						LCDPutGraphic(&greyimg);  
						
						switch(KEYRead())
						{
						case KEY1: set_drv_parameters();
							break;		/* DRV: set driving parameters */
						case KEY2: Set_CameraParams();
							break;		/* CAM: set camera 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();
			move_camera(0);
      LCDClear();
			
      break;
      
    case KEY4: exit_program = TRUE; break;      /* - - - - END - - - - */
    default: break;
    }
  } while (!exit_program);
  
  End_Servos();
  End_Drive();
  End_PSD();
  return 0;
}

/*@}*/