/**

 * @file findball.c

 * @brief This program identifies the ball and the size and centerpoint of that ball.

 *

 * @author Eric Lim, UWA 2002

 * @author Thomas Braunl, UWA 2002

 *

 * A ball is identified in the digital image taken by an eyebot.

 * The program computes the image hue and runs a histogram over rows and columns

 * to determine the "maximum ball probability" in an image.

 * This is being used to caluculate the ball position and size.

 * The ball color can be set by the user.

 */



#include <eyebot.h>



#define NO 0

#define YES 1

#define NO_HUE -1

#define FOUND 1

#define NOTFOUND 0



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

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



#define MAXHUE 255

#define MINHUE 0



#define WHITEPIX 255



#define PICHEIGHT 55

#define PICWIDTH 80



#define ILLEGALHUE 255

#define NOTDONE 0

#define DONE 1

#define FIRSTINIT 81



#define TWENTIETH 2

#define QUARTERSEC 2

#define HALFSEC 50



#define XTHRESDFLT 2

#define YTHRESDFLT 2

#define RANGEDFLT 8

#define HUEDFLT 62

#define STEPSIZEDFLT 2

#define ILLEGALXY -1





/**

 * @brief Used to pass Red, Green, Blue and Hue values between functions.

 *

 * Used in: defBallColour, searchBall2d, frontBallCol, frontSearch, main.

 */

typedef struct rgbhue

{

  int red;    /**< holds red value from 0-255. */

  int green;  /**< holds green value from 0-255. */

  int blue;   /**< holds blue value from 0-255. */

  int hue;    /**< holds hue value from 0-255. */

} rgbhue;





/**

 * @brief Initialises camera.

 * 

 * Shamelessly copied from imageglobal.c by Birgit Graf and Thomas Braunl. 

 * Modified by Eric Lim for more advanced hardware.

 * 

 */

void camInit(void)

{

  int camera;		/* just used for checking camera status */



  camera = CAMInit(NORMAL);	/* CAMInit is an eyebot library func */



  if (camera == NOCAM)

    OSError("Camera not detected!\n Press any key to continue.\n", 4133, 0);

  else

    if (camera == INITERROR)

      OSError("Camera Initialisation Error!\n Press any key to continue.\n", 4133, 0);

}





/**

 * @brief Releases camera resources.

 */

void camRelease(void)

{

  if(CAMRelease()==-1)

    LCDPrintf("Warning: Camera Resources Not Released\n");

}





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

    

    Called by defBallColour, searchBall2d, frontSearch.

    @author Thomas Braunl, UWA 1998.

    @see defBallColour()

    @see searchBall2d()

    @see frontSearch()

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

    @return hue for single RGB value 

*/



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

{

  int hue /*,sat, val*/, delta, max, min;



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

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

  delta = max - min;

  hue =0;

  /* initialise hue*/

  

  /* val   = max;

     if (max != 0) sat = delta / max; else sat = 0;

     if (sat == 0) hue = NO_HUE;

  */

  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;

}



/**

 * @brief Defines the ball's colour.

 *

 * Called by frontBallCol()

 * 

 * @see frontBallCol()

 * @return red, green, blue and hue values in rgbhue datatype

 */

rgbhue defBallColour(void)

{

  colimage img;

  int row, column, count=0;

  rgbhue pixave;



  pixave.red=pixave.green=pixave.blue=pixave.hue=0;

  

  /* Capture the colour image */

  CAMGetColFrame(&img, NO);



  /* Analyse the colour image */

  /* Use the middle 5 (odd num easier) squares */



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

  { 

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

    {

      /* update average values */

      pixave.red+=img[row][column][0];

      pixave.green+=img[row][column][1];

      pixave.blue+=img[row][column][2];

      count++;

    }

  }



  pixave.red/=count;

  pixave.green/=count;

  pixave.blue/=count;



  pixave.hue=RGBtoHue(pixave.red, pixave.green, pixave.blue);

  return pixave;

}



/**

 * @brief Searches an image for the location of a ball.

 *

 * Called by frontSearch.

 *

 * @see frontSearch() 

 * @param x column where ball's center is located (used for output)

 * @param y row where ball's center is located (used for output)

 * @param xthres number of hue matches that constitute a ball being found in the x

 * @param ythres number of hue matches that constitute a ball being found in the y

 * @param image input colour image

 * @param pixbuff rgbhue struct containing values to search for

 * @param xsize width of ball (used for output)

 * @param ysize height of ball (used for output)

 * @param hue hue of ball on image

 * @param huerange range of hues to search for

 * @param stepsize check every "stepsize" pixel

 * @param xhist histogram of hue density in x (used for output) 

 * @param yhist histogram of hue density in y (used for output)

 * @return FOUND or NOTFOUND

 */



