/**
  @file path_follow.c
  @author Yves Hwang.
  @date 25/2/2002
  @brief Path following program for the eyebot equipped with cameras.
 */

/****************************************
*       -=| PATH FOLLOWING DEMO |=-     *
*                                       *
*       The program follows a path on   *
*       a board using the distinct      *
*       colour/contrast differences to  *
*       differentiate the path from     *
*       other possible glitches.        *
*                                       *
*       written by Yves Hwang           *
*       hwang-y@ee.uwa.edu.au           *       
*       25/2/2002                       *
*                                       *
*****************************************/


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


#define PROCESS_LOWER_ROW 47 /** The lower bound for the area of interest on the image*/
#define PROCESS_UPPER_ROW 25 /** The upper bound for the area of interest on the image*/
#define PIX_DIFF 70          /** The maximum amout of pixel difference to determine path on the image*/
#define LCD_DISPLAY_WIDTH 54 /*LCD display width for strings*/
#define LCD_STRING_ROW 6        /* maximum number of lines(characters) down the LCD display*/ 
#define LCD_STRING_COLUMN 15    /* maximum number of character across the LCD display*/
#define INVALID -1              /*Arbitrary value for an invalid co-ordinate*/
#define INFINITY 1000000        /*Arbitrary value for infinity*/
#define CENTRE_OF_SCREEN_X (imagecolumns-2)/2   /*The x co-ordinate value for the centre of screen*/
#define MAX_OFFSET 20           /*Maximum allowable offset before values read becomes unreliable*/
#define FORWARD_SPEED 0.03      /*Eyebot forward speed*/
#define TURN_SPEED 0.25         /*Eyebot turn speed*/
#define FORWARD_GRADIENT 1.5    /*Minimum gradient needed for forward motion*/
#define FORTY_FIVE_DEGREES 1.0  /*gradient of 45 degrees*/
#define DELTA_GRADIENT 0.5      /*Change in gradient*/
#define PI M_PI                 /*Definition of PI using the math library*/
#define MARGIN 10               /*Definition of the margin from the sides of the image*/
#define DELTA_RAD PI/8          /*Definition for the change (increment) in rad*/

typedef struct
{
        int x; 
        int y;
}point;
/*a point consist of co-ordinate x and y*/

typedef struct
{
        point point_one;
        point point_two;
        float gradient;
        int offset;
        int direction;
}path;
/*a path consist of two points, a gradient, an offset from the centre of eyebot and a possible direction.*/

enum {
  SERVO_MIN_POSITION =   0, /**< enum value SERVO_MIN_POSITION. */
  SERVO_MAX_POSITION = 255  /**< enum value SERVO_MAX_POSITION. */
};
enum {
        NO_EDGE =0,             /**< enum value NO_EDGE.*/
        ONE_EDGE=1,             /**< enum value ONE_EDGE.*/     
        MORE_THAN_TWO_EDGS=2    /**< enum value MORE_THAN_TWO_EDGS.*/
};
enum{
        UP =-1,                 /**< enum value UP.*/
        DOWN=1                  /**< enum value DOWN.*/
};
enum{
        STRAIGHT = 0,           /**< enum value STRAIGHT.*/
        RIGHT=1,                /**< enum value RIGHT.*/
        LEFT=-1                 /**< enum value LEFT.*/
};
#define INIT_CAM_POS 130        /*Initial camera position*/
#define DELTA_CAM_MOVEMENT 25    /*Camera position increment*/

static void initcam(int, ServoHandle *, int);
static void release_cam(ServoHandle *);
static void initVW(VWHandle *);
static void release_VW(VWHandle *);
static path path_determination();
static point point_detection(image *, int, int *, int *, int, int);
static float calc_gradient(point *, point *);
static int calc_path_offset(point *);
static void path_follow(path *, VWHandle *);
static int horizontal_detection(image *, int, int);

/**
 * The main() for the path following demo. Initialisation of VW handle and camera servo is done here. 
 * Then the path_follow() is called and the eyebot will follow the path according to the path layout. 
 * The program exists when KEY4 is pressed.
 */
