/***********************************************************************/

/** @name image.c

    Contains all necessary image processing routines.



    @author Birgit Graf,   UWA, 1998

    @author Thomas Braunl, UWA, 1998 (HSV)

    @author Andrew Berry,  UWA, 1999

*/

/***********************************************************************/



/*@{*/



#include "global.h"

#include "general.h"

#include "servos.h"

#include "image.h"



#define NO_HUE 255



/** size at bottom of ball (where it should be detected) */

#define BALLSIZE 0.03



/** Weighting for latest velocity info */

#define VELWEIGHT 0.6



/** size modifer for maximum object size */

#define MAXOBJSIZE 1.5



/** Ball coordinates when ball has been detected (position on field) */

float ball_x, ball_y;



/** threshold for comparison of colours to ballcolour */

int thresh_ball = 5;

/** threshold for comparison of colours to goalcolour */

int thresh_goal = 10;

/** threshold for comparison of colours to goalcolour */

int thresh_wall = 50;

/** ball colour */

int BallColour = 42;

/** goal colour */

int GoalColour = 220;

/** wall colour */

int WallColour = 150;





/** permanent flag to select whether robot is attacking blue or yellow

    goal */

int drive_to_blue_goal = TRUE;



/***********************************************************************/

/**

    Update_Ball_Location



    This function updates the balls location i.e. middle of image;

    left edge or right edge.



    @param ImageX,ImageY the row and column values from the image

    (NOT absolute co-ords.)



    @author Andrew Berry,  UWA, 1999



    Called by PPball_test()

    @see  PPball_test()

 */

/***********************************************************************/

void Update_Ball_Location(int ImageX, int ImageY)

{



  static int OldY;





  if(ImageY > -1)

  {



    if (ImageY < LEFT_EDGE_ZONE)

      BallImagePos = LEFT_EDGE;

    else

      BallImagePos = RIGHT_EDGE;

    LCDSetPos(0,0);

    LCDPrintf("updating \n");

    OldY = ImageY;

    just_lost_flag = FALSE;

    UpdateFlags();

    LCDSetPos(0,0);

    LCDPrintf("done     \n");

    return;

  }

  if ( (just_lost_flag == FALSE) && (OldY != -1.0) )

  {

    just_lost_flag = TRUE;

    OldY = -1.0;

    LCDSetPos(0,0);

    LCDPrintf("just lost\n");

    UpdateFlags();

    return;

  }

  else

    if ( just_lost_flag == TRUE)



    {

      /* it's gone, give up looking */

      just_lost_flag = FALSE;

      LCDSetPos(0,0);

      LCDPrintf("gone    \n");

      UpdateFlags();





    }

   LCDSetPos(0,0);

   LCDPrintf("done    \n");

}





/***********************************************************************/

/**

   Update_Ball_Direction

   This function is used to update the ball's direction.

   Must be called after all attempts

   to locate ball.



   @param New_X,New_Y the new co-ordinates of the ball. If ball is lost, use (-1,-1).

          update_type the type of update 1 = I see the ball at these co-ords

                                         2 = I no not currently see the ball

    @author Andrew Berry,  UWA, 1999



    Called by PPball_test()

    @see  PPball_test()

*/

/***********************************************************************/



void Update_Ball_Direction(float New_X, float New_Y, int update_type)

{

  static float Old_X = 0.0, Old_Y = 0.0;



  switch(update_type){

  case 1:

    {



      BallDeltaX = (New_X - Old_X)*VELWEIGHT + BallDeltaX*(1.0-VELWEIGHT);

      BallDeltaY = (New_Y - Old_Y)*VELWEIGHT + BallDeltaX*(1.0-VELWEIGHT);

      Old_X = New_X;

      Old_Y = New_Y;



      just_lost_flag = FALSE;

      break;

    }

  case 2:

      {

        if ( (just_lost_flag == FALSE) && (Old_X != -1.0) )

        {

          just_lost_flag = TRUE;

          Old_X = -1.0;

        }

        else

        {

          /* it's gone, give up looking */

          just_lost_flag = FALSE;

        }

        break;

      }

  }

}



/***********************************************************************/

/** Init camera.



    Called by main().

    @see  main()

*/

/***********************************************************************/



void InitCam()

{

  int camera;

  int bright, hue, sat;

  /*  int i,x,y;*/



  camera = CAMInit(WIDE);



  if (camera < COLCAM)

    error("No colour camera!");

  else

  {

    if (camera == NOCAM)

      error("No camera!\n");

    else

      if (camera == INITERROR)

        error("CAMInit!\n");

  }



  /* set brightness and hue */



  CAMGet(&bright, &hue, &sat);

  CAMMode(NOAUTOBRIGHTNESS);

  CAMSet(160, 110, sat);





}





/***********************************************************************/

