/***********************************************************************************************************/
/*                 L I B V I S I O N   v2.00                                                               */
/*                                                                                                         */
/* Philippe LECLERCQ                                                                                       */
/* -----------------                                                                                       */
/*                  November 16th 2000                                                                     */
/*                                                                                                         */
/*                                                                                   lecle-p@ee.uwa.edu.au */
/***********************************************************************************************************/

#include "LibVision.h"
#include <string.h>

/* Algorithm information, ONLY depending on VIS_MSB in the .h */
#define VIS_LSB      (8-VIS_MSB)  /* Less significant bits */
#define VIS_VALUES   (1<<VIS_MSB) /* Number of intervals */
#define VIS_SHIFT    (1<<VIS_LSB) /* Shift for loops */
#define NO_HUE       255          /* Default value for VIS_RGB2Hue() function */

/* Global variables */
VIS_HueTable       HueTable;       /* Global hue-table conversion */
BYTE               HueTableOK=0;   /* Flag if hue-table is initialised or not */
VIS_Algo           AlgoInfo;       /* Global algorithm information */
BYTE               AlgoInfoOK=0;   /* Flag if the algorithm informations are initialised or not */
VIS_CameraOffset   CamInfo;        /* Camera setup for distance */
BYTE               CamInfoOK=0;    /* Flag of the camera informations are initialised or not */
VIS_DistanceTables DistTables;     /* Distance tables */
BYTE               DistTablesOK=0; /* Flag if the distance tables are initialised or not*/

/* convert radian to degrees */
/*static double rad2deg(radians rad) { return 180*rad/M_PI;}*/


/* Minimum function */
#define MIN(a,b) (a<b?a:b)
/* Maximum function */
#define MAX(a,b) (a>b?a:b)

/***********************************************************************************************************/
/* LibVision functions                                                                                     */
/***********************************************************************************************************/
/****f* LibVision/VIS_RGB2Hue
 * SYNOPSIS
 *   BYTE VIS_RGB2Hue(BYTE R, BYTE G, BYTE B)  
 * DESCRIPTION
 *   Converts (R,G,B) triplet into Hue value.
 *   Formula taken from the "Digital Image Processing" (1992) Allison Wesley
 * SEE ALSO
 *   VIS_RGB2Sat VIS_RGB2Int
 * Change RBG to HSV -- use hue only.
 * Thomas Braunl, UWA 1998.
 ****/
