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

/***********************************************************************************************************/
/* Header file for the LibVision v2.00                                                                     */
/* Declaration of constants, structures and user functions                                                 */
/***********************************************************************************************************/

/****h* LibVision/LibVision
 *DESCRIPTION    
 *
 * All the 'LibVision' components (#define, structures and functions) begin with VIS to avoid
 * confusion with any other modules.
 *
 * Only the values in the header files may need to be changed. The values in the program source
 * are based on the values in the header and should not be changed.
 *
 * * ************** *
 * * #define values *
 * * ************** *
 *
 * Library default informations
 * ----------------------------
 *   #define VIS_MSB         5   : Number of most significant bits used for the 3D tables
 *   #define VIS_NONE        0   : No colour class 
 *   #define VIS_CONFLICT    255 : Colour-conflict class
 *   #define VIS_SAT_THRES   32  : Threshold for limit saturation
 * @ RGB Values are coded on 8 bits, but for a RGB 3D space, ((2^8)^3)=16MB...
 * @ VIS_MSB defines how many MSB should be used : as an exemple for VIS_MSB=5 -> ((2^5)^3)=32kB.
 * @ VIS_NONE and VIS_CONFLICT define the two defaults values for the no-colour class and the colour-conflict
 * class.
 * @ VIS_SAT_THRES defines the default saturation threshold to define colours within the RGB space avoiding
 * defining white shades as colours. 
 *
 * Defaults for the algorithm
 * --------------------------
 *   #define VIS_ALGO_DEPTH 1                 : One pixel around
 *   #define VIS_ALGO_FAST  1                 : Use fast
 *   #define VIS_ALGO_STEP  2                 : Every 2nd pixel
 *   #define VIS_ALGO_LOOP  (VIS_ALGO_STEP-1) : defined internally (in LibVision.c)
 * @ These values define the way to process the image. Some function are optimised for certain parameters.
 * Look the VIS_Algo structure for more informations on these values.
 *
 * Image size information
 * ----------------------
 *   #define VIS_IMROW imagerows    : number of rows
 *   #define VIS_IMCOL imagecolumns : number of columns
 * @ These two values are used to avoid the ANSI C warning about variable-size variables definition.
 *
 * Maximum number of classes to be processed
 * -----------------------------------------
 *   #define VIS_MAX_CLASS 10 : No more than 10 different classes
 * @ This value is used to avoid the ANSI C warning about variable-size variables definition.
 * It defines the maximum number of colour classes to be processed by the library.
 *
 * LCD Display informations, used by VIS_ComputeBW
 * -----------------------------------------------
 *   #define VIS_LCD_DISPLAY_ROW  64
 *   #define VIS_LCD_DISPLAY_COL  128
 *   #define VIS_LCD_DISPLAY_SIZE ((VIS_LCD_DISPLAY_ROW*VIS_LCD_DISPLAY_COL)>>3)
 * @ These values are used to define the LCD display properties. The VIS_LCD_DISPLAY_SIZE defines the 1D
 * memory length of a complete B/W image for the LCD display.
 *
 * Algorithm information, ONLY depending on VIS_MSB in the .h, (declared in LibVision.c)
 * -------------------------------------------------------------------------------------
 *   #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
 * @ These values are used internally and are based on the VIS_MSB value. They are used to define internal
 * values to deal with the RGB 3D colour space.
 *
 * Distances lookup tables : normal table, border table, angle tables
 * ------------------------------------------------------------------
 *   #define VIS_ROW    0
 *   #define VIS_COL    1
 *   #define VIS_ALPHA  0
 *   #define VIS_BETA   1
 *   #define VIS_NORMAL 0
 *   #define VIS_BORDER 1
 * @ These values are used internally to access the right part of the distance tables.
 * Look the VIS_DistanceTables for more informations.
 *
 * * ****************** *
 * * typedef structures *
 * * ****************** *
 *
 * RGB space
 * ---------
 *   typedef BYTE VIS_RGBSpace[1<<VIS_MSB][1<<VIS_MSB][1<<VIS_MSB];
 * @ Definition of a RGB 3D colour space type. Each dimension is depending on the number of MSB used.
 *
 * RGB -> Hue conversion table
 *   typedef BYTE VIS_HueTable[1<<VIS_MSB][1<<VIS_MSB][1<<VIS_MSB];
 * @ Definition of a RGB->Hue 3D conversion table. Each dimension is depending on the number of MSB used.
 *
 * Median hue and automatic range structure
 * ----------------------------------------
 *   typedef struct {
 *     int Hue;   : Median hue value
 *     int Range; : Range around the median value 
 * } VIS_DefineHue;
 * @ Used to get the 'median hue' and the 'hue range' from the function 'VIS_MedianHue'.
 *
 * Image processing struct : table -> undenoised class, index, X and Y_list denoised data
 * --------------------------------------------------------------------------------------
 *   typedef struct {
 *     BYTE *table;  : Binary table
 *     BYTE *X_list; : X positions list
 *     BYTE *Y_list; : Y positions list
 *     int  index;   : Number of elements in X_list and Y_list
 *   } VIS_Process;
 * @ The VIS_Process structure is used internally and contains a binary picture (table) of the colour-class,
 * a pixel list of the denoised binary picture (X and Y list) and the number of pixel in these lists (index).
 * 'index' is used to look for the limit pixels of the object in the denoised pixel lists. 
 *
 * Algorithm options struct
 * ------------------------
 *   typedef struct {
 *     BYTE depth; : denoise one dot around
 *     BYTE fast;  : use fast algorithm, only for depth=1
 *     BYTE step;  : process every dots or two dots only
 *     BYTE loop;  : shift for loops, modified internally loop=step-1
 *   } VIS_Algo;
 * @ These values enable to choose between several optimised functions for image processing.
 * 'depth' indicates on how many pixels the "erosion" is applied.
 * 'fast'  indicates the use of an easier loop if the "erosion" is applied on only 1 pixel (depth==1).
 * 'step'  is either every pixels or every second pixels.
 * 'loop'  is used internally to define a shift value depending on 'step'.
 *
 * Object structure : class and limits
 * -----------------------------------
 *   typedef struct {
 *     BYTE class;                 : Class number of the object
 *     BYTE top, bot, right, left; : Coordinates of the found object
 *   } VIS_Object;
 * @ This structure is created by the user to get back the result. If an object has been found, 'class' will
 * contain the class number, i.e. VIS_NONE if no object from the current colour has been found.
 * 'top', 'bot', 'right' and 'left' contain the pixel limits of the found object in the image.
 *
 * Camera angles and positions offsets to calibrate
 * ------------------------------------------------
 *   typedef struct {
 *     int   alpha;  : Servo alpha offset
 *     int   height; : Camera height
 *     int   lenght; : Camera distance to the front of the robot
 *     float angle;  : Lens angle in radians
 *     float beta;   : Camera beta offset
 *   } VIS_CameraOffset;
 * @ This structure defines the camera properties and position.
 * 'alpha'  gives the servo position when the camera looks straight forward,
 * 'beta'   gives the angle offset between the camera middle axis and the horizontal axis,
 * 'lenght' gives the distance offset from the camera to the robot front,
 * 'height' gives the camera heigth to the ground,
 * 'angle'  defines the camera view angle.
 * @ These values are used to calibrate the ball distance to the camera and the angle positions.
 *
 * Camera offset default informations
 * ----------------------------------
 *   static const int   VIS_CAM_HEIGHT[2][5]={{-1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1}};
 *   static const int   VIS_CAM_LENGHT[2][5]={{-1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1}};
 *   static const BYTE  VIS_CAM_ALPHA[2][5]={{-1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1}};
 *   static const float VIS_CAM_BETA[2][5]={{-1, -1, -1, -1, -1}, {-1, -1, -1, -1, -1}};
 *   static const float VIS_CAM_ANGLE[2][5]={{0.85, 0.85, 0.85, 0.85, 0.85}, {0.85, 0.85, 0.85, 0.85, 0.85}};
 * @ These values are used as defaults for the camera calibration (VIS_CameraOffset).
 * @ Angles informations have to be set up in radians.
 *
 * Distances lookup tables : normal table, border table, angle tables
 * ------------------------------------------------------------------
 *   typedef struct {
 *     short Dist_dist[2][imagerows][imagecolumns];     : Direct relative distance, border or normal
 *     short Dist_coord[2][2][imagerows][imagecolumns]; : Relative distance in x and y, border or normal
 *     float Dist_angle[2][imagerows][imagecolumns];    : Alpha and beta angles
 *   } VIS_DistanceTables;
 * @ This structure defines the distance table and is a global variable.
 * 'dist'  defines the direct distance between the robot and the ball,
 * 'coord' defines the side and depth coordinates of the ball compared to the camera,
 * 'angle' defines the object angle position compared to the camera.
 *
 * Distance structure
 * ------------------
 *   typedef struct {
 *     short d_row; : depth distance
 *     short d_col; : side distance
 *     short dist;  : direct distance : dist^2=d_row^2+d_col^2
 *     float alpha; : sideways angle
 *     float beta;  : up-down angle
 *   } VIS_Distance;
 * @ The distance structure is for the user to get the ball position using VIS_GetPosition.
 * @ The distance is only valid for the ball, but the angle position can be used for any object.
 *
 * * ***************************************** *
 * * Global variables (defined 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
 * @ These are the global variables declared in the LibVision.c program. They are the basic informations the
 * library needs to work.
 ****/

   /* C Libraries */