/** Change camera parameters.

    Changes brightness, hue and saturation in case default values

    aren't good enough (e.g. different light conditions). Function as

    in RoBiOS-Setup, but without camera-initialisation, works only for

    colour-camera.



    Called by main().

    @see  main()

*/

/***********************************************************************/



void set_cam_parameters ()

{

  int boc[] = {0, 0, 0, 0};

  int ind = 0;

  /*  int test = 0;*/

  image greyimg;

  colimage img;



  int end_proc = FALSE;



  LCDClear();

  LCDMenu(" + "," - ","Nxt","END");

  LCDPrintf("Camera param.:\n");



  while (!end_proc)

  {

    CAMGet(&boc[0], &boc[1], &boc[2]);



    if (boc[3] == 0) CAMMode(NOAUTOBRIGHTNESS);

    else CAMMode(AUTOBRIGHTNESS);



    LCDSetPos(2, 0);

    LCDPrintf("Brigh.:   %d\n", boc[0]);

    LCDPrintf("Hue   :   %d\n", boc[1]);

    LCDPrintf("Satur.:   %d\n", boc[2]);



    CAMGetColFrame(&img, FALSE);

    IPColor2Grey(&img, &greyimg);



    LCDPrintf("AutoBr.:  ");

    if (boc[3] == 0) LCDPrintf("OFF\n");

    else LCDPrintf("ON \n");

    LCDSetChar(2 + ind, 15, '*');



    LCDPutGraphic (&greyimg);



    switch (KEYRead())

    {

    case KEY1:      boc[ind] ++;

      if (ind == 0 || ind == 3)

      {

        if (ind == 0)

        {

          if (boc[ind] > 179) boc[ind] = 0;

        }

        else

          if (boc[ind] > 1) boc[ind] = 0;

      }

      else

        if (boc[ind] > 255) boc[ind] = 0;

      CAMSet(boc[0], boc[1], boc[2]);

      break;



    case KEY2:      boc[ind] --;

      if (ind == 0 || ind == 3)

      {

        if (ind == 0)

        {

          if (boc[ind] < 0) boc[ind] = 179;

        }

        else

          if (boc[ind] < 0) boc[ind] = 1;

      }

      else

        if (boc[ind] < 0) boc[ind] = 255;

      CAMSet(boc[0], boc[1], boc[2]);

      break;



    case KEY3:

      LCDSetChar(2 + ind, 15, ' ');

      ind ++;

      if (ind > 3) ind = 0;

      break;



    case KEY4:

      LCDClear(); end_proc = TRUE; break;



    default: break;

    }

  }

}





/***********************************************************************/

/** Change parameters for image processing.

    Changes thresholds which are used to select ball/goal coloured

    pixels from others and size of ball/goal.



    Called by main().

    @see  main()

*/

/***********************************************************************/



void set_img_parameters()

{

  int ind = 0;

  int end_proc = FALSE;



  LCDClear();

  LCDMenu("CHG", "NXT", " ", "END");



  while (!end_proc)

  {

    LCDSetPos(0, 0);

    LCDPrintf("Image parameters\n");



    LCDSetPos(2, 0);

    LCDPrintf("Thresh ball\n");

    LCDSetPos(3, 0);

    if (!I_am_goalie())

      LCDPrintf("Thresh goal\n");



    LCDSetChar(2 + ind, 15, '*');



    switch (KEYGet())

    {

    case KEY1:

      switch(ind)

      {

      case 0:

        thresh_ball = set_iparam("Thresh ball", 0, thresh_ball, 100, 1);

        break;

      case 1:

        thresh_goal = set_iparam("Thresh goal", 0, thresh_goal, 100, 1);

        break;

      default: break;

      }



      LCDMenu("CHG", "NXT", " ", "END");

      break;



    case KEY2:

      LCDSetChar(2 + ind, 15, ' ');

      ind ++;

      if (I_am_goalie())

        if (ind > 0) ind = 0;

      if (ind > 1) ind = 0;

      break;



    case KEY4:

      LCDClear(); end_proc = TRUE; break;



    default: break;

    }

  }

}







/***********************************************************************/

/** Change RBG to HSV -- use hue only.

    Thomas Braunl, UWA 1998.



    Called by HSVhue() and set_colour().



    @see HSVhue()

    @see set_colour()

    @param r,g,b rgb value of single pixel

    @return hue for single RGB value

*/

/***********************************************************************/



#define MIN(a,b) (a<b?a:b)

#define MAX(a,b) (a>b?a:b)



int RGBtoHue(BYTE r, BYTE g, BYTE b)

{

  int hue, delta, max, min;

  max   = MAX(r, MAX(g,b));

  min   = MIN(r, MIN(g,b));

  delta = max - min;



  if (2 * delta <= max)

    hue = NO_HUE;

  else

  {

    if (r == max)

      hue =  42 + 42*(g-b) / delta; /* 1*42 */

    else

      if  (g == max)

        hue = 126 + 42*(b-r) / delta; /* 3*42 */

    else

      if (b == max)

        hue = 210 + 42*(r-g) / delta; /* 5*42 */

    /* now: hue is in range [0..252] */

  }



  return hue;

}