int main(void)
{
        path path_a;
        ServoHandle cam_servo;
        VWHandle vw_handle;
        
        release_cam(&cam_servo); /*release before init to ensure proper functionality of the hardware.*/
        release_VW(&vw_handle);
        initVW(&vw_handle);
        initcam(AUTOBRIGHTNESS, &cam_servo,INIT_CAM_POS );
        LCDPrintf("PATH FOLLOWING\n\nYves Hwang 2002\n\n");
        LCDPrintf("Press anykey\n");
        while(KEYRead()==0);
        path_follow(&path_a, &vw_handle);
        release_cam(&cam_servo);
        release_VW(&vw_handle);

        return 0;
}

/**
 * The function is called when a path is determined and the eyebot needs to decide where it wants to go.
 * The fuction is called by main(). After vigorous testing and experimenting, the values for 
 * FORWARD_SPEED and TURN_SPEED is then chosen. However, a possible improvement is to construct a
 * mathmatical model to determine the optimum speed and turning angle using the gradient and offset as 
 * its constraints. The mathematical model may provide a greater accuracy, simplify the algoritm and 
 * perhaps a smoother transition through the path.   
 * @param path *p, VWHandle *vw_handle.
 * @return NONE.
 * @see main().
 */
static void path_follow(path *p, VWHandle *vw_handle)
{
        int prev_direction=STRAIGHT;
        double prev_gradient=0.0, rad=DELTA_RAD;

        LCDClear();
        do{
                *p=path_determination();
                if(prev_gradient!= 0.0) rad=PI/4;
                /*reset the turning radians*/

                if(p->gradient == 0.0 && p->offset == 0)
                {
                        switch(prev_direction)
                        {
                                case STRAIGHT: /*possible horizontal path*/
                                        VWSetSpeed(*vw_handle,0,0);/* 0 m per sec, 0 rad per sec*/
                                        
					/*now the eyebot will have to choose which direction to turn and
					  search for a horizontal path using previously recorded values*/
					if(rad<PI)/*maximum turning is 180 degrees*/ 
                                        {
                                                /*using the previous gradient to determine which direction 
						  to turn */
						if(prev_gradient>0) 
                                                        VWDriveTurn(*vw_handle, -rad, 5*FORWARD_SPEED);
                                                else VWDriveTurn(*vw_handle, rad, 5*FORWARD_SPEED);
                                                rad+=DELTA_RAD;
                                                VWDriveWait(*vw_handle);
                                        }
                                        break;
                                case LEFT:
                                        VWSetSpeed(*vw_handle, FORWARD_SPEED, TURN_SPEED);
                                        break;
                                case RIGHT:
                                        VWSetSpeed(*vw_handle, FORWARD_SPEED, -TURN_SPEED);
                                        break;
                        }       
                }
                if(abs(p->offset) < MAX_OFFSET)/*reliable reading of the path*/
                {
                        /* forward motion: gradient > FORWARD_GRADIENT
                         * meterspersec: FORWARD_SPEED
                         * radpersec:   0
                         */
                        if(abs(p->gradient) >FORWARD_GRADIENT) VWSetSpeed(*vw_handle, FORWARD_SPEED, 0);
                
                        /* curve left and right: gradient > 45 degrees
                         * meterspersec: FORWARD_SPEED
                         * radpersec:   FORWARD_SPEED/6 (optimum value) 
                         */
                        if(p->gradient > FORTY_FIVE_DEGREES)
                               VWSetSpeed(*vw_handle, FORWARD_SPEED, -FORWARD_SPEED/6);
                        if(p->gradient < -FORTY_FIVE_DEGREES)
                               VWSetSpeed(*vw_handle, FORWARD_SPEED, FORWARD_SPEED/6);
                        
                        /* curve left and right
                         * meterspersec: FORWARD_SPEED
                         * radpersec:   FORWARD_SPEED/5 (optimum value) 
                         */
                        if(p->gradient > FORTY_FIVE_DEGREES-DELTA_GRADIENT && p->gradient < FORTY_FIVE_DEGREES)
                                VWSetSpeed(*vw_handle, FORWARD_SPEED, -FORWARD_SPEED/5);
                        if(p->gradient < -FORTY_FIVE_DEGREES+DELTA_GRADIENT && p->gradient > -FORTY_FIVE_DEGREES)
                                VWSetSpeed(*vw_handle, FORWARD_SPEED, FORWARD_SPEED/5);

                        if(p->gradient > 0 && p->gradient < FORTY_FIVE_DEGREES-DELTA_GRADIENT)
                                VWSetSpeed(*vw_handle, FORWARD_SPEED, -FORWARD_SPEED);
                        if(p->gradient < 0 && p->gradient > -FORTY_FIVE_DEGREES+DELTA_GRADIENT)
                                VWSetSpeed(*vw_handle, FORWARD_SPEED, FORWARD_SPEED);                   
                }
                else /*offset larger than max_offset: unreliable reading*/
                {       
                        if(p->offset < 0 ) 
                                VWSetSpeed(*vw_handle, FORWARD_SPEED, FORWARD_SPEED);
                        if(p->offset > 0 ) 
                                VWSetSpeed(*vw_handle, FORWARD_SPEED, -FORWARD_SPEED);
                }
                prev_gradient = p->gradient; /*remember the previous gradient*/
                prev_direction= p->direction;   /*remember the previous direction (horizontal)*/
        }while(KEYRead()!=KEY4);
}