int searchBall2d(int *x, int *y, int xthres, int ythres, int *xsize, int *ysize, colimage image, rgbhue pixbuff, int huerange, int stepsize, int *xhist, int *yhist)

{

  int xfirst,xlast, yfirst,ylast;

  int currenthue=ILLEGALHUE, huemax, huemin; 

  int xcounter, ycounter; 

  int xstatus=NOTFOUND, ystatus=NOTFOUND;



  huemin=pixbuff.hue-huerange;    /* set minimum hue allowed */

  huemax=pixbuff.hue+huerange;    /* set maximum hue allowed */



  if(pixbuff.hue>MAXHUE || pixbuff.hue<MINHUE) return NOTFOUND;

  if(huemin<MINHUE || huemax>MAXHUE) return NOTFOUND;



  /* initialise histograms */

  for(xcounter=0;xcounter<imagecolumns;xcounter++) xhist[xcounter]=0;

  for(ycounter=0;ycounter<imagerows;ycounter++) yhist[ycounter]=0;

  

  /* fill histograms */

  for(xcounter=1;xcounter<imagecolumns-1;xcounter+=stepsize)

  {

    for(ycounter=1;ycounter<imagerows-1;ycounter+=stepsize)

    {

      /* Making histogram */

      currenthue=RGBtoHue(image[ycounter][xcounter][0], image[ycounter][xcounter][1], image[ycounter][xcounter][2]);

      if((currenthue>huemin)&&(currenthue<huemax))  /* Check if in hue range */      

      {

        xhist[xcounter]++;

	yhist[ycounter]++;

      }

    }

  }



  /* check histograms */

  xfirst=0;  /* first x where ball is found */

  xlast =0;  /* first x where ball is found */

  xstatus=NOTFOUND;

  /* Stop checking when center is found or end of array */

  for((xcounter)=1; ((xcounter<imagecolumns-1)&&(xstatus!=FOUND)); xcounter+=stepsize)

  {

    /* if it is a ball */

    if(xhist[xcounter]>=xthres)

    {

      if(!xfirst) xfirst=xcounter;

      xlast = xcounter;

      if(xhist[xcounter+stepsize] < xthres) xstatus=FOUND;

    }

  }

  (*xsize) = xlast - xfirst;

  (*x)     = xfirst + ((*xsize)/2);



  yfirst=0;

  ylast =0;

  ystatus=NOTFOUND;

  for(ycounter=1; ((ycounter<imagerows-1)&&(ystatus!=FOUND)); ycounter+=stepsize)

  {

    if(yhist[ycounter]>=ythres)

    {

      if(!yfirst) yfirst=ycounter;

      ylast = ycounter;

      if(yhist[ycounter+stepsize] < ythres) ystatus=FOUND;

    }

  }

  (*ysize) = ylast - yfirst;

  (*y)     = yfirst + ((*ysize)/2);





  if((ystatus==FOUND)&&(xstatus==FOUND)) return FOUND;

  else return NOTFOUND;

}





/**

 * @brief Creates the Main Menu.

 * @param sethue The hue that the program is looking for.

 * @return KEY1, KEY2, KEY3, KEY4

 */

int menuMain(int sethue)

{

  LCDClear();

  LCDSetPos(0,3);

  LCDPrintf("Find  Ball\n\n");

  LCDPrintf("[1] Search (%03d)", sethue);

  LCDPrintf("[2] SetHue\n");

  LCDPrintf("[3] Settings\n");



  LCDMenu("[1]", "[2]", "[3]", "EXIT");

  return KEYGet();

}



/**

 * @brief Handles interaction for defBallColour

 *

 * Its just another pretty frontend.

 *

 * @see defBallColour()

 * @param oldpix Previous rgbhue that we were looking for.

 */

void frontBallCol(rgbhue *oldpix)