/***********************************************************************/

/** Convert an rgb image to hue in grayscale.

    Thomas Braunl, UWA 1998.



    Called by find_object().



    @see find_object()

    @param imageIn, imageOut input / output picture

*/

/***********************************************************************/



void HSVhue(BYTE *imageIn, BYTE *imageOut)

{

  int i,j;



  for(i=0, j=0; i < imagerows*imagecolumns; i++, j+=3)

    imageOut[i] = RGBtoHue(imageIn[j],imageIn[j+1],imageIn[j+2]);

}



/***********************************************************************/

/** Converts just a row of the image to HSV

    Andrew Berry, UWA 1999





    Called by find_object().

    @see find_object()

    @param in, out:  input / output picture

    @param row:      the row to be converted

*/

/***********************************************************************/





void ConvertRowToHSV(BYTE *in, BYTE *out, int row)

{

  int column;



  for(column=0;column<imagecolumns;column++)

  {

    out[row*imagecolumns + column]=RGBtoHue(in[(row*imagecolumns + column)*3],in[(row*imagecolumns + column)*3+1],in[(row*imagecolumns + column)*3+2] );

  }

}



/***********************************************************************/

/** Converts just a row of the image to HSV

    Andrew Berry, UWA 1999



    Used in CheckWall.



    @param in, out:  input / output picture

    @param row:      the row to be converted



*/

/***********************************************************************/



void ConvertColumnToHSV(BYTE *in, BYTE *out, int column)

{

  int row;

  for(row=0;row<imagerows;row++)

  out[row*imagecolumns + column]=RGBtoHue(in[(row*imagecolumns + column)*3],in[(row*imagecolumns + column)*3+1],in[(row*imagecolumns + column)*3+2] );





}





/***********************************************************************/

/** Change RBG to HSV -- use saturation only.

    Andrew Berry, UWA 1999.



    Called by HSVhue() and set_colour().



    @see HSVhue()

    @see set_colour()

    @param r,g,b rgb value of single pixel

    @return hue for single RGB value

*/

/***********************************************************************/





int RGBtoSaturation(BYTE r, BYTE g, BYTE b)

{

  int intensity, min_col;

  float sat, min3;



  intensity = (int)r+(int)g+(int)b;

  if (intensity == 0)

    intensity = 1;



  if( (r<g) && (r<b) )

    min_col = (int)r;

  else

    if( (g<r) && (g<b))

      min_col = (int)g;

    else

      min_col = (int)b;

  min3 = 3 * min_col;

  sat = ( 1 - (min3)/intensity);







  return (int)255*sat;



}



/***********************************************************************/

/** Convert an rgb image to intensity in grayscale.

    AndrewBerry, UWA 1999.





    @param imageIn, imageOut input / output picture

*/

/***********************************************************************/



void HSVsaturation(BYTE *imageIn, BYTE *imageOut)

{

  int i,j;



  for(i=0, j=0; i < imagerows*imagecolumns; i++, j+=3)

    imageOut[i] = RGBtoSaturation(imageIn[j],imageIn[j+1],imageIn[j+2]);

}







/***********************************************************************/

/** NextRow



    Determines the next row to search.



    Returns the row number of the next row, or -1 if all rows have been

    searched. It also updates the lastup, lastdown and lastdir.



    @param *lastup, *lastdown: the topmost and bottommost rows searched

    @param *lastdir:           whether the last row search was above(0) or

                               below(1) the orginal row.



    Called by find_object()

    @see  find_object()



*/

/***********************************************************************/



int NextRow(int *lastup, int *lastdown, int *lastdir)

{



  if (*lastup==0 && *lastdown==imagerows-1) /*scanned whole image! */

    return -1;

  else

    if (*lastdir == DOWN)                  /* last move was down */

      if (*lastup != 0)                   /* haven't reached the top yet */

      {

        *lastup=(*lastup)-1;

        *lastdir = UP;

        return *lastup;

      }

      else                 /* at the top, must go down */

      {

        *lastdown = (*lastdown)+1;

        *lastdir = DOWN;

        return *lastdown;

      }

    else

      if (*lastdir == UP)                          /* last move was up */

      {

        if (*lastdown != imagerows)         /* haven't reached bottom yet */

        {

          *lastdown = (*lastdown)+1;

          *lastdir = DOWN;

          return *lastdown;

        }

        else                                /* reached bottom, must go up */

        {

          *lastup=(*lastup)-1;

          *lastdir = UP;

          return *lastup;

        }

      }

      else

        error("Can't find next row\n");

  return 0;

}



/***********************************************************************/

/** Set Ball/Goal colour.

    Set ball/goal colour to colour detected in middle of picture

    (mean value of 5x5 area around middle of picture).



    Called by set_ball_colour() and set_goal_colour().



    @see set_ball_colour()

    @see set_goal_colour()

    @param img colour picture

    @return mean colour of image middle and surroundings

*/