/**
 * The function is called by main() to initialise the VW handle.
 * @param VWHanel *vw_handle.
 * @return none.
 * @see main().
 */
static void initVW(VWHandle *vw_handle)
{
        *vw_handle = VWInit(VW_DRIVE, 1);
        if (*vw_handle == 0) {
                LCDPrintf("VWInit() error\n");
                return;
        }
        VWStartControl(*vw_handle, 7, 0.3, 7, 0.1); /*optimum drive control*/
        /* Vv: 7 (proportional component of the v-controller)
         * Tv: 0.3 (integrating component of the v-controller)
         * Vw: 7 (proportional component of the w-controller)
         * Tw: 0.1 (integrating component of the w-controller)
         */
}

/**
 * The function is called by main() to release the VW handle.
 * @param VWHanel *vw_handle.
 * @return none.
 * @see main().
 */
static void release_VW(VWHandle *vw_handle)
{
        VWStopControl(*vw_handle);
        VWRelease(*vw_handle);
}

/**
 * The function is called by path_determination() to determine the offset of the path.
 * It calculates how far off the first point of the path is from the centre of the eyebot.
 * @param point *pt.
 * @return returns the offset as an int.
 * @see path_determination().
 */
static int calc_path_offset(point *pt)
{
        /*used to calculate how far is the eybot from the centre of path*/
        if( pt->x == INVALID) return 0; /*horizontal gradient; eyebot is not on the path*/
        return pt->x - CENTRE_OF_SCREEN_X;
}

/**
 * The function is called by path_determination() to determine the gradient of the path.
 * It calculates the gradient using two points on the path.
 * @param point *one, point *two.
 * @return returns the gradient as a float.
 * @see path_determination().
 */
static float calc_gradient(point *one, point *two)
{
        if ( one->x == INVALID || one->y == INVALID || two->x == INVALID || two->y == INVALID) 
                return 0.0; /*horizontal gradient*/
        else 
        {
                if( ( two->x - one->x) == 0) return INFINITY; /*vertical asymtote*/     
                return ((float)(-two->y - -one->y)/(float)(two->x - one->x));
        }
                /*the minus signs in front of the y co-ordinates help to translate the y value into the 
                  correct mathematical cartesian value.*/
}

/**
 * The function is used to determine a possible direction for the eyebot. The value obtained using this 
 * function will only be useful if the path has a 90 degrees sharp turn. This function is called in 
 * path_determination().
 * @param image *img, int margin_from_side, int pix_difference.
 * @return returns direciton as an int.
 * @see path_determination().
 */
