// EyeBot Demo Program: Color Detection (red), T. Bräunl, June 2015
#include "eyebot.h"
#define SIZE QVGA_SIZE

// change image array to 1 (match) or 0 (no match) for red hue
void match(BYTE image [SIZE])
{ int red, green, blue;
  for (int i=0; i<SIZE; i+=3)
  { red=image[i]; green=image[i+1]; blue=image[i+2];
    if (red>50 && red>2*green && red>2*blue)
    { image[i]=1; image[i+1]=1; image[i+2]=1;}
    else
    { image[i]=0; image[i+1]=0; image[i+2]=0;}
  }  
}

// find position of max element in array
int maxpos(BYTE f[], int size)
{ int i, pos=0, m=0;
  for (i=0; i<size; i++)
    if (f[i]>m)
    { m = f[i]; pos = i;
    }
  return pos;
}


int main()
{ BYTE image[SIZE];
  BYTE row[160], col[120];
  int a, b;

  if (CAMInit(QQVGA) != CAM_SUCCESS)
  { LCDPrintf("ERROR cam init"); return 1; }

  for (int i=0; i<150; i++)
  { CAMGet(image);
    LCDImage(image);
    match(image);

    // Histogram summatation over each row
    for (int j=0; j<160; j++)
    { row[j] = 0;
      for (int i=0; i<120; i++)
        row[j] += image[3*(i*160+j)];
    }

    // Histogram summatation over each column
    for (int i=0; i<120; i++)
    { col[i] = 0;
      for (int j=0; j<160; j++)
        col[i] += image[3*(i*160+j)];
    }

    a = maxpos(row, 160);
    b = maxpos(col, 120);
    LCDCircle(a,b, 10, GREEN, 1);
    LCDRefresh();
  }
    
  CAMRelease();
  return 0;
}