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

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



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=0;x<imagecolumns;x++)

  { count = 0;

    for (y=0;y<imagerows;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; }

		LCDLine(x,53, x, 53-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);

		OSWait(100);

  }



  LCDClear();

	LCDPrintf("Detect Color\n");

  LCDMenu("","","","END");

  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, 1);  /* vertical line */

  }



  return 0;

}