#include <eyebot.h>
#include <math.h>

   /* Camera offset default informations */
static const int   VIS_CAM_HEIGHT[2][5]={{59, 62, 55, 52, -1}, {-1, -1, -1, -1, -1}};
static const int   VIS_CAM_LENGHT[2][5]={{30, 33, 38, 20, -1}, {-1, -1, -1, -1, -1}};
static const BYTE  VIS_CAM_ALPHA[2][5]={{118, 139, 132, -1, -1}, {-1, -1, -1, -1, -1}};
static const float VIS_CAM_BETA[2][5]={{0.147, 0.026, 0.037, -1, -1}, {-1, -1, -1, -1, -1}};
static const float VIS_CAM_ANGLE[2][5]={{0.85, 0.85, 0.85, 0.85, 0.85}, {0.85, 0.85, 0.85, 0.85, 0.85}};

   /* Algorithm informations */
#define VIS_MSB         5   /* Most significant bits used for the 3D tables */
#define VIS_NONE        0   /* No colour class */
#define VIS_CONFLICT    255 /* Colour-conflict class */
#define VIS_SAT_THRES   32  /* Threshold for limit saturation */
#define VIS_BALL_RADIUS 21  /* ball radius */

    /* Defaults for the algorithm */
#define VIS_ALGO_DEPTH 1 /* "erosion" one dot around */
#define VIS_ALGO_FAST  1 /* Use fast function */
#define VIS_ALGO_STEP  2 /* Process every second pixel */
/* VIS_ALGO_LOOP is defined in the LibVision.c and is based on VIS_ALGO_STEP */

    /* Image size information */
