/* *********************************************************************** */
/* Filename: obstacle.c                                                    */
/*                                                                         */
/* Description:  Obstacle Detection Application Program                    */ 
/*                                                                         */
/* This application is designed to demonstrate the image processing        */
/* capability of the EyeBot Mobile Robot Platform with its QuickCam        */  
/* digital greyscale camera.  It uses image data to detect obstacles,      */
/* avoid collisions, and navigate in a dynamically changing environment.   */
/*                                                                         */
/* Author:          Barbara Linn    (bsl0560@cs.rit.edu)                   */
/*                                                                         */
/* Last changes:     26-Apr-97                                             */
/*                   20-Aug-97 Thomas Braunl                               */
/*                   28-Feb-00 Thomas Braunl, adapt for V3.0               */
/* Version:          1.1                                                   */
/* *********************************************************************** */

#define REALTIME        0
#define SLOWMODE        1

#include "eyebot.h"

/* global variables */

int version;  
char z;
image img1, img2, img3, img4;
image *ip1, *ip2, *ip3, *ip4;
int angle, speed;
int mymaxrow, mymaxcol;
int half, third;
IPL_CONFIG _IPLConfig;

int main(void)
/* ********************************************************************* */
/*                                                                       */
/* This is the application's top level routine.  It controls the endless */
/* loop that offers users the choice of running in either Realtime or    */
/* Slow mode.  Control returns to this routine after the user requests   */
/* END inside the application                                            */
/*                                                                       */
/* ********************************************************************* */
{
int initob(void);
int mainmenu(void);
int obstacle(int);

	initob();

	/* Endless main application loop */

	mainmenu();

	while (1==1)
	{
		z = KEYGet();
		switch (z)
		{
			case KEY1:
				LCDMode(NONSCROLLING);
				obstacle(REALTIME);
				mainmenu();
				break;

			case KEY2:      
				LCDMode(NONSCROLLING);
				obstacle(SLOWMODE);
				mainmenu();
				break;

			case KEY3:
					break;

			case KEY4:      
					return (0); 
					break;      
			
			default:        break;

		}
	}
}

int initob()                                                               
/* ********************************************************************* */
/*                                                                       */
/* This routine initializes the Image Processing system, the camera,     */
/* and the screen constants needed by the Obstacle Detection routines.   */
/*                                                                       */
/* ********************************************************************* */
{
/* Initialize screen constants */
	mymaxrow = 54;
	mymaxcol = 82;
	half = (mymaxcol-2)/2;
	third = mymaxcol/3;

/* Initialize the Image Processing System */

	_IPLConfig.colorBlack = 0;
	_IPLConfig.colorWhite = 15;
	_IPLConfig.imageWidth = mymaxcol;
	_IPLConfig.imageHeight = mymaxrow;

	IPL_init(&_IPLConfig);  

	/* Clear Display */
	LCDClear();
	
	/* Display warning in case camera doesn't initialize */
	LCDPrintf("Camera?....");

	/* Initialize camera */
	version = CAMInit(WIDE);
	while(version==254)     /* In case CamInit failed the first time */
		{ 
		version=CAMInit(WIDE);
		LCDPrintf("I");
		}

	if (version==255)       /* CamInit failed due to no camera */
		{
		LCDSetPos(2,0);
		LCDPrintf("No Camera !!\a");
		KEYWait(0);
		} 
	return(0);
}