static int horizontal_detection(image *img, int margin_from_side, int pix_difference)
{
        int i, column, left_edges[margin_from_side], right_edges[margin_from_side], right_edge, left_edge; 
        double direction_left = 0.0, direction_right= 0.0;
        
	/* init to NO_EDGE*/
	for(i=1;i<margin_from_side; i++)
        {
                left_edges[i]=NO_EDGE;
                right_edges[i]=NO_EDGE;
        }
        for(column=1;column<margin_from_side; column++)
        {
                for(i=1;i<(imagerows-2);i++)
                {
			/* detect horizontal edges using edge detection; ie. if the difference in pixel 
                           value is greater than pix_difference.*/
                        if( abs( (*img)[i][column] - (*img)[i+1][column]) >pix_difference)
                        {
                                if(left_edges[column]==NO_EDGE) left_edges[column]=ONE_EDGE;
                                if(left_edges[column]==ONE_EDGE) left_edges[column]=MORE_THAN_TWO_EDGS;
                        }
                        if( abs( (*img)[i][(imagecolumns-2)-column] - (*img)[i+1][(imagecolumns-2)-column]) >pix_difference)
                        {
                                if(right_edges[column]==NO_EDGE) right_edges[column]=ONE_EDGE;
                                if(right_edges[column]==ONE_EDGE) right_edges[column]=MORE_THAN_TWO_EDGS;
                        }
                }
        }
        for(i=1;i<margin_from_side;i++) /*add up all the values of the edges. (detected and non-detected)*/
        {
                direction_left+=(double)left_edges[i];
                direction_right+=(double)right_edges[i];
        }       
        direction_left/=margin_from_side; /*average the value added*/
        direction_right/=margin_from_side;      
        
	/* the average value must be greater or equal to MORE_THAN_TWO_EDGS+ ONE_EDGE divide by 2*/
	if(direction_left >= (MORE_THAN_TWO_EDGS + ONE_EDGE)/2 ) left_edge = MORE_THAN_TWO_EDGS;
        if(direction_right>= (MORE_THAN_TWO_EDGS + ONE_EDGE)/2 ) right_edge = MORE_THAN_TWO_EDGS;

        /*to determine a possible horizontal/90 degrees turn..*/  
	if(left_edge==MORE_THAN_TWO_EDGS && right_edge!=MORE_THAN_TWO_EDGS) return LEFT;
        else if(right_edge==MORE_THAN_TWO_EDGS && left_edge!=MORE_THAN_TWO_EDGS) return RIGHT;
        else return STRAIGHT;
}       
         
                                
/**
 * The function is called to determine a point on the path in the captured image. The method used to
 * determine a point is of an edge detection method. A possible point in a path consists of two edges and
 * a path consists of at least two points.
 * @param image *img, int array, int *flag, int *edge_count, int pix_difference, int up_down.
 * @return returns direciton as an int.
 * @see path_determination().
 */
