///////////////////////////////////////////////////////////////////////////////////////
//
// File: car.c
//
// Author: Timothy Bruton (c),
// 1999 BCSE Honours, Monash University,
// http://www.csse.monash.edu.au/hons/projects/1999/Tim.Bruton/
//
// Description: This file uses the VL library to input a video stream 
// from the composite video in, on VINO cards in Silicon Graphics Indy machines.
// When the program is being run, there are several options to select from
// by using the function keys.
//
// The program is run by entering: car n m 
// where n=0 for no screen output during the run, n=1 for screen output
// and m=0 for open loop control, m=1 for closed loop control
// Note it is a good idea to calibrate the system before the car is run,
// this measures the contrast difference and width of the two lines on the carpet
// which must be about 25cm apart and in the centre of the screen.
// Program thresholds are then set.
//
// All of the options continually grab a frame and perform processing until the Esc
// key is hit. At this stage if the F10 key is hit then the program exits.
// 
// The F3 pixel view option prints the value of the pixel under the purple dot on the screen,
// the dot's position is changed by hitting F1 or F2 or F3 of F4, and a global
// minimum/maximum range of the pixels searched so far is updated by pressing F5.
// Pressing F6 then colours all the pixels in this range pink. F7 resets this colouring
// and the global min/max values
//
// The F5 option involves hitting F11 when the camera has moved so that the flow field
// may be measured between these two frames. This option looks for the file called 
// car.data placed in the  current directory. This file hold the size of the sub string
// that matching is performed on.
//
// Hitting F8 any time will capture the screen to capt.ppm in the current directory.
//
//////////////////////////////////////////////////////////////////////////////////////

#include <stdio.h>                                          
#include <stdlib.h>
#include <gl/gl.h>
#include <gl/device.h>
#include <dmedia/vl_vino.h>
#include <vl/vl.h>
#include <unistd.h>
#include <sys/types.h>
#include <signal.h>

extern "C" {
#include <image.h>
}

#define ANALOG 1
#define DIGITAL 0

// program constants and mode flags
// v_flag is for turning off screen write
// cl_flage is for open/closed loop control
// widthT width of the track
// CENTRE = 64 = xsize/2*b_width
// OFFSET how far up to look for the track 
#define NEG_NUMBER -1
#define BIG_NUMBER 10000
#define MAX_SIZE 50
#define CENTRE 64
int v_flag=1;
int cl_flag=0;
IMAGE *out;
int OFFSET=80;

// printing flags
int flag1=2;
int mflag=1;
int printflag=0;
int printinfo=0;
// bitmask value
int bmask=255;

// pixel sub-block size for finding the track
int b_width=6;
int b_height=1;

// line thresholds
float gradth=70;
float minlinewidth=2;
float maxlinewidth=40;
int line1=300+OFFSET;

// recovery variables
int num_one_line=0;
int off_track=0;
int BACK_LEFT=0;
int BACK_RIGHT=0;
#define LOST 2000
#define FINISHED 1000

// motion vector variables
#define mot_size 200

// multiple process variables for signalling etc
int s_flag=0;
int child_id,parent_id;
int quit_flag=0;
int dont_send_to_parent=0;

// finish line flag
// width of track
int FINISH_LINE_FLAG=0;
int widthT=90;

// computer output->car variables
FILE* serial;
char move_car[1];
char move1_car[1];

// feed back controller car controls
// starting values
#define D_FORWARD 250000 
#define D_FORWARD_MIN 120000
#define D_FORWARD_MAX 100000
#define D_TURN_MIN 140000 
#define D_TURN_MAX 300000 
#define D_TURN_W_B_MIN 300000
#define D_TURN_W_B_MAX 380000
#define D_BRAKE_MIN 60000 
#define D_BRAKE_MAX 80000 
// adjustable values
float FORWARD=D_FORWARD;
float FORWARD_MIN=D_FORWARD_MIN;
float FORWARD_MAX=D_FORWARD_MAX;
float TURN_MIN=D_TURN_MIN;
float TURN_MAX=D_TURN_MAX;
float BRAKE_MIN=D_BRAKE_MIN;
float BRAKE_MAX=D_BRAKE_MAX;
float TURN_W_B_MIN=D_TURN_W_B_MIN;
float TURN_W_B_MAX=D_TURN_W_B_MAX;
// positions for different actions
int TURN_POS=7;
int TURN_POS_W_B=15; 
int END=100;
float fgain=0.003;

// fixed gain controller car controls
// starting controls
#define D_F 250000
#define D_LT 160000
#define D_LF 140000
#define D_BT 180000
#define D_BF 120000
#define D_BBT 260000
#define D_ET 280000
#define D_EB 60000
#define D_EBT 350000
#define D_EBB 70000
// adjustable controls
float f_F=D_F;
float f_LT=D_LT;
float f_LF=D_LF;
float f_BT=D_BT;
float f_BF=D_BF;
float f_BBT=D_BBT;
float f_ET=D_ET;
float f_EB=D_EB;
float f_EBT=D_EBT;
float f_EBB=D_EBB;
// positions for different actions
#define FORWARD_CAR 7
#define LITTLE_TURN 10
#define BIG_TURN 14
#define BIGGEST_TURN 17
#define END 21

// VINO video variables
VLServer svr;
VLPath path;
VLNode src,drn;
VLControlValue val;
VLBuffer buffer;
VLInfoPtr info;
char *data;
long win;
int xsize, ysize, x, y;

// colour analysis (view_pixels()) variables
int rlocalmin=255,rglobalmin=255,rlocalmax=0,rglobalmax=0;
int glocalmin=255,gglobalmin=255,glocalmax=0,gglobalmax=0;
int blocalmin=255,bglobalmin=255,blocalmax=0,bglobalmax=0;
#define rval (*(data+4*x+3+(4*xsize*y)))
#define gval (*(data+4*x+2+(4*xsize*y)))
#define bval (*(data+4*x+1+(4*xsize*y)))

// timing variables
struct timeval tp;
struct timezone tzp;
long int start_sec, start_usec, end_sec, end_usec,sec,usec;
struct timeval frtp;
struct timezone frtzp;
double frstart_sec, frstart_usec, frend_sec, frend_usec,frsec,frusec;

// time struct
struct t
	{
		long int sec;
		long int usec;
	};

// calibration data structure
struct cal
	{
        	float gradient;
                float width;
        };

// y=mx+c data structure
struct sim{
        float m;
        float c;
};

void error_exit()
{
	printf("Abort\n");
	exit(1);
}

// set up source and drain nodes and a datapath between them
// set VINO composite input, PAL timing
// create video buffer
void open_video()
{
	if (!(svr = vlOpenVideo("")))
	error_exit();
	src = vlGetNode(svr, VL_SRC, VL_VIDEO, ANALOG);
	//src = vlGetNode(svr, VL_SRC, VL_VIDEO, DIGITAL);
	// the above line containing DIGITAL will use the Indycam
	drn = vlGetNode(svr, VL_DRN, VL_MEM, VL_ANY);
	path = vlCreatePath(svr,VL_ANY, src, drn);
	if ((vlSetupPaths(svr, (VLPathList)&path, 1, VL_SHARE, VL_SHARE)) < 0) 
		error_exit();

	val.intVal = VL_PACKING_RGB_8;
	vlSetControl(svr, path, drn, VL_PACKING, &val);

	val.intVal = VL_VINO_COMPOSITE;
	vlSetControl(svr, path, src, VL_MUXSWITCH, &val);

	val.intVal = VL_TIMING_625_SQ_PIX;
        vlSetControl(svr, path, src, VL_TIMING, &val);

	val.intVal = 255;
        vlSetControl(svr, path, src, VL_VINO_PAL_SENS, &val);

	val.intVal = 1;
        vlSetControl(svr, path, src, VL_VINO_AUFD, &val);

	vlGetControl(svr, path, drn, VL_SIZE, &val);
	xsize = val.xyVal.x; 
	ysize = val.xyVal.y;
	x=(4*xsize)/2;
	y=ysize/2;

	buffer = vlCreateBuffer(svr, path, drn, 1);
	if (buffer == NULL)
		error_exit();

	vlRegisterBuffer(svr, path, drn, buffer);
	if (vlBeginTransfer(svr, path, 0, NULL))
		error_exit();
}

// clean up paths and the buffer
void close_video()
{
	vlEndTransfer(svr, path);
	vlDeregisterBuffer(svr, path, drn, buffer);
	vlDestroyBuffer(svr, buffer);
	vlCloseVideo(svr);
}

// window to display video
void open_window()
{
	prefsize(xsize,ysize);
	foreground();
	win = winopen("Driver View");
	RGBmode();
	pixmode(PM_TTOB, 1);
	gconfig();
}

void close_window()
{
	winclose(win);
}

// wait for a valid frame and fill buffer with it
void wait_for_frame()
{
	long v[2];
	int i;
	do {
		sginap(1);
		info = vlGetLatestValid(svr, buffer);
		//info = vlGetNextValid(svr, buffer);
	}while (!info);
	data = vlGetActiveRegion(svr, buffer, info); 
}

// write the frame to the window
void write_frame()
{
	lrectwrite(0, 0, xsize-1, ysize-1, (ulong *)data);
	vlPutFree(svr, buffer);
}

// pass in (x1,y1) (x2,y2) and this return the gradient
// and constant of the y=mx+c equation
struct sim solve(float x1, float y1, float x2, float y2)
{
        struct sim sim1;
 
        sim1.m=(y2-y1)/(x2-x1);
        sim1.c=(y1-sim1.m*x1);
        return sim1;
}

// set global variables start_sec and start_usec
// to be used by end_time() 
void begin_time()
{
        gettimeofday(&tp,&tzp);
        start_sec=tp.tv_sec;
        start_usec=tp.tv_usec;
}

// using the global variables start_sec and start_usec
// which are set by begin_time()
// this function will return the elapsed time
// http://www.csse.monash.edu.au/hons/projects/1999/Tim.Bruton/
struct t end_time()
{
	struct t ttime;

        gettimeofday(&tp,&tzp);
        end_sec=tp.tv_sec;
        end_usec=tp.tv_usec;
        sec=end_sec-start_sec;
        usec=end_usec-start_usec;
        if(usec<0)
        {       
                usec=1000000-start_usec+end_usec;
                sec--; 
        }
	
	ttime.sec=sec;
	ttime.usec=usec;

	return ttime;
}

// set global variables frstart_sec and frstart_usec
// to be used by frame_end_time()    
void frame_begin_time()
{
        gettimeofday(&frtp,&frtzp);
        frstart_sec=frtp.tv_sec;
        frstart_usec=frtp.tv_usec;
}

// using the global variables frstart_sec and frstart_usec
// set by frame_begin_time()
// this function will place the elapsed time into
// the global variables frsec and frusec 
void frame_end_time()
{
        gettimeofday(&frtp,&frtzp);
        frend_sec=frtp.tv_sec;
        frend_usec=frtp.tv_usec;
        frsec=frend_sec-frstart_sec;
        frusec=frend_usec-frstart_usec;
        if(frusec<0)
        {
                frusec=1000000-frstart_usec+frend_usec;
                frsec--;
        }
}

// PRE: (line,offset) are the (y,x) video image coordinates,
//      colour is the colour selection 0-4,
// POST: the function draws a 4*4 square at these co-ordinates
void draw_square(int offset, int line, int colour)
{
	int i,j;
	offset=offset*4;

	for(j=line;j<(line+5);j++)
	{
		for(i=(j*4*xsize+offset);i<(j*4*xsize+16+offset);i=i+4)
		{
			if(colour==0)
			{
				*(data+i+3)=255;
                		*(data+i+2)=0;
                		*(data+i+1)=0;
			}
			if(colour==1)
                        {
                                *(data+i+3)=0;
                                *(data+i+2)=255;
                                *(data+i+1)=0;
                        }
			if(colour==2)
                        {
                                *(data+i+3)=0;
                                *(data+i+2)=0;
                                *(data+i+1)=255;
                        }
			if(colour==3)
                        {
                                *(data+i+3)=255;
                                *(data+i+2)=0;
                                *(data+i+1)=255;
                        }
			if(colour==4)
                        {
                                *(data+i+3)=128;
                                *(data+i+2)=255;
                                *(data+i+1)=255;
                        }
		}
	}
}