/***********************************************************************/



int set_colour(colimage img)

{

  int row, column;

  int count = 0;

  int my_hue, hue;



  my_hue = 0;



  for (row = imagerows/2 - 2; row < (imagerows/2 + 3); row ++)

    for (column = imagecolumns/2 - 2; column < imagecolumns/2 + 3; column ++)

    {

      hue = RGBtoHue(img[row][column][0], img[row][column][1],

                     img[row][column][2]);

      if (hue != NO_HUE)

      {

        my_hue += hue;

        count ++;

      }

    }



  if (count != 0)

    my_hue /= count;



  LCDClear();

  LCDMenu("", "", "", "OK");

  LCDSetPos(7,0);

  LCDPrintf("HUE: %3d\n", my_hue);



  KEYWait(KEY4); LCDClear();



  return my_hue;

}



/***********************************************************************/

/** Set  colour saturation.

    Returns the colour intensety to colour detected in middle of picture

    (mean value of 5x5 area around middle of picture).



    Called by set_ball_colour() and set_goal_colour().



    @see set_wall_colour()

    @param img colour picture

    @return mean colour intesity of image middle and surroundings

*/

/***********************************************************************/



int set_colour_saturation(colimage img)

{

  int row, column;

  int count = 0;

  int my_saturation;



  my_saturation = 0;



  for (row = imagerows/2 - 2; row < (imagerows/2 + 3); row ++)

    for (column = imagecolumns/2 - 2; column < imagecolumns/2 + 3; column ++)

    {



        my_saturation = RGBtoSaturation(img[row][column][0], img[row][column][1],

                     img[row][column][2]);

        count ++;

    }



  if (count != 0)

    my_saturation /= count;



  LCDClear();

  LCDMenu("", "", "", "OK");

  LCDSetPos(7,0);

  LCDPrintf("INTE: %3d\n", my_saturation);



  KEYWait(KEY4); LCDClear();



  return my_saturation;

}



/***********************************************************************/

/** Set Wall colour.



    Called by main().



    @see main()

    @param colour[] array of 3 values which contain r,g,b-values for

    colour

    @param img colour picture

    @return detected colour in referenced array

*/

/***********************************************************************/



void set_wall_colour(colimage img)

{

  /*WallColour = set_colour_saturation(img);*/

  WallColour = set_colour(img);



}





/***********************************************************************/

/** Set Ball colour.



    Called by main().



    @see main()

    @param colour[] array of 3 values which contain r,g,b-values for

    colour

    @param img colour picture

    @return detected colour in referenced array

*/

/***********************************************************************/



void set_ball_colour(colimage img)

{

  BallColour = set_colour(img);

}







/***********************************************************************/

/** Set Goal colour.



    Called by main().



    @see main()

    @param colour[] array of 3 values which contain r,g,b-values for

    colour

    @param img colour picture

    @return detected colour in referenced array

*/

/***********************************************************************/



void set_goal_colour(colimage img)

{

  GoalColour = set_colour(img);

  drive_to_blue_goal = GoalColour > 150;

}







/***********************************************************************/

/** Compare colours.

    Check if 2 colour-values are alike (difference < thresh).



    Called by find_object().



    @see find_object()

    @param val1, val2 colours to compare

    @param thresh threshold for comparison

    @return TRUE or FLASE, depending on comparison of colour values

*/

/***********************************************************************/



int alike(int val1, int val2, int thresh)

{

      int distance;

      if (val1 != NO_HUE && val2!= NO_HUE)

      {

        //val1 and val2 can have values between 0..252

        //we have 253 hues in total and 126 is the middle

        //we need to calculate the 'distance' between

        //these two hues

        distance = abs(val1-val2);

        //if the distance is larger than 126 than the other

        //way round is shorter

        if (distance > 126)

          distance = 253 - distance;



        if (distance <= thresh)

          return TRUE;

      }



      return FALSE;

}





/***********************************************************************/

/** Mark object.

    Convert colour picture into greyscale (for LCD output),

    mark ball/goal position by drawing a vertical and horizontal (opposite to

    the image's underlying colour) through it. Ball and it's size is

    displayed by black lines. Ball and size not diplayed currently.



    Called by find_object().



    @see find_object()

    @param x_middle, y_middle pixel-coordinates of object

    @param object_size width of marking

*/

/***********************************************************************/



void mark_object(image greyimg, int x_middle, int y_middle, int object_size)

{

  int row, column;



  /* mark object position: WHITE */

  for (row = 1; row < imagerows; row ++)

    if(greyimg[row][y_middle]==WHITE)

      greyimg[row][y_middle] = BLACK;

    else

      greyimg[row][y_middle] = WHITE;



  for (column = 1; column < imagecolumns; column ++)

    if(greyimg[x_middle][column]==WHITE)

      greyimg[x_middle][column] = BLACK;

    else

      greyimg[x_middle][column] = WHITE;



}