static point point_detection(image *greyimg, int array, int *flag, int *edge_count, int pix_difference, int up_down)
{
        point pt; 
        int i;
        
	/* specified array is out of the valid range*/
	if(array > PROCESS_LOWER_ROW || array < PROCESS_UPPER_ROW)  
        {
                pt.x = INVALID;
                pt.y = INVALID;
                return pt;
        }
        for(i=1;i<(imagecolumns-2); i++)
        {
                /*edge detection; if the difference in pixel is greater than pix_difference*/
		if( abs( (*greyimg)[array][i] - (*greyimg)[array][i+1]) >pix_difference)
                {
                        switch(*flag)
                        {
                                case NO_EDGE:
                                        *flag=ONE_EDGE;
                                        pt.x=i+1;
                                        (*edge_count)=1;
                                        break;
                                case ONE_EDGE:
                                        *flag=MORE_THAN_TWO_EDGS;
                                        pt.x+=i+1;
                                        (*edge_count)++;
                                        break;
                                case MORE_THAN_TWO_EDGS:
                                        pt.x+=i+1;
                                        (*edge_count)++;
                                        break;
                        }
                }
        }
        /*average the two edges to get the 1st pt on path in the lower bound*/
        switch(*flag) 
        {
                case NO_EDGE:
			/* recursive call until the specified array is out of the valid range*/
                        pt=point_detection(greyimg, array+up_down, flag, edge_count, pix_difference, up_down);
                        break;
                case ONE_EDGE:
                        if(pt.x==(imagecolumns-2)) /*if the screen is blank..*/
                        {
                                pt.x = INVALID;
                                pt.y= INVALID;
                        }
                        else pt.y=array;
                        break;
                case MORE_THAN_TWO_EDGS:
			/* average the value of detected edges to reduce error*/
                        pt.x/=*edge_count;
                        pt.y=array;
                        break;
        }
        return pt;
}
/**
 * The function is called in path_follow() to determine all the attribute of a possible path.
 * @param none.
 * @return returns path. 
 * @see path_follow().
 */ 
static path path_determination(void)
{
        path path_one;
        image greyimg, sobel_image;
        int lower_edge, upper_edge, lower_edge_count, upper_edge_count;
        
        lower_edge=NO_EDGE;
        upper_edge=NO_EDGE;
        CAMGetFrame(&greyimg);
        IPSobel(&greyimg, &sobel_image);        
        LCDPutGraphic(&sobel_image);
        /*determine pt1 on lower bound*/
        path_one.point_one=point_detection(&sobel_image, PROCESS_LOWER_ROW, &lower_edge, &lower_edge_count, PIX_DIFF, UP);
        /*determine pt2 on upper bound*/
        path_one.point_two=point_detection(&sobel_image, PROCESS_UPPER_ROW, &upper_edge, &upper_edge_count, PIX_DIFF, DOWN);
        path_one.gradient=calc_gradient(&path_one.point_one, &path_one.point_two);
        path_one.offset = calc_path_offset(&path_one.point_one);
        LCDPrintf("(%d,%d) (%d,%d)\ngrad %0.2f offset %d\n" ,path_one.point_one.x,path_one.point_one.y, path_one.point_two.x, path_one.point_two.y, path_one.gradient, path_one.offset);                
        path_one.direction = horizontal_detection(&sobel_image, MARGIN, PIX_DIFF);      
          return path_one;      
}

/**
 * The function is called by main() to initialise the camera and its servo.
 * @param int camera_mode, ServoHandle *camera_servo, int init_cam_position.
 * @return none.
 * @see main().
 */
static void initcam(int camera_mode, ServoHandle *camera_servo, int init_cam_pos)
{
        int camera, bright, hue, sat;
        
        /* Initialize servos for the camera*/
        if(!(*camera_servo = SERVOInit(SERVO11)))
        LCDPrintf("CameraSerInit\nError!\n");
        else SERVOSet(*camera_servo, init_cam_pos);
        
        
        camera = CAMInit(NORMAL);
        if (camera < COLCAM) LCDPrintf("No colour camera!");
        else
        {
                if (camera == NOCAM)
                LCDPrintf("No camera!\n");
                else
                if (camera == INITERROR)
                LCDPrintf("CAMInit!\n");
        }
        if (camera_mode == AUTOBRIGHTNESS) CAMMode(AUTOBRIGHTNESS);
        else
        {
                LCDPrintf("no autobrightness\n");
                CAMGet(&bright, &hue, &sat);
                CAMMode(NOAUTOBRIGHTNESS);
                CAMSet(135, 130, sat);
                /*ideal value for camera setting
                 * brightness:  135
                 * offset:      130 */
        }
}

/**
 * The fucntion is called in main() to release the camera and its servo handle.
 * @param ServoHandle *cam_servo.
 * @return NONE.
 */
static void release_cam(ServoHandle *cam_servo)
{
        SERVORelease(*cam_servo);
        CAMRelease();
}