// this function accepts two line numbers and two coloumn numbers
// to define a square. lineu is the upper line, linl is the lower
// line, coll is the left column, colr is the right column
// All pixels in this square are converted to grey scale
// http://www.csse.monash.edu.au/hons/projects/1999/Tim.Bruton/
void change_format_square(int lineu,int linel,int coll,int colr)
{
        int i,j;
        int red, green, blue, intensity;

        // convert to grey scale
        // and mask off least significant bits, according to bmask
	// for each line
	for(i=(lineu*4*xsize);i<((linel)*4*xsize);i=i+4*xsize)
	{
		// for each column
        	for(j=coll*4;j<colr*4;j=j+4)
        	{
                	red=*(data+i+j+3);
                	green=*(data+i+j+2);
                	blue=*(data+i+j+1);
                	intensity=(int)sqrt((red*red+green*green+blue*blue)/3.0);
                	*(data+i+j+3)=intensity;
                	*(data+i+j+2)=intensity;
                	*(data+i+j+1)=intensity;

                	if(bmask!=255)
                	{
                        	*(data+i+j+3)=bmask&(*(data+i+j+3));
                        	*(data+i+j+2)=bmask&(*(data+i+j+2));
                        	*(data+i+j+1)=bmask&(*(data+i+j+1));
                	}
        	}
	}
}

// for the square defined by two lines and two columns
// this area is averaged into pixel sub blocks the size of
// b_width*b_height which are global variables.
// The averaged value is written only into the top left blue byte
// pixel, ie byte offset of 1, for 4 bytes/pixel.
void average_lines_square(int lineu, int linel,int coll,int colr)
{
        int i,m,n,o;
        int ave_1=0;

        // for each line spaced b_height apart
        for(i=(lineu*4*xsize);i<((linel)*4*xsize);i=i+b_height*4*xsize)
        {
               	// for columns spaced b_width apart 
               	for(m=4*coll;m<4*colr;m=m+4*b_width)
               	{
			// for the next b_height lines
			for(o=0;o<b_height*4*xsize;o=o+4*xsize)
			{
				// for the next b_width columns
				for(n=0;n<4*b_width;n=n+4)
				{
                       			// average each 4 pixel sub-block in a line, for 4 consecutave lines
                       			ave_1=ave_1+(*(data+i+m+n+o+1));
				}
			}

			// colour in top left pixel blue field only
			(*(data+i+m+1))=(ave_1)/(b_width*b_height);
			ave_1=0;
               	}
        }
}

// this function edges detects a square area of the screen.
// this is achieved by using the pixel sub block size defined
// by b_height*b_width, and expects the averaged value to be in
// the top left position of this block.
// By calculating blocks 1+2-(3+4), if this value is > than the 
// gradth value an edge is detected.
void edge_detect_square_x(int lineu,int linel,int coll,int colr){
        int i,j,k,n,grad;

        // colour in according to gradient
        for(i=lineu*4*xsize;i<(linel*4*xsize);i=i+b_height*4*xsize)
        {
		for(n=4*coll;n<4*colr-12*b_width;n=n+4*b_width)
		{
           		//grad=(*(data+i+n+1))+(*(data+i+n+1+4*b_width))+(*(data+i+n+1+8*b_width))-(*(data+i+n+1+12*b_width))-(*(data+i+n+1+16*b_width))-(*(data+i+n+1+20*b_width));
           		grad=(*(data+i+n+1))+(*(data+i+n+1+4*b_width))-(*(data+i+n+1+8*b_width))-(*(data+i+n+1+12*b_width));
           		//grad=(*(data+i+n+1))-(*(data+i+n+1+4*b_width));
                	grad=abs(grad);
                	if(grad>255||grad>gradth)
                	{
                        	grad=255;
                	}
                	else
                	{
                        	grad=0;
                	}
			// for the next b_height lines
                        for(j=0;j<b_height*4*xsize;j=j+4*xsize)
                        {
                                // for the next b_width columns
                                for(k=0;k<4*b_width;k=k+4)
                                {
                                        (*(data+i+j+n+k+1))=grad;
                                        (*(data+i+j+n+k+2))=grad;
                                        (*(data+i+j+n+k+3))=grad;
                                }
                        }
		}
        }
}

// This function copies a small area of the current frame into a 2D array.
// If fill_flag==1 then a search window defined by lineu,linel,coll,colr
// is converted to grey scale and averaged, then copied into mot_vect1.
// If fill_flag==0 this area is copied into mot_vect2
void calculate_flow_area(int lineu,int linel,int coll,int colr,int mot_vect1[10][mot_size],int mot_vect2[10][mot_size],int fill_flag)
{
	int i,n,row_ctr=0,column_ctr=0;
        // convert to grey scale
        change_format_square(lineu,linel,coll,colr);

        // average into 4*4 blocks
        average_lines_square(lineu,linel,coll,colr);

	for(i=lineu*4*xsize;i<(linel*4*xsize);i=i+b_height*4*xsize)
	{
		for(n=4*coll;n<4*colr;n=n+4*b_width)
		{
			if(fill_flag==1)
			{
				mot_vect1[row_ctr][column_ctr]=(*(data+i+n+1));
				//printf("\t%d",(*(data+i+n+1)));
			}
			else
			{
				mot_vect2[row_ctr][column_ctr]=(*(data+i+n+1));
				//printf("\t%d",(*(data+i+n+1)));
			}
			column_ctr++;
		}	
		column_ctr=0;
		row_ctr++;
	}
}

// The position printed on the screen is the minimum MSE match of a small section
// of the middle of the search window of the first frame (mot_vect2)
// compared to the entire search window of the second frame (mot_vect1).
// http://www.csse.monash.edu.au/hons/projects/1999/Tim.Bruton/
void calculate_flow_vector(int top,int bottom,int mot_vect1[10][mot_size],int mot_vect2[10][mot_size],int tslide_size,int sq_left,int sq_right,int num_col,int sl_start,int centre,int cd_spread)
{
	int i,n,l,d,num_ops=0;
	int match_mot_error=0;
	long min_mot_error=1000000000;
	int xmot_shift=9990;
	int ymot_shift=9990;
	// l is the row counter
	// i is the sliding window counter
	// n is the inside matching window counter

       	// line number
       	for(l=0;l<((bottom-top)/b_height);l++)
       	{
               	// column number
               	for(i=0;i<num_col-tslide_size;i=i+1)
               	{
			match_mot_error=0;
                       	// slide number
                      	for(n=0;n<tslide_size;n=n+1)
                       	{
                       		num_ops++;
                               	match_mot_error=match_mot_error+(mot_vect2[((bottom-top)/(2*b_height))][n+sl_start]-mot_vect1[l][i+n])*(mot_vect2[((bottom-top)/(2*b_height))][n+sl_start]-mot_vect1[l][i+n]);
                       	}
			// find minimum MSE and set position
                       	if(min_mot_error>match_mot_error)
                       	{
                               	min_mot_error=match_mot_error;
                               	xmot_shift=i-(sl_start);
                               	ymot_shift=((bottom-top)/(2*b_height))-l;	
                       	}
               	}
       	}
	printf("\n# Comparisons:%d MSE:%ld x_shift:%d y_shift:%d",num_ops,min_mot_error,xmot_shift,ymot_shift);
}

// This function expects two lines to be placed on the floor spaced
// roughly 25cm apart and in the middle of the screen.
// This function returns a cal structure which contains
// the gradient value for contrast difference, and the 
// line width. 
struct cal calibrate(int line_position1[MAX_SIZE][3], int line1)
{
	int i,grad,gradmax=-1000,gradmin=1000,r=0,c=0,state1,state2,num_marks=0,ave=0;
	struct cal cal_datal;

	// set gradient values
        for(i=line1*4*xsize+4*b_width;i<(line1*4*xsize+4*xsize-4*b_width);i=i+4*b_width)
        {
		grad=(*(data+i+1))+(*(data+i+1+4*b_width))-((*(data+i+1+8*b_width))+(*(data+i+1+12*b_width)));
		if(grad>gradmax)
		{
			gradmax=grad;
		}
		if(grad<gradmin)
		{
			gradmin=grad;
		}
	}
	cal_datal.gradient=(gradmax-gradmin)/2;
	
	// step along line until find W to B peak
        i=line1*4*xsize+2*b_width;
	grad=(*(data+i+1))+(*(data+i+1+4*b_width))-((*(data+i+1+8*b_width))+(*(data+i+1+12*b_width)));

        while(grad<0.5*cal_datal.gradient)
        {
                i=i+16;
		grad=(*(data+i+1))+(*(data+i+1+4*b_width))-((*(data+i+1+8*b_width))+(*(data+i+1+12*b_width)));
        }
        line_position1[r][c]=(i-(line1*4*xsize+16))/16;
        c=1;
        state1=0;
        state2=1;

        // for remaining part of line
        for(i;i<(line1*4*xsize+4*xsize-4*b_width);i=i+4*b_width)
        {
                // positive = W to B, negative = B to W
		grad=(*(data+i+1))+(*(data+i+1+4*b_width))-((*(data+i+1+8*b_width))+(*(data+i+1+12*b_width)));
                // if W to B
                if(state1)
                {
                        if(grad>(0.5*cal_datal.gradient))
                        {
                                line_position1[r][c]=(i-(line1*4*xsize+16))/16;
                                c=1;
                                state1=0;
                                state2=1;
                        }
                }
                // if B to W
                if(state2)
                {
                        if(grad<-(0.5*cal_datal.gradient))
                        {
                                line_position1[r][c]=(i-(line1*4*xsize+16))/16;
                                r++;
                                c=0;
                                state2=0;
                                state1=1;
                        }
                }
        }

	// Identify possible lines and mark them
        i=0;
        while((line_position1[i][0]!=NEG_NUMBER)&&(line_position1[i][1]!=NEG_NUMBER)&&(i<MAX_SIZE))
        {
                line_position1[i][2]=(line_position1[i][1]-line_position1[i][0]);
		ave=ave+line_position1[i][2];
		num_marks++;
                i++;
        }
	ave=ave/num_marks;
	
	// Clear array
        i=0;
        while(i<MAX_SIZE)
        {
                line_position1[i][0]=NEG_NUMBER;
                line_position1[i][1]=NEG_NUMBER;
                line_position1[i][2]=NEG_NUMBER;
                i++;
        }

	cal_datal.width=ave;
	return cal_datal;
}

// This function fills the array called line_position1.
// The line_position1[x][1] is a white to black identification,
// The line_position1[x][2] is a black to white identification.
void find_black(int line_position1[MAX_SIZE][3],int line1,int search_l,int search_r)
{
	int i,r=0,c=0;
	int grad,state1=1,state2=1;

        i=0;
	// step along line until find W to B peak
	i=line1*4*xsize+4*search_l;
	grad=(*(data+i+1))+(*(data+i+1+4*b_width))-((*(data+i+1+8*b_width))+(*(data+i+1+12*b_width)));
        while(grad<gradth)
        {
		i=i+4*b_width;
		grad=(*(data+i+1))+(*(data+i+1+4*b_width))-((*(data+i+1+8*b_width))+(*(data+i+1+12*b_width)));
        }
	line_position1[r][c]=(i-line1*4*xsize)/(4*b_width);
        c=1;
        state1=0;
        state2=1;

	// for remaining part of line
	for(i;i<((line1*4*xsize)-(12*b_width)+(4*search_r));i=i+4*b_width)
	{
		// positive = W to B, negative = B to W
		grad=(*(data+i+1))+(*(data+i+1+4*b_width))-((*(data+i+1+8*b_width))+(*(data+i+1+12*b_width)));
                // if W to B
		if(state1)
		{
                        if(grad>(gradth))
                        {
                                line_position1[r][c]=(i-line1*4*xsize)/(4*b_width);
                                c=1;
                                state1=0;
                                state2=1;
                        }
		}
                // if B to W
		if(state2)
		{
                        if(grad<-(gradth))
                        {
                                line_position1[r][c]=(i-line1*4*xsize)/(4*b_width);
                                r++;
                                c=0;
                                state2=0;
                                state1=1;
                        }
		}
	}
}
	