/***********************************************************************/

/** Search for ball/goal.

    Search for reagions in rows, who's mean colour value fits the

    colour of the ball/goal, return coordinates of its middle.



    Called by find_ball() and find_goal().



    @see find_ball()

    @see find_goal()

    @param *x_middle, *y_middle pointers to pixel-coordinates of middle

    of ball/goal

    @param *object_size pointer to ball's/goal's size

    @param size minimum size of ball/goal

    @param colour colour of searched object (ball/goal)

    @param thresh threshold for colour-comparison

    @return TRUE or FALSE if ball/goal was found or not

    @return changed ball/goal coordinates and size in referenced

    variables

*/

/***********************************************************************/



int find_object(int *x_middle, int *y_middle, int *object_size, float bottom_size, int colour, int thresh)

{

  colimage img;

  image greyimg, greyimg2, hue;

  int row, column;

  int must_size;                /* desired size of object depending on

                                   position on screen */

  int first, last;              /* first and last pixel of searched

                                   colour */



  int know_first, know_last;    /* flag to indicate whether new object

                                   coloured pixel on right/left side

                                   has been searched yet */

  int new_first, new_last;      /* new test area/test values */



  int sum;                      /* sum of pixel values in examined area */

  int count;                    /* no.of pixels with valid hue */

  int mean;                     /* mean of pixel values in examined area */



  int first_sum, last_sum;      /* sum of pixel values in new examined areas */



  int is_ball;                  /* flag to determine whether detected

                                   object is ball/goal (not too big) */



  int last_up, last_down;          /* the last row searched above and below the

                                   first row searched */

  int last_dir;                   /* 0 = last row searched was above starting

                                         row

                                   1 = last row searched was below

                                       starting row

                                */





  CAMGetColFrame(&img, FALSE);





  last_up = *y_middle;

  last_down = *y_middle;

  last_dir = DOWN; /* could be the other direction */

  row = *y_middle;

  while(row>=0)

  {

    if (yfact[cam_pos][row] != 0.0)

    {

      must_size = (int) (bottom_size / yfact[cam_pos][row]);

      if (must_size > imagecolumns - 2)

        must_size = imagecolumns - 2;

    }

    else

      must_size = imagecolumns - 2; /* size of ball/goal,

                                       depending on distance of object

                                       ball -> must_size = radius */



    if (must_size < 5)

      {

        if (!I_am_goalie())

          must_size = 5;

        else

          must_size = 3;

      }

    first = 1;                  /* first: index of first object

                                   coloured pixel in row */

    last = imagecolumns - 1;    /* last: index of first non object

                                   coloured pixel in row after series

                                   of object coloured ones */





    ConvertRowToHSV((BYTE *)img, (BYTE *)hue, row);



    /* find 'left' edge */

    while (!alike(hue[row][first], colour, thresh) &&

           (first < (last - must_size / 2)))

      first += must_size / 2;



    if (alike(hue[row][first], colour, thresh) &&

        (first < (last - must_size / 2)))

    {





      first -= must_size / 2;



      while (!alike(hue[row][first], colour, thresh))

        first ++;               /* go back to first pixel of object colour */

    }



    /* find 'right edge' */

    while (!alike(hue[row][last - 1], colour, thresh) &&

           (last > (first + must_size / 2)))

      last -= must_size / 2;

    if (alike(hue[row][first], colour, thresh) &&

        (last > (first + must_size / 2)))

    {

      last += must_size / 2;



      while (!alike(hue[row][last], colour, thresh))

        last  --;               /* go back to last pixel of object

                                   colour */

    }



    if ((last - first) >= must_size) /* object big enough? */

    {

      know_first = FALSE;               /* reset flags */

      know_last = FALSE;



      sum   = 0;                        /* reset sum */

      count = 0;



      for (column=first;column < last;column ++) /* check region from first to

                                          last */

       if (hue[row][column] != NO_HUE)

       {

         sum += abs(hue[row][column] - BallColour);

         count ++;

       }

    }





    while((last - first) >= (must_size))        /* object big enough? */

    {

      /* get mean value for region between columns "first" and "last" */

      mean = sum / count;



      /* test if there is the desired object between columns "first" and

         "last" */

      if (mean < thresh)

      {

        *object_size = last - first;

        *y_middle = first + *object_size / 2;

        *x_middle = row;



        is_ball = *object_size < MAXOBJSIZE * must_size; /* object shoulden't be

                                                   too big (might be

                                                   goal instead of

                                                   ball) */

        if (!competition_mode)

        {

          IPColor2Grey(&img, &greyimg);

          IPSobel(&greyimg,&greyimg2);

          if (is_ball)

            mark_object(greyimg2, *x_middle, *y_middle, *object_size);



          /*  LCDPutGraphic(&greyimg2);*/

        }

        return is_ball;

      }







      /*     ----- find new region to test ----- */

      if (!know_first)

      {                         /* find next pixel of searched colour

                                   (from left) */

        first_sum = 0;

        column = first;

        do

        {

          if (hue[row][column] != NO_HUE)

            first_sum += abs(hue[row][column] - BallColour);



          column ++;

        }

        while (!alike(hue[row][column], colour, thresh) && (column <= last));



        new_first = column;

        know_first = TRUE;

      }



      if (!know_last)

      {                 /* find next pixel of searched colour

                                   (from right) */

        last_sum = 0;

        column = last;

        do

        {

          if (hue[row][column] != NO_HUE)

            last_sum += abs(hue[row][column - 1] - BallColour);

          column --;

        } while (!alike(hue[row][column - 1], colour, thresh) &&

                 (column >= first));



        new_last = column;

        know_last = TRUE;

      }



      if ((new_first - first) > (last - new_last)) /* new possible

                                                      object region */

      {

        first = new_first;

        sum  -= first_sum;

        know_first = FALSE;

      }

      else

      {

        last = new_last;

        sum -= last_sum;

        know_last = FALSE;

      }

    }

    row = NextRow(&last_up,&last_down,&last_dir);

  }



  if (!competition_mode)

  {

    IPColor2Grey(&img, &greyimg);

    IPSobel(&greyimg,&greyimg2);

    /*LCDPutGraphic(&greyimg2);*/

  }

  see_ball_flag = FALSE;

  return FALSE;                 /* searched whole picture */

}