{

  rgbhue pixbuff, oldbuff;

  oldbuff=*oldpix;

  while(KEYRead()!=KEY4)

  {

    pixbuff=defBallColour();

    LCDClear();

    LCDPrintf("Setting Ball Hue\n");

    if(pixbuff.hue==NO_HUE)

    {

      LCDPrintf("Too Dark/Light\n");

      LCDPrintf("Hue not captured\n");

      LCDPrintf("Old:%d ", oldbuff.hue);

      LCDPrintf(" R:%d\n G:%d\n B:%d\n", pixbuff.red, pixbuff.green, pixbuff.blue);

    }

    else

    {

      LCDPrintf("Old:%d New:%d\n", oldbuff.hue, pixbuff.hue);

      LCDPrintf(" R:%d\n G:%d\n B:%d\n", pixbuff.red, pixbuff.green, pixbuff.blue);

    }

    LCDMenu(" ", " ", " ", "EXIT");

  }

  if(pixbuff.hue!=NO_HUE)

    *oldpix=pixbuff;

}





/**

 * @brief Changes the number of rows/columns skipped.

 *

 * Changes the variable stepsize from main which determines which 

 * rows/columns are checked.

 * 

 * @param stepsize Variable from main.

 */

void setStepSize(int *stepsize)

{

  int stepbuff, keybuff;

  

  stepbuff=*stepsize;

  while(keybuff!=KEY4)

    {

    LCDClear();

    LCDSetPos(0,3);

    LCDPrintf("Step Range\n\n");

    LCDPrintf("Currently: %d\n", stepbuff);

  

    LCDMenu("UNDO"," <- "," -> ","EXIT");

    switch(KEYGet())

    {

      case KEY1:

        stepbuff=*stepsize;

        break;

      case KEY2:

        --stepbuff;

	break;

      case KEY3:

        ++stepbuff;

	break;

      case KEY4:

      {

        *stepsize=stepbuff;

        keybuff=KEY4;

	break;

      }

    }

  }

}





/**

 * @brief Changes the range of hues that program looks for.

 * 

 * Changes the variable huerange from main which determines how much

 * deviation from the value set in defBallColour is allowed before the

 * pixel is no longer recognised as a ball.

 *

 * @param huerange Variable from main.

 */

void setHueRange(int *huerange)

{

  int rangebuff, keybuff;

  

  rangebuff=*huerange;

  while(keybuff!=KEY4)

    {

    LCDClear();

    LCDSetPos(0,3);

    LCDPrintf("Hue  Range\n\n");

    LCDPrintf("Currently: %d\n", rangebuff);

  

    LCDMenu("UNDO"," <- "," -> ","EXIT");

    switch(KEYGet())

    {

      case KEY1:

        rangebuff=*huerange;

	break;

      case KEY2:

        --rangebuff;

	break;

      case KEY3:

        ++rangebuff;

	break;

      case KEY4:

      {

        *huerange=rangebuff;

        keybuff=KEY4;

	break;

      }

    }

  }

}





/**

 * @brief Sets the X threshold level.

 * 

 * The X threshold is the number of pixels in a column that 

 * have to have a hue within acceptable limits before a 

 * ball is recognised as being in that column.

 * 

 * @param xthres Variable from main

 */

void setXthres(int *xthres)

{

  int thresbuff, keybuff;

  

  thresbuff=*xthres;

  while(keybuff!=KEY4)

    {

    LCDClear();

    LCDSetPos(0,2);

    LCDPrintf("X  Threshold\n\n");

    LCDPrintf("Old: %d\n", *xthres);

    LCDPrintf("New: %d\n", thresbuff);

  

    LCDMenu("UNDO"," <- "," -> ","EXIT");

    switch(KEYGet())

    {

      case KEY1:

        thresbuff=*xthres;

	break;

      case KEY2:

        --thresbuff;

	break;

      case KEY3:

      ++thresbuff;

	break;

      case KEY4:

      {

        *xthres=thresbuff;

        keybuff=KEY4;

	break;

      }

    }

  }

}





/**

 * @brief Sets the Y threshold level.

 *

 * The Y threshold is the number of pixels in a row that

 * have to have a hue within acceptable limits before a ball

 * is recognised as being in that row.

 *

 * @param ythres Variable from main

 */

void setYthres(int *ythres)

