#include "fsm.h"#include <stdio.h>#include <stdlib.h>#include <sys/types.h>#include <sys/socket.h>#include <netinet/in.h>#include <netdb.h> #include <linux/poll.h>#include "serial.h"#define MAX_CLIENT	5#define MAX_PATH	20#define BUF_SIZE	256#define MAX_CROSS	10char *ur_path[MAX_CROSS]={	"ES#",	"S#",	"ESS#",	"SS#",	"WSS#",	"SESS#",	"SWSS#",	NULL};int cur_ur_path = 0;typedef struct _path{	char start[5];	char end[5];	char *crosses[10];	char dirs[10-1];} path;path paths[MAX_PATH]={{	"A",	"B",	{"A","2","4","3","B",NULL},	{'N','N','W','W','\0'}},{	"A",	"C",	{"A","2","4","C",NULL},	{'N','N','N','\0'}},{	"A",	"D",	{"A","2","4","3","6","D",NULL},	{'N','N','W','N','N','\0'}},{	"A",	"E",	{"A","2","4","5","7","E",NULL},	{'N','N','E','N','N','\0'}},{		"B",	"A",	{"B","3","4","2","A",NULL},	{'E','E','S','S','\0'}},{		"B",	"C",	{"B","3","4","C",NULL},	{'E','E','N','\0'}},{		"B",	"D",	{"B","3","6","D",NULL},	{'E','N','N','\0'}},{		"B",	"E",	{"B","3","4","5","7","E",NULL},	{'E','E','E','N','N','\0'}},{		"C",	"A",	{"C","4","2","A",NULL},	{'S','S','S','\0'}},{		"C",	"B",	{"C","4","3","B",NULL},	{'S','W','W','\0'}},{		"C",	"D",	{"C","4","3","6","D",NULL},	{'S','W','N','N','\0'}},{		"C",	"E",	{"C","4","5","7","E",NULL},	{'S','E','N','N','\0'}},{		"D",	"A",	{"D","6","3","1","2","A",NULL},	{'S','S','S','E','S','\0'}},{		"D",	"B",	{"D","6","3","B",NULL},	{'S','S','W','\0'}},{		"D",	"C",	{"D","6","3","4","C",NULL},	{'S','S','E','N','\0'}},{		"D",	"E",	{"D","6","7","E",NULL},	{'S','E','N','\0'}},{		"E",	"A",	{"E","7","5","4","2","A",NULL},	{'S','S','W','S','S','\0'}},{		"E",	"B",	{"E","7","5","4","3","B",NULL},	{'S','S','W','W','W','\0'}},{		"E",	"C",	{"E","7","5","4","C",NULL},	{'S','S','W','N','\0'}},{		"E",	"D",	{"E","7","6","D",NULL},	{'S','W','N','\0'}},NULL};int wless_fd, serv_fd;char answer[32];void does(const unsigned char *none){}/*void turn_left(const unsigned char *arc){	sprintf(answer,"turn_left|%d", atoi(arc));}void turn_right(const unsigned char *arc){	sprintf(answer,"turn_right|%d", atoi(arc));}void stop(const unsigned char *none){	sprintf(answer,"stop");}*/struct func_table funcv[]={	{"do", does},};struct FSM *map_fsm[MAX_CLIENT];char client_dir[MAX_CLIENT];char *client_name[MAX_CLIENT]={	"sword",	NULL};intfind_client(char *name){	int i;	for( i = 0; i < MAX_CLIENT; i++ )	{			if( !strcmp(name, client_name[i]) )			return i;	}	return -1;}intfind_answer(int client_id, char *start, char *end, int cur_cross){	int 	i;	char	event[BUF_SIZE];	path 	*p = NULL;	int	ur_p;	for( i = 0; i < MAX_PATH; i++ )	{		if( !strcmp(paths[i].start,start) && !strcmp(paths[i].end, end) )		{			p = &paths[i];			break;		}	}		if( p != NULL )	{					if( cur_cross == 0 )		{			ur_p = atoi(p->crosses[1]);			sprintf( answer, "%s|%c", ur_path[ur_p-1], p->dirs[0] );			return 0;		}			sprintf(event, "%c", client_dir[client_id]);		fsm_doevent( map_fsm[client_id], event  );		if( !strcmp(p->crosses[cur_cross], end ) )		{			strcpy( answer, "#|#" );		}		else		{			ur_p = atoi( p->crosses[cur_cross+1] );			sprintf( answer, "%s|%c", ur_path[ur_p-1], p->dirs[cur_cross] );		}		client_dir[client_id] = p->dirs[cur_cross];	}}intcomm_send( int fd, char* buf ){	return write( fd, buf, strlen(buf) );}intcomm_recv( int fd, char* buf ){	return read(fd,buf,BUF_SIZE);}intcomm_send_cmd( int fd , char * cmd){	char temp[BUF_SIZE];	int size;	sprintf( temp, "SERVER|CMD|%s|SERVER \0", cmd);	size = comm_send( fd, temp );	printf("send: %d,%s\n", size,temp);	return 0;}int parse_ask_dir( fd, input ){	char	*temp;	char 	*t;		char 	*start,		*end;	char	cur_dir;	int	cur_cross;	int	client_id;	char buf[BUF_SIZE];	char state[BUF_SIZE];		temp = strstr( input, "|CLIENT" );	*temp = '\0';	input += strlen("CLIENT|ASK_DIR|");		t = strtok( input, "|" );	client_id = find_client(t);		if( -1 == client_id )	{		return -1;	}	start = strtok( NULL, "|" );	end = strtok( NULL, "|" );	t = strtok( NULL, "|" );	cur_cross = atoi(t);	t = strtok( NULL, "|" );	cur_dir = *t;	if( cur_cross == 0 )		client_dir[client_id] = cur_dir;	else if( cur_dir != client_dir[client_id] )	{		sprintf(buf, "%s|stop", client_name[client_id]);		usleep(100);		comm_send_cmd(fd, buf);		return -1;	}		find_answer(client_id, start, end, cur_cross);	sprintf(buf, "%s|%s", client_name[client_id], answer );			usleep(100);	comm_send_cmd( fd, buf );		return 0;}intparse_client_input( int fd, char *input){	char *input_cpy;	int len = strlen(input);	input_cpy = malloc( len + 1 );	strcpy( input_cpy, input );	if( strncmp( input, "CLIENT", 6 ) != 0 ){		free( input_cpy );		return -1;	}	input += 7;	if( strncmp( input, "ASK_DIR", 7 ) == 0 ){		parse_ask_dir( fd, input_cpy );		free( input_cpy );		return 0;	}	free( input_cpy );	return 0;}intparse_new_job( int fd, char *input ){		char	*temp;	char 	*t;		char 	*start,			*end;	char	*base_id;	char	*client;		int		client_id;	char 	buf[BUF_SIZE];		temp = strstr( input, "|CENTRE" );	*temp = '\0';	input += strlen("CENTRE|NEW_JOB|");		base_id = strtok( input, "|" );	client = strtok( NULL, "|" );		start = strtok( NULL, "|" );	end = strtok( NULL, "|" );	sprintf(buf, "SERVER|SET_DEST|%s|%s|%s|SERVER \0", client, start, end);	comm_send(wless_fd, buf);		//FIXME need to be changed	printf("send: %s\n", buf);	return 0;}intparse_centre_input( int fd, char *input){	char *input_cpy;	int len = strlen(input);	input_cpy = malloc( len + 1 );	strcpy( input_cpy, input );	if( strncmp( input, "CENTRE", 6 ) != 0 ){		free( input_cpy );		return -1;	}	input += 7;	if( strncmp( input, "NEW_JOB", 7 ) == 0 ){		parse_new_job( fd, input_cpy );		free( input_cpy );		return 0;	}	free( input_cpy );	return 0;}/*intcomm_wait_input( int fd ){	int tmp_size,size;	char tmp[BUF_SIZE];	char buf[BUF_SIZE];	char *input;	memset(buf, '\0', BUF_SIZE);	size = comm_recv(fd, buf);	buf[size]='\0';	while( !strstr(buf,"|CLIENT") ){		tmp_size = comm_recv(fd, tmp);		tmp[tmp_size] = '\0';		strcat(buf,tmp);		size += tmp_size;	}	buf[size]='\0';	printf("before strstr, recv:%s\n",input);	input = strstr(buf,"CLIENT|");	printf("after strstr, recv:%s\n",input);	parse_input( fd, input );	return 0;}*/#define WIRELESS_DEV	"/dev/ttyS0"int open_wireless( int *fd ){	*fd = open( WIRELESS_DEV, O_RDWR );	if( -1 == *fd )	{		perror("Can't open serial port!");		return -1;	}//	set_speed( fd, 9600);//	set_Parity(fd, 8, 1, 'N');	set_serial(*fd);	return 0;}#define CENTER_PORT 10000	//#define CENTER_IP	"127.0.0.1"#define CENTER_IP	"192.168.1.80"//#define CENTER_PORT 5000	//#define CENTER_IP	"192.168.1.88"int connect_to_center( int *fd ){	struct sockaddr_in serv_addr;	struct hostent *server;	*fd = socket( AF_INET, SOCK_STREAM, 0 );	if( *fd < 0 )	{		perror("Can't open socket!");		return -1;	}	bzero( (char*)&serv_addr, sizeof(serv_addr) );	serv_addr.sin_family = AF_INET;	serv_addr.sin_addr.s_addr = inet_addr(CENTER_IP);	serv_addr.sin_port = htons(CENTER_PORT);		if( connect( *fd, &serv_addr, sizeof(serv_addr) ) < 0 )	{		perror("Can't connect to center!");		return -1;	}	perror("Connect success!\n");		return -1;	return 0;}#define CEN	0#define CAR	1intget_input( int role, int fd ){	int tmp_size;	char tmp[BUF_SIZE];	static int size[2];	static char buf[2][BUF_SIZE];	char *input;	switch(role)	{	case CEN:		buf[0][size[0]]='\0';		tmp_size = comm_recv(fd, tmp);		strcat(buf,tmp);		tmp[tmp_size] = '\0';		size[0] += tmp_size;				if( strstr(buf[0],"|CENTRE") )		{			buf[0][size[0]]='\0';			input = strstr(buf[0],"CENTRE|");			printf("after strstr, recv:%s\n",input);			parse_centre_input( fd, input );			size[0] = 0;		}		break;	case CAR:		buf[1][size[1]]='\0';		tmp_size = comm_recv(fd, tmp);		strcat(buf[1],tmp);		tmp[tmp_size] = '\0';		size[1] += tmp_size;				if( strstr(buf[1],"|CLIENT") )		{			buf[1][size[1]]='\0';			input = strstr(buf[1],"CLIENT|");			printf("after strstr, recv:%s\n",input);			parse_client_input( fd, input );			size[1] = 0;		}		break;	}	return 0;}#define FD_MAX	10intmain(){	int i;		struct pollfd fds[FD_MAX];	int	pollfd_num;	open_wireless(&wless_fd);	connect_to_center(&serv_fd);	for( i = 0; i < MAX_CLIENT; i++)		map_fsm[i] = fsm_load("map.fsm", funcv, 1);	fds[0].fd = wless_fd;	fds[0].events = POLLRDNORM;	fds[1].fd = serv_fd;	fds[1].events = POLLRDNORM;	pollfd_num = 2;	while(1)	{		poll( fds, pollfd_num, 100 );				if( fds[0].revents & POLLRDNORM )       //FIXME POLLERR handle		{			get_input(CAR,fds[0].fd);		}			if( fds[1].revents & POLLRDNORM )		{			get_input(CEN,fds[1].fd);		}		//comm_wait_input(wless_fd);	}	for( i = 0; i < MAX_CLIENT; i++)		fsm_free(map_fsm[i]);	close(serv_fd);	close(wless_fd);	return 0;}