/***********************************************************************/

/** Search for ball.



    Called by PPball_test().



    @see PPball_test()

    @param *x, *y pointers to pixel-coordinates of middle of ball

    @param *size pointer to ball size

    @return TRUE or FALSE if ball was found or not

    @return changed ball coordinates and size in referenced

    variables

*/

/***********************************************************************/



int find_ball(int *x, int *y, int *size)

{

  return find_object(x, y, size, BALLSIZE, BallColour, thresh_ball);

}





/***********************************************************************/

/** Search for goal.



    Called by drive_to_goal().



    @see drive_to_goal()

    @param *x, *y pointers to pixel-coordinates of middle of goal

    @param *size pointer to goal size

    @return TRUE or FALSE if goal was found or not

    @return changed goal coordinates and size in referenced

    variables

*/

/***********************************************************************/



int find_goal(int *x, int *y, int *size)

{

  return find_object(x, y, size, 0.6 * GOALWIDTH, GoalColour, thresh_goal);

}



/***********************************************************************/

/** Search for goal.



    Called by  PPball_test()



    @see PPball_test()



    @return 2 if the goal is found, 1 if it is not found.

    @return changed goal coordinates and size in referenced

    variables



    Why return 1 and 2 and not TRUE of FALSE? Because 0 is used to tell

    the drive thread that the the image thread hasn't checked yet.

*/

/***********************************************************************/



int search_for_goal()

