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

/****h* LibVision/LibVision
 *DESCRIPTION    
 *
 * --------------
 * #define values
 * --------------
 *
 * 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
 *
 * Picture characteristics
 *   #define BYTES_PER_PIXEL 3
 *   #define BYTES_PER_LINE  (imagecolumns*BYTES_PER_PIXEL)
 *
 * 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)
 *
 * LCD Display informations
 *   #define LCD_DISPLAY_ROW  64
 *   #define LCD_DISPLAY_COL  128
 *   #define LCD_DISPLAY_SIZE ((LCD_DISPLAY_ROW*LCD_DISPLAY_COL)/8)
 *
 * 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
 *
 * 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
 *
 * ------------------
 * typedef structures
 * ------------------
 *
 * 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];
 *
 * 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;
 *
 * 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}};
 *
 * 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;
 *
 * ----------------
 * 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
 ****/

#include <eyebot.h>
#include <math.h>

   /* Camera offset default informations */
#define MAX_ROBOTS 5
static const int   VIS_CAM_HEIGHT[2][MAX_ROBOTS]={{59, 62, 55, 52, -1}, {-1, -1, -1, -1, -1}};
static const int   VIS_CAM_LENGHT[2][MAX_ROBOTS]={{30, 33, 38, 20, -1}, {-1, -1, -1, -1, -1}};
static const BYTE  VIS_CAM_ALPHA[2][MAX_ROBOTS]={{118, 139, 132, 128, -1}, {-1, -1, -1, -1, -1}};
static const float VIS_CAM_BETA[2][MAX_ROBOTS]={{0.147, 0.026, 0.037, 0.265, -1}, {-1, -1, -1, -1, -1}};
static const float VIS_CAM_ANGLE[2][MAX_ROBOTS]={{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  /* balll radius */

    /* Picture characteristics */
#define BYTES_PER_PIXEL 3
#define BYTES_PER_LINE  (imagecolumns*BYTES_PER_PIXEL)

    /* Defaults for the algorithm */
#define VIS_ALGO_DEPTH 1
#define VIS_ALGO_FAST  1
#define VIS_ALGO_STEP  2
#define VIS_ALGO_LOOP  (VIS_ALGO_STEP-1)

    /* LCD Display informations */
#define LCD_DISPLAY_ROW  64
#define LCD_DISPLAY_COL  128
#define LCD_DISPLAY_SIZE ((LCD_DISPLAY_ROW*LCD_DISPLAY_COL)/8)

   /* 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

    /* 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];

    /* 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(int HueMin, int HueMax, 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_FindClasses(colimage *Pic, VIS_RGBSpace *RGBSpace, VIS_Process *Process, BYTE ColourClass);
    /*Finds median hue out of a the middle part of the picture */
BYTE VIS_MedianHue(colimage *Pic, int size);
    /* Fills the hue conversion table and initialise the algorithm defaults */
void VIS_Init(void);
    /* Modify algorithm values */
int VIS_ModifyAlgo(BYTE NewDepth, BYTE NewFast, BYTE NewStep);
    /* Modify camera offset values */
int VIS_ModifyCam(VIS_CameraOffset *NewCam);
    /* Clears the RGB space */
void VIS_ColClear(VIS_RGBSpace *RGBSpace);
    /* Initialise a new colour class */
void VIS_ColInit(VIS_RGBSpace *RGBSpace, BYTE ColourClass);
		/*   Camera angle offset */
void VIS_CamCal(VIS_RGBSpace *RGBSpace, BYTE ColourClass);
    /* Finds the specified colour class */
int VIS_ColFind(colimage *Pic, VIS_RGBSpace *RGBSpace, VIS_Object *Object, BYTE ColourClass);

/***********************************************************************************************************/
/* 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);