// this function returns the positive or negative error direction 
// by analysing line_position1[][] which is filled by void find_black.
// Possible track lines are the lines which have white to black
// followed by black to white changes occuring at a width of the expected width
// found by performing a calibration.
// This function also takes the running average of track width.
// http://www.csse.monash.edu.au/hons/projects/1999/Tim.Bruton/
int find_track(int line_position1[MAX_SIZE][3],int line1,int midprev){

        int i=0,j,k,midpoint1=NEG_NUMBER,num_marks1=0;
	int l_track1=0,r_track1=0,found=0;
	int direction=0;
	int width1=9999;

	// Identify possible lines and mark them
	i=0;
	while((line_position1[i][0]!=NEG_NUMBER)&&(line_position1[i][1]!=NEG_NUMBER)&&(i<MAX_SIZE))
	{
		if(((line_position1[i][1]-line_position1[i][0])<maxlinewidth)&&((line_position1[i][1]-line_position1[i][0])>minlinewidth))
		{
			line_position1[i][2]=(line_position1[i][0]+line_position1[i][1])/2;			
			num_marks1++;
		}
		i++;
	}

	// print line array
	if(printinfo!=0)
	{
		printf("\n\n");
		printf("\nLine %d",line1);
        	i=0;
        	while((i<MAX_SIZE)&&(line_position1[i][0]!=NEG_NUMBER)&&(line_position1[i][1]!=NEG_NUMBER))
        	{
			if(line_position1[i][2]!=NEG_NUMBER)
			{
                        	printf("\nPossible Line positions:%d",line_position1[i][2]);
			}
                	i++;
                }
	}

	// if no lines, force lost state
	i=0;	
	if(num_marks1==0)
	{
		width1=0;
	}
	else
	{
		// if one line is found and if first time, then predict the mid point
		// of the track, else if it is the second time in a row then set
		// BACK_LEFT or BACK_RIGHT to cause the car to back up with the wheels turned
		if(num_marks1==1)
        	{
			if(off_track<1)
			{
				// step over shadows to left of track
		                while(line_position1[i][2]==NEG_NUMBER)
        		        {
                		        i++;
                		}
				if(line_position1[i][2]>midprev)
				{
					r_track1=line_position1[i][2];
					midpoint1=r_track1-(int)(0.5*widthT);
					l_track1=r_track1-(int)(1.0*widthT);
					if(v_flag)
						draw_square(b_width*midpoint1,line1,0);
					width1=widthT;
				}
				else
				{
					l_track1=line_position1[i][2];
					midpoint1=l_track1+(int)(0.5*widthT);
					r_track1=l_track1+(int)(1.0*widthT);
					if(v_flag)
						draw_square(b_width*midpoint1,line1,0);
					width1=widthT;
				}	
			}
			else
			{
				 // step over shadows to left of track
	                        while(line_position1[i][2]==NEG_NUMBER)
        	                {
                	                i++;
                        	}
	                        if(midprev>CENTRE)
        	                {
					BACK_LEFT=1;
					r_track1=line_position1[i][2];
                                        midpoint1=r_track1-(int)(0.5*widthT);
                                        l_track1=r_track1-(int)(1.0*widthT);
                                        if(v_flag)
                                                draw_square(b_width*midpoint1,line1,0);
                                        width1=widthT;
	                        }
        	                else
                	        {
					BACK_RIGHT=1;
					l_track1=line_position1[i][2];
                                        midpoint1=l_track1+(int)(0.5*widthT);
                                        r_track1=l_track1+(int)(1.0*widthT);
                                        if(v_flag)
                                                draw_square(b_width*midpoint1,line1,0);
                                        width1=widthT;
	                        }
			}
                	off_track++;
			num_one_line++;
	        }
		// if 2 track borders are found then use the width of the track
		// to determine if this is the correct track
        	else if(num_marks1==2)
	        {	
			off_track=0;
			i=0;
                	// step over shadows to left of track
        	        while(line_position1[i][2]==NEG_NUMBER)
                	{
                        	i++;
	                }
			l_track1=line_position1[i][2];
			// step over shadows to left of track
			i++;
        	        while(line_position1[i][2]==NEG_NUMBER)
                	{
                		i++;
	                }
			r_track1=line_position1[i][2];
			width1=r_track1-l_track1;
			widthT=(widthT+width1)/2;
        	        midpoint1=4+(l_track1+r_track1)/2;
                	if(printinfo!=0)
                	{
                        	printf("\n=2midpoint:%d",midpoint1);
	                }
        	        if(v_flag)
				draw_square(b_width*midpoint1,line1,1);
        	}
		// else there are more than 2 possible track borders, so starting at the 
		// previous position of the track search outward and select the first two
		// possible lines found and call this the track. Then check if the width
		// is consistent with the running average of track width, and either
		// accept and continue or force a lost condition on the car.
		else
        	{
			off_track=0;
        		i=0;
			found=0;
			while((i<MAX_SIZE)&&(!found))
			{
				if((line_position1[i][2]!=NEG_NUMBER)&&(line_position1[i][2]<=midprev))
				{
					l_track1=line_position1[i][2];
				}
				if((line_position1[i][2]!=NEG_NUMBER)&&(line_position1[i][2]>=midprev))
				{
					r_track1=line_position1[i][2];
					found=1;
					width1=r_track1-l_track1;
					widthT=(widthT+width1)/2;
					midpoint1=4+(l_track1+r_track1)/2;
                        		if(printinfo!=0)
                        		{
                                		printf("\n>2midpoint:%d",midpoint1);
                        		}
				}
				i++;
			}
			if(v_flag)
				draw_square(b_width*midpoint1,line1,2);
		}
	}
	direction=midpoint1-CENTRE;

	if(!BACK_LEFT&&!BACK_RIGHT)
	{
		// check for valid track width
		if((width1<0.5*widthT)||(width1>1.6*widthT))
		{
			direction=LOST;	
		}
	}
	i=0;
        while(i<MAX_SIZE)
        {
                line_position1[i][0]=NEG_NUMBER;
	        line_position1[i][1]=NEG_NUMBER;
               	line_position1[i][2]=NEG_NUMBER;
                i++;
	}
	return(direction);
}

// function prints the value of the pixel under the purple dot on the screen,
// the dot's position is changed by hitting F1 or F2 or F3 of F4, and a global
// minimum/maximum range of the pixels searched so far is updated by pressing F5.
// Pressing F6 then colours all the pixels in this range pink. F7 resets this colouring
// and the global min/max values
// http://www.csse.monash.edu.au/hons/projects/1999/Tim.Bruton/
void pixel_view()
{

	int i, ctr; 
	int blueu, bluel, redu, redl, greenu, greenl;

	// test for position updates in diagnostic area
	if(getbutton(F1KEY))
	{
        	x=x-4;
		rlocalmin=255;
		rlocalmax=0;
		glocalmin=255;
		glocalmax=0;
		blocalmin=255;
		blocalmax=0;
	}
        if(getbutton(F2KEY))
	{
        	x=x+4;
		rlocalmin=255;
		rlocalmax=0;
		glocalmin=255;
		glocalmax=0;
		blocalmin=255;
		blocalmax=0;
	}
        if(getbutton(F3KEY))
	{
        	y=y-1;
		rlocalmin=255;
		rlocalmax=0;
		glocalmin=255;
		glocalmax=0;
		blocalmin=255;
		blocalmax=0;
	}
        if(getbutton(F4KEY))
	{
        	y=y+1;
		rlocalmin=255;
		rlocalmax=0;
		glocalmin=255;
		glocalmax=0;
		blocalmin=255;
		blocalmax=0;
	}

	// Determine colour ranges for local RGB
	if(rval<rlocalmin)
        {
                rlocalmin=rval;
        }
        if(rlocalmax<rval)
        {
                rlocalmax=rval;
        }
	// Green local updates
	if(gval<glocalmin)
        {
                glocalmin=gval;
        }
        if(glocalmax<gval)
        {
                glocalmax=gval;
        }
	// Blue local updates
	if(bval<blocalmin)
        {
                blocalmin=bval;
        }
        if(blocalmax<bval)
        {
                blocalmax=bval;
        }
	// RGB global updates
        if(getbutton(F5KEY))
        {
                if(rglobalmax<rlocalmax)
                {
                        rglobalmax=rlocalmax;
                }
		if(rglobalmin>rlocalmin)
                {
                        rglobalmin=rlocalmin;
                }
		if(gglobalmin>glocalmin)
                {
                        gglobalmin=glocalmin;
                }
                if(gglobalmax<glocalmax)
                {
                        gglobalmax=glocalmax;
                }
		if(bglobalmin>blocalmin)
                {
                        bglobalmin=blocalmin;
                }
                if(bglobalmax<blocalmax)
                {
                        bglobalmax=blocalmax;
                }
        }

	// draw diagnostic line
	if(printflag)
	{
		printf("\nRGB: %d %d %d\tRmin: %d Rmax: %d Gmin: %d Gmax: %d Bmin: %d Bmax: %d",rval,gval,bval,rglobalmin,rglobalmax,gglobalmin,gglobalmax,bglobalmin,bglobalmax);
		if(v_flag)
			draw_square(x/4,y,3);
	}
	
	// Find regions of colour within bounds
	// and colour purple
	if(getbutton(F6KEY)){
		flag1=1;
	}
	if(getbutton(F7KEY))
	{
		rlocalmin=255,rglobalmin=255,rlocalmax=0,rglobalmax=0;
		glocalmin=255,gglobalmin=255,glocalmax=0,gglobalmax=0;
		blocalmin=255,bglobalmin=255,blocalmax=0,bglobalmax=0;
		flag1=0;
	}
	
	if(flag1==1)
	{
	 	redl=rglobalmin;
                redu=rglobalmax;
                greenl=gglobalmin;
                greenu=gglobalmax;
                bluel=bglobalmin;
                blueu=bglobalmax;
		for(i=0;i<4*((xsize)*(ysize));i=i+4)
        	{
                	//if((*(data+i+1)>blue)&&(*(data+i+3)<red)&&(*(data+i+2)<green))
                	if((*(data+i+3)>redl)&&(*(data+i+3)<redu)&&(*(data+i+2)>greenl)&&(*(data+i+2)<greenu)&&(*(data+i+1)>bluel)&&(*(data+i+1)<blueu))
                	{
                        	*(data+i+3)=255;
                        	*(data+i+2)=0;
                        	*(data+i+1)=255;
                	}
        	}

	}
	
}

// send the char move to the serial port to 
// to cause the interface to actuate the car.
void exe_move(char move_car[])
{
        fprintf(serial,"%c",move_car[0]);
        fflush(serial);
}

// capture the current frame to capt.ppm.
// if n=1 100% size, n=2 50% size etc.
void capture_frame(int n)
{
	int r,g,b,row,col;
	float nn=100/n;

	printf("\nCapturing frame to capt.ppm at %.0f percent resolution!",nn);
	out=i_mktemp(ysize/n,xsize/n,24);
	i_zero(out);

	for(row=0;row<out->rows*n;row=row+n)
	{
		for(col=0;col<out->cols*n;col=col+n)
		{
			r=(*(data+4*col+3+(4*xsize*row)));
			g=(*(data+4*col+2+(4*xsize*row)));
			b=(*(data+4*col+1+(4*xsize*row)));
			i_putrgb(out,row/n,col/n,r,g,b);
		}
	}
	printf("\n");
	i_dump(out,"capt","ppm");
	i_close(out);
}
// http://www.csse.monash.edu.au/hons/projects/1999/Tim.Bruton/

// continually count the clock until a delay amount of time has passed
int delay(int delay)
{
        long delayl,begin_time,end_time,start_time,wait_time,startu,starts;
        float del=1.6*delay;
        struct timeval tp;
        struct timezone tzp;

	delayl=(long)delay;
        gettimeofday(&tp,&tzp);
        startu=tp.tv_usec;
	starts=tp.tv_sec;

	start_time=starts*1000000+startu;
	begin_time=start_time;
        wait_time=start_time+delay;
        while((wait_time-start_time)>0)
        {
                gettimeofday(&tp,&tzp);
		start_time=(tp.tv_sec)*1000000+tp.tv_usec;
        }
        // if difference between actual time and desired time
        // is to far different then stop the car
	gettimeofday(&tp,&tzp);
	end_time=1000000*(tp.tv_sec)+tp.tv_usec;
        if(fabs(end_time-begin_time)>del)
        {
                printf("\nTIME DELAY ERROR!\n");
        }
}