{



  colimage img;

  image hue;

  int row = 0, column;

  int stepsize = 8;             /* desired size of object depending on

                                   position on screen */

  int first, last;              /* first and last pixel of searched

                                   colour */



  int know_first, know_last;    /* flag to indicate whether new object

                                   coloured pixel on right/left side

                                   has been searched yet */

  int new_first, new_last;      /* new test area/test values */



  int sum;                      /* sum of pixel values in examined area */

  int count;                    /* no.of pixels with valid hue */

  int mean;                     /* mean of pixel values in examined area */



  int first_sum, last_sum;      /* sum of pixel values in new examined areas */



  int i= 0;



  move_camera(UP);

  /*

    LCDSetPos(4,0);

    LCDPrintf("find goal start\n");

  */



  CAMGetColFrame(&img, FALSE);



  while( row < imagerows)

  {

    first = 1;                  /* first: index of first object coloured pixel in row */

    last = imagecolumns - 1;    /* last: index of first non object coloured pixel in row

                                   after series of object coloured ones */



    ConvertRowToHSV((BYTE *)img, (BYTE *)hue, row);



    /* find 'left' edge */



    while (!alike(hue[row][first],GoalColour, thresh_goal)

           && (first < (last - stepsize / 2)))

      first += stepsize / 2;



    if (alike(hue[row][first], GoalColour, thresh_goal) &&

        (first < (last - stepsize / 2)))

    {

      first -= stepsize / 2;

      while (!alike(hue[row][first], GoalColour, thresh_goal))

        first ++; /* go back to first pixel of object colour */

    }



    /* find 'right edge' */



    while (!alike(hue[row][last - 1], GoalColour, thresh_goal) &&

           (last > (first + stepsize / 2)))

      last -= stepsize / 2;

    if (alike(hue[row][first], GoalColour, thresh_goal) &&

        (last > (first + stepsize / 2)))

    {

      last += stepsize / 2;

      while (!alike(hue[row][first], GoalColour, thresh_goal))

        last  --;               /* go back to last pixel of object

                                   colour */

    }



    if ((last - first) >= stepsize) /* object big enough? */

    {

      know_first = FALSE;               /* reset flags */

      know_last = FALSE;



      sum   = 0;                        /* reset sum */

      count = 0;



      for (column=first;column < last;column ++) /* check region from first to

                                                    last */

       if (hue[row][column] != NO_HUE)

       {

         sum += abs(hue[row][column] - GoalColour);

         count ++;

       }

    }



    while((last - first) >= (stepsize)) /* object big enough? */

    {

      /* get mean value for region between columns "first" and "last" */

      mean = sum / count;



      /* test if there is the desired object between columns "first" and

         "last" */

      if (mean < thresh_goal)

      {

        AUBeep();

        return 2; /* I'm looking at the goal */

      }

      else

      {

        AUBeep();

        AUBeep();



      }

      /*     ----- find new region to test ----- */

      if (!know_first)

      {                         /* find next pixel of searched colour

                                   (from left) */

        first_sum = 0;

        column = first;

        do

        {

          if (hue[row][column] != NO_HUE)

            first_sum += abs(hue[row][column] - GoalColour);

          column ++;

        }

        while (!alike(hue[row][column], GoalColour, thresh_goal)

               && (column <= last));



        new_first = column;

        know_first = TRUE;

      }



      if (!know_last)

      {                 /* find next pixel of searched colour

                                   (from right) */

        last_sum = 0;

        column = last;

        do

        {

          if (hue[row][column] != NO_HUE)

            last_sum += abs(hue[row][column - 1] - GoalColour);

          column --;

        } while (!alike(hue[row][column - 1], GoalColour, thresh_goal) &&

                 (column >= first));

        new_last = column;

        know_last = TRUE;

      }



      if ((new_first - first) > (last - new_last)) /* new possible

                                                      object region */

      {

        first = new_first;

        sum  -= first_sum;

        know_first = FALSE;

      }

      else

      {

        last = new_last;

        sum -= last_sum;

        know_last = FALSE;

      }

    }

    i++;

    row++;

  }



  return 1;                     /* ball not found */

}





/***********************************************************************/

/** Transform local to global Position.

    change local dx, dy from robot position into global coordinates



    @author  Thomas Braunl

*/

/***********************************************************************/



void local2global(PositionType pos, PositionType local, PositionType *global)

{

  float cosphi, sinphi;



  if (I_am_goalie())    /* change coordinates for goalie */

  {

    if (cam_pos == UP)

    {

      local.x *= 0.7;

      local.y *= 0.7;

    }

    pos.phi += PI / 2.0;

  }

  cosphi = cos(pos.phi);

  sinphi = sin(pos.phi);



  global->x   = pos.x + cosphi*local.x + sinphi*local.y;

  global->y   = pos.y + cosphi*local.y + sinphi*local.x;

  global->phi = 0.0;

}







/***********************************************************************/

/** Check for wall.

    Andrew Berry 1999



    Looks straigh ahead and checks to see if it can see the wall.

    If it finds the wall, it sets is_wall_flag to true.

    Always sets the check_wall_flag to false upon completion.







    Called by PPball_test()

    @see PPball_test()

*/

/***********************************************************************/



void CheckForWall()

{

  colimage img;

  int count = 0;

  int row, column;

  /*  int left = -1,right = -1;*/

  int my_hue, hue;





  move_camera(UP);

  CAMGetColFrame(&img, FALSE);

  my_hue = 0;





   for (row = imagerows/2 - 2; row < (imagerows/2 + 3); row ++)

    for (column = imagecolumns/2 - 2; column < imagecolumns/2 + 3; column ++)

    {

      hue = RGBtoHue(img[row][column][0], img[row][column][1],

                     img[row][column][2]);

      if (hue != NO_HUE)

      {

        my_hue += hue;

        count ++;

      }

    }



  if (count != 0)

    my_hue /= count;





  if(alike(my_hue,WallColour,thresh_wall))

    /* obstacle is a wall! */

    {

      LCDSetPos(1,0);

      LCDPrintf("W  front\n");

      is_wall_flag = TRUE;

      OSReschedule();

    }





  else

    {

      LCDSetPos(1,0);

      LCDPrintf("Not Wall\n");



      is_wall_flag = FALSE;

    }

  check_wall_flag = FALSE;



}





/***********************************************************************/

/** Check for ball.

    Analyse picture by calling find_object and checking camera

    position, set see_ball_flag and got_ball_flag respectively.



    Process started by main().

    @see main()

*/

/***********************************************************************/



void PPball_test()

