#include"m9402.h"
#define S_MAX 20
int kyori;
void sss_io()
{
	int	data;
	void	sss_search(void);
	static int sss_no;
	extern int initial[02];
	static int	old;	/* 読み込んだdataの場所 */
	int s_get_data(void);	/* dataの読込み */

	/* 初期化 */
	if(initial[02] == 1)
	{
		old = 0;
		initial[02] = 2;
		sss_no = 0;
	}

	data = s_get_data();

	/* sss send */
        outportb(TCR2,0xb3);     /*Timer enable*/
	outportb(PADR2,sss_no);	 /* 8bit data */
        outportb(PACR2,0x28);       /* H2 asert */
        outportb(PACR2,0x20);       /* H2 negate */


	sys8(02,old+1,data);	/* BBにdataを書き込む */
	sys8(02,4,old+1);		/* どこのBBの値を変えたか報告 */

	sss_search();

	old = sss_no;
	++sss_no;
}

int s_get_data()
{
	float	d;
	int	e;
	int	data;
	int	h,m,l;
	float	f;
	int	underf;

	/* read count data */
	h = inportb(CNTR2+2) & 0xff;
	m = inportb(CNTR2+4) & 0xff;
	l = inportb(CNTR2+6) & 0xff;

	/* sss send */
	/*outportb(PADR2,sss_no);*/	 /* 8bit data */

	/* change 3 piece of data to 1 piece data */
	data = (h << 16) + (m << 8) +l;

	/* calucuration */
	d = 34000 * (SSS_LIMIT - data) * 32 / 2;	/* 計測距離　*/
	e = d / (double)8000000;			/* 実際の距離　*/
        kyori=e;
	d = d / (double)8000000;

	/* 四捨五入　*/
	f = d - e;
	if(f >= 0.5)
		e = e + 1;

	data = e;

	/* under flow check */
	underf = inportb(PADR2) & 0x04;

	if(underf != 0)
		data = 0;

	return data;
}

void sss_search(void)
{
	int i,j,k;
	int rest;
	int dist;
	int divi;
	int diff;
	int data;
	int direction;			
	int sss_no;			/* 超音波番号 */
	int hit;			/* get opponent */
	int resp;			/* response */
	int front,left,right;		/* data changer */
	static int old_divi;
	static int d[3][S_MAX+1];		/* data lib */
	extern int initial[02];	/* 初期化用変数 */
	extern struct pos
	{
		int x;
		int y;
		int v;
		int theta;
		int thetav;
		int way;
	}
	rot_position_i;
   

	/* 初期化 */
	if(initial[02] == 2)
		direction = 1;

	resp = 0x0;
	hit = 0;

	/* 壁と平行の位置にあるか判別 ただし､指向性は５゜ */
	divi = (rot_position_i.theta) / 90;
	if(divi != 0)
		rest = rot_position_i.theta % (90 * divi);
	if(rest > 5)
		diff = ((90 - rest) <= 5)? 1 : 0;	/* diffが"1"なら平行 */
	else if(rest <= 5)
		diff = 1;

	/* 自分の向き */
	for(i = 0 ; i <= 10 ; ++i)
		if(divi == 4*i)		 front =250-rot_position_i.x,right = rot_position_i.y;
		else if(divi == (1+4*i)) front =250-rot_position_i.y,left = rot_position_i.x;
		else if(divi == (2+4*i)) left = rot_position_i.y,front = rot_position_i.x;
		else if(divi == (3+4*i)) front = rot_position_i.y,right = rot_position_i.x;

	/* 方向転換したか判別 */
	if(old_divi != divi)
		direction = 1;
	else
		direction = 0;
	old_divi = divi;

	/* dataの補正 */	
	if(direction != 0)
	{
		for(i = 0 ; i < 3 ; ++i)
			for(j = 0 ; j < S_MAX ; ++j)
				switch(i)
				{
					case 0:	d[i][j] = right;
						break;
					case 1: d[i][j] = front;
						break;
					case 2: d[i][j] = left;
						break;
				}
		initial[02] = 0;
	}

	/****** read data ******/
	sss_no = sys9(02,4);
	data = sys9(02,sss_no+1);		/* "data = 0" の時は　underflow */		
	
	/* 壁と平行の時　*/
	if(diff == 1)
	{	
		for ( i = 1 ; i <= S_MAX ; ++i)
			d[sss_no][i-1] = d[sss_no][i];

		d[sss_no][S_MAX] = data;
	/* 相手を検索 */
		k = 0;
		for(i = 1 ; i <= S_MAX ; ++i)
		{
			if(abs(d[sss_no][i] - d[sss_no][i-1]) < 10)	
			{
				if(k=1)
					++hit;
			}
			else if(abs(d[sss_no][i] - d[sss_no][i-1]) > 25)	/* 突然の変化 */
			{
				if(k = 0)
				{
					k = 1;
					++hit;
				}
				else 
					k = 0;
			}
		}
	}
	else
	{
		if(data != 0) /* 壁と平行でないのにdataがある */
			++hit;
	}
	/******      返す値作成     ********/
		if(!hit)
			resp = 0;
		else
		{
			resp = resp | 0x8;
			resp = resp + sss_no;	/************ 改良の余地あり **************/
		}
}