#include "Camera.h"
#include "ImageFilter.h"
#include "eyebot.h"

int cubeColor = 55;
int allowedCubeColorDeviation = 10;

bool g_hasCubeColor (BYTE hue)
{
	if (hue != 255)
	{
		//hue and cubeColor 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
		int distance = (int)hue-cubeColor;
    if (distance < 0)
      distance = -distance;
		//if the distance is larger than 126 than the other
		//way round is shorter
		if (distance > 126)
			distance = 253 - distance;
		if (distance <= allowedCubeColorDeviation)
			return true;
	}
	return false;
}



int main(void)
{
  Camera* camera;
  camera = Camera::getCamera();
  ImageFilter imageFilter;

  image hues; //to store the hues of the currentColorImage
  colimage colorImage;
  int key = -1;

  LCDClear(); 
  while (1)
  {
    camera->getColorFrame(colorImage);

     for (int row = 1; row <=imagerows-2; row++)
      imageFilter.calculateHue(colorImage, row, hues);
    for (int row = 1; row <=imagerows-2; row++)
      for (int column = 1; column <=imagecolumns-2; column++)
        if (g_hasCubeColor(hues[row][column]))
          hues[row][column] = 30;
        else
          hues[row][column] = 255;
     LCDPutGraphic(&hues);
   
   
    int key = KEYRead();
    if (key == KEY1)
    {
      cubeColor++;
      if (cubeColor>252)
        cubeColor = 0;
      
    }
    if (key == KEY2)
    {
      cubeColor--;
      if (cubeColor<0)
        cubeColor = 252;
    }
     
      LCDSetPos(0,0);
      LCDPrintf("color %d\n", cubeColor);
   
  }
  
  return 0;
}