#include "visual.h"
#define MIN(a,b)	(a<b?a:b)
#define MAX(a,b)	(a>b?a:b)
#include <math.h>
#include <stdlib.h>

/** Table of distances */
static const float distTable[]={0, 30, 29, 28, 27, 26, 25, 24, 23, 22, 21, 20, 19, 18, 16, 15, 14, 13, 12, 12,
                                11, 10.75, 10.25, 10, 9.25, 9.5, 9, 8.5, 8, 7.5, 7, 6.75, 6.5, 6.25, 6, 5.75, 5.5, 5.25, 5, 4.75,
		                4.5, 4.25, 4, 4, 2.5, 2.25, 2.25, 2, 1.75, 1.75, 1.5, 1.25, 1, 1, 0.25, 0};
/** Size of table of distances */

int RGB2Hue2 (BYTE r, BYTE g, BYTE b) {
	int hue, delta, max, min;

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

	if (2*delta <= max) hue = NO_HUE;
        else {
		if (r==max) hue = 42 + 42*(g-b)/delta;
		else if (g==max) hue = 126 + 42*(b-r)/delta;
		else if (b==max) hue = 210 + 42*(r-g)/delta;
	}
	return (BYTE) hue;
}

void ColSearch2 (colimage img, int obj_hue, int thres, int *pos, int *val) {
	int x, y, count, h, distance;
	*pos = -1; *val = 0;
	for (x=0;x<imagecolumns;x++) {
		count = 0;
		for (y=0;y<imagerows;y++) {
			h = RGB2Hue2(img[y][x][0], img[y][x][1], img[y][x][2]);

			if (h != NO_HUE) {
				distance = abs ((int)h-obj_hue);
				if (distance > 126) distance = 253 - distance;
				if (distance < thres) count++;
			
			}
		}
	if (count > *val) { *val = count; *pos = x; }
	}
}


int findObj(float *distance, colimage *img, rgbhue *pixsearch,lastsizes *dis)
{	
	int foundvar, size;
	int x=0, y=0;
	int xsize=0;
  	int ysize=0;
  	int xhist[imagecolumns], yhist[imagerows];
  
	CAMGetColFrame(img, 0);
#ifdef DEBUG
	LCDPutColorGraphic(img);
	ColSearch2 (*img, RED_HUE, 10, &pos, &val);
	LCDSetPos(1,0);
	LCDPrintf("p%2d v%2d\n", pos, val);
#endif
  
	foundvar=searchBall2d(&x, &y, XTHRESDFLT, YTHRESDFLT, &xsize, &ysize, *img, *pixsearch, RANGEDFLT, STEPSIZEDFLT, xhist, yhist);
	if(foundvar==FOUND)
	{
		size = (xsize+ysize)/2;
		insertSizelib(dis, size);
		*distance = scanDist(dis);
		return FOUND;
	}
	else
	{
		*distance = -1;
		return NOTFOUND;
	}
}


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;
}

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 an all white (clear) image.
 * @return An all white image.
 */
void imgWhite(image clearer)
{
  int ycounter, xcounter;
  for(xcounter=0;xcounter<imagecolumns;xcounter++)
    for(ycounter=0;ycounter<imagerows;ycounter++)
	clearer[ycounter][xcounter]=WHITEPIX;
}

/**
 * @brief Initialises the sizelib structure
 */

// last changes
void initSizelib(lastsizes *x)
{
  int counter;
  
  x->oldest=0;
  x->filled=0;
  for(counter=0; counter<FRAMESIZEAVE; counter++)
  {
    x->sizearray[counter]=0;
  }
}


/**
 * @brief Inserts a size into sizelib.
 */
void insertSizelib(lastsizes *x, int size)
{
  if(x->filled<FRAMESIZEAVE) x->filled++;
  x->sizearray[x->oldest]=size;
  if(x->oldest<FRAMESIZEAVE) x->oldest++;
  else x->oldest=0;
}

/**
 * @brief Displays hue Histograms on the LCD.
 * @param xhist The x histogram populated by searchBall2d.
 * @param yhist The y histogram populated by searchBall2d.
 * @param xsize The width of the ball.
 * @param ysize The height of the ball.
 * @param pix_size size of ball in pixel.
 * @param x co-ordinate of ball (x).
 * @param y co-ordinate of ball (y).
 * @param ballfound Specifies if the ball is found.
 */
void dispHist(int *xhist, int *yhist, int xsize, int ysize, int ballfound, int pix_size, int x, int y )
{
  image clearer;
  int counter;
  
  /* initalise clearer graphic */
  imgWhite(clearer);

  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);
    LCDSetPos(1,10);
    LCDPrintf("X:%3d", x);

    LCDSetPos(2,10);
    LCDPrintf("Y:%3d", y);

    if(ballfound==FOUND)
    {
      LCDSetPos(3,10);
      LCDPrintf("S:%3d", pix_size);
    }
    
    LCDSetPos(0,10);
    if(ballfound==FOUND) 
    {
      LCDPrintf("FOUND");
      LCDArea((x)-xsize/2, (y)-ysize/2, 
              (x)+xsize/2, (y)+ysize/2, 1);
    }
    else LCDPrintf("-----");
}

/* no scanRound*/
/* no scanmaximise*/
/* no scanAngle*/

/**
 * @brief Determines the distance to the ball.
 * @return Distance between 0 and 0.3(m).
 */
float scanDist(lastsizes *x)
{
  float distance;
  int counter, sizeave=0;
  
  for(counter=0;((counter<FRAMESIZEAVE)&&(counter<x->filled));counter++)
    sizeave+=x->sizearray[counter];
  if(x->filled>FRAMESIZEAVE) sizeave/=FRAMESIZEAVE;
  else sizeave/=x->filled;

  if((sizeave>55)||(sizeave<0)) return distance=0.0;
  else return distance=(distTable[sizeave]);
}

/* no servotoRadian */
/* no move to ball*/