/***********************************************************************/
/** @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
    @author Andrew Berry, UWA, 1999

    @version 6
*/
/***********************************************************************/

/*@{*/


#include "global.h"

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

#define SETPOSITION FALSE

/** 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 (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()
{
  /* -----------= processes =-------------- */
  /** process to check for end (keypress) */
  struct tcb *end_p = NULL;

  /** process to look for obstacles */
  struct tcb *obstacle_p = NULL;

  /** 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 exit_colour2 = FALSE;
  int blue_goal;                /* playing on blue goal? */

  int key;                      /* keypress */

   /** picture to work on */
  colimage img;

  /** picture for LCD-output */
  image greyimg;

  /* ---------------------------------------------------- */
  /* ---------------initialisation process--------------- */
  /* ---------------------------------------------------- */

  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;
    }
  if (I_am_goalie())
    MyRole = GOALIE;
  else
    if (player_pos == 2 || player_pos == 3)
      MyRole = DEFENDER; /* would normally be defender */
    else
      MyRole = ATTACKER;

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

  LCDMenu("yes","no"," "," ");
  LCDSetPos(2,0);
  LCDPrintf("Use comms?\n");
  comms_flag = (KEYGet() == KEY1);
  LCDClear();

  /*comms_flag = TRUE;*/

  if (I_use_comms())
  {
    LCDPrintf("comms flag %d \n",comms_flag);
    LCDPrintf("init comms\n");
    if(RADIOInit()!=0)
      OSPanic("Can't init commlink\n");
    LCDPrintf("comms done\n");

    play_flag = FALSE;

  }
  else
    play_flag = TRUE;

  /*set the flag to run the set position function */

  if (SETPOSITION)
  {
    LCDMenu("yes","no"," "," ");
    LCDSetPos(2,0);
    LCDPrintf("set position?\n");
    find_loc_flag = (KEYGet() == KEY1);
    LCDClear();
  }

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

   do
     {
       if(MyRole == ATTACKER)
         LCDMenu("SET", "GO", "ATTK", "END");
       else
         if(MyRole == DEFENDER)
           LCDMenu("SET", "GO", "DEFN", "END");
         else
           LCDMenu("SET", "GO", "GOAL", "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();

       /* --------------------------------------------------------------- */
       /* ---------------------- Main Menu ------------------------------ */
       /* --------------------------------------------------------------- */

       switch(key)
         {
           /* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
         case KEY1:
           /* ---------Option:  SET --------------------------------------- */
           LCDClear();
           do
             {
               move_camera(DOWN);
               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", " ", "...", "END");
                       else
                         LCDMenu("BALL", "GOAL", "...", "END");

                       CAMGetColFrame(&img, FALSE);
                       IPColor2Grey(&img,&greyimg);
                       /* mark_object causes problems with the EyeCams - cause unknown - don't
                          use it */
                       /*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 */
                           move_camera(DOWN);
                           set_ball_colour(img);
                           break;
                         case KEY2:             /* GOAL: colour of goal */
                           if (!I_am_goalie())
                           {
                             move_camera(UP);
                             set_goal_colour(img);

                           }
                           break;
                         case KEY3:
                           do
                             {

                               LCDMenu("WALL", "   ", "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:     /* set the colour of the walls */
                                   move_camera(UP);

                                   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 */

                                   set_wall_colour(img);

                                   break;

                                 case KEY2:
                                   break;

                                   /* INFO: infromation about what to do */

                                 case KEY3:
                                   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();
                                   LCDMenu("WALL", "   ", "INFO", "END");

                                   break;

                                 case KEY4:
                                   exit_colour2 = TRUE;
                                   move_camera(DOWN);
                                   break; /* END */
                                 default: break;

                                 }
                             }while (!exit_colour2);
                           exit_colour2 = TRUE;

                         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;
           /* ---------End Option: Set ----------------------------------- */

         case KEY2:
           /* ---------Option: Go --------------------------------------- */

           LCDClear();
           if(!competition_mode)
             {
               if (I_am_goalie())
                 LCDMenu("RESET", "  ", "STOP", "  ");
               else
                 LCDMenu("  ", "  ", "STOP", "  ");
             }

           end_flag = FALSE;            /* reset flags for motion control*/
           obstacle_flag = FALSE;
           see_ball_flag = FALSE;
           got_ball_flag = FALSE;
           VWGetPosition(vw,&reset_pos);
           start_flag = TRUE;   /* drive to home position without turning */
           start_flag1 = TRUE;
           next_home = 0;
           reset_flag = FALSE;

           /* 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(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);
           }

           VWSetSpeed(vw, 0, 0);
           LCDClear();

           break;

           /* -------------End Option: Go --------------------------------*/

         case KEY3:
           /* ----------Option: Ano/Aof-----------------------------------*/

           if (MyRole == ATTACKER)
             MyRole = DEFENDER;
           else
             if (MyRole == DEFENDER)
               MyRole = ATTACKER;
           break;
           /* -------------End Option: Aon/Aof ---------------------------*/

         case KEY4:
           /* ----------Option: End --------------------------------------*/
           exit_program = TRUE;
           break;
           /* ----------End Option: End ----------------------------------*/

         default: break;
         }
     }
   while (!exit_program);

   if (I_use_comms())   /* don't need comms any more */
     RADIOTerm();

   ReleaseServos();
   VWRelease(vw);
   PSDRelease();

   return 0;
}

/*@}*/