#define VIS_IMROW imagerows    /* number of camera image rows */
#define VIS_IMCOL imagecolumns /* number of camrea image columns */

    /* Maximum number of classes to be processed */
#define VIS_MAX_CLASS 10 /* No more than 10 different classes, avoiding ANSI C warning */

    /* LCD Display informations, used by VIS_ComputeBW */
#define VIS_LCD_DISPLAY_ROW  64
#define VIS_LCD_DISPLAY_COL  128
#define VIS_LCD_DISPLAY_SIZE ((VIS_LCD_DISPLAY_ROW*VIS_LCD_DISPLAY_COL)>>3)

    /* RGB space */
typedef BYTE VIS_RGBSpace[1<<VIS_MSB][1<<VIS_MSB][1<<VIS_MSB];

    /* RGB -> Hue conversion table */
typedef BYTE VIS_HueTable[1<<VIS_MSB][1<<VIS_MSB][1<<VIS_MSB];

    /* Median hue and automatic range structure */
typedef struct {
  int Hue;   /* Median hue value */
  int Range; /* Range around the median value */
} VIS_DefineHue;

    /* Image processing struct : table -> undenoised class, index, X and Y_list denoised data */
typedef struct {
  BYTE *table;  /* Binary table */
  BYTE *X_list; /* X positions list */
  BYTE *Y_list; /* Y positions list */
  int  index;   /* Number of elements in X_list and Y_list */
} VIS_Process;

    /* Algorithm options struct */