BYTE VIS_RGB2Hue(BYTE r, BYTE g, BYTE b)
{
  BYTE hue=0, delta, max, min;

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

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

/****f* LibVision/VIS_RGB2Sat
 * SYNOPSIS
 *   BYTE VIS_RGB2Sat(int R, int G, int B)  
 * DESCRIPTION
 *   Converts (R,G,B) triplet into Saturation value.
 *   Formula taken from the "Digital Image Processing" (1992) Allison Wesley
 * SEE ALSO
 *   VIS_RGB2Hue VIS_RGB2Int
 ****/
BYTE VIS_RGB2Sat(BYTE R, BYTE G, BYTE B)
{
	if ((R+G+B) == 0)
		return 255;
	else
	  return ((BYTE)((255-(765/(R+G+B)))*MIN(MIN(R,G),B)));
}

/****f* LibVision/VIS_RGB2Int
 * SYNOPSIS
 *   BYTE VIS_RGB2Int(int R, int G, int B)  
 * DESCRIPTION
 *   Converts (R,G,B) triplet into Intensity value.
 *   Formula taken from the "Digital Image Processing" (1992) Allison Wesley
 * SEE ALSO
 *   VIS_RGB2Hue VIS_RGB2Sat
 ****/
BYTE VIS_RGB2Int(BYTE R, BYTE G, BYTE B)
{
  return ((BYTE)((R+G+B)/3));
}

/****f* LibVision/VIS_FillHueTable
 * SYNOPSIS
 *   void VIS_FillHueTable(VIS_HueTable *HueTable)  
 * DESCRIPTION
 *   Fills the Hue table for faster conversion. Sets the HueTableOK flag to 1.
 * SEE ALSO
 *   VIS_FillRGBSpace
 ****/
void VIS_FillHueTable(void)
{
  int  max_loop=VIS_VALUES*VIS_VALUES; /* Last index for loop progress display */
  BYTE R_index, G_index, B_index;

  /* Initialise progress display */
  VIS_InitShowProgress("   RGB -> Hue");

  for (R_index=0; R_index<=VIS_VALUES; R_index++) {
    for (G_index=0; G_index<=VIS_VALUES; G_index++) {
      /* Display progress */
      VIS_ShowProgress(G_index+(R_index<<VIS_MSB),max_loop);

      for (B_index=0; B_index<=VIS_VALUES; B_index++) {
	/* Fill in table */
	HueTable[R_index][G_index][B_index]=VIS_RGB2Hue(R_index<<VIS_LSB, G_index<<VIS_LSB,
							B_index<<VIS_LSB);
      }
    }
  }
  HueTableOK=1;
}

/****f* LibVision/VIS_FillRGBSpace()
 * SYNOPSIS
 *   void VIS_FillRGBSpace(int HueMin, int HueMax, int ColourClass, 
 *                         VIS_HueTable *HueTable, VIS_RGBSpace *RGBSpace)  
 * DESCRIPTION
 *   Fills the RGB space cube with a colour class, using a saturation threshold (VIS_SAT_THRES).
 * SEE ALSO
 *   VIS_ColInit VIS_ColClear VIS_ColFind VIS_FillHueTable
 ****/
void VIS_FillRGBSpace(int HueMin, int HueMax, BYTE ColourClass, VIS_RGBSpace *RGBSpace)
{
  int  max_loop=VIS_VALUES*VIS_VALUES; /* Last index for loop progress display */
  BYTE R_index, G_index, B_index;
  BYTE RGBValue, HueValue;

  /* Initialise progress display */
  VIS_InitShowProgress("    Fill RGB");

  for (R_index=0; R_index<VIS_VALUES; R_index++) {
    for (G_index=0; G_index<VIS_VALUES; G_index++) {
      /* Display progress */
      VIS_ShowProgress(G_index+(R_index<<VIS_MSB),max_loop);

      for (B_index=0; B_index<VIS_VALUES; B_index++) {
	/* Init inner loop variables */
	RGBValue=(*RGBSpace)[R_index][G_index][B_index];
	HueValue=HueTable[R_index][G_index][B_index];

	/* Check the Saturation, to avoid white */
	if (VIS_RGB2Sat(R_index, G_index, B_index)>VIS_SAT_THRES) {

	  /* Sat>Threshold -> Check for any colour class conflict */
	  if ((RGBValue==VIS_CONFLICT) || (RGBValue!=VIS_NONE)) {
	    (*RGBSpace)[R_index][G_index][B_index]=VIS_CONFLICT; /* YES, use default colour conflict class */
	    LCDSetPrintf(3, 0, "    Conflict"); /* Display if there was a class conflict */
	  }
	  else {
	    /* NO conflict, check if corresponding Hue is within the limits */
	    if ((HueValue<=HueMax) && (HueValue>=HueMin)) {
	      (*RGBSpace)[R_index][G_index][B_index]=ColourClass; /* YES, use the given class number */
	      LCDSetPrintf(3, 0, "Col"); /* Display when it finds class */
	    }
	    else { 
	      (*RGBSpace)[R_index][G_index][B_index]=VIS_NONE; /* NO, use default no colour class number */
              LCDSetPrintf(3, 0, "   "); LCDSetPrintf(3, 12, "   "); /* Erase when nothing is found */
	    }
	  }
	}
	else { LCDSetPrintf(3, 12, "SAT");} /* Display when it's saturated */
      }
    }
  }  
}

/****f* LibVision/VIS_ReduceNoise
 * SYNOPSIS
 *   int VIS_ReduceNoise(VIS_Process *Process, int len)  
 * DESCRIPTION
 *   Denoise every pixels.
 * SEE ALSO
 *   VIS_ReduceNoisef VIS_ReduceNoise2 VIS_ReduceNoise2f
 ****/
int VIS_ReduceNoise(VIS_Process *Process, int len)
{
  int  l_index=0;
  int  row;
  int  imgcols=imagecolumns;
  BYTE *temp=Process->table, *X=Process->X_list, *Y=Process->Y_list;

  for (row=len+1; row<=(imagerows-len-2); row++) { 
    int col;
    for (col=len+2; col<=(imgcols-len-2); col++) {
      int res=0, depth;
      for (depth=1; depth<=len; depth++) {
	int pos=row*imgcols+col;
        res+=(temp[pos-imgcols*depth] & temp[pos+depth] & temp[pos+imgcols*depth] & temp[pos-depth]);
      }
      if (res==len) { X[l_index]=row; Y[l_index]=col; l_index++;}
    }
  }
  return l_index-1;
}

/****f* LibVision/VIS_ReduceNoisef
 * SYNOPSIS
 *   int VIS_ReduceNoisef(VIS_Process *Process)  
 * DESCRIPTION
 *   Denoise every pixels f i.e. no for, one pixel around.
 * SEE ALSO
 *   VIS_ReduceNoise VIS_ReduceNoise2 VIS_ReduceNoise2f
 ****/
int VIS_ReduceNoisef(VIS_Process *Process)
{
  int  l_index=0;
  int  row;
  int  imgcols=imagecolumns;
  BYTE *temp=Process->table, *X=Process->X_list, *Y=Process->Y_list;

  for (row=2; row<=imagerows-1-2; row++) {
    int col;
    for (col=3; col<=imgcols-1-2; col ++) {
      int pos=row*imgcols+col;
      if (temp[pos-imgcols] & temp[pos+1] & temp[pos+imgcols] & temp[pos-1]) 
	{ X[l_index]=row; Y[l_index]=col; l_index++;}
    }
  }
  return l_index-1;
}

/****f* LibVision/VIS_ReduceNoise2
 * SYNOPSIS
 *   int VIS_ReduceNoise2(VIS_Process *Process, int len)  
 * DESCRIPTION
 *   Denoise 2 i.e. every two pixels.
 * SEE ALSO
 *   VIS_ReduceNoise VIS_ReduceNoisef VIS_ReduceNoise2f
 ****/
int VIS_ReduceNoise2(VIS_Process *Process, int len)
{
  int  l_index=0;
  int  row_index;
  int  imrows=(imagerows>>1), imcols=(imagecolumns>>1);
  BYTE *temp=Process->table, *X=Process->X_list, *Y=Process->Y_list;

  for (row_index=(len+1); row_index<=imrows-len-1; row_index++) {
    int row=(row_index<<1);
    int col_index;
      for (col_index=len+1; col_index<=imcols-len-1; col_index++) {
      int res=0, depth;
      for (depth=1<<1; depth<=(len<<1); depth+=2) {
	int pos=row_index*imcols+col_index;
        res+=(temp[pos-imcols*depth] & temp[pos+depth] & temp[pos+imcols*depth] & temp[pos-depth]);
      }
      if (res==len) {
	int col=(col_index<<1); 
	X[l_index]=row; Y[l_index]=col; l_index++;
      }
    }
  }
  return l_index-1;
}

/****f* LibVision/VIS_ReduceNoise2f
 * SYNOPSIS
 *   int VIS_ReduceNoise2f(VIS_Process *Process)  
 * DESCRIPTION
 *   Denoise 2 i.e. every two pixels f i.e. fast (no for, one pixel around)
 * SEE ALSO
 *   VIS_ReduceNoise VIS_ReduceNoisef VIS_ReduceNoise2
 ****/
int VIS_ReduceNoise2f(VIS_Process *Process)
{
  int  l_index=0;
  int  row_index;
  int  imrows=(imagerows>>1), imcols=(imagecolumns>>1);
  BYTE *temp=Process->table, *X=Process->X_list, *Y=Process->Y_list;

  for (row_index=1; row_index<=imrows-2; row_index++) {
    int row=(row_index<<1);
    int col_index;
    for (col_index=3; col_index<=imcols-2; col_index++) {
      int pos=row_index*imcols+col_index;
      if (temp[pos-imcols] & temp[pos+1] & temp[pos+imcols] & temp[pos-1]) {
	int col=(col_index<<1);
	X[l_index]=row; Y[l_index]=col; l_index++;
      }
    }
  }
  return l_index-1;
}

/****f* LibVision/VIS_FindLimits
 * SYNOPSIS
 *   void VIS_FindLimits(int* pTop, int* pBot, int* pRight, int* pLeft, VIS_Object *Object)  
 * DESCRIPTION
 *   Find the limits of the segmented objects.
 * SEE ALSO
 *   VIS_FindClasses
 ****/
void VIS_FindLimits(int* pTop, int* pBot, int* pRight, int* pLeft, VIS_Process *Process)
{
  int  i;
  int  right=*pRight, left=*pLeft;
  int  l_index=Process->index;
  BYTE *Y=Process->Y_list, *X=Process->X_list;

  /* Look for right and left values */
  for (i=l_index; i>=0; i--) { /*for (i=0; i<=l_index; i++) {*/
    int y_value=Y[i];
    if      (y_value<left) { left=y_value;}   /*if (Y[i]<*pLeft) { *pLeft=Y[i];}*/
    else if (y_value>right) { right=y_value;} /*if (Y[i]>*pRight) { *pRight=Y[i];}*/
  }
  /* Assign values */
  *pTop=X[0]; *pBot=X[l_index]; /* Values are inserted from TOP to BOTTOM in denoising function... */
  *pLeft=left; *pRight=right;
}

/****f* libvision/VIS_FindClasses
 * SYNOPSIS
 *   void VIS_FindClasses(Picture *PicInfo, VIS_RGBSpace *RGBSpace, VIS_Object *Object, int ColourClass)
 * DESCRIPTION
 *   Build a binary map for a specific colour class.
 * SEE ALSO
 *   VIS_FindLimits VIS_ColFind
 ****/
void VIS_FindClasses(colimage *Pic, VIS_RGBSpace *RGBSpace, VIS_Process *Process, BYTE ColourClass)
{
  int           loop=AlgoInfo.loop; /* AlgoInfo is global */
  int           loop_offset=2>>loop;
  int           imrows=(imagerows>>loop), imcols=(imagecolumns>>loop);
  int           row_index=0; /* Processed picture index */
  unsigned char *table=Process->table;
  
  /* row_index and col_index : processed picture indexes (i.e. object indexes) */
  for (row_index=0; row_index<=imrows-loop_offset; row_index++) {
    int row=(row_index<<loop);   /* Camera picture index */
    int col_index=0;             /* Processed picture index */
    for (col_index=loop_offset+1; col_index<=imcols-loop_offset; col_index++) {
      int col=(col_index<<loop); /* Camera picture index */
      
      /* Get class of the current pixel */
      unsigned char PixelClass=(*RGBSpace)[(*Pic)[row][col][0]>>VIS_LSB]
	                                  [(*Pic)[row][col][1]>>VIS_LSB]
	                                  [(*Pic)[row][col][2]>>VIS_LSB];

      /* Fill in the binary tables (undenoised) for the object */
      int offset=row_index*imcols+col_index;   /* Objects table linear index */
      table[offset]=(PixelClass==ColourClass); /* Fills table with 0 or 1 */
    }
  }
}

/****f* libvision/VIS_MedianHue
 * SYNOPSIS
 *   int VIS_MedianHue(Picture *PicInfo, int size)
 * DESCRIPTION
 *   Finds median hue out of the middle part of the picture.
 *   "size" can't be more than 10.
 * SEE ALSO
 *
 ****/
BYTE VIS_MedianHue(colimage *Pic, int size)
{
  int Row, Col, RowStart, ColStart, RowPos, ColPos, Pos, Index;
  int MedIndex=(size*size)>>1;
  int min, minIndex=0;
  int Array[100]; /* 10x10 is the limit, to get rid of the ansi C warning... */
  
  /* Start indexes */
  RowStart=(imagerows-size)>>1; /* RowStart=(height/2)-(size/2)*/
  ColStart=(imagecolumns-size)>>1;  /* ColStart=(width/2)-(size/2)*/
  
  /* Filling "Unsorted" table */
  Index=0;
  for (Row=0; Row<size; Row++) {
    for (Col=0; Col<size; Col++) {
      RowPos=RowStart+Row; ColPos=ColStart+Col; /* Picture indexes */
      Array[Index++]=VIS_RGB2Hue((*Pic)[RowPos][ColPos][0], /* Filling "Unsorted" table */
				 (*Pic)[RowPos][ColPos][1],
				 (*Pic)[RowPos][ColPos][2]);
    }
  }
  
  /* Main loop finding the median value */
  for (Index=0; Index<=MedIndex; Index++) {
    /* Sub-loop looking for the left local min */
    min=255; /* Init. min value */
    for (Pos=0; Pos<size*size; Pos++) {
      if (Array[Pos]<min) { /* Looking for min */ 
	min=Array[Pos]; /* YES, update min value and min position */
	minIndex=Pos;
      }
    }
    if (Index!=MedIndex) { /* Last Value ? Do not overwrite last value ! */
      Array[minIndex]=255; /* NO, Update "Unsorted" table*/
    }
  }
  
  /* Returning median value */
  return Array[minIndex];
}

/****f* libvision/VIS_Init
 * SYNOPSIS
 *   void VIS_Init(void)
 * DESCRIPTION
 *   Fills the hue conversion table and initialise the algorithm and camera offset defaults.
 * SEE ALSO
 *
 ****/
void VIS_Init(void)
{
  int RobotId;
  int team, id;

  /* Fill the global hue table conversion */
  VIS_FillHueTable(); /* HueTableOK is also modified by VIS_FillHueTable */

  /* Initialise the default algorithm values */
  AlgoInfo.depth=VIS_ALGO_DEPTH;
  AlgoInfo.fast=VIS_ALGO_FAST;
  AlgoInfo.step=VIS_ALGO_STEP;
  AlgoInfo.loop=VIS_ALGO_LOOP;
  AlgoInfoOK=1; /* Flag for Algorithm info structure initialised */

  /* Initialise the default camera offset values */
  RobotId=VIS_TeamRobot(); team=((int)(RobotId/10)); id=RobotId-10*team;
  CamInfo.angle=VIS_CAM_ANGLE[team][id];
  CamInfo.alpha=VIS_CAM_ALPHA[team][id];
  CamInfo.beta=VIS_CAM_BETA[team][id];
  CamInfo.height=VIS_CAM_HEIGHT[team][id];
  CamInfo.lenght=VIS_CAM_LENGHT[team][id];
  if ((CamInfo.angle==-1)||(CamInfo.alpha==-1)||(CamInfo.beta==-1)||
      (CamInfo.height==-1)||(CamInfo.lenght==-1)) {
    VIS_ErrorDisplay("VIS_Init", -1, "CamInfo incomp");
    CamInfoOK=0; /* One value is not set up properly... */
  }
  else { CamInfoOK=1;} /* Flag for camera offset structure initialised */
}

/****f* libvision/VIS_ModifyAlgo
 * SYNOPSIS
 *   void VIS_ModifyAlgo(VIS_Algo *NewAlgo)
 * DESCRIPTION
 *   Modify algorithm values.
 *   Input  : a new 'VIS_algo' structure.
 *            'VIS_Algo' is 4 integers : depth, fast, step and loop (modified automatically).
 *            Program is checking error in 'VIS_Algo' values, display them and return an error code.
 *            If an error occurs, no changes are made.
 *   Output : an error code : ==0 -> no errors; >0 -> an error occured.
 * SEE ALSO
 *   VIS_ModifyCam
 ****/
int VIS_ModifyAlgo(BYTE NewDepth, BYTE NewFast, BYTE NewStep)
{
  int error=0;

  /* error checking */
  if ((NewStep!=1) && (NewStep!=2)) {
    error=100; VIS_ErrorDisplay("! Param. error !", error, "step=only 1 or 2"); return error;
  }
  if ((NewFast==1) && (NewDepth>1)) {
    error=110; VIS_ErrorDisplay("! Param. error !", error, "fast for depth=1"); return error;
  }
  
  /* Modify if no errors */
  if (!error) {
    /* Modify Algo_info struct */
    AlgoInfo.depth=NewDepth;
    AlgoInfo.fast=NewFast;
    AlgoInfo.step=NewStep;
    AlgoInfo.loop=NewStep-1;
  }
  
  /* No errors occured... */
  return error;
}

/****f* libvision/VIS_ModifyCam
 * SYNOPSIS
 *   void VIS_ModifyCam(VIS_CamOffset *NewCam)
 * DESCRIPTION
 *   Modify camera offset values.
 *   Input  : a new 'VIS_CamOffset' structure.
 *            'VIS_CamOffset' is 5 values : angle and beta as floats
 *                                          alpha as BYTE
 *                                          height and lenght as int 
 *   Output : 0 for no errors, 1 is the NewCam is not valid.
 * SEE ALSO
 *   VIS_ModifyAlgo
 ****/
int VIS_ModifyCam(VIS_CameraOffset *NewCam)
{
  if ((NewCam->angle==-1)||(NewCam->alpha==-1)||(NewCam->beta==-1)||
      (NewCam->height==-1)||(NewCam->lenght==-1)) {
    VIS_ErrorDisplay("   ModifyCam", -1, "NewCam invalid");
    CamInfoOK=0;
    return -1;
  }
  else {
    CamInfo.angle=NewCam->angle;
    CamInfo.alpha=NewCam->alpha;
    CamInfo.beta=NewCam->beta;
    CamInfo.height=NewCam->height;
    CamInfo.lenght=NewCam->lenght;
    CamInfoOK=1;
    return 0;
  }
}

/****f* libvision/VIS_ColClear
 * SYNOPSIS
 *   void VIS_ColClear(VIS_RGBSpace *RGBSpace)
 * DESCRIPTION
 *   Clears the RGB space.
 * SEE ALSO
 *   VIS_ColInit VIS_ColFind
 ****/
void VIS_ColClear(VIS_RGBSpace *RGBSpace)
{
  int  max_loop=VIS_VALUES*VIS_VALUES; /* Last index for loop progress display */
  BYTE R_index, G_index, B_index;

  /* Initialise progress display */
  VIS_InitShowProgress("   Clear RGB");

  for (R_index=0; R_index<VIS_VALUES; R_index++) {
    for (G_index=0; G_index<VIS_VALUES; G_index++) {
      /* Display progress */
      VIS_ShowProgress(G_index+(R_index<<VIS_MSB), max_loop);
      for (B_index=0; B_index<VIS_VALUES; B_index++) {
	/* Clear RGB space */
	(*RGBSpace)[R_index][G_index][B_index]=0;
      }
    }
  }
}

/****f* libvision/VIS_ColInit
 * SYNOPSIS
 *   void VIS_ColInit(VIS_HueTable *HueTable, VIS_RGBSpace *RGBSpace, int ColourClass)
 * DESCRIPTION
 *   Initialise a new colour class.
 * SEE ALSO
 *   VIS_ColClear VIS_ColFind
 ****/
void VIS_ColInit(VIS_RGBSpace *RGBSpace, BYTE ColourClass)
{
  int      size=4, Key;
  int      Hue, HueStep=5;
  colimage Pic;
  image greyimg;

  /* Offset calibration display */
  LCDClear();
  LCDSetPrintf(0,0, "* Colour cal. *");
  LCDSetPrintf(1,0, "---------------");
  LCDSetPrintf(2,0, "  Align objet");
  LCDSetPrintf(3,0, "  Choose size");
  LCDSetPrintf(4,0, " with : + or -");
  LCDSetPrintf(5,0, "Cal : calibrate");
  LCDMenu(" ", " ", " ", "Nxt");

  /* Wait for "Nxt" */
  KEYWait(KEY4);
  
  /* Initialise display */
  LCDClear();
  LCDSetPrintf(0,11, "Size\n");
  LCDSetPrintf(2,11, "Hue\n");
  LCDMenu(" - ", " + ", " ", "Cal");
  Key=0;

  /* Waiting for "Col" */
  while (Key!=KEY4) {

    /* Get a picture */
    CAMGetColFrame(&Pic, 0);
 		IPColor2Grey(&Pic,&greyimg);
		VIS_MarkObject(greyimg, imagerows / 2 - 1,  imagecolumns / 2 - 1, 0);
		LCDPutGraphic(&greyimg);  
   
    /* Getting hue */
    Hue=VIS_MedianHue(&Pic, size);

    /* Display it... (still destroying picture ??? -> after VIS_MedianHue) */
/*    LCDPutColorGraphic(&Pic); */

    /* Reading a key from keyboard */
    Key=KEYRead();
    
    /* Changing distance value and displaying it */
    if (Key==KEY1) { size--;}
    if (Key==KEY2) { size++;}
    
    /* Display information on LCD */
    LCDSetPrintf(1,11, " %d ", size);
    LCDSetPrintf(3,11, " %d ", Hue);
  }

  /* Displaying result, ask for HueStep */
  LCDClear();
  LCDSetPrintf(0,0, "* Colour cal. *");
  LCDSetPrintf(1,0, "---------------");
  LCDSetPrintf(2,0, "  Hue : %d", Hue);
  LCDSetPrintf(4,0, "  Adjust Step");
  LCDSetPrintf(5,0, "  with + or -");
  LCDMenu("-", "+", " ", "Nxt");
  Key=0;

  /* Waiting for "Nxt" */
  while (Key!=KEY4) {

    /* Display information on LCD */
    LCDSetPrintf(3,0, "  Step : %d", HueStep);
    
    /* Reading a key from keyboard */
    Key=KEYRead();
    
    /* Changing distance value and displaying it */
    if (Key==KEY1) { HueStep--;}
    if (Key==KEY2) { HueStep++;}
  }  

  /* Filling RGB space with found Hue */
  VIS_FillRGBSpace(Hue-HueStep, Hue+HueStep, ColourClass, RGBSpace);
}

/****f* libvision/VIS_ColFind
 * SYNOPSIS
 *   void VIS_ColFind(colimage *Pic, VIS_RGBSpace *RGBSpace, VIS_Object *Object, BYTE ColourClass)
 * DESCRIPTION
 *   Finds the specified colour class.
 *   Returns the ColourClass found : either ColourClass, either VIS_NONE.
 * SEE ALSO
 *   VIS_ColInit VIS_ColClear
 ****/
int VIS_ColFind(colimage *Pic, VIS_RGBSpace *RGBSpace, VIS_Object *Object, BYTE ColourClass)
{
  VIS_Process Process;
  BYTE        table[imagerows*imagecolumns];
  BYTE        X_list[imagerows*imagecolumns];
  BYTE        Y_list[imagerows*imagecolumns];

  /* Initialise Process structure */
  Process.X_list=&X_list[0];
  Process.Y_list=&Y_list[0];  
  Process.table=&table[0];

  /* Find class */
  VIS_FindClasses(Pic, RGBSpace, &Process, ColourClass);

  /* Reduce noise function selection : normal, fast, every 2nd pixels... */
  if      (AlgoInfo.fast && AlgoInfo.loop)  { Process.index=VIS_ReduceNoise2f(&Process);}
  else if (AlgoInfo.fast && !AlgoInfo.loop) { Process.index=VIS_ReduceNoisef(&Process);}
  else if (!AlgoInfo.fast && AlgoInfo.loop) { Process.index=VIS_ReduceNoise2(&Process, AlgoInfo.depth);}
  else                                      { Process.index=VIS_ReduceNoise(&Process, AlgoInfo.depth);}

  /* If an object has been found */
  if (Process.index>=0) {
    /* Find limits of object */
    int offset=(AlgoInfo.depth)*(1<<AlgoInfo.loop);
    int top=imagerows+1,  bot=-1, right=-1, left=imagecolumns+1;
    VIS_FindLimits(&top, &bot, &right, &left, &Process);
    
    /* Update object structure */
    Object->class=ColourClass;
    Object->top=top-offset;
    Object->bot=bot+offset;
    Object->right=right+offset;
    Object->left=left-offset;
  }
  /* No object has been found */
  else {
    Object->class=VIS_NONE;
  }

  /* Return result */
  return Object->class;
}

/****f* libvision/VIS_CamCal
 * SYNOPSIS
 *   void VIS_CamCal()
 * DESCRIPTION
 *   Camera angle offset
 * SEE ALSO
 *   
 ****/
void VIS_CamCal(VIS_RGBSpace *RGBSpace, BYTE ColourClass)
{
  int         imgrows=imagerows, imgcols=imagecolumns;
  int         middle=0;
  int         distance=260; /* in mm */
  char        Key=0;
  BYTE        test=0;
  BYTE        alpha_offset=CamInfo.alpha;
  BYTE        bwimg[LCD_DISPLAY_SIZE]; /* Declaration of LCD display 1D array */
  BYTE        FoundColour;
  float       beta_offset=CamInfo.beta;
  float       beta_th=0, beta_r=0;
  colimage    ColPic;
  VIS_Object  Object;
  ServoHandle panning_camera=0;


  /* Ask for calibration */
  VIS_ShowOffset(alpha_offset, beta_offset);
  LCDMenu("No", " ", " ", "Yes");

  /* Wait for Yes or No */
  while (Key!=KEY1 && Key!=KEY4) {
    Key=KEYRead();
  }

  /* Yes, calibration... */
  if (Key==KEY4) {

    /*--------------------------*/
    /* Alpha offset calibration */
    /*--------------------------*/

    /* Panning camera servo init */
    panning_camera=SERVOInit(SERVO11);

    /* BW picture initialisation */
    VIS_InitBwimg(bwimg);

    /* Draw border of camera picture */
    VIS_DrawBorder(bwimg, imagerows, imagecolumns);

    /* Alpha offset display */
    LCDClear();
    LCDSetPrintf(0,11, "Alpha");
    LCDSetPrintf(2,11, "Mid.");
    LCDMenu(" ", " ", " ", "Stop");
    
    /* Pan from 0 to 255 until middle is found */
    alpha_offset=0; test=0; Key=0;

    while ((!test) && (Key=KEYRead()!=KEY4)) 
		{
      SERVOSet(panning_camera, alpha_offset);

      /* Getting image */
      OSWait(50); /* Avoid tilting camera problems... */
      CAMGetColFrame(&ColPic, 0);

      /* Process to find classes */
      FoundColour=VIS_ColFind(&ColPic, RGBSpace, &Object, ColourClass);
      
      if (FoundColour==ColourClass) 
			{
        middle=(Object.right+Object.left)>>1;
        VIS_DrawLimits(bwimg, &Object, 1);
			}
      /* Display... */
      LCDPutImage(bwimg);
      if (FoundColour==ColourClass) 
			{
        VIS_DrawLimits(bwimg, &Object, 0);
      }

      /* Display values */
      LCDSetPrintf(1,11, "%d ", alpha_offset);
      LCDSetPos(3,11);
      if (FoundColour==ColourClass) { LCDPrintf("%d ", middle-(imgcols/2));}
      else { LCDPrintf("    ");}
      LCDSetPrintf(4,11, "%d ", Object.right);
      LCDSetPrintf(5,11, "%d ", Object.left);
      
      if (middle==(imgcols/2)) { test=1;}
      else { alpha_offset++;}

      if (alpha_offset>255) { AUTone(110,500); alpha_offset=0;}
    }

    /* Wait for anykey */
    LCDMenu("  A", "N Y", "K E", "Y");
    KEYWait(ANYKEY);

    /* Panning camera servo release */
    SERVORelease(panning_camera);

    /*-------------------------*/
    /* Beta offset calibration */
    /*-------------------------*/

    /* Offset calibration display */
    LCDClear();
    LCDSetPrintf(0,0, "* Offset cal. *");
    LCDSetPrintf(1,0, "---------------");
    LCDSetPrintf(2,0, "  Align. ball");
    LCDSetPrintf(3,0, " Adj. distance");
    LCDSetPrintf(4,0, " with : + or -");
    LCDSetPrintf(5,0, "Cal : calibrate");
    LCDMenu(" ", " ", " ", "Nxt");

    /* Wait for "Nxt" */
    KEYWait(KEY4);

    /* Initialise display */
    LCDClear();
    LCDSetPrintf(0,11, "Dist\n");
    LCDSetPrintf(3,10, "Offset");
    LCDMenu(" - ", " + ", " ", "Cal");
    Key=0;

    /* BW picture initialisation */
    VIS_InitBwimg(bwimg);

    /* Draw border of camera picture */
    VIS_DrawBorder(bwimg, imagerows, imagecolumns);

    /* Waiting for "Cal" */
    while (Key!=KEY4) {

      /* Getting image */
      CAMGetColFrame(&ColPic, 0);

      /* Process to find classes */
      FoundColour=VIS_ColFind(&ColPic, RGBSpace, &Object, ColourClass);

      /* Display border of ball class */
      if (FoundColour==ColourClass) {
        middle=(Object.right+Object.left)/2;
        VIS_DrawLimits(bwimg, &Object, 1);
     }
      else { middle=-1;}

      /* Display... */
      LCDPutImage(bwimg);
      if (FoundColour==ColourClass) {
        VIS_DrawLimits(bwimg, &Object, 0);
      }

      /* Reading a key from keyboard */
      Key=KEYRead();

      /* Changing distance value and displaying it */
      if (Key==KEY1) { distance-=10;}
      if (Key==KEY2) { distance+=10;}

      /* Process angles, offset and distance */
      beta_th=atan( (float) (CamInfo.height-VIS_BALL_RADIUS)/distance);
      beta_r=( (float) 0.75*CamInfo.angle/2)*
           ((( (float) (Object.top+Object.bot)/2)-( (float) imgrows/2))/
             ( (float) imgrows/2));
      beta_offset=beta_th-beta_r;

      /* Display information on LCD */
      LCDSetPrintf(1,11, "%d cm", (int) (distance/10));
      LCDSetPrintf(4,11, "%1.2f ", beta_offset);
    }

    /* Modifiy values */
    CamInfo.alpha=alpha_offset;
    CamInfo.beta=beta_offset;

    /* Check if now all informations are filled */
    if ((CamInfo.angle==-1)||(CamInfo.alpha==-1)||(CamInfo.beta==-1)||
			(CamInfo.height==-1)||(CamInfo.lenght==-1)) {
      VIS_ErrorDisplay("VIS_Init", -1, "CamInfo incomp");
      CamInfoOK=0; /* One value is not set up properly... */
    }
    else { CamInfoOK=1;} /* Flag for camera offset structure initialised */

    /* Angle have been changed, need to recalculate distance table */
    DistTablesOK=0;

    /* Display new values */
    VIS_ShowOffset(alpha_offset, beta_offset);
    LCDMenu(" ", " ", " ", "Nxt");

    /* Wait for Nxt */
    KEYWait(KEY4);
  }
}

/***********************************************************************************************************/
/* Distance functions                                                                                      */
/***********************************************************************************************************/
/****f* libvision/VIS_InitDistance
 * SYNOPSIS
 *   BYTE VIS_InitDistance(void)
 * DESCRIPTION
 *   Check the CamInfo and initialise distances lookup tables.
 *   Output : returns 0 if VIS_struct is set up properly, >0 if not or if beta angle not set up.
 * SEE ALSO
 *   VIS_GetPosition()
 ****/
BYTE VIS_InitDistance(void)
{
  int   error=0;
  int   row=0, col=0;
  int   d_norm=0, d_bord=0;
  int   imgrows=imagerows, imgcols=imagecolumns;
  float alpha=0, beta=0;
  float temp, cos_alpha, sin_alpha;
  
  /* Check if the CamInfo struct has been filled or not */
  if (CamInfoOK) {

    /* Initialise show progress display */
    VIS_InitShowProgress("* Distance cal.\n");
    
    for (row=0; row<imgrows; row++) {
      beta=(0.75*(0.5*CamInfo.angle))*(row-(0.5*imgrows))/(0.5*imgrows);
      beta+=CamInfo.beta;
      
      temp=(float) (CamInfo.height-VIS_BALL_RADIUS);
      
      d_norm=(short) (temp/beta);
      d_bord=(short) ((temp-(0.5*VIS_BALL_RADIUS))/beta);
      
      for (col=0; col<imgcols; col++) {
	
	alpha=0.5*CamInfo.angle*(( (float) (2*col)/(imgcols))-1);
	cos_alpha=cos(alpha);
	sin_alpha=sin(alpha);
	
	DistTables.Dist_angle[VIS_ALPHA][row][col]=alpha;
	DistTables.Dist_angle[VIS_BETA][row][col]=beta;
	
	DistTables.Dist_dist[VIS_NORMAL][row][col]=d_norm;
	DistTables.Dist_dist[VIS_BORDER][row][col]=d_bord;
	
	DistTables.Dist_coord[VIS_NORMAL][VIS_ROW][row][col]=(short)d_norm*cos_alpha;
	DistTables.Dist_coord[VIS_NORMAL][VIS_COL][row][col]=(short)d_norm*sin_alpha;
	DistTables.Dist_coord[VIS_BORDER][VIS_ROW][row][col]=(short)d_bord*cos_alpha-VIS_BALL_RADIUS;
	DistTables.Dist_coord[VIS_BORDER][VIS_COL][row][col]=(short)d_bord*sin_alpha;
      }
      
      /* Still alive display... */
      VIS_ShowProgress(row,imgrows);
    }
    DistTablesOK=1; /* Distance lookup table initialsed */
  }
  else { /* CamInfo has not been defined */
    VIS_ErrorDisplay("   DistTables", -1, "CamInfo empty");
    error=-1;
  }
  return error;
}
/****f* libvision/VIS_GetPosition
 * SYNOPSIS
 *   int VIS_GetPosition(VIS_Object *Object, VIS_Distance *ObjDist)
 * DESCRIPTION
 *   Returns the distance of the ball by looking into the lookup tables.
 *   A VIS_distance struct is made of 3 integers : d_row, d_col and dist.
 *   Where dist is the direct distance between the robot and the ball,
 *         (d_row,d_col) are the relative coordinates of the ball compared to the robot position,
 *   So that dist^2=d_row^2+d_col^2.
 *   d_row is the depth of the ball compared to the robot position,
 *   d_col is the side distance of the ball compared to the robot.
 *   Input  : an object structure (VIS_Object) and a VIS_Distance result structure.
 *   Output : 0 on success, -1 if distance table is not initialised.
 * SEE ALSO
 *   VIS_Init_distance()
 ****/
int VIS_GetPosition(VIS_Object *Object, VIS_Distance *ObjDist) {
  int       border=0;
  int       pcol=0, prow=0;
  int       error=0;

  /* Check if distance lookup table is updated */
  if (!DistTablesOK) {
    VIS_ErrorDisplay("  GetPosition", -1, "   DistTables");
    error=-1;
  }

  /* Look for ball position */
  if (Object->bot>=imagerows-5) {
    if (Object->left<=4) {
      prow=Object->top; pcol=Object->right;
      border=VIS_BORDER;
    }
    else if (Object->right>=imagecolumns-5) {
      prow=Object->top; pcol=Object->left;
      border=VIS_BORDER;
    }
    else {
      prow=Object->top; pcol=(Object->right+Object->left)>>1;
      border=VIS_BORDER;
    }
  }
  else if (Object->left<=5) {
    prow=(Object->top+Object->bot)>>1; pcol=Object->right;
    border=VIS_NORMAL;
  }
  else if (Object->right>=imagecolumns-5) {
    prow=(Object->top+Object->bot)>>1; pcol=Object->left;
    border=VIS_NORMAL;
  }
  else {
    prow=(Object->top+Object->bot)>>1;
    pcol=(Object->right+Object->left)>>1;
    border=VIS_NORMAL;
  }

  /* Get values in the tables */
  ObjDist->d_row=DistTables.Dist_coord[border][VIS_ROW][(short) prow][(short) pcol];
  ObjDist->d_col=DistTables.Dist_coord[border][VIS_COL][(short) prow][(short) pcol];
  ObjDist->dist=DistTables.Dist_dist[border][(short) prow][(short) pcol];
  ObjDist->alpha=DistTables.Dist_angle[VIS_ALPHA][(short) prow][(short) pcol];
  ObjDist->beta=DistTables.Dist_angle[VIS_BETA][(short) prow][(short) pcol];

  return 0;
}

/***********************************************************************************************************/
/* Display functions                                                                                       */
/***********************************************************************************************************/
/****f* libvision/VIS_InitBwimg
 * SYNOPSIS
 *   void VIS_InitBwimg(BYTE *bwimg)
 * DESCRIPTION
 *   BW LCD picture initialisation.
 *   Input  : 'Picture' structure.
 *   Output : 0 no errors, -1 NULL bwimg pointer.
 * SEE ALSO
 *   VIS_ComputeBW VIS_DrawLimits
 ****/
int VIS_InitBwimg(BYTE *bwimg)
{
  /* Init picture */
  if (NULL!=bwimg) {
    memset(bwimg, 0, LCD_DISPLAY_SIZE);
    return 0; /* Pointer was not NULL */
  }
  else { return -1;} /* Error pointer was NULL */
}

/****f* libvision/VIS_ComputeBW
 * SYNOPSIS
 *   void VIS_ComputeBW(BYTE *bwimg, BYTE *temp)
 * DESCRIPTION
 *   Get a 2D binary array and make a linear BW array for LCD display.
 *   Input  : a BW linear array for LCD display (bwimg), a 2D binary array (temp).
 * SEE ALSO
 *   VIS_InitBwimg VIS_DrawLimits
 ****/
void VIS_ComputeBW(BYTE *bwimg, BYTE *temp)
{
  int row=0;
  int imgrows=LCD_DISPLAY_ROW, imgcols=LCD_DISPLAY_COL;
  int datasize=LCD_DISPLAY_SIZE;

  for (row=imgrows-1; row>=0; row--) { /*for (row=0; row<imagerows; row++) {*/
    int col;
    for (col=0; col<imgcols; col+=8) {
      int value=0x00;
      int i;
      for (i=7; i>=0; i--) { /*for (i=0; i<8; i++) {*/
        int offset=((row*imgcols)+(col+7-i)); /* [row][col+(7-i)] */
        if ((offset<datasize) && (temp[offset])) {
          value |= (1<<i);
        }
      }
      bwimg[(row<<4)+(col>>3)]=value; /* bwimg[row*128/8+col/8]=value; */
    }
  }
}

/****f* libvision/VIS_DrawLimits
 * SYNOPSIS
 *   void VIS_DrawLimits(BYTE *bwimg, VIS_Object *Object, int value)
 * DESCRIPTION
 *   Draw object limits and center on bw picture.
 *   value==0 -> erase, value>0 -> draw.
 *   Input  : a BW linear array, the region to draw and a draw (i.e. >0) / erase (i.e. ==0) value.
 *   Output : N/A
 * SEE ALSO
 *   VIS_InitBwimg VIS_ComputeBW
 ****/
void VIS_DrawLimits(BYTE *bwimg, VIS_Object *Object, int value)
{
  int top=Object->top, bot=Object->bot;
  int right=Object->right, left=Object->left;

  if (value) { /* Display limits of object (value>0) */
    bwimg[(top<<4)+(right>>3)] |= 1<<(7-(right) % 8); /* bwimg[top*128/8+right/8] |= 1<<(7-(right) % 8); */
    bwimg[(top<<4)+(left>>3)] |= 1<<(7-(left) % 8);   /* bwimg[top*128/8+left/8] |= 1<<(7-(left) % 8); */
    bwimg[(bot<<4)+(right>>3)] |= 1<<(7-(right) % 8); /* bwimg[bot*128/8+right/8] |= 1<<(7-(right) % 8); */
    bwimg[(bot<<4)+(left>>3)] |= 1<<(7-(left) % 8);   /* bwimg[bot*128/8+left/8] |= 1<<(7-(left) % 8); */
    bwimg[((top+bot)<<3)+((right+left)>>4)] |= 1<<(7-((right+left)>>1) % 8);
    /* bwimg[((top+bot)/2)*128/8+((right+left)/2)/8] |= 1<<(7-((right+left)/2) % 8);*/
  }
  else { /* Erase limits of object (value==0) */
    bwimg[(top<<4)+(right>>3)] &= ~(1<<(7-(right) % 8)); /* bwimg[top*128/8+right/8]=~(1<<(7-(right) % 8)) */
    bwimg[(top<<4)+(left>>3)] &= ~(1<<(7-(left) % 8));  /* bwimg[top*128/8+left/8]=~(1<<(7-(left) % 8)) */
    bwimg[(bot<<4)+(right>>3)] &= ~(1<<(7-(right) % 8)); /* bwimg[bot*128/8+right/8]=~(1<<(7-(right) % 8)) */
    bwimg[(bot<<4)+(left>>3)] &= ~(1<<(7-(left) % 8));  /* bwimg[bot*128/8+left/8]=~(1<<(7-(left) % 8)) */
    bwimg[((top+bot)<<3)+((right+left)>>4)] &= ~(1<<(7-((right+left)>>1) % 8)); 
    /* bwimg[((top+bot)/2)*128/8+((right+left)/2)/8]=~(1<<(7-((right+left)>>1) % 8)) */
  }
}

/****f* libvision/VIS_DrawBorder
 * SYNOPSIS
 *   void VIS_DrawBorder(BYTE *bwimg, int rows, int cols)
 * DESCRIPTION
 *   Draw borders in a 1D array for LCD display
 *   Input  : a 1D BW array, line and column of the border.
 *   Output : N/A
 * SEE ALSO
 *   
 ****/
void VIS_DrawBorder(BYTE *bwimg, int rows, int cols)
{
  int i;

  for (i=cols; i>=0; i--) {                       /* for (i=0; i<=cols; i++) {*/
    bwimg[(rows<<4)+(i>>3)] |= 1<<(7-(i) % 8);    /* bwimg[rows*128/8+i/8] |= 1<<(7-(i) % 8); */
  }
  for (i=rows-1; i>=0; i--) {                     /* for (i=0; i<rows; i++) {*/
    bwimg[(i<<4)+(cols>>3)] |= 1<<(7-(cols) % 8); /* bwimg[i*128/8+cols/8] |= 1<<(7-(cols) % 8); */
  }
}

/****f* libvision/VIS_MarkObject
 * SYNOPSIS
 *   void VIS_MarkObject(image greyimg, int x_middle, int y_middle, int object_size)
 * DESCRIPTION
 *   Mark object position by drawing a vertical and horizontal white
 *   line through it. Ball size is displayed by black lines.
 *   Birgit Graf, UWA 1998.
 ****/
/* -----------= colours =-------------- */
#define BLACK  0
#define WHITE 15

void VIS_MarkObject(image greyimg, int x_middle, int y_middle, int object_size)
{
  int row, column;
  
  /* mark object position: WHITE */  
  for (row = 1; row < imagerows; row ++)
    greyimg[row][y_middle] = WHITE;
  
  for (column = 1; column < imagecolumns; column ++)
    greyimg[x_middle][column] = WHITE;

  
  /* mark object size: BLACK */
  for (row = x_middle - 2; row < x_middle + 3; row ++)
  {
    greyimg[row][y_middle - object_size/2] = BLACK;
    greyimg[row][y_middle + object_size/2] = BLACK;
  }  

  for (column = y_middle - object_size / 2 + 1; column < y_middle +
	 object_size / 2; column ++)
    greyimg[x_middle][column] = BLACK;
}

/****f* libvision/VIS_ErrorDisplay
 * SYNOPSIS
 *   void VIS_ErrorDisplay(char title[16], int error, char msg[16])
 * DESCRIPTION
 *   Display error messages.
 *   Input  : a box title (16 char max.), an error code an error message (16 char max.).
 *   Output : N/A
 * SEE ALSO
 *   
 ****/
void VIS_ErrorDisplay(char title[16], int error, char msg[16])
{
  AUTone(110, 500);
  LCDClear();
  LCDSetPrintf(0,0, title);
  LCDSetPrintf(1,0, "----------------");
  LCDSetPrintf(2,0, "  Error code :");
  LCDSetPrintf(3,0, "      %d", error);
  LCDSetPrintf(4,0, "   Message :");
  LCDSetPrintf(5,0, msg);
  LCDMenu("", "", "", "Cont");
  KEYWait(KEY4);
}

/****f* libvision/VIS_InitShowProgress
 * SYNOPSIS
 *   void VIS_InitShowProgress(char title[16])
 * DESCRIPTION
 *   Initialise the progress show of a loop (clear screen, display title,...).
 *   Input  : a loop title (16 char max.).
 *   Output : N/A
 * SEE ALSO
 *   
 ****/
void VIS_InitShowProgress(char title[16])
{
  LCDClear();
  LCDSetPrintf(0, 0, title);
  LCDSetPrintf(2, 0, "I          I\n");
  LCDSetPrintf(2,13, "[ ]\n");
}

/****f* libvision/VIS_ShowProgress
 * SYNOPSIS
 *   void VIS_ShowProgress(int actual, int max)
 * DESCRIPTION
 *   Displays the progress of a loop.
 *   Input  : the number of the actual loop and the total number of loops.
 *   Output : N/A
 * SEE ALSO
 *   
 ****/
void VIS_ShowProgress(int actual, int max)
{
  int  col;
  char rotation[]={'|', '/', '-', '\\'};

  if (actual==0) {
    LCDSetPrintf(4,0, "  Please Wait");
  }
  col=(int) (actual/(max/10))+1;
  if (col>10) { col=10;}
  LCDSetChar(2,col, '.');
  LCDSetChar(2,14, rotation[actual % 4]);
}

/****f* libvision/VIS_ShowOffset
 * SYNOPSIS
 *   void VIS_ShowOffset()
 * DESCRIPTION
 *   Displays the actual offset of the camera.
 * SEE ALSO
 *   
 ****/
void VIS_ShowOffset(int alpha_offset, float beta_offset)
{
  LCDClear();
  LCDSetPrintf(0,0, "* Offset cal. *");
  LCDSetPrintf(1,0, "---------------");
  LCDSetPrintf(3,0, "  Alpha: %d ", alpha_offset);
  LCDSetPrintf(4,0, "  Beta : %1.3f ", beta_offset);
}

/***********************************************************************************************************/
/* Camera functions                                                                                        */
/***********************************************************************************************************/
/****f* libvision/VIS_InitCam
 * SYNOPSIS
 *   int VIS_InitCam(int mode)
 * DESCRIPTION
 *   Initialisation of the camera. 10 retries.
 *   Input  : mode of the camera.
 *   Output : camversion or INITERROR.
 * SEE ALSO
 *   N/A
 ****/
int VIS_InitCam(int cammode)
{
  int  camversion=0;
  int  number=0;
  char rotation[]={'|', '/', '-', '\\'};

  LCDClear();
  LCDSetPrintf(0,0, "  Camera Init.");
  LCDSetPrintf(1,0, "  ------------");
  LCDSetPrintf(3,6, "[ ]");

  /* Wait for camera initialisation, get camera type */
  while ((INITERROR==(camversion=CAMInit(NORMAL))) && (number<=10) && (NOCAM!=camversion)){
    OSWait(CAMWAIT);
    LCDSetChar(3,7, rotation[(number++)%4]);
  }

  /* Bad initialisation after 10 times or no camera connected */
  if ((INITERROR==camversion) || (NOCAM==camversion)) { return camversion;}

  /* Camera initialised, return version */
  CAMMode(cammode);
  AUBeep();
  return camversion;
}

/****f* libvision/VIS_AutobrightnessDelay
 * SYNOPSIS
 *   void VIS_AutobrightnessDelay(int number)
 * DESCRIPTION
 *   Let time to camera for autobrightness.
 *   Input  : number of waits.
 *   Output : N/A
 * SEE ALSO
 *   N/A
 ****/
void VIS_AutobrightnessDelay(int number)
{
  char     Key=0;
  colimage Pic;

  /* Initialise show progress display */
  VIS_InitShowProgress("*Autobrightness*\n");
  LCDMenu("No", " ", " ", "Yes");

  /* Wait for Yes or No */
  KEYGetBuf(&Key);
  while (Key!=KEY1 && Key!=KEY4) {
    KEYGetBuf(&Key);
  }
  AUBeep();

  /* Wait loop... */
  if (Key==KEY4) {
    int i;
    LCDMenu(" ", " ", " ", " ");
    for (i=0; i<number; i++) {
      /* Wait : i.e. take a picture */
      CAMGetColFrame (&Pic, 0);
      
      /* Still alive display */
      VIS_ShowProgress(i, number);
    }
  }
}
/****f* libvision/VIS_TeamRobot
 * SYNOPSIS
 *   int VIS_TeamRobot(void)
 * DESCRIPTION
 *   Returns a robot team and ID.
 *   Input  : N/A
 *   Output : a two digits integer xy where x is the team and y is the ID.
 * SEE ALSO
 *   N/A
 ****/
int VIS_TeamRobot(void)
{
  int  team=0;
  BYTE robot_id=0;
  char Key;
  
  robot_id=OSMachineID(); /* Get ID of the robot */
  team=CAMInit(NORMAL);   /* Quickcam : COLCAM=16, Eyecam : COLCAM+1 (i.e. 17) */

  if ((robot_id<1)||(robot_id>5)) {
    robot_id=1;
    VIS_ErrorDisplay("TeamRobot", -1, "bad robot_id");
    LCDClear();
    LCDSetPrintf(0, 0, "Enter Robot ID");
    LCDSetPrintf(3, 3, "Robot ID : %d", robot_id);
    LCDMenu("-", "+", "", "Cont");
    Key=0;
    while (Key!=KEY4) {
      Key=KEYRead();
      if (Key==KEY1)
			{
				robot_id--;
				if (robot_id < 1)
						robot_id=1;
			}
      if (Key==KEY2) 
			{
				robot_id++;
				if (robot_id>MAX_ROBOTS)
					robot_id=MAX_ROBOTS;
			}
      LCDSetPrintf(3, 3, "Robot ID : %d", robot_id);
    }
  }

  switch(team)
    {
    case COLCAM+1: /* EyeCam */
    case 32 :      /* Fixes the 32 instead of 17 (COLCAM+1) bug for EyeCam */
      return (robot_id-1);
      break; 
    default :      /* QuickCam */  
      return (10+robot_id-1);
      break;
    }
}