// MIRSAK パトロール走行
#include <stdio.h>
#include <linux/errno.h>
#include <unistd.h>
#include <pthread.h>
#include <math.h>
#include <sys/io.h>

#include "mirsak.h"
//#include "patrol_sequence.h"
#include "hardware.h"
#include "direction.h"
#include "run_ctrl.h"
#include "number.h"
#include "io.h"		//追加
#include "uss.h"	//追加
#include "vel_ctrl.h"	//追加


static void rotate_change();		


static void endflag_check();


static int st_value = 30;	//直進速度
static int ro_value = 28;	//回転速度

static	int angle;	//回転角の変数


void mirsak_patrol(){

	number_set_nums(1);	//ボード上の数字の数が1つ

/*スタート*/
	//int number[0];
	int number;

/*エンコーダを用いて250[cm]進む*/
	run_ctrl_set(STRAIGHT, st_value); 
	run_ctrl_set_dist((float)250);
	run_ctrl_create();

	// endflag が 1 になるまで loop する
	while(1){	
		if(run_ctrl_get_endflag()==1) break;
		 usleep(20000);
	}
	run_ctrl_cancel();


	//3秒待機
	usleep(3000000);


/*エンコーダを用いて50[cm]進む*/
	run_ctrl_set(STRAIGHT, 25); 
	run_ctrl_set_dist((float)50);
	run_ctrl_create();

	// endflag が 1 になるまで loop する
	while(1){	
		if(run_ctrl_get_endflag()==1) break;
		 usleep(20000);
	}
	run_ctrl_cancel();

	//direction_correct(50);	//正体補正

	//1.5秒待機
	usleep(1500000);


/*数字認識を開始する*/
	//number[0] = 
	number_detect(45); //数字認識
    number_get(&number);

	//1.5秒待機
	usleep(1500000);


/*左右の超音波センサで万が一のエラー回避を行う*/
	int uss_data[5];	//uss3は後ろからみて左uss4は後ろからみて右
	int uss_end;
	int uss_last;
	int cnt;
	for(cnt=3;cnt<=4;cnt++){	//uss3,uss4の値を取る
		uss_data[cnt] = uss_get(cnt);		//uss_dataを配列に代入する
		printf("uss[%d]=%3d    ",cnt,uss_data[cnt]);
		usleep(50000);
	}
	printf("\n");


	if(uss_data[3] > uss_data[4]){		//超音波センサで取得した値を比較し、
		uss_end = 4;				//大きい値を出力する
		printf("uss_end in if 3 > 4 = %d\n",uss_end);
	}

	if(uss_data[3] < uss_data[4]){
		uss_end = 2;
		printf("uss_end in if 3 < 4 = %d\n",uss_end);
	}

	uss_last = uss_end;

	//if(number != uss_data){		//uss_dataは超音波センサ計測の結果。つまり、2,8(右回転) or 4,6(左回転)
								//これがもし数字と一致しなかったら
		//number[0] = 
		number_detect(45); //数字認識
        number_get(&number);	//再画像処理

		if((number && uss_end) == 2 || (number == 8 && uss_end == 2)){		//右回転
			angle = 88;
		}
		if((number && uss_end) == 4 || (number == 6 && uss_end == 4)){		//左回転
			angle = -88;
			ro_value = ro_value*(-1);
		}
		else{
			if(uss_last == 2){	//右回転
				angle = 88;
			}
			if(uss_last == 4){
				angle = -88;	//左回転
				ro_value = ro_value*(-1);
			}
		}
		printf("angle = %d\n",angle);	//必ず曲がれる
	//}

	//1.5秒待機
	usleep(2000000);


/*エンコーダを用いて90°回転*/
	run_ctrl_set(ROTATE, ro_value); 
	run_ctrl_set_dist((float)angle);
	run_ctrl_create();

	// endflag が 1 になるまで loop する
	while(1){	
		if(run_ctrl_get_endflag()==1) break;
		 usleep(20000);
	}
	run_ctrl_cancel();

	//3秒待機
	usleep(3000000);


/*エンコーダを用いて100[cm]進む*/
	run_ctrl_set(STRAIGHT, st_value); 
	run_ctrl_set_dist((float)100);
	run_ctrl_create();

	// endflag が 1 になるまで loop する
	while(1){	
		if(run_ctrl_get_endflag()==1) break;
		 usleep(20000);
	}
	run_ctrl_cancel();

	//direction_correct(50);	//正体補正

	//3秒待機
	usleep(3000000);



/*エンコーダを用いて90°回転*/
	run_ctrl_set(ROTATE, ro_value); 
	run_ctrl_set_dist((float)angle);
	run_ctrl_create();

	// endflag が 1 になるまで loop する
	while(1){	
		if(run_ctrl_get_endflag()==1) break;
		 usleep(20000);
	}
	run_ctrl_cancel();

	//3秒待機
	usleep(3000000);


/*条件分岐によりゴール位置を判断*/
	if(number==-1){		//最終手段のエラー回避2,4のみ
		number=uss_last;
	}
	switch(number){
		case 2:
			run_ctrl_set(STRAIGHT, st_value); 
			run_ctrl_set_dist((float)57);
			run_ctrl_create();

			endflag_check();
			break;

		case 4:
			run_ctrl_set(STRAIGHT, st_value); 
			run_ctrl_set_dist((float)57);
			run_ctrl_create();

			endflag_check();
			break;

		case 6:
			run_ctrl_set(STRAIGHT, st_value); 
			run_ctrl_set_dist((float)208);
			run_ctrl_create();

			endflag_check();
			break;

		case 8:
			run_ctrl_set(STRAIGHT, st_value); 
			run_ctrl_set_dist((float)208);
			run_ctrl_create();

			endflag_check();
			break;

	}

	//3秒待機
	usleep(3000000);

/*ゴール手前の回転動作(最初の2回とは逆の方向に回転する)*/
	rotate_change();	//反対回転動作

	//3秒待機
	usleep(3000000);

/*正面のタッチセンサが1になるまで小刻みに走り続ける*/
	int count;
	char io_mdata[13];

        while(1) {
			io_get(io_mdata);
	       	for(count=0;count<12;count++){
	  			printf("I/O[%d] = %c\n",count+1,io_mdata[count]);
     		}
	   		printf("\n");

			//タッチセンサが'0'なら直進し続ける
			run_ctrl_set(STRAIGHT, 20); 
			run_ctrl_set_dist((float)20);
			run_ctrl_create();

			while(1){	
				if(run_ctrl_get_endflag()==1) break;
				 usleep(20000);	
			}
			run_ctrl_cancel();

       		printf("I/O[7] = %c\n",io_mdata[7]);
       		
       		if(io_mdata[7] == '1'){		//条件分岐
						//タッチセンサが'1'なら止まる
				printf("We can gole!\n");
				st_value = st_value*(-1);	//後退する
				run_ctrl_set(STRAIGHT, st_value); 
				run_ctrl_set_dist((float)40);
				run_ctrl_create();

				while(1){	
					if(run_ctrl_get_endflag()==1) break;
					 usleep(20000);
				}
				run_ctrl_cancel();
	
				vel_ctrl_set(0.,0.);     //	モータが完全停止  			
				run_ctrl_stop();
       			break;			
       		}
       			
			usleep(1000*1000);
			
		}



}


void rotate_change(){		
	ro_value = ro_value*(-1);
	angle = angle*(-1);
	run_ctrl_set(ROTATE, ro_value); 
	run_ctrl_set_dist((float)angle);
	run_ctrl_create();

	// endflag が 1 になるまで loop する
	while(1){	
		if(run_ctrl_get_endflag()==1) break;
		 usleep(20000);
	}
	run_ctrl_cancel();
}


void endflag_check(){
	// endflag が 1 になるまで loop する
	while(1){	
		if(run_ctrl_get_endflag()==1) break;
		 usleep(20000);
	}
	run_ctrl_cancel();
}