typedef struct {
  BYTE depth; /* denoise one dot around */
  BYTE fast;  /* use fast algorithm, only for depth=1 */
  BYTE step;  /* process every dots or two dots only */
  BYTE loop;  /* shift for loops, modified internally loop=step-1 */
} VIS_Algo;

    /* Object structure : class and limits */
typedef struct {
  BYTE class;                 /* Class number of the object */
  BYTE top, bot, right, left; /* Coordinates of the found object */
} VIS_Object;

    /* Camera angles and positions offsets to calibrate */
typedef struct {
  int   alpha;   /* Servo alpha offset */
  int   height;  /* Camera height */
  int   lenght;  /* Camera distance to the middle of the robot */
  float angle;   /* Lens angle in radians */
  float beta;    /* Camera beta offset */
} VIS_CameraOffset;

   /* Distances lookup tables : normal table, border table, angle tables */
typedef struct {
  short Dist_dist[2][imagerows][imagecolumns];     /* Direct relative distance, border or normal */
  short Dist_coord[2][2][imagerows][imagecolumns]; /* Relative distance in x and y, border or normal */
  float Dist_angle[2][imagerows][imagecolumns];    /* Alpha and beta angles */
} VIS_DistanceTables;

   /* Distance structure */
typedef struct {
  short d_row; /* depth distance */
  short d_col; /* side distance */
  short dist;  /* direct distance : dist^2=d_row^2+d_col^2 */
  float alpha; /* sideways angle */
  float beta;  /* up-down angle */
} VIS_Distance;

/***********************************************************************************************************/
/* LibVision functions                                                                                     */
/***********************************************************************************************************/
    /* Converts (R,G,B) triplet into Hue value */
BYTE VIS_RGB2Hue(BYTE R, BYTE G, BYTE B);
    /* Converts (R,G,B) triplet into Saturation value */
BYTE VIS_RGB2Sat(BYTE R, BYTE G, BYTE B);
    /* Converts (R,G,B) triplet into Intensity value */
BYTE VIS_RGB2Int(BYTE R, BYTE G, BYTE B);
    /* Fills the Hue table for faster conversion */
void VIS_FillHueTable(void);
    /* Fills the RGB space cube with a colour class */
void VIS_FillRGBSpace(BYTE HueMin, BYTE HueMax, BYTE Saturation, BYTE ColourClass, VIS_RGBSpace *RGBSpace);
    /* Fills a white class in the RGB space. The shade between black and white is defined by the intensity */
void VIS_FillWhiteClass(BYTE SatMin, BYTE SatMax, BYTE IntMin, BYTE IntMax,
		        BYTE ColourClass, VIS_RGBSpace *RGBSpace);
    /* Denoise every pixels */
int  VIS_ReduceNoise(VIS_Process *Process, int len);
    /* Denoise every pixels f i.e. no for, one pixel around */
int  VIS_ReduceNoisef(VIS_Process *Process);
    /* Denoise 2 i.e. every two pixels */