{

  PositionType pos;

  int x_middle = 0, y_middle = 0, ball_size = 0; /* parameters of ball */

  /* float ball_angle;  */      /* angle to ball */



  /* ball coordinates (robot's local/ global coord.syst.) */

  PositionType local, global;

  /*  float new_y;*/

  /* threshold for moving camera down - to middle */

  float thresh_middle_down = 0.06;

  /* threshold for moving camera to middle - up */

  float thresh_up_middle = 0.15;



  int factor = 0;                       /* counts pictures without ball */

  int frame_counter, new_counter;



  frame_counter = OSGetCount();

  while(!end_flag)

  {

    /* first, see if we need to check for a wall */

    if (check_wall_flag)

      CheckForWall();



    /* now see if we need to check for the goal */

    if (look_for_goal_flag)

      {

        found_goal_flag = search_for_goal();

        look_for_goal_flag = FALSE;

      }



    /* now that's out of the way, look for the ball */



    new_counter = OSGetCount();

    LCDSetPos(5, 10);           /* print frame rate */

    if (new_counter != frame_counter)

      LCDPrintf("f:%1.1f\n", 100.0 / (float)(new_counter - frame_counter));

    frame_counter = new_counter;



    VWGetPosition(vw, &pos);



    got_ball_flag = FALSE;      /* haven't caught it yet */

    if (find_ball(&x_middle, &y_middle, &ball_size))

    {

      /* ------ see ball -> set coordinates and see_ball_flag ------ */

      local.y = (float)(imagecolumns / 2 - 1 - y_middle) *

        yfact[cam_pos][x_middle];

      local.x = x2m[cam_pos][x_middle];

      local.phi = 0.0; /* unused */



      local2global(pos, local, &global); /* get global coordinates */





      /* check whether ball is in field */

      if ((global.x > 0.0) && (global.x < LENGTH) && (fabs(global.y) < WIDTH2))

      {

        see_ball_flag = TRUE;   /* really seeing ball -> set flag + coordinates */

        ball_x = global.x;

        ball_y = global.y;



        Update_Ball_Location(x_middle,y_middle);



        factor = 3;             /* allow factor pictures without ball

                                   before looking elsewhere */



        if (local.x < thresh_middle_down)

        {

          if (I_am_goalie())

            kick_ball();  /* kick it away */

          else

            move_camera(DOWN);

          if ((local.x < 0.5 * thresh_middle_down) &&

              (fabs(local.y) < thresh_middle_down))

          {

            see_ball_flag = TRUE; /* don't need to drive to ball position */

            got_ball_flag = TRUE;

          }

        }

        else

          if (local.x < thresh_up_middle)

            move_camera(MIDDLE);

          else

            move_camera(UP);

      }

      else                      /* no ball (found object out of field,

                                   propably goal) */

      {

        Update_Ball_Location(-1,-1);

        if (cam_pos == DOWN)    /* look up for ball */

          move_camera(MIDDLE);

        else

          if (cam_pos == MIDDLE)

          {

            if (I_am_goalie())

              kick_ball();  /* make sure ball is not in front of robot -

                               kick it away before looking up */

            move_camera(UP);

          }

      }

    }

    else                        /* no ball */

    {

      Update_Ball_Location(-1,-1);

      see_ball_flag = FALSE;



      if (factor)               /* keep on looking to presumed ball

                                   position */

        factor  --;

      else

      {

        if (cam_pos == DOWN)    /* look up for ball */

          move_camera(MIDDLE);

        else

          if (cam_pos == MIDDLE)

          {

            if (I_am_goalie())

              kick_ball();  /* make sure ball is not in front of robot -

                               kick it away before looking up */

            move_camera(UP);

          }

      }

    }

    LCDSetPos(0,0);

    LCDPrintf("image done\n");

    OSReschedule();

  }

  end_flag += 1;

  OSKill(0);

}





/***********************************************************************/

/** Get ball coordinates.

    Get position of ball on field as found out in image processing

    routines and orientation of ball towards goal.



    Called by PPdrive_field() and PPdrive_goal().



    @see PPdrive_field()

    @see PPdrive_goal()

    @param *ball pointer to array containing coordinates of ball

    @return ball position and orientation towards goal in referenced

    parameter

*/

/***********************************************************************/



void get_ball_coord(PositionType *ball)

{

 ball->x = ball_x;

 ball->y = ball_y;

 ball->phi = -atan2(ball_y, LENGTH - ball_x); /* neg: angle ball to goal */

}





/***********************************************************************/

/** Get ball colour.

    Get current values for ball colour.



    Called by main().



    @see main()

    @param colour array containing colour values for ball

    @return current colour in referenced parameter

*/

/***********************************************************************/



int get_ball_col()

{

    return BallColour;

}





/***********************************************************************/

/** Get goal colour.

    Get current values for goal colour.



    Called by main().



    @see main()

    @param colour array containing colour values for goal

    @return current colour in referenced parameter

*/

/***********************************************************************/



int get_goal_col(int *blue_goal)

{

  *blue_goal = drive_to_blue_goal;

  return GoalColour;

}







/*@}*/