// these signal handerls are used for signalling in the multithreaded mode.
// sighdlr causes the parent process to pass track data to the child, only
// when the child has finished and calls this function.
void sighdlr(int signo)
{
        s_flag=1;
}
// sighdlr2 causes the process to terminate when required, and print out the 
// track information.
void sighdlr2(int signo)
{
	quit_flag=1;
}

void main(int argc, char *argv[])
{
	int i,direct,directp=0,flag=1;
	int vec1=0,vec2=0,vec3=0,ctr=10;
	int neg_flag=0;
	int off_track_flag=0,fill_flag,num_off_track=0,num_lost_track=0,no_enter_flag=1;
	int search_l=0,search_r=xsize;
	int t_error[1200];
	int ave_error=0;
	int pause_time=115000;
	float minwidth,maxwidth,mingrad;
	long turn;
	int s_counter=0;
	int tslide_size=20;
	int cd_spread=2;
	FILE *f_pointer;
	FILE *f2_pointer;
	char line[50];
	int num_frames=0, num_frames_child=0;
	int line_position1[MAX_SIZE][3];
	double frames_per_sec;
	struct cal cal_data1; 
	struct t ttemp;
	struct t t1;
	struct t t2;
	struct t t3;
	struct t t4;
	struct t t5;
	struct t t6;
	struct t t7;
	struct t t8;
	struct t t9;
	
	long forward_time,turn_time2,forward_time2,turn_time3,brake_time;
        struct sim TURN_region2;
        struct sim TURN_region3;
        struct sim FORWARD_region2;
        struct sim BRAKE_region3;
        struct sim V_CAR_BRAKE;

	char message1[90];
	char message2[90];
	char message3[90];
	char message4[90];
	char message5[90];
	char message6[90];
	char message7[90];
	char message8[90];
	char message9[90];

	// motion vector variables
	int sq_top;
        int sq_bottom;
        int sq_left;
        int sq_right;
        int num_col;
        int sl_start;
        int centre;

	int mot_vect1[10][mot_size];
	int mot_vect2[10][mot_size];

	// multiple processes
	char p_c_buffer[5];
	int nread=1,p_c_fildes[2];

	p_c_buffer[0]=0;
	p_c_buffer[1]=0;
	p_c_buffer[2]=0;
	p_c_buffer[3]=0;
	p_c_buffer[4]=0;

	pipe(p_c_fildes);

	// to display video on the screen or not
	if(argc==2)
        {
		if(atoi(argv[1])==1||atoi(argv[1])==0)
                	v_flag=atoi(argv[1]);
        }
	// to set open or closed loop control
	if(argc==3)
        {
		 if(atoi(argv[1])==1||atoi(argv[1])==0)
                        v_flag=atoi(argv[1]);

		if(atoi(argv[2])==1||atoi(argv[2])==0)
                	cl_flag=atoi(argv[2]);
	}

	for(i=0;i<1200;i++)
		t_error[i]=BIG_NUMBER;

	i=0;
        while((line_position1[i][0]!=NEG_NUMBER)&&(i<MAX_SIZE))
        {
                line_position1[i][0]=NEG_NUMBER;
                line_position1[i][1]=NEG_NUMBER;
                line_position1[i][2]=NEG_NUMBER;
                i++;
        }

	serial = fopen("/dev/ttyd1","w");

	printf("\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n");

	while(!getbutton(F10KEY))
	{
		if(flag==1)
		{
			printf("\n\n__________________________________________________________________________________");
			printf("\nHit F1 to calibrate the system");
        		printf("\nHit F2 for tele-operation mode");
        		printf("\nHit F3 to view pixel values");
        		printf("\nHit F4 for processing time information");
			printf("\nHit F5 for camera position change");
        		printf("\nHit F6 for 24 position steering, simple execution");
        		printf("\nHit F7 for 200 position steering, simple execution");
        		printf("\nHit F8 for 200 position steering, multithreaded execution");
        		printf("\nHit F9 for 200 position steering, simple execution with track recognition done while driving");
        		printf("\nHit F10 to quit");
			printf("\n>> ");
			flag=0;
		}
		// calibration option which sets gradth, minlinewidth, maxlinewidth
		if(getbutton(F1KEY))
		{
			open_video();
			search_l=0;
			search_r=xsize;
			do{
                        	wait_for_frame();
                        	change_format_square(line1,line1+b_height,search_l,search_r);
                        	average_lines_square(line1,line1+b_height,search_l,search_r);
                        	cal_data1=calibrate(line_position1,line1);
				minlinewidth=0.20*cal_data1.width;
				maxlinewidth=2.7*cal_data1.width;
				gradth=0.30*cal_data1.gradient;
				ctr--;
				vlPutFree(svr,buffer);
			}while(((minlinewidth<=0)||(maxlinewidth>40))&&(ctr>0));
			if(((minlinewidth<=0)||(maxlinewidth>40))&&(ctr<1))
			{
				printf("\n\nCalibration failed, please move car and try again!");
				
			}
			else
			{
				printf("\n\nMininmum line width:%f\nMaximum line width:%f\nGradient threshold:%f",minlinewidth,maxlinewidth,gradth);
			}
                        close_video();
			ctr=10;
			flag=1;
		}
		// tele-operatrion mode, no processing, just display video on the screen
		if(getbutton(F2KEY))
                {
			frame_begin_time();
                        open_video();
                        open_window();
                        do{
                                wait_for_frame();
/*
				for(i=0;i<ysize;i=i+b_height)
				{
					change_format_square(i,i+b_height,0,xsize);
                                	average_lines_square(i,i+b_height,0,xsize);
					edge_detect_square_x(i,i+b_height,0,xsize);
				}
*/
                                if(getbutton(F8KEY))
                                {
                                        capture_frame(2);
                                }
                                write_frame();
			num_frames++;
                        }while (!getbutton(ESCKEY));
			frame_end_time();
                        close_video();
                        close_window();
                        frames_per_sec=(num_frames)/(frsec+frusec/1000000);
                        printf("\n\n\tTotal time: %.0lf seconds + %.0lf microseconds.",frsec,frusec);
                        printf("\n\t%lf frames per second.",frames_per_sec);
			frames_per_sec=0;
                      	num_frames=0; 
                        flag=1;
                }
		//view pixel RGB values, and colour them in according to the range selected
		if(getbutton(F3KEY))
                {
                        open_video();
                        open_window();
			printflag=1;
                        do{
                                wait_for_frame();
				pixel_view();
                                if(getbutton(F8KEY))
                                {
                                        capture_frame(2);
                                }
                                write_frame();
                        }while (!getbutton(ESCKEY));
                        close_video();
                        close_window();
			printflag=0;
                        flag=1;
                }
		// print processing times to the screen for grabbing a frame
		// converting to grey scale
		// averaging into blocks
		// edge detecting
		// and writing the frame to the screen
		if(getbutton(F4KEY))
                {
                        open_video();
                        open_window();
			search_l=0;
			search_r=xsize;
			strcpy(message1,"aquiring the frame");
                        strcpy(message2,"converting to grey-scale");
                        strcpy(message3,"averaging into blocks");
                        strcpy(message4,"edge_detect");
                        strcpy(message5,"identifing the track");
                        strcpy(message6,"writing the frame to the screen");
			frame_begin_time();
                        do{
				begin_time();
                                	wait_for_frame();
                                	vlPutFree(svr,buffer);
                                	wait_for_frame();
                                ttemp=end_time();
                                t1.sec=(ttemp.sec+t1.sec)/2;
                                t1.usec=(ttemp.usec+t1.usec)/2;
                                begin_time();
                                	change_format_square(line1,line1+b_height,search_l,search_r);
                                ttemp=end_time();
                                t2.sec=(ttemp.sec+t2.sec)/2;
                                t2.usec=(ttemp.usec+t2.usec)/2;
                                begin_time();
                                	average_lines_square(line1,line1+b_height,search_l,search_r);
                                ttemp=end_time();
                                t3.sec=(ttemp.sec+t3.sec)/2;
                                t3.usec=(ttemp.usec+t3.usec)/2;
                                begin_time();
                                	find_black(line_position1,line1,search_l,search_r);
                                ttemp=end_time();
                                t4.sec=(ttemp.sec+t4.sec)/2;
                                t4.usec=(ttemp.usec+t4.usec)/2;
                                begin_time();
                                	direct=find_track(line_position1,line1,directp+CENTRE);
                                ttemp=end_time();
                                t5.sec=(ttemp.sec+t5.sec)/2;
                                t5.usec=(ttemp.usec+t5.usec)/2;
				directp=direct;
				if(getbutton(F8KEY))
                                {
                                        capture_frame(2);
                                }
				begin_time();
                                	write_frame();
                                ttemp=end_time();
                                t6.sec=(ttemp.sec+t6.sec)/2;
                                t6.usec=(ttemp.usec+t6.usec)/2;
				num_frames++;
				begin_time();
				ttemp=end_time();
                                t7.sec=(ttemp.sec+t7.sec)/2;
                                t7.usec=(ttemp.usec+t7.usec)/2;
                                if(getbutton(F8KEY))
                                {
                                        capture_frame(2);
                                }
                        }while (!getbutton(ESCKEY));
			frame_end_time();
			t7.sec=1000000*t7.sec+t7.usec;
			frames_per_sec=(num_frames)/(frsec+frusec/1000000);
                        printf("\n\n\tTotal time: %.0lf seconds + %.0lf microseconds.",frsec,frusec);
                        printf("\n\t%lf frames per second.",frames_per_sec);
                        printf("\n\n\tTime to execute %s is: %ldusec.",message1,(1000000*t1.sec+t1.usec)-t7.sec);
                        printf("\n\tTime to execute %s is: %ldusec.",message2,(1000000*t2.sec+t2.usec)-t7.sec);
                        printf("\n\tTime to execute %s is: %ldusec.",message3,(1000000*t3.sec+t3.usec)-t7.sec);
                        printf("\n\tTime to execute %s is: %ldusec.",message4,(1000000*t4.sec+t4.usec)-t7.sec);
                        printf("\n\tTime to execute %s is: %ldusec.",message5,(1000000*t5.sec+t5.usec)-t7.sec);
                        printf("\n\tTime to execute %s is: %ldusec.",message6,(1000000*t6.sec+t6.usec)-t7.sec);
                        t1.sec=0;
                        t1.usec=0;
                        t2.sec=0;
                        t2.usec=0;
                        t3.sec=0;
                        t3.usec=0;
                        t4.sec=0;
                        t4.usec=0;
                        t5.sec=0;
                        t5.usec=0;
                        t6.sec=0;
                        t6.usec=0;
                        t7.sec=0;
                        t7.usec=0;
                        num_frames=0;
                        flag=1;
                        close_video();
                        close_window();
                        flag=1;
                }
		// flow field measurement for a camera position change 
		// in two frames
		if(getbutton(F5KEY))
                {
			strcpy(message3,"find motion vector");
			open_video();
                        open_window();
                        printinfo=0;

			b_width=4;
			b_height=4;

			sq_top=130;
        		sq_bottom=sq_top+10*b_height;
        		sq_left=4*b_width;
        		sq_right=xsize-4*b_width;
			num_col=(sq_right-sq_left)/b_width;
        		sl_start=sq_left*4;
        		centre=(num_col/2);

			f_pointer=fopen("car.data","r");
			fgets(line,50,f_pointer);
			tslide_size=atoi(strtok(line," \t"));
			printf("\nSliding window size:%d",tslide_size);
			frame_begin_time();
                        do{

				i=0;
				while(!getbutton(F11KEY))
				{
					if(i==0)
						printf("\nHit F11 to contiune!");
					i=1;
				}
				i=0;
                                wait_for_frame();
				vlPutFree(svr,buffer);
                                wait_for_frame();
				begin_time();
				// this method of using alternative function calls depending on the frame number
				// avoids shifting data around, and alternates the order of using arrays
				// if num_frame is even
				if(num_frames%2==0)
				{
					fill_flag=1;
                                	calculate_flow_area(sq_top,sq_bottom,sq_left,sq_right,mot_vect1,mot_vect2,fill_flag);
				}
				else
				{
					fill_flag=0;
                                	calculate_flow_area(sq_top,sq_bottom,sq_left,sq_right,mot_vect1,mot_vect2,fill_flag);
				}
				if((num_frames%2==0)&&(num_frames>0))
				{
					calculate_flow_vector(sq_top,sq_bottom,mot_vect1,mot_vect2,tslide_size,sq_left,sq_right,num_col,sl_start,centre,cd_spread);
				}
				else if((num_frames%2!=0)&&(num_frames>0))
				{
					calculate_flow_vector(sq_top,sq_bottom,mot_vect2,mot_vect1,tslide_size,sq_left,sq_right,num_col,sl_start,centre,cd_spread);
				}
                		ttemp=end_time();
				t3.sec=(ttemp.sec+t3.sec)/2;
				t3.usec=(ttemp.usec+t3.usec)/2;
				directp=direct;
                                if(getbutton(F8KEY))
                                {
                                        capture_frame(1);
                                }
                                write_frame();
				num_frames++;
                        }while (!getbutton(ESCKEY));
			// reset variables
 			frame_end_time();
                        frames_per_sec=(num_frames)/(frsec+frusec/1000000);
                        printf("\n\n\tTotal time: %.0lf seconds + %.0lf microseconds.",frsec,frusec);
                        printf("\n\t%lf frames per second.\n",frames_per_sec);
                        printf("\n\tTime to execute %s is: %ldusec.",message3,1000000*t3.sec+t3.usec);
			frames_per_sec=0;
			t1.sec=0;
			t1.usec=0;
			t3.sec=0;
			t3.usec=0;
			t6.sec=0;
			t6.usec=0;
                        num_frames=0;
			printinfo=0;
                        close_video();
                        close_window();
                        flag=1;
			b_width=6;
			b_height=1;
                }
		// 24 position steering controller
		if(getbutton(F6KEY))
                {
			f2_pointer=fopen("terr.data","w");
                        open_video();
			if(v_flag)
                        	open_window();
			frame_begin_time();
			off_track=0;
			directp=CENTRE;
			direct=CENTRE;
			widthT=50;
			OFFSET=80;
                        do{
				// double buffered so grab two frames
                                wait_for_frame();
				vlPutFree(svr,buffer);
                                wait_for_frame();
				// conver to grey scale
				change_format_square(line1,line1+b_height,search_l,search_r);
				// average into pixel blocks
                                average_lines_square(line1,line1+b_height,search_l,search_r);
				// find large contrast changes 
				find_black(line_position1,line1,search_l,search_r);
				// find the track
                                direct=find_track(line_position1,line1,CENTRE+directp);
				// car is lost so back up
				if(direct==LOST)
				{
                                        if(off_track_flag==2)
                                        {
                                                FINISH_LINE_FLAG=1;
						printf("\n\n\t*****************************");
                                                printf("\n\tThe car is lost, bailing out!");
						printf("\n\t*****************************");
                                        }
                                        else
                                        {
                                                off_track_flag++;
                                                move_car[0]='1';
                                                exe_move(move_car);
                                                delay(200000);
                                                exe_move(move_car);
                                                delay(200000);
						num_off_track++;
						t_error[num_frames]=t_error[num_frames-1];
                                        }
				}
				// car had lost 1 line so back up with wheels turned
				else if(BACK_LEFT||BACK_RIGHT)
				{
					if(BACK_LEFT)
					{
						BACK_LEFT=0;
						move_car[0]='9';
                                                exe_move(move_car);
                                                delay(200000); 
						directp=direct+10;
                                                delay(100000); 
						num_lost_track++;
						t_error[num_frames]=directp;
					}
					else 
					{
						BACK_RIGHT=0;
						move_car[0]='5';
                                                exe_move(move_car);
                                                delay(200000); 
                                                exe_move(move_car);
                                                delay(100000); 
						directp=direct-10;
						num_lost_track++;
						t_error[num_frames]=directp;
					}
				}
				else
				{
					off_track_flag=0;
                                        vec2=vec1;
                                        vec1=direct;
                                        t_error[num_frames]=direct;
                                        if(cl_flag)
                                        {
                                                //sign change in the error, system is overshooting
                                                // decrease gain
                                                if(((vec2>0)&&(vec1<0))||((vec2<0)&&(vec1>0)))
                                                {
							if(f_LT>(0.8*D_LT))
								f_LT=f_LT-(fgain*f_LT*abs(vec1));
							if(f_BT>(0.8*D_BT))
								f_BT=f_BT-(fgain*f_BT*abs(vec1));
							if(f_BBT>(0.8*D_BBT))
								f_BBT=f_BBT-(fgain*f_BBT*abs(vec1));
							if(f_ET>(0.8*D_ET))
								f_ET=f_ET-(fgain*f_ET*abs(vec1));
							if(f_EBT>(0.8*D_EBT))
								f_EBT=f_EBT-(fgain*f_EBT*abs(vec1));
							if(f_EB>(0.9*D_EB))
								f_EB=f_EB-(fgain*f_EB*abs(vec1));
							if(f_EBB>(0.9*D_EBB))
								f_EBB=f_EBB-(fgain*f_EBB*abs(vec1));
                                                }
                                                // else system is undershooting
                                                // increase the gain
                                                else if(((vec2>0)&&(vec1>0))||((vec2<0)&&(vec1<0)))
                                                {
                                                       if(f_LT<(1.2*D_LT))
                                                                f_LT=f_LT+(fgain*f_LT*abs(vec1));
                                                        if(f_BT<(1.2*D_BT))
                                                                f_BT=f_BT+(fgain*f_BT*abs(vec1));
                                                        if(f_BBT<(1.2*D_BBT))
                                                                f_BBT=f_BBT+(fgain*f_BBT*abs(vec1));
                                                        if(f_ET<(1.2*D_ET))
                                                                f_ET=f_ET+(fgain*f_ET*abs(vec1));
                                                        if(f_EBT<(1.2*D_EBT))
                                                                f_EBT=f_EBT+(fgain*f_EBT*abs(vec1));
                                                        if(f_EB<(1.1*D_EB))
                                                                f_EB=f_EB+(fgain*f_EB*abs(vec1));
                                                        if(f_EBB<(1.1*D_EBB))
                                                                f_EBB=f_EBB+(fgain*f_EBB*abs(vec1));
                                                }
                                        }
					// the car isn't lost so drive forward using the low level controller
                                        directp=direct;
					if(direct>0)
					{
						move_car[0]='6';
					}
					else
					{
						move_car[0]=':';
					}

					direct=abs(direct);
					// go straight 
					if(direct<FORWARD_CAR)
					{
						move1_car[0]='2';
                                		exe_move(move1_car);
						delay((int)f_F);
					}
					// else turn
					else 
					{
						// turn a little then forward a little
						if(direct<LITTLE_TURN)
						{
							move1_car[0]='2';
							exe_move(move1_car);
							delay((int)f_LF);
                                			exe_move(move_car);
							delay((int)f_LT);
						}
						// turn a bit more with forward
						else if(direct<BIG_TURN)
						{
							move1_car[0]='2';
							exe_move(move1_car);
							delay((int)f_BF);
        	                                        exe_move(move_car);
                	                                delay((int)f_BT);

						}
						// turn even more no forward
						else if(direct<BIGGEST_TURN)
						{
                                	                exe_move(move_car);
                                        	        delay((int)f_BBT);
						}
						// apply brakes then turn a lot  
                                        	else if(direct<END)
                                        	{
							move1_car[0]='1';
							exe_move(move1_car);
        	                                        delay((int)f_EB);
                	                                exe_move(move_car);
                        	                        delay((int)f_ET);
                                	        }
						// apply brakes for longer then turn a lot
						else
						{
							move1_car[0]='1';
	                                        	exe_move(move1_car);
        	                                	delay((int)f_EBB);
                	                        	exe_move(move_car);
                        	                	delay((int)f_EBT);
						}
					}
				}
				move1_car[0]=' ';
				exe_move(move1_car);
                                if(getbutton(F8KEY))
				{
					move1_car[0]='1';
                                        exe_move(move1_car);
                                        capture_frame(2);
				}
				if(v_flag)
                                        write_frame();
                                else
                                        vlPutFree(svr, buffer);
				if(num_frames%7==0)
				{
				 	move1_car[0]='1';
                                        exe_move(move1_car);
					delay(50000);
				}
				num_frames++;
                        }while((!getbutton(ESCKEY))&&(!FINISH_LINE_FLAG));
			FINISH_LINE_FLAG=0;
			frame_end_time();
			move_car[0]='1';
			exe_move(move_car);
			delay(80000);
			move_car[0]=' ';
			exe_move(move_car);
                        close_video();
			if(v_flag)
                         	close_window();
                        frames_per_sec=(num_frames)/(frsec+frusec/1000000);
                        printf("\n\n\tTotal time: %.0lf seconds + %.0lf microseconds.",frsec,frusec);
                        printf("\n\t%lf frames per second.",frames_per_sec);
			printf("\n\tThe car drove %d times only seeing one line.",num_one_line);
			printf("\n\tThe car reversed with wheels turned %d times.",num_lost_track);
			printf("\n\tThe car ran off the track and reversed %d times.",num_off_track);
			frames_per_sec=0;
			off_track=0;
			off_track_flag=0;
			num_off_track=0;
			num_lost_track=0;
                        flag=1;
			BACK_LEFT=0;
			BACK_RIGHT=0;
			search_l=0;
			search_r=xsize;
			i=0;
			while(t_error[i]!=BIG_NUMBER)
			{
				fprintf(f2_pointer,"%d\n",t_error[i]);
				ave_error=ave_error+abs(t_error[i]);
				t_error[i]=BIG_NUMBER;
				i++;
			}
			fclose(f2_pointer);
			printf("\n\tAverage position error/frame:%d\n",(int)(ave_error/num_frames));
			if(cl_flag)
                        {
                                printf("\n\tFINAL f_LT:%f",f_LT);
                                printf("\n\tFINAL f_BT:%f",f_BT);
                                printf("\n\tFINAL f_BBT:%f",f_BBT);
                                printf("\n\tFINAL f_ET:%f",f_ET);
                                printf("\n\tFINAL f_EBT:%f",f_EBT);
                                printf("\n\tFINAL f_EB:%f",f_EB);
                                printf("\n\tFINAL f_EBB:%f",f_EBB);
                        }
			num_frames=0;
			ave_error=0;
			num_one_line=0;
			f_LT=D_LT;
			f_BT=D_BT;
			f_BBT=D_BBT;
			f_ET=D_ET;
			f_EB=D_EB;
			f_EBT=D_EBT;
			f_EBB=D_EBB;
                }

		// 200 position steering control
		if(getbutton(F7KEY))
                {
			f2_pointer=fopen("terr.data","w");
                        open_video();
			search_l=0;
			search_r=xsize;
			directp=CENTRE;
			direct=CENTRE;
			widthT=CENTRE-10;
			if(v_flag)
                        	open_window();
			OFFSET=80;
                        line1=300+OFFSET;
                        FORWARD=0.8*D_FORWARD;
                        FORWARD_MIN=0.8*D_FORWARD_MIN;
                        FORWARD_MAX=0.8*D_FORWARD_MAX;
                        TURN_MIN=1.1*D_TURN_MIN;
                        TURN_MAX=1.1*D_TURN_MAX;
                        TURN_W_B_MIN=0.9*D_TURN_W_B_MIN;
                        TURN_W_B_MAX=0.9*D_TURN_W_B_MAX;
                        BRAKE_MIN=1.05*D_BRAKE_MIN;
                        BRAKE_MAX=1.05*D_BRAKE_MAX;

			frame_begin_time();
                        do{

                                wait_for_frame();
				vlPutFree(svr,buffer);
                                wait_for_frame();
                                change_format_square(line1,line1+b_height,search_l,search_r);
                                average_lines_square(line1,line1+b_height,search_l,search_r);
                                find_black(line_position1,line1,search_l,search_r);
                                direct=find_track(line_position1,line1,CENTRE+directp);
				if(direct==LOST)
                                {
					if(off_track_flag==2)
					{
						FINISH_LINE_FLAG=1;
						printf("\n\n\t*****************************");
                                                printf("\n\tThe car is lost, bailing out!");
                                                printf("\n\t*****************************");

					}
					else
					{
						off_track_flag++;	
                                		move_car[0]='1';
                                        	exe_move(move_car);
                                        	delay(250000);
                                        	exe_move(move_car);
                                        	delay(200000);
						num_off_track++;
						t_error[num_frames]=t_error[num_frames-1];
					}
                                }
                                else if(BACK_LEFT||BACK_RIGHT)
                                {
                                        if(BACK_LEFT)
                                        {
                                                BACK_LEFT=0;
                                                move_car[0]='9';
                                                exe_move(move_car);
                                                delay(200000);
                                                exe_move(move_car);
                                                delay(200000);
                                                directp=direct+10;
						num_lost_track++;
						t_error[num_frames]=directp;
                                        }
                                        else
                                        {
                                                BACK_RIGHT=0;
                                                move_car[0]='5';
                                                exe_move(move_car);
                                                delay(200000);
                                                exe_move(move_car);
                                                delay(200000);
                                                directp=direct-10;
						num_lost_track++;
						t_error[num_frames]=directp;
                                        }
                                }
                                else
                                {
					off_track_flag=0;
                                	vec2=vec1;
                                	vec1=direct;
					t_error[num_frames]=direct;
                                        if(cl_flag)
                                        {
                                                //sign change in the error, system is overshooting
                                                // decrease gain
                                                if(((vec2>0)&&(vec1<0))||((vec2<0)&&(vec1>0)))
                                                {
                                                        if((TURN_MIN)>(0.8*D_TURN_MIN))
                                                                TURN_MIN=TURN_MIN-fgain*TURN_MIN*abs(vec1);
                                                        if((TURN_MAX)>(0.8*D_TURN_MAX))
                                                                TURN_MAX=TURN_MAX-fgain*TURN_MAX*abs(vec1);
                                                        if((TURN_W_B_MIN)>(0.8*D_TURN_W_B_MIN))
                                                                TURN_W_B_MIN=TURN_W_B_MIN-fgain*TURN_W_B_MIN*abs(vec1);
                                                        if((TURN_W_B_MAX)>(0.8*D_TURN_W_B_MAX))
                                                                TURN_W_B_MAX=TURN_W_B_MAX-fgain*TURN_W_B_MAX*abs(vec1);
                                                        if((BRAKE_MIN)>(0.9*D_BRAKE_MIN))
                                                                BRAKE_MIN=BRAKE_MIN-fgain*BRAKE_MIN*abs(vec1);
                                                        if((BRAKE_MAX)>(0.9*D_BRAKE_MAX))
                                                                BRAKE_MAX=BRAKE_MAX-fgain*BRAKE_MAX*abs(vec1);
                                                }
                                                // else system is undershooting
                                                // increase the gain
                                                else if(((vec2>0)&&(vec1>0))||((vec2<0)&&(vec1<0)))
                                                {
                                                        if((TURN_MIN)<(1.2*D_TURN_MIN))
                                                                TURN_MIN=TURN_MIN+fgain*TURN_MIN*abs(vec1);
                                                        if((TURN_MAX)<(1.2*D_TURN_MAX))
                                                                TURN_MAX=TURN_MAX+fgain*TURN_MAX*abs(vec1);
                                                        if((TURN_W_B_MIN)<(1.2*D_TURN_W_B_MIN))
                                                                TURN_W_B_MIN=TURN_W_B_MIN+fgain*TURN_W_B_MIN*abs(vec1);
                                                        if((TURN_W_B_MAX)<(1.2*D_TURN_W_B_MAX))
                                                                TURN_W_B_MAX=TURN_W_B_MAX+fgain*TURN_W_B_MAX*abs(vec1);
                                                        if((BRAKE_MIN)<(1.1*D_BRAKE_MIN))
                                                                BRAKE_MIN=BRAKE_MIN+fgain*BRAKE_MIN*abs(vec1);
                                                        if((BRAKE_MAX)<(1.1*D_BRAKE_MAX))
                                                                BRAKE_MAX=BRAKE_MAX+fgain*BRAKE_MAX*abs(vec1);
                                                }
                                        }
                                	directp=direct;
                                	if(direct>0)
                                	{
                                        	move_car[0]='6';
                                	}
                                	else
                                	{
                                        	move_car[0]=':';
                                	}
                                	direct=abs(direct);
        				if(direct<TURN_POS)
        				{
						move1_car[0]='2';
                                                exe_move(move1_car);
                                                delay((int)FORWARD);
                                        }
                                        else if(direct<TURN_POS_W_B)
                                        {
	                                        TURN_region2=solve(TURN_POS,TURN_MIN,TURN_POS_W_B,TURN_MAX);
                                                turn_time2=(long)(TURN_region2.m*direct+TURN_region2.c);
                                                FORWARD_region2=solve(TURN_POS,FORWARD_MIN,TURN_POS_W_B,FORWARD_MAX);
                                                forward_time2=(long)(FORWARD_region2.m*direct+FORWARD_region2.c);
                                                move1_car[0]='2';
                                                exe_move(move1_car);
                                                delay((int)forward_time2);
                                                exe_move(move_car);
                                                delay((int)turn_time2);
                                        }
                                        else
                                        {
                                                TURN_region3=solve(TURN_POS_W_B,TURN_W_B_MIN,END,TURN_W_B_MAX);
                                                turn_time3=(long)(TURN_region3.m*direct+TURN_region3.c);
                                                BRAKE_region3=solve(TURN_POS_W_B,BRAKE_MIN,END,BRAKE_MAX);
                                                brake_time=(long)(BRAKE_region3.m*direct+BRAKE_region3.c);

                                                move1_car[0]='1';
                                                exe_move(move1_car);
                                                delay((int)brake_time);
                                                exe_move(move_car);
                                                delay((int)turn_time3);
					}

				}
				move_car[0]=' ';
                                exe_move(move_car);
                                if(getbutton(F8KEY))
                                {
                                        move_car[0]='1';
                                        exe_move(move_car);
                                        capture_frame(2);
                                }
				if(v_flag)
                                        write_frame();
                                else
                                        vlPutFree(svr, buffer);
				num_frames++;
				if(num_frames%7==0)
                                {
                                        move_car[0]='1';
                                        exe_move(move_car);
                                        delay(50000);
                                }
                        }while((!getbutton(ESCKEY))&&(!FINISH_LINE_FLAG));
			FINISH_LINE_FLAG=0;
			frame_end_time();
                        move1_car[0]='1';
                        exe_move(move1_car);
                        delay(80000);
                        move1_car[0]=' ';
                        exe_move(move1_car);
                        close_video();
                        if(v_flag)
				close_window();
			frames_per_sec=(num_frames)/(frsec+frusec/1000000);
			printf("\n\n\tTotal time: %.0lf seconds + %.0lf microseconds.",frsec,frusec);
			printf("\n\t%lf frames per second.",frames_per_sec);
			printf("\n\tThe car drove %d times only seeing one line.",num_one_line);
			printf("\n\tThe car reversed with wheels turned %d times.",num_lost_track);
			printf("\n\tThe car ran off the track and reversed %d times.",num_off_track);
			frames_per_sec=0;
			off_track=0;
			off_track_flag=0;
			num_off_track=0;
			num_lost_track=0;
			vec3=0;
                        vec2=0;
                        vec1=0;
                        flag=1;
                        printinfo=0;
			search_l=0;
                        search_r=xsize;
			i=0;
                        while(t_error[i]!=BIG_NUMBER)
                        {
                                fprintf(f2_pointer,"%d\n",t_error[i]);
				ave_error=ave_error+abs(t_error[i]);
                                t_error[i]=BIG_NUMBER;
                                i++;
                        }
                        fclose(f2_pointer);
			printf("\n\tAverage position error/frame:%d\n",(int)(ave_error/num_frames));
			if(cl_flag)
			{
				printf("\n\tFINAL TURN_MIN:%f",TURN_MIN);
				printf("\n\tFINAL TURN_MAX:%f",TURN_MAX);
				printf("\n\tFINAL TURN_W_B_MIN:%f",TURN_W_B_MIN);
				printf("\n\tFINAL TURN_W_B_MAX:%f",TURN_W_B_MAX);
				printf("\n\tFINAL BRAKE_MIN:%f",BRAKE_MIN);
				printf("\n\tFINAL BRAKE_MAX:%f",BRAKE_MAX);
			}
			num_frames=0;
                        ave_error=0;
			num_one_line=0;
			FORWARD=D_FORWARD;
			FORWARD_MIN=D_FORWARD_MIN;
			FORWARD_MAX=D_FORWARD_MAX;
			TURN_MIN=D_TURN_MIN;
			TURN_MAX=D_TURN_MAX;
			BRAKE_MIN=D_BRAKE_MIN;
			BRAKE_MAX=D_BRAKE_MAX;
			TURN_W_B_MIN=D_TURN_W_B_MIN;
			TURN_W_B_MAX=D_TURN_W_B_MAX;

		}
		// 200 position multithreaded control
                if(getbutton(F8KEY))
                {
			f2_pointer=fopen("terr.data","w");
			off_track_flag=0;
                        open_video();
			search_l=0;
			search_r=xsize;
			directp=CENTRE;
			direct=CENTRE;
			widthT=CENTRE-10;
                        if(v_flag)
				open_window();
			parent_id=getpid();
			OFFSET=80;
			line1=300+OFFSET;
			FORWARD=D_FORWARD;
			FORWARD_MIN=D_FORWARD_MIN;
			FORWARD_MAX=D_FORWARD_MAX;
			TURN_MIN=1.25*D_TURN_MIN;
			TURN_MAX=1.25*D_TURN_MAX;
			TURN_W_B_MIN=1.15*D_TURN_W_B_MIN;
			TURN_W_B_MAX=1.15*D_TURN_W_B_MAX;
			BRAKE_MIN=0.95*D_BRAKE_MIN;
			BRAKE_MAX=0.95*D_BRAKE_MAX;
			sigset(SIGUSR1,sighdlr);
			sigset(SIGUSR2,sighdlr2);
			child_id=fork();
			// parent
			if(child_id!=0)
			{
				num_frames=0;
				close(p_c_fildes[0]);
                        	frame_begin_time();
				do{
                                	wait_for_frame();
					vlPutFree(svr,buffer);
                                	wait_for_frame();
	                                change_format_square(line1,line1+b_height,search_l,search_r);
        	                        average_lines_square(line1,line1+b_height,search_l,search_r);
                                	find_black(line_position1,line1,search_l,search_r);
                                	direct=find_track(line_position1,line1,CENTRE+directp);
					if(BACK_LEFT)
                                        {
                                        	directp=direct+10;
                                        }
					else if(BACK_RIGHT)
                                        {
                                        	directp=direct-10;
                                        }
					else
						directp=direct;
					if(quit_flag)
					{
						FINISH_LINE_FLAG=1;
                                                printf("\n\n\t*****************************************");
                                                printf("\n\tThe car is lost or finished, bailing out!");
                                                printf("\n\t*****************************************");
					}
					else
					{
						if(v_flag)
                                       			write_frame();
	                                	else
        	                               		vlPutFree(svr, buffer);
						if(direct==LOST)
						{
							off_track_flag++;
                                                        move_car[0]='1';
                                                        exe_move(move_car);
                                                        delay(250000);
							direct=254;
						}
						if(direct<0)
						{
							direct=abs(direct);
							neg_flag=1;
						}
						p_c_buffer[0]=direct;
						p_c_buffer[1]=BACK_LEFT;
						p_c_buffer[2]=BACK_RIGHT;
						p_c_buffer[3]=num_one_line;
						p_c_buffer[4]=neg_flag;
						BACK_LEFT=0;
						BACK_RIGHT=0;
						neg_flag=0;
						if(s_counter==0)
						{
							write(p_c_fildes[1],p_c_buffer,5);
						}
						if(s_flag)
						{
							write(p_c_fildes[1],p_c_buffer,5);
							s_flag=0;
						}
						if(v_flag)
                                       			write_frame();
                         	       		else
                                	       		vlPutFree(svr, buffer);
						s_counter=1;
						num_frames++;
					}
				}while((!getbutton(ESCKEY))&&(!FINISH_LINE_FLAG));
				frame_end_time();
				if(!quit_flag)
				{
					p_c_buffer[0]=255;
					p_c_buffer[1]=255;
					p_c_buffer[2]=255;
					p_c_buffer[3]=255;
					write(p_c_fildes[1],p_c_buffer,1);
				}
                        	if(v_flag)
					close_window();
				frames_per_sec=(num_frames)/(frsec+frusec/1000000);
                                printf("\n\t%lf frames per second in Parent.\n\n",frames_per_sec);
				delay(500000);
				delay(500000);
				delay(500000);
				delay(500000);
				delay(500000);
				delay(500000);
                                kill(child_id,SIGKILL);
				exit(0);
			}
			// child
			else
			{
				close(p_c_fildes[1]);
				f2_pointer=fopen("terr.data","w");
                        	off_track_flag=0;
                        	frame_begin_time();
				num_frames=0;
				num_one_line=0;
				BACK_LEFT=0;
				BACK_RIGHT=0;
				do{
					nread=read(p_c_fildes[0],p_c_buffer,5);
					if(nread>0)
					{
                                		direct=p_c_buffer[0];
                                		BACK_LEFT=p_c_buffer[1];
                                		BACK_RIGHT=p_c_buffer[2];
                                		num_one_line=p_c_buffer[3];
						neg_flag=p_c_buffer[4];
					}
					// force printout of data
					if(neg_flag)
						direct=-direct;
					if(direct==255)
					{
						direct=254;
						off_track_flag=2;
						dont_send_to_parent=1;
					}
					if(direct==254)
                                	{
                                        	if(off_track_flag==2)
                                        	{
							frame_end_time();
                                			move_car[0]='1';
                                			exe_move(move_car);
                                			delay(80000);
                                			move_car[0]=' ';
                                			exe_move(move_car);
	                                		printf("\n\n\tTotal time: %.0lf seconds + %.0lf microseconds.",frsec,frusec);
                			                frames_per_sec=(num_frames)/(frsec+frusec/1000000);
                        			        printf("\n\t%lf frames per second in Child.",frames_per_sec);
		                                	printf("\n\tThe car drove %d times only seeing one line.",num_one_line);
                		                	printf("\n\tThe car reversed with wheels turned %d times.",num_lost_track);
                                			printf("\n\tThe car ran off the track and reversed %d times.",num_off_track);
                                			i=0;
			                                while(t_error[i]!=BIG_NUMBER)
                        			        {
			                                        fprintf(f2_pointer,"%d\n",t_error[i]);
                        			                ave_error=ave_error+abs(t_error[i]);
			                                        t_error[i]=BIG_NUMBER;
                        			                i++;
			                                }
                        			        fclose(f2_pointer);
			                                printf("\n\tAverage position error/frame:%d\n",(int)(ave_error/num_frames));
							if(cl_flag)
				                        {
                                				printf("\n\tFINAL TURN_MIN:%f",TURN_MIN);
				                                printf("\n\tFINAL TURN_MAX:%f",TURN_MAX);
				                                printf("\n\tFINAL TURN_W_B_MIN:%f",TURN_W_B_MIN);
				                                printf("\n\tFINAL TURN_W_B_MAX:%f",TURN_W_B_MAX);
				                                printf("\n\tFINAL BRAKE_MIN:%f",BRAKE_MIN);
				                                printf("\n\tFINAL BRAKE_MAX:%f",BRAKE_MAX);
                        				}
							off_track_flag=3;
							if(!dont_send_to_parent)
							{
								sigsend(P_PID,parent_id,SIGUSR2);
							}
							delay(500000);
							delay(500000);
							delay(500000);
							delay(500000);
        	                                }
                	                        else
                        	                {
                                	                off_track_flag++;
                                        	        move_car[0]='1';
                                                	exe_move(move_car);
	                                                delay(250000);
                                                	exe_move(move_car);
	                                                delay(70000);
                                	                num_off_track++;
                                        	        t_error[num_frames]=t_error[num_frames-1];
	                                        }
        	                        }
                	                else if(BACK_LEFT||BACK_RIGHT)
                        	        {
                                	        if(BACK_LEFT)
                                        	{
							BACK_LEFT=0;
	                                                move_car[0]='9';
        	                                        exe_move(move_car);
                	                                delay(250000);
        	                                        exe_move(move_car);
                	                                delay(70000);
	                                                num_lost_track++;
        	                                        t_error[num_frames]=directp;
                	                        }
                        	                else
                                	        {
							BACK_RIGHT=0;
	                                                move_car[0]='5';
        	                                        exe_move(move_car);
                	                                delay(250000);
        	                                        exe_move(move_car);
                	                                delay(70000);
        	                                        num_lost_track++;
                        	                        t_error[num_frames]=directp;
                                	        }
                                	}
	                                else
        	                        {
                	                        off_track_flag=0;
                                	        vec2=vec1;
                                        	vec1=direct;
        	                                t_error[num_frames]=direct;
	                                        if(cl_flag)
						{
	                                                //sign change in the error, system is overshooting
                                               		// decrease gain
	                                                if(((vec2>0)&&(vec1<0))||((vec2<0)&&(vec1>0)))
        	                                        {
                	                                        if((TURN_MIN)>(1.15*0.8*D_TURN_MIN))
                        	                                        TURN_MIN=TURN_MIN-fgain*TURN_MIN*abs(vec1);
                                	                        if((TURN_MAX)>(1.15*0.8*D_TURN_MAX))
                                        	                        TURN_MAX=TURN_MAX-fgain*TURN_MAX*abs(vec1);
                                                	        if((TURN_W_B_MIN)>(1.15*0.8*D_TURN_W_B_MIN))
                                                        	        TURN_W_B_MIN=TURN_W_B_MIN-fgain*TURN_W_B_MIN*abs(vec1);
	                                                        if((TURN_W_B_MAX)>(1.15*0.8*D_TURN_W_B_MAX))
        	                                                        TURN_W_B_MAX=TURN_W_B_MAX-fgain*TURN_W_B_MAX*abs(vec1);
                	                                        if((BRAKE_MIN)>(1.15*0.9*D_BRAKE_MIN))
                        	                                        BRAKE_MIN=BRAKE_MIN-fgain*BRAKE_MIN*abs(vec1);
                                	                        if((BRAKE_MAX)>(1.15*0.9*D_BRAKE_MAX))
                                        	                        BRAKE_MAX=BRAKE_MAX-fgain*BRAKE_MAX*abs(vec1);
                	                                }
	                                                // else system is undershooting
        	                                        // increase the gain
                	                                else if(((vec2>0)&&(vec1>0))||((vec2<0)&&(vec1<0)))
                        	                        {
                                	                        if((TURN_MIN)<(1.15*1.2*D_TURN_MIN))
                                        	                        TURN_MIN=TURN_MIN+fgain*TURN_MIN*abs(vec1);
                                                	        if((TURN_MAX)<(1.15*1.2*D_TURN_MAX))
                                                        	        TURN_MAX=TURN_MAX+fgain*TURN_MAX*abs(vec1);
	                                                        if((TURN_W_B_MIN)<(1.15*1.2*D_TURN_W_B_MIN))
        	                                                        TURN_W_B_MIN=TURN_W_B_MIN+fgain*TURN_W_B_MIN*abs(vec1);
                	                                        if((TURN_W_B_MAX)<(1.15*1.2*D_TURN_W_B_MAX))
                        	                                        TURN_W_B_MAX=TURN_W_B_MAX+fgain*TURN_W_B_MAX*abs(vec1);
                                	                        if((BRAKE_MIN)<(1.15*1.1*D_BRAKE_MIN))
                                        	                        BRAKE_MIN=BRAKE_MIN+fgain*BRAKE_MIN*abs(vec1);
                                                	        if((BRAKE_MAX)<(1.15*1.1*D_BRAKE_MAX))
                                                        	        BRAKE_MAX=BRAKE_MAX+fgain*BRAKE_MAX*abs(vec1);
                                                	}
                                        	}
                	                        if(direct>0)
                        	                {
                                	                move_car[0]='6';
                                        	}
                                        	else
                                        	{
                                                	move_car[0]=':';
	                                        }
        	                                direct=abs(direct);
                                        	if(direct<TURN_POS)
                	                        {
                        	                        move1_car[0]='2';
                                	                exe_move(move1_car);
                                                	delay((int)FORWARD);
                                        	}
	                                        else if(direct<TURN_POS_W_B)
        	                                {
							 TURN_region2=solve(TURN_POS,TURN_MIN,TURN_POS_W_B,TURN_MAX);
                                                	turn_time2=(long)(TURN_region2.m*direct+TURN_region2.c);
	                                                FORWARD_region2=solve(TURN_POS,FORWARD_MIN,TURN_POS_W_B,FORWARD_MAX);
        	                                        forward_time2=(long)(FORWARD_region2.m*direct+FORWARD_region2.c);
                                	                move1_car[0]='2';
                                        	        exe_move(move1_car);
                                                	delay((int)forward_time2);
                	                                exe_move(move_car);
                        	                        delay((int)turn_time2);
                        	                }
                                	        else
                                        	{
                                                	TURN_region3=solve(TURN_POS_W_B,TURN_W_B_MIN,END,TURN_W_B_MAX);
	                                                turn_time3=(long)(TURN_region3.m*direct+TURN_region3.c);
        	                                        BRAKE_region3=solve(TURN_POS_W_B,BRAKE_MIN,END,BRAKE_MAX);
                	                                brake_time=(long)(BRAKE_region3.m*direct+BRAKE_region3.c);
		
                	                                move1_car[0]='1';
                                	                exe_move(move1_car);
                        	                        delay((int)brake_time);
                                        	        exe_move(move_car);
                                                	delay((int)turn_time3);
	                                        }
        	                        }
					if(num_frames>1)
					{
						move1_car[0]=' ';
						exe_move(move1_car);
						delay(125000);
					}
					num_frames++;
					sigsend(P_PID,parent_id,SIGUSR1);
                                }while(nread!=0);
			}
                }
	 	// 200 position steering, with track recognition performed while driving the car 
                if(getbutton(F9KEY))
                {
                        f2_pointer=fopen("terr.data","w");
                        open_video();
                        search_l=0;
                        search_r=xsize;
                        directp=CENTRE;
                        direct=CENTRE;
                        widthT=CENTRE-10;
                        if(v_flag)
                                open_window();
			OFFSET=77;
                        line1=300+OFFSET;
                        FORWARD=0.60*D_FORWARD;
                        FORWARD_MIN=0.6*D_FORWARD_MIN;
                        FORWARD_MAX=0.6*D_FORWARD_MAX;
                        TURN_MIN=0.80*D_TURN_MIN;
                        TURN_MAX=0.80*D_TURN_MAX;
                        TURN_W_B_MIN=0.8*D_TURN_W_B_MIN;
                        TURN_W_B_MAX=0.8*D_TURN_W_B_MAX;
                        BRAKE_MIN=1.25*D_BRAKE_MIN;
                        BRAKE_MAX=1.25*D_BRAKE_MAX;
                        for(i=0;i<10;i++)
                        {
                                begin_time();
                                wait_for_frame();
                                vlPutFree(svr,buffer);
                                wait_for_frame();
                                change_format_square(line1,line1+b_height,search_l,search_r);
                                average_lines_square(line1,line1+b_height,search_l,search_r);
                                find_black(line_position1,line1,search_l,search_r);
                                direct=find_track(line_position1,line1,CENTRE+directp);
                                ttemp=end_time();
                                t1.sec=(ttemp.sec+t1.sec)/2;
                                t1.usec=(ttemp.usec+t1.usec)/2;
                        	vlPutFree(svr,buffer);
                        }
                        printf("\nTime to acquire a frame: %ld seconds + %ld useconds",t1.sec,t1.usec);
			no_enter_flag=0;
                        frame_begin_time();
                        do{
                                if(no_enter_flag)
                                {
                                        wait_for_frame();
                                        vlPutFree(svr,buffer);
                                        wait_for_frame();
                                        change_format_square(line1,line1+b_height,search_l,search_r);
                                        average_lines_square(line1,line1+b_height,search_l,search_r);
                                        find_black(line_position1,line1,search_l,search_r);
                                        direct=find_track(line_position1,line1,CENTRE+directp);
                                        no_enter_flag=0;
                                }
                                if(direct==LOST)
                                {
                                        if(off_track_flag==2)
                                        {
                                                FINISH_LINE_FLAG=1;
                                                printf("\n\n\t*****************************");
                                                printf("\n\tThe car is lost, bailing out!");
                                                printf("\n\t*****************************");

                                        }
                                        else
                                        {
                                                off_track_flag++;
                                                move_car[0]='1';
                                                exe_move(move_car);
                                                delay(250000);
                                                exe_move(move_car);
                                                delay(250000);
                                                num_off_track++;
                                                t_error[num_frames]=t_error[num_frames-1];
                                        }
                                }
                                else if(BACK_LEFT||BACK_RIGHT)
                                {
                                        if(BACK_LEFT)
                                        {
                                                BACK_LEFT=0;
                                                move_car[0]='9';
                                                exe_move(move_car);
                                                delay(260000);
                                                exe_move(move_car);
                                                delay(250000);
                                                directp=direct+10;
                                                num_lost_track++;
                                                t_error[num_frames]=directp;
                                        }
                                        else
                                        {
                                                BACK_RIGHT=0;
                                                move_car[0]='5';
                                                exe_move(move_car);
                                                delay(260000);
                                                exe_move(move_car);
                                                delay(250000);
                                                directp=direct-10;
                                                num_lost_track++;
                                                t_error[num_frames]=directp;
                                        }
                                }
                                else
                                {
                                        off_track_flag=0;
                                        vec2=vec1;
                                        vec1=direct;
                                        t_error[num_frames]=direct;
                                        if(cl_flag)
                                        {
                                                //sign change in the error, system is overshooting
                                                // decrease gain
                                                if(((vec2>0)&&(vec1<0))||((vec2<0)&&(vec1>0)))
                                                {
							if((TURN_MIN)>(0.8*D_TURN_MIN))
                                                        	TURN_MIN=TURN_MIN-fgain*TURN_MIN*abs(vec1);
							if((TURN_MAX)>(0.8*D_TURN_MAX))
                                                        	TURN_MAX=TURN_MAX-fgain*TURN_MAX*abs(vec1);
							if((TURN_W_B_MIN)>(0.8*D_TURN_W_B_MIN))
                                                        	TURN_W_B_MIN=TURN_W_B_MIN-fgain*TURN_W_B_MIN*abs(vec1);
							if((TURN_W_B_MAX)>(0.8*D_TURN_W_B_MAX))
                                                        	TURN_W_B_MAX=TURN_W_B_MAX-fgain*TURN_W_B_MAX*abs(vec1);
							if((BRAKE_MIN)>(0.9*D_BRAKE_MIN))
                                                        	BRAKE_MIN=BRAKE_MIN-fgain*BRAKE_MIN*abs(vec1);
							if((BRAKE_MAX)>(0.9*D_BRAKE_MAX))
                                                        	BRAKE_MAX=BRAKE_MAX-fgain*BRAKE_MAX*abs(vec1);
                                                }
                                                // else system is undershooting
                                                // increase the gain
                                                else if(((vec2>0)&&(vec1>0))||((vec2<0)&&(vec1<0)))
                                                {
							if((TURN_MIN)<(1.2*D_TURN_MIN))
                                                                TURN_MIN=TURN_MIN+fgain*TURN_MIN*abs(vec1);
                                                        if((TURN_MAX)<(1.2*D_TURN_MAX))
                                                                TURN_MAX=TURN_MAX+fgain*TURN_MAX*abs(vec1);
                                                        if((TURN_W_B_MIN)<(1.2*D_TURN_W_B_MIN))
                                                                TURN_W_B_MIN=TURN_W_B_MIN+fgain*TURN_W_B_MIN*abs(vec1);
                                                        if((TURN_W_B_MAX)<(1.2*D_TURN_W_B_MAX))
                                                                TURN_W_B_MAX=TURN_W_B_MAX+fgain*TURN_W_B_MAX*abs(vec1);
                                                        if((BRAKE_MIN)<(1.1*D_BRAKE_MIN))
                                                                BRAKE_MIN=BRAKE_MIN+fgain*BRAKE_MIN*abs(vec1);
                                                        if((BRAKE_MAX)<(1.1*D_BRAKE_MAX))
                                                                BRAKE_MAX=BRAKE_MAX+fgain*BRAKE_MAX*abs(vec1);
                                                }
                                        }
                                        directp=direct;
                                        if(direct>0)
                                        {
                                                move_car[0]='6';
                                        }
                                        else
                                        {
                                                move_car[0]=':';
                                        }
                                        direct=abs(direct);
                                        if(direct<TURN_POS)
                                        {
                                                move1_car[0]='2';
                                                exe_move(move1_car);
                                                //printf("\n%ld FORWARD:%f",(1000000*t1.sec+t1.usec),FORWARD);
                                                if((1000000*t1.sec+t1.usec)<(FORWARD+pause_time))
                                                {
                                                        delay((int)((FORWARD+pause_time)-(1000000*t1.sec+t1.usec)));
                                                        wait_for_frame();
                                                        vlPutFree(svr,buffer);
                                                        wait_for_frame();
                                                        change_format_square(line1,line1+b_height,search_l,search_r);
                                                        average_lines_square(line1,line1+b_height,search_l,search_r);
                                                        find_black(line_position1,line1,search_l,search_r);
                                                        direct=find_track(line_position1,line1,CENTRE+directp);
                                                        no_enter_flag=0;
                                                }
                                                else
                                                {
                                                        delay((int)FORWARD);
                                                        no_enter_flag=1;
                                                }

                                        }
                                        else if(direct<TURN_POS_W_B)
                                        {
                                                TURN_region2=solve(TURN_POS,TURN_MIN,TURN_POS_W_B,TURN_MAX);
                                                turn_time2=(long)(TURN_region2.m*direct+TURN_region2.c);
                                                FORWARD_region2=solve(TURN_POS,FORWARD_MIN,TURN_POS_W_B,FORWARD_MAX);
                                                forward_time2=(long)(FORWARD_region2.m*direct+FORWARD_region2.c);

                                                move1_car[0]='2';
                                                exe_move(move1_car);
                                                delay((int)forward_time2);
                                                no_enter_flag=1;
                                                exe_move(move_car);
                                                //printf("\n%ld turn_time2:%ld",(1000000*t1.sec+t1.usec),turn_time2);
                                                if((1000000*t1.sec+t1.usec)<(turn_time2+pause_time))
                                                {
                                                        delay((int)((turn_time2+pause_time)-(1000000*t1.sec+t1.usec)));
                                                        wait_for_frame();
                                                        vlPutFree(svr,buffer);
                                                        wait_for_frame();
                                                        change_format_square(line1,line1+b_height,search_l,search_r);
                                                        average_lines_square(line1,line1+b_height,search_l,search_r);
                                                        find_black(line_position1,line1,search_l,search_r);
                                                        direct=find_track(line_position1,line1,CENTRE+directp);
                                                        no_enter_flag=0;
                                                }
                                                else
                                                {
                                                        delay((int)turn_time2);
                                                        no_enter_flag=1;
                                                }
                                        }
                                        else
                                        {
                                                TURN_region3=solve(TURN_POS_W_B,TURN_W_B_MIN,END,TURN_W_B_MAX);
                                                turn_time3=(long)(TURN_region3.m*direct+TURN_region3.c);
                                                BRAKE_region3=solve(TURN_POS_W_B,BRAKE_MIN,END,BRAKE_MAX);
                                                brake_time=(long)(BRAKE_region3.m*direct+BRAKE_region3.c);

                                                move1_car[0]='1';
                                                exe_move(move1_car);
                                                delay((int)brake_time);
                                                exe_move(move_car);
                                                //printf("\n%ld turn_time3:%ld",(1000000*t1.sec+t1.usec),turn_time3);
                                                if((1000000*t1.sec+t1.usec)<(turn_time3+pause_time))
                                                {
                                                        delay((int)((turn_time3+pause_time)-(1000000*t1.sec+t1.usec)));
                                                        wait_for_frame();
                                                        vlPutFree(svr,buffer);
                                                        wait_for_frame();
                                                        change_format_square(line1,line1+b_height,search_l,search_r);
                                                        average_lines_square(line1,line1+b_height,search_l,search_r);
                                                        find_black(line_position1,line1,search_l,search_r);
                                                        direct=find_track(line_position1,line1,CENTRE+directp);
                                                        no_enter_flag=0;
                                                }
                                                else
                                                {
                                                        delay((int)turn_time3);
                                                        no_enter_flag=1;
                                                }
                                        }
                                }
                                if(getbutton(F8KEY))
                                {
                                        move_car[0]='1';
                                        exe_move(move_car);
                                        capture_frame(2);
                                }
                                if(v_flag)
                                        write_frame();
                                else
                                        vlPutFree(svr, buffer);

                                num_frames++;
                                if(num_frames>3)
                                {
                                        move_car[0]=' ';
                                        exe_move(move_car);
                                        delay(pause_time);
                                }
                        }while((!getbutton(ESCKEY))&&(!FINISH_LINE_FLAG));
                        FINISH_LINE_FLAG=0;
                        frame_end_time();
                        move1_car[0]='1';
                        exe_move(move1_car);
                        delay(80000);
                        move1_car[0]=' ';
                        exe_move(move1_car);
                        close_video();
                        if(v_flag)
                                close_window();
                        frames_per_sec=(num_frames)/(frsec+frusec/1000000);
                        printf("\n\n\tTotal time: %.0lf seconds + %.0lf microseconds.",frsec,frusec);
                        printf("\n\t%lf frames per second.",frames_per_sec);
                        printf("\n\tThe car drove %d times only seeing one line.",num_one_line);
                        printf("\n\tThe car reversed with wheels turned %d times.",num_lost_track);
                        printf("\n\tThe car ran off the track and reversed %d times.",num_off_track);
                        frames_per_sec=0;
                        off_track=0;
                        off_track_flag=0;
                        num_off_track=0;
                        num_lost_track=0;
                        vec3=0;
                        vec2=0;
                        vec1=0;
                        flag=1;
                        printinfo=0;
                        search_l=0;
                        search_r=xsize;
                        i=0;
                        while(t_error[i]!=BIG_NUMBER)
                        {
                                fprintf(f2_pointer,"%d\n",t_error[i]);
                                ave_error=ave_error+abs(t_error[i]);
                                t_error[i]=BIG_NUMBER;
                                i++;
                        }
                        fclose(f2_pointer);
                        printf("\n\tAverage position error/frame:%d\n",(int)(ave_error/num_frames));
                        if(cl_flag)
                        {
                                printf("\n\tFINAL TURN_MIN:%f",TURN_MIN);
                                printf("\n\tFINAL TURN_MAX:%f",TURN_MAX);
                                printf("\n\tFINAL TURN_W_B_MIN:%f",TURN_W_B_MIN);
                                printf("\n\tFINAL TURN_W_B_MAX:%f",TURN_W_B_MAX);
                                printf("\n\tFINAL BRAKE_MIN:%f",BRAKE_MIN);
                                printf("\n\tFINAL BRAKE_MAX:%f",BRAKE_MAX);
                        }
                        num_frames=0;
                        ave_error=0;
                        num_one_line=0;
                        FORWARD=D_FORWARD;
                        FORWARD_MIN=D_FORWARD_MIN;
                        FORWARD_MAX=D_FORWARD_MAX;
                        TURN_MIN=D_TURN_MIN;
                        TURN_MAX=D_TURN_MAX;
                        BRAKE_MIN=D_BRAKE_MIN;
                        BRAKE_MAX=D_BRAKE_MAX;
                        TURN_W_B_MIN=D_TURN_W_B_MIN;
                        TURN_W_B_MAX=D_TURN_W_B_MAX;
                }
	}
}
// http://www.csse.monash.edu.au/hons/projects/1999/Tim.Bruton/
// (c) Tim Bruton 1999