int  VIS_ReduceNoise2(VIS_Process *Process, int len);
    /* Denoise 2 i.e. every two pixels f i.e. fast (no for, one pixel around) */
int  VIS_ReduceNoise2f(VIS_Process *Process);
    /* Find the limits of the segmented objects */
void VIS_FindLimits(int* pTop, int* pBot, int* pRight, int* pLeft, VIS_Process *Process);
    /* Build a binary map for a specific colour class */
void VIS_FindClass(colimage *Pic, VIS_RGBSpace *RGBSpace, VIS_Process *Process, BYTE ColourClass);
    /* Build a binary map for an several colour classes */
void VIS_FindClasses(BYTE number, colimage *Pic, VIS_RGBSpace *RGBSpace, VIS_Process *Process,
		     BYTE *ColourList);
    /* Finds median hue and the hue range out of the middle part of the picture */
VIS_DefineHue VIS_MedianHue2(colimage *Pic, int size);
    /* Fills the hue conversion table and initialise the algorithm defaults */
void VIS_Init(void);
    /* Clears the RGB space */
void VIS_ColClear(VIS_RGBSpace *RGBSpace);
    /* Initialise a new colour class */
void VIS_ColInit(VIS_RGBSpace *RGBSpace, BYTE ColourClass);
    /* Finds the specified colour class */
int  VIS_ColFindOne(colimage *Pic, VIS_RGBSpace *RGBSpace, VIS_Object *Object, BYTE ColourClass);
    /* Finds several colour classes pur in a list */
int  VIS_ColFind(int number, colimage *Pic, VIS_RGBSpace *RGBSpace, 
		 VIS_Object *Object, BYTE *ColourList);
    /* Camera angle offset */
void VIS_CamCal(VIS_RGBSpace *RGBSpace, BYTE ColourClass);
    /* Modify camera offset values */
int  VIS_ModifyCam(VIS_CameraOffset *NewCam);
    /* Modify algorithm values */
int  VIS_ModifyAlgo(BYTE NewDepth, BYTE NewFast, BYTE NewStep);

/***********************************************************************************************************/
/* Distance functions                                                                                      */
/***********************************************************************************************************/
    /* Check the CamInfo and initialise distances lookup tables */
BYTE VIS_InitDistance(void);
    /* Returns the distance of the ball by looking into the lookup tables */
int  VIS_GetPosition(VIS_Object *Object, VIS_Distance *ObjDist);

/***********************************************************************************************************/
/* Display functions                                                                                       */
/***********************************************************************************************************/
    /* BW LCD picture initialisation */
int  VIS_InitBwimg(BYTE *bwimg);
    /* Get a 2D binary array and make a linear BW array for LCD display */
void VIS_ComputeBW(BYTE *bwimg, BYTE *temp);
    /* Draw object limits and center on bw picture */
void VIS_DrawLimits(BYTE *bwimg, VIS_Object *Object, int value);
    /* Draw borders in a 1D array for LCD display */
void VIS_DrawBorder(BYTE *bwimg, int rows, int cols);
    /* Mark object position by drawing a vertical and horizontal white line through it */
void VIS_MarkObject(image greyimg, int x_middle, int y_middle, int object_size);
    /* Display error messages */
void VIS_ErrorDisplay(char title[16], int error, char msg[16]);
    /* Initialise the progress show of a loop (clear screen, display title,...) */
void VIS_InitShowProgress(char title[16]);
    /* Displays the progress of a loop */
void VIS_ShowProgress(int actual, int max);
    /* Displays the actual offset of the camera */
void VIS_ShowOffset(int alpha_offset, float beta_offset);

/***********************************************************************************************************/
/* Camera functions                                                                                        */
/***********************************************************************************************************/
    /* Initialisation of the camera. 10 retries */
int  VIS_InitCam(int cammode);
    /* Let time to camera for autobrightness */
void VIS_AutobrightnessDelay(int number);
    /* Returns a robot team and ID */
int  VIS_TeamRobot(void);