{

  int thresbuff, keybuff;

  

  thresbuff=*ythres;

  while(keybuff!=KEY4)

  {

    LCDClear();

    LCDSetPos(0,2);

    LCDPrintf("Y  Threshold\n\n");

    LCDPrintf("Old: %d\n", *ythres);

    LCDPrintf("New: %d\n", thresbuff);

  

    LCDMenu("UNDO"," <- "," -> ","EXIT");

    switch(KEYGet())

    {

      case KEY1:

        thresbuff=*ythres;

	break;

      case KEY2:

        --thresbuff;

	break;

      case KEY3:

        ++thresbuff;

	break;

      case KEY4:

      {

        *ythres=thresbuff;

        keybuff=KEY4;

	break;

      }

    }

  }

}





/**

 * @brief Creates the Settings Menu

 * 

 * Calls setXthres, setYthres, setStepSize, setHueRange.

 * 

 * @param xthres The x threshold

 */

void menuSettings(int *xthres, int *ythres, int *stepsize, int *huerange)

{

  int key, threskey;

  key=KEY1;

  while(key!=KEY4)

  {

    LCDClear();

    LCDSetPos(0,4);

    LCDPrintf("Settings\n\n");

    LCDPrintf("[1]Thres(%02d,%02d)\n", *xthres, *ythres);

    LCDPrintf("[2]Step Size(%02d)", *stepsize);

    LCDPrintf("[3]Hue Range(%02d)\n", *huerange);

  

    LCDMenu("[1]","[2]","[3]","EXIT");

    switch(key=KEYGet())

    {

    case KEY1:

    {

      LCDClear();

      LCDSetPos(1,0);

      LCDPrintf("Choose Dimension\n");



      LCDMenu(" X", " Y", " ", "EXIT");

      threskey=KEYGet();

      if(threskey==KEY1) setXthres(xthres);

      if(threskey==KEY2) setYthres(ythres);

      if(threskey==KEY4) break;

      break;

    }

    case KEY2:

      setStepSize(stepsize);

      break;

    case KEY3:

      setHueRange(huerange);

      break;

    }

  }

}





/**

 * @brief Handles displaying results and images for searchBall2d.

 *

 * 3 different modes:

 *  - VAL

 *   - Shows various relavent numerical values.

 *  - GRA

 *   - Shows the current frame of the picture.

 *  - HIST

 *   - Shows the histograms and detected ball in graphical form.

 * 

 * @see searchBall2d()

 * @param x column where ball's center is located (used for output)

 * @param y row where ball's center is located (used for output)

 * @param xthres number of hue matches that constitute a ball being found in the x

 * @param ythres number of hue matches that constitute a ball being found in the y

 * @param size diameter of ball

 * @param pixbuff rgbhue struct containing values to search for 

 * @param range variation in hue allowed whilst still being recognised as a ball

 * @param stepsize determines which rows/columns are checked 

 */

void frontSearch(int *x,int *y,int xthres,int ythres,int *size,rgbhue pixbuff,int range,int stepsize)