int obstacle (int mode)
/* ********************************************************************* */
/*                                                                       */
/* This routine is the heart of the application.  It is called by the    */
/* main routine with an integer value for mode (0 = realtime, 1 = slow). */
/* It executes the algorithm that gets the image data, processes it with */
/* edge-detection and thresholding functions, draws the sector borders,  */
/* computes lowest and highest edges, and computes the new steering      */
/* angle and speed.                                                      */
/*                                                                       */
/* ********************************************************************* */
{
int steer(int, int, int, int, int, int, int);
int marker(int, int);
int display();

	int i;
	BYTE th = 6;
	BYTE blk = 0;
	BOOL blank, stop;
	int row, col;
	int xL, yL, xC, yC, xR, yR;
	char zp;

	LCDClear();
	if (mode == REALTIME)
		LCDMenu("","","","END");
	else    LCDMenu("","DMP","GO","END");
		
	ip1=&img1;ip2=&img2;ip3=&img3;ip4=&img4;
	angle = 0;
	speed = 0;

	while (1==1) {
		
		/* Get image */
		CAMGetFrame(ip1);

		/* Do edge detection */
		IPSobel(ip1, ip2);
		
		/* Thresholding */ 
		IPL_threshold((BYTE *)ip2, (BYTE *)ip3, th);
		
		/* Display the Edges */ 
		LCDPutGraphic(ip3);                                        
		
		/* Draw the border lines for left, center, and right */
		for (i=1; i<=2; i++) {
			col = i*third;
			for (row=0; row < mymaxrow; row++) {
				LCDSetPixel(row, col, TRUE);
			}
		}

		/* Find Left lowest point */
		yL = 1;
		xL = 1;
		row = mymaxrow - 1;
		stop = FALSE;
		
		for (col=2; col<third; col++) {
			while (row > 1 && stop == FALSE) {
				if ( img3[row][col] == blk) {/* pixel black? */
					if (row >= yL) {
						stop = TRUE;
						yL = row;
						xL = col;
					}
				}
				row -= 1;
			}
			stop = FALSE;           /* Reset stop flag */
			row = mymaxrow - 1; 
		}                                                      

		/* Display the left marker */
		marker(yL, xL);
		
		/* Find Center Highest point */
		yC = mymaxrow - 1;              /* Init y to bottom */
		xC = half;                      /* Init x to center */
		row = mymaxrow - 1;
		blank = TRUE;                   /* Init blank region flag */
		stop = FALSE;                   /* Init stop flag */
		for (col=third+1; col<third*2; col++) {
			while (row > 1 && stop == FALSE) {
				if ( img3[row][col] == blk) {/*pixel black?*/
					blank = FALSE;    /* Not blank */ 
					stop = TRUE;      /* Highest in row*/
					if (row < yC) {   /*Highest overall?*/
						yC = row;
						xC = col;
					}
				}
				row -= 1;
			}
			if (row == 1) {         /* All the way w/o edge? */
				yC = 1;         /* This is highest */
				xC = col;
			}
			row = mymaxrow - 1;             /* Reset to bottom */
			stop = FALSE;                   /* Reset stop flag */
		}                                                      
		
		/* No edges in this Center region? */
		if (blank == TRUE) {                /* Set to Mid-region */
			yC = 1;
			xC = half;
		}
		   
		/* Display the center marker */
		marker(yC, xC);

		/* Find Right lowest point */
		yR = 1;
		xR = mymaxcol-4;
		row = mymaxrow - 1;
		stop = FALSE;
		for (col= mymaxcol-4; col>third*2; col--) {
			while (row > 1 && stop == FALSE) {
				if (img3[row][col] == blk) {/* pixel black? */
					if (row >= yR) {
						stop = TRUE;
						yR = row;
						xR = col;
					}
				}
				row -= 1;
			}
			stop = FALSE;
			row = mymaxrow - 1;
		}                                                      

		/* Display the right marker */
		marker(yR, xR);
			 
		/* Compute new Angle and Speed */
		steer(mode, xL, yL, xC, yC, xR, yR);

		/* Display new Angle and Speed */
		display();

		/* Slow mode */
		if (mode == SLOWMODE) { 
			
			/* Check for Termination */
			KEYGetBuf(&zp);
			if (zp == KEY4)
				break;
			
			if (zp == KEY3) {
				LCDClear();
				LCDMenu("","DMP","GO","END");
			}
			else {
				/* Dump data */
				LCDMenu("","","","GO");
				LCDSetPos(6,10); 
				LCDPrintf("%2d %2d", xL, yL);
				LCDSetPos(7,0);
				LCDPrintf("%2d %2d %2d %2d", xC, yC, xR, yR);

				KEYWait(KEY4);
				LCDClear();
				LCDMenu("","DMP","GO","END");
			}
		}
		else { 
			/* Check for Termination */
			z = KEYRead();
			if (z == KEY4)
				break;
		}
	}
	return(0);
}

		

int marker(int crow, int ccol)    
/* ********************************************************************* */
/*                                                                       */
/* This routine places the arrow markers on the display at the requested */
/* values of row and column.  It ascertains whether the marker should    */
/* point up or down in order to stay within the bounds of the display,   */
/* and inverts its pixels based on what would be most visible in its     */ 
/* current image location.                                               */
/*                                                                       */
/* ********************************************************************* */
{
int i, j;
BOOL set;
	
	if (crow+4 < mymaxrow) {                  /* Marker off bottom? */
		for (i=crow; i<=crow+4; i++) {    /* Yes, point up at pixel */
			if (LCDGetPixel(i, ccol)) /* Pixel already on? */
				set = FALSE;      /* Invert for visibility*/
			else    set = TRUE;
			LCDSetPixel(i, ccol, set);
			if (i == crow+1)  {
				for (j=ccol-1; j<=ccol+1; j++) {
					if (LCDGetPixel(i, j) && j!=ccol)
						set = FALSE;   /* Invert */
					else    set = TRUE;
					LCDSetPixel(i, j, set);
				}
			}
		}
	}
	else {                                  /* Point down at pixel */                   
		for (i=crow; i>=crow-4; i--) {
			if (LCDGetPixel(i, ccol))  /* Pixel already on? */
				set = FALSE;       /* Invert for visibility*/
			else    set = TRUE;
			LCDSetPixel(i, ccol, set);
			if (i == crow-1)  {
				for (j=ccol-1; j<=ccol+1; j++) {
					if (LCDGetPixel(i, j) || j==ccol)
						set = FALSE;   /* Invert */
					else    set = TRUE;
					LCDSetPixel(i, j, set);
				}
			}
		}
	}
	return(0);
}

