#include "LibVision.h"

/* To remember the declared global variables (in LibVision.c) */
/* 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*/

int main(void)
{
  BYTE         ColourClass=1;               /* This example handles only one class */
  BYTE         FoundClass;                  /* Colour class found */
  BYTE         bwimg[VIS_LCD_DISPLAY_SIZE]; /* Declaration of LCD display 1D array */
  BYTE         draw;                        /* Flag if object drawn or not */
  int	       error;                       /* Error number */
  int          timea, timeb;                /* Main function timers */
  char         Key;                         /* Keyboard... */
  colimage     ColPic;                      /* User's colour image */
  VIS_Object   Object;                      /* User's object */
  VIS_RGBSpace RGBSpace;                    /* User's RGB space */
  VIS_Distance ObjDist;                     /* User's distance structure */
  
  /******************/
  /* Initialisation */
  /******************/

  /* Camera Initialisation */
  error=VIS_InitCam(NORMAL);
  if (error==INITERROR) { VIS_ErrorDisplay("   Cam Init.", INITERROR, "");}

  /* Fill the hue conversion table and initialise the algorithm defaults */
  VIS_Init();
  
  /* Fill the user's RGB space with zeros */
  VIS_ColClear(&RGBSpace);
  
  /* Finding the median hue and filling the RGBSpace */
  VIS_ColInit(&RGBSpace, ColourClass);
  
  /* calibrate camera */
  VIS_CamCal(&RGBSpace, ColourClass);
  
  /* init distance table */
  VIS_InitDistance();
  
  /***************/
  /* Detect loop */
  /***************/

  /* Clear LCD display, display menu, clear the LCD 1D array and draw the camera picture borders */
  LCDClear();
  LCDMenu("", "", "", "END");

  /* Clear the LCD 1D array */
  error=VIS_InitBwimg(bwimg);
  if (error==-1) { VIS_ErrorDisplay(" bwimg pointer", error, "NULL");}

  /* Draw camera picture borders */
  VIS_DrawBorder(bwimg, 61, 81);

  /* Init variables... */
  draw=0; /* First loop, nothing has been drawn... */
  Key=0;

  /* Init display */
  LCDSetPrintf(0, 10, "Ball:\n");
  LCDSetPrintf(3, 10, "Time:\n");

  while (Key!=KEY4) {
    /* Erase object in LCD 1D array */
    if (draw) { VIS_DrawLimits(bwimg, &Object, 0);}

    /* Read Keyboard... */
    Key=KEYRead();

    /* Get a picture */
    CAMGetColFrame(&ColPic, 0);

    /* Find class */
    timea=OSGetCount();
    FoundClass=VIS_ColFindOne(&ColPic, &RGBSpace, &Object, ColourClass);
    timeb=OSGetCount();

    /* Draw limits of object if found */
    if (Object.class==ColourClass) { /* Could be 'if (FoundClass==ColourClass)' */
      VIS_DrawLimits(bwimg, &Object, 1); /* Draw limits in bwimg image */
      draw=1; /* Set flag that something has been drawn */
      VIS_GetPosition(&Object, &ObjDist);
      LCDSetPrintf(1, 11, "%3d  \n", ObjDist.d_row/10);
      LCDSetPrintf(2, 11, "%3d  \n", ObjDist.d_col/10);
    }
    else { /* Not object found, nothing drawn */
      draw=0;
      LCDSetPrintf(1, 11, "    ");
      LCDSetPrintf(2, 11, "    ");
    }

    /* Display timer */
    LCDSetPrintf(4, 11, "%d ", timeb-timea);

    /* Display bwimg (LCD 1D array) */
    LCDPutImage(bwimg);
  }
  return 0;
}
