/* ********************************* */
/* color-detect.c                    */
/* Simple Color Detection Program    */
/* Thomas Braunl, UWA, 2002          */
/* ********************************* */

#include "eyebot.h"
#include <stdlib.h>
#define MIN(a,b) (a<b?a:b)
#define MAX(a,b) (a>b?a:b)
#define NO_HUE -1
#define DEBUG 1

int RGBtoHue(BYTE r, BYTE g, BYTE b)
/* return hue value for RGB color */
{ int hue, delta, max, min;

  max   = MAX(r, MAX(g,b));
  min   = MIN(r, MIN(g,b));
  delta = max - min;
  hue =0; /* init hue*/

  if (2*delta <= max) hue = NO_HUE; /* gray, no color */
  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 */
  }
  return (BYTE) hue; /* now: hue is in range [0..252] */
}


void ColSearch(colimage img, int obj_hue, int thres,
               int *pos, int *val)
/* find x position of color object, return pos and value*/
{ int x,y, count, h, distance;
  *pos = -1; *val = 0;  /* init */
  for (x=1;x<imagecolumns-1;x++)
  { count = 0;
    for (y=1;y<imagerows-1;y++)
    { h = RGBtoHue(img[y][x][0],img[y][x][1],img[y][x][2]);

      if (h != NO_HUE)
      { distance = abs((int)h-obj_hue);  /* hue distance */
        if (distance > 126) distance = 253 - distance;
        if (distance < thres) count++;
      }
    }
    if (count > *val) { *val = count; *pos = x; }
    if (count == 0) count = 1; /* display at least line */
    LCDLine(x,imagerows-2, x, imagerows-2-count, 2); /* demo only */
  }
}


#define X 40
#define Y 40
int main()
{ colimage c;
  int hue, pos, val;
  LCDPrintf("Teach Color\n");
  LCDMenu("TEA","","","");
  CAMInit(NORMAL);
  while ((KEYRead() != KEY1))
  { CAMGetColFrame(&c,0);
    LCDPutColorGraphic(&c);
    hue = RGBtoHue(c[Y][X][0], c[Y][X][1], c[Y][X][2]);
    LCDSetPos(1,0);
    LCDPrintf("R%3d G%3d B%3d\n", c[Y][X][0], c[Y][X][1], c[Y][X][2]);
    LCDPrintf("hue %3d\n", hue);
	if (DEBUG) fprintf(stderr, "hue %3d\n", hue);
    OSWait(100);
  }

  LCDClear();
  LCDPrintf("Detect Color\n");
  LCDMenu("","","","END");
  if (hue==255) LCDPrintf("Invalid Hue\n");
  else
    while (KEYRead() != KEY4)
    { CAMGetColFrame(&c,0);
      LCDPutColorGraphic(&c);
      ColSearch(c, hue, 10, &pos, &val);  /* search image */
      LCDSetPos(1,0);
      LCDPrintf("h%3d p%2d v%2d\n", hue, pos, val);
      LCDLine  (pos, 0, pos, 53, 2);  /* vertical line */
    }
  return 0;
}