/************************************************************************

	Title: 　モードの選択

	File :SELECT.C		Ver.1.0	 written on Jan.31,1996 by J.W.
				Ver.1.1		    Feb.6,1996

*************************************************************************/
#include "m9401.h"
#include "mathf.h"

#define TH_EPS PIQ/4.5
#define TIME 7000

extern int wait();
extern int command();
extern int inportu();
extern int escape();
extern void calc_state();
extern void control();

extern float Z;
extern float Th;
extern float Th_ref;
extern int Z_ref;
extern float Xc, Yc;
extern int Tc,Sc;
extern int Ts_d,Ps_d;
extern int Th_e;
extern int Ze;
extern int Posi;
extern int Dist[];
extern int Turn;
extern int mode;
extern int Seg[];
extern int Led2;/* Red Led Data */
extern int flag;
extern int Act; /* 0:go 1:turn_left 2:turn_right 3:back 4:stop */

int select()
{
	if(flag==ON){
		Sc=0;
		/* Th_e=0; */
		Ze=0;
	}

	switch(mode){
		case NOMAL:
			nomal();
			if(Ps_d!=0x00){
				flag=ON;
				mode=PS_MODE;
			}
			else if((Ts_d & 0x80) == 0x80){
				flag=ON;
				mode=ESCAPE;
			}
			else if (Posi!=0 && Act==0 && Turn==0){
				if (Dist[0]<50 || Dist[1]<50){
					flag=ON;
					mode=SS_MODE;
				}
			}
			else if (Posi!=0 && Act==0 && Turn==1){
				if (Dist[0]<50 || Dist[2]<50){
					flag==ON;
					mode=SS_MODE;
				}
			}
		break;
		case PS_MODE:
			ps_mode();
		break;
		case ESCAPE:
			escape();
		break;
		case SS_MODE:
			ss_mode();
		break;
		case INI_MODE:
			ini_m();
		break;
		default:
			Act=4;
			flag=ON;
			mode=NOMAL;
		break;
	}
	/* if(flag==ON)
		Act=4; */
	/* Seg[4]=mode; */
	wait(TIME);
	return(ON);
}