{

  colimage img;

  image clearer;  /* Used to clear histogram pos */

  int xhist[imagecolumns], yhist[imagerows], counter, xcounter, ycounter, xsize,ysize,ballfound;

  int keybuff=KEY3;

  int lastkey=KEY3;  /* last key pressed */

  int starttime, endtime;

  

  /* initialise var clearer */

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

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

      clearer[ycounter][xcounter]=WHITEPIX;    /* Make every pixel white */

  

  LCDClear();

  starttime=OSGetCount();

  while(lastkey!=KEY4)

  {

    endtime=OSGetCount();

    LCDSetPos(5,10);

    if(starttime!=endtime) LCDPrintf("f:%1.1f\n", 100.0 / (float)(endtime-starttime));

    starttime=endtime;

    CAMGetColFrame(&img, NO);

    

    if((keybuff=KEYRead())!=0) lastkey=keybuff;  /* KEYRead returns 0 if no key is pressed */

    if(lastkey==KEY2)  /* Display the picture from the camera */

    {

      if(keybuff!=0) LCDClear(); /* Clear if new */

      /* First line: FOUND? */

      LCDSetPos(0,10);

      ballfound = searchBall2d(x, y, xthres, ythres, &xsize, &ysize, img, pixbuff, range, stepsize, xhist, yhist);

      (*size) = (xsize+ysize)/2;

      if (ballfound==FOUND)

	    LCDPrintf("FOUND");

      else  LCDPrintf("-----");



      /* Second line: RED */

      LCDSetPos(1,10);

      LCDPrintf("R:%3d", img[imagerows/2][imagecolumns/2][0]);

  

      /* Third line: GREEN */

      LCDSetPos(2,10);

      LCDPrintf("G:%3d", img[imagerows/2][imagecolumns/2][1]);



      /* Fourth line: BLUE */

      LCDSetPos(3,10);

      LCDPrintf("B:%3d", img[imagerows/2][imagecolumns/2][3]);



      /* Fifth line: HUE */

      LCDSetPos(4,10);

      LCDPrintf("H:%3d", RGBtoHue(img[imagerows/2][imagecolumns/2][0], img[imagerows/2][imagecolumns/2][1], img[imagerows/2][imagecolumns/2][2]));



      LCDPutColorGraphic(&img);

    }

    

    if(lastkey==KEY3)  /* Display histogram */

    {

      if(keybuff!=0) LCDClear();

      LCDSetPos(0,10);

      ballfound = searchBall2d(x, y, xthres, ythres, &xsize, &ysize, img, pixbuff, range, stepsize, xhist, yhist);

      (*size) = (xsize+ysize)/2;

      if (ballfound==FOUND) 

           LCDPrintf("FOUND");

      else LCDPrintf("-----");

      /* Clear histogram space */

      LCDPutGraphic(&clearer);

      /* Draw Y histogram */

      for(counter=1;counter<PICHEIGHT;counter++)

      {

        /* Draw a line for each Y value */

        if(yhist[counter]>0) LCDLine(0,counter-1,yhist[counter],counter-1,1);

      }



      /* Draw X histogram */

      for(counter=1;counter<PICWIDTH;counter++)

      {

        /* Draw a line for each X value */

        if(xhist[counter]>0) LCDLine(counter-1,0,counter-1,xhist[counter],1);

      }

      

      /* Draw thresholds */

      if (ballfound==FOUND     /* mark ball */)

         LCDArea((*x)-xsize/2, (*y)-ysize/2, 

                 (*x)+xsize/2, (*y)+ysize/2, 1);



      LCDSetPos(1,10);

      LCDPrintf("X:%3d", *x);



      LCDSetPos(2,10);

      LCDPrintf("Y:%3d", *y);



      LCDSetPos(3,10);

      LCDPrintf("S:%3d", *size);

    }

    

    if(lastkey==KEY1)

    {

      if(keybuff!=0) LCDClear();

      LCDSetPos(0,0);

      LCDPrintf("Found: ");

      if(searchBall2d(x, y, xthres, ythres, &xsize, &ysize, img, pixbuff, range, stepsize, xhist, yhist)==FOUND) LCDPrintf("Y\n");

      else  LCDPrintf("N\n");

      LCDSetPos(1,0);

      LCDPrintf("X:%3d Y:%3d Size:%3d\n", *x, *y, *size);

      LCDSetPos(2,0);

      LCDPrintf("%03d,%03d,%03d,%03d\n", img[imagerows/2][imagecolumns/2][0], img[imagerows/2][imagecolumns/2][1], img[imagerows/2][imagecolumns/2][2], RGBtoHue(img[imagerows/2][imagecolumns/2][0], img[imagerows/2][imagecolumns/2][1], img[imagerows/2][imagecolumns/2][2]));

      LCDSetPos(3,0);

      LCDPrintf("%03d,%03d,%03d,%03d\n", pixbuff.red, pixbuff.green, pixbuff.blue, pixbuff.hue);

    }

    

    LCDMenu("VAL","GRA","HIST"," OK");

  }

}



int main(void)

{

  int x, y, size, xthres, ythres, range, stepsize, keybuff;

  rgbhue pixbuff;

  

  x=y=ILLEGALXY;

  size=0;

  pixbuff.hue=HUEDFLT;

  pixbuff.red=pixbuff.green=pixbuff.blue=0;

  xthres=XTHRESDFLT;

  ythres=YTHRESDFLT;

  range=RANGEDFLT;

  stepsize=STEPSIZEDFLT;



  camInit();

  LCDMode(NOCURSOR);

  LCDMode(NONSCROLLING);

  while((keybuff=menuMain(pixbuff.hue))!=KEY4)

  {

    switch(keybuff)

    {

      case KEY2:

        frontBallCol(&pixbuff);

	break;

      case KEY1:

        frontSearch(&x, &y, xthres, ythres, &size, pixbuff, range, stepsize);

	break;

      case KEY3:

	menuSettings(&xthres, &ythres, &stepsize, &range);

    }

  }

  camRelease();

  return 0;

}