int steer(int md, int xlt, int ylt, int xcn, int ycn, int xrt, int yrt)
/* ********************************************************************* */
/*                                                                       */
/* This routine computes the new steering angle and speed, based on the  */
/* execution mode (Realtime or Slow) and the (x,y) values of the sector  */
/* markers.                                                              */
/*                                                                       */
/* ********************************************************************* */

{
int new_angle;
int border_rt, border_lt;
int band = 5;
float const1 = 0.025;
float const2 = 0.75;
float const3 = 0.25;

	/* Adjust Y values to reflect origin at base of LCD */
	ylt = mymaxrow - ylt;             
	ycn = mymaxrow - ycn;             
	yrt = mymaxrow - yrt;

	/* Adjust X values to reflect origin at center of LCD */
	xlt = xlt - half;
	xcn = xcn - half;
	xrt = xrt - half;

	/* Compute adjusted x-value for sector boundaries */
	border_lt = third - half;
	border_rt = 2 * third - half;
	
	/* Compute the new speed */
	if (md == REALTIME)             /* include previous speed */
		speed = (const2 * speed) + (const3 * (ycn - 5));

	else                            /* new speed from image data only */
		speed = ycn - 5;

	if (speed < 5)                  /* obstacle immediately ahead? */
		speed = 0;              

	/* If path blocked, execute right-angle turn */
	if (speed==0) 
		angle = 90;
	
	/* Otherwise, compute current image angle and new steering angle */
	else { 
		/* Is there a blank left or right sector condition? */
		if ((xlt <= -half && ylt >= mymaxrow)
					|| (xrt >= half && yrt >= mymaxrow))
			new_angle = xcn;
		else {
			/* Is there either diagonal condition? */
			if (ylt >= mymaxrow-band && yrt <= band)
		
				new_angle = xcn;
			 
			else {
				if (ylt <= band && yrt >= mymaxrow-band)
					new_angle = xcn;
				
				else
					/* Normal condition */
					new_angle = xcn 
					   + const1 * (yrt-ylt) * (xrt-xlt);

			}
		}
	
		/* Compute the new steering angle */
		if (md == REALTIME)     /* include previous angle */
			angle = (const2 * angle) + (const3 * new_angle);

		else                    /* new angle from image data only */
			angle = new_angle;

		/* Computed angle out of legal range? */
		if (angle > 90) 
			angle = 90;
		
		if (angle < -90)      
			angle = -90;

	}
	return(0);
}

int display() 
/* ********************************************************************* */
/*                                                                       */
/* This routine displays the new steering angle and speed on the LCD,    */
/* using the global variables for angle an speed.                        */
/*                                                                       */
/* ********************************************************************* */
{
	
	LCDSetPos(0,10);                
	if (speed == 0) {               /* Check if stopped */
		LCDPrintf("STOP ");
		LCDSetPos(1,10);
		LCDPrintf("       ");
	}
	else {   
		LCDPrintf("SPEED");        /* Display speed */
		LCDSetPos(1,10);
		LCDPrintf("%3d", speed);
	}

	/* Display angle */
	if (angle != 0) {               /* Check not straight ahead */
		LCDSetPos(3,10);
		LCDPrintf("TURN ");
		
		LCDSetPos(4,10);        /* Display turn angle */
		LCDPrintf("%3d ", angle);

		LCDSetPos(5,10);        /* Display turn direction */
		if (angle < 0)        
			LCDPrintf("LEFT ");
		else    LCDPrintf("RIGHT");
	}
	else {
		LCDSetPos(3,10);        /* Display straight ahead */
		LCDPrintf("AHEAD");
		LCDSetPos(4,10);
		LCDPrintf("       ");
		LCDSetPos(5,10);
		LCDPrintf("       ");

	}
	return(0);
}


int mainmenu()
/* ********************************************************************* */
/*                                                                       */
/* This routine displays the application's main menu on the LCD          */
/*                                                                       */
/* ********************************************************************* */

{
	LCDClear();
	LCDMode(SCROLLING|CURSOR);
	LCDMenu("RT","SL","","END");

	LCDPrintf("Obstacle\n");
	LCDPrintf("Avoidance: READY\n");

	LCDPrintf("Choose Mode:\n");
	LCDPrintf("Realtime\n");
	LCDPrintf("or Slow");

	return(0);
}