/////////////////////////////////////////////////////////////
//2009年6月15日
//鉄道シュミレーション
//演算周期0.5[ms]で200[m]地点から定位置停止を行う
//TASCパターンの代わりにファジィ推論を利用する
//fazzy_tasc2からの追加機能
//・シングルトンの厚みをなくす(○)
//・スピードメンバシップのパラメータ(○)
//・乱変数により、タイミングのズレた距離を入力する(○)
//・ジャークを評価するメンバシップ関数を作成する(○)
//
//5からの変更点
//・シングルトンに10%を追加
//・パラメータ調整
//
/////////////////////////////////////////////////////////////
#include<time.h>
#include<stdio.h>
#include<math.h>
#include<stdlib.h>
/////////////////////////////////////////////////////////////
//初期設定
/////////////////////////////////////////////////////////////
#define sample_time 0.05	//演算周期[s]
#define t 3.0			//時定数 t
#define mu 0.3			//摩擦係数μ
#define weight 26000		//車両質量[kg]
#define g 9.8			//重力定数
#define first_speed 80.0	//初速度[m/s]
#define first_distance 200	//残走距離の初期値
//rule1で使用する値
#define sp_low 18.8		//スピードファジィ命題のLowメンバシップ関数が1になるときのxの値
#define sp_mid 44.8		//スピードファジィ命題のMidメンバシップ関数が1になるときのxの値
#define sp_high 69.8		//スピードファジィ命題のHighメンバシップ関数が1になるときのxの値
//rule2で使用する値
#define dis_ne 14.8		//距離ファジィ命題のNeメンバシップ関数が1になるときのxの値
#define dis_zo 64.8		//距離ファジィ命題のZoメンバシップ関数が1になるときのxの値
#define dis_po 114.8		//距離ファジィ命題のPoメンバシップ関数が1になるときのxの値
//rule3で使用する値
#define jerk_go -30.0		//ジャーク命題のgoodメンバシップ関数が1になるときの値
#define jerk_nor -40.0		//ジャーク命題のnormalメンバシップ関数が1になるときの値
#define jerk_ba -50.0		//ジャーク命題のbadメンバシップ関数が1になるときの値
//singletonで使用する値
#define x0_singleton 0.0	//0%
#define x1_singleton 0.035	//3.6%
#define x2_singleton 0.087	//7.3%
#define x3_singleton 0.10	//10% 
#define x4_singleton 0.45	//45%
/////////////////////////////////////////////////////////////
//関数宣言
/////////////////////////////////////////////////////////////
float friction(float v);
void rule1(float v_rule1,float speed_rule1[]);
void rule2(float distance_rest2,float distance_rule2[]);
void rule3(float jerk3,float jerk_rule3[]);
void alg_times(float output_times[][3][3],float jerk_times[],float speed_times[],float distance_times[]);
float singleton(float output_plus[][3][3]);
/////////////////////////////////////////////////////////////
//グローバル変数
/////////////////////////////////////////////////////////////
int main(){
	//[0]=Lo,[1]=Mid,[2]=Hi
	float fazzy_speed[3]={0,0,0};	
	//[0]=Ne,[1]=Zo,[2]=Po
	float fazzy_distance[3]={0,0,0};
	//[0]=good,[1]=normal,[2]=bad
	float fazzy_jerk[3]={0,0,0};
	//[0][0][0]=Go&Lo&Ne,[0][1]=Lo&Zo,[0][2]=Lo&Po
	//[1][0]=Mi&Ne,[1][1]=Mi&Zo,[1][2]=Mi&Po
	//[2][0]=Hi&Ne,[2][1]=Hi&Zo,[2][2]=Mi&po	
	float output[3][3][3]={{{0,0,0},{0,0,0},{0,0,0}},
			       {{0,0,0},{0,0,0},{0,0,0}},
			       {{0,0,0},{0,0,0},{0,0,0}}};
	float speed_past,speed_next,speed;				//過去のスピード,現在のスピード,次のスピード
	float down_rate;						//スピードを落とす割合
	float train_accel[2]={0,0};					//加減速度{=(現在のスピード)-(過去のスピード)}
	float train_time = 0.0;							//時間
	float distance_rest = first_distance;				//残走距離
	float l;							//移動距離
	float jerk=0.0;							//ジャーク
	int random1,random2;						//乱変数
	int count1=0;							//カウンター
	int count2=0;							//カウンター
	float mark1=200.0;						//地上子1の場所,停止地点から200m手前
	float mark2=20.0;						//地上子2の場所,停止地点から20m手前
	float gap2=5.0;							//地上子2とのズレの最大値
	float mark3=2.0;						//地上子3の場所,停止地点から2m手前
	float gap3=0.5;							//地上子3とのズレの最大値
	speed_past = first_speed;					//現在のスピードを初期速度に設定	
	float distance = 0.0;						//移動距離	
	

///////////////////////////////
//乱変数設定
//randomには0〜11を代入
///////////////////////////////
	srand(time(NULL));
	random1 = rand()%12;
	srand(time(NULL));
	random2 = rand()%12;

/////////////////////////////////////////////////////////////
//ループ条件
//移動スピードが0.0以上の間ループする.
/////////////////////////////////////////////////////////////
	while(speed_past>0.0){			
		speed=friction(speed_past);						//摩擦によって現在のスピードが変化
		rule1(speed,fazzy_speed);						//ルール1実行

////////////////////////////////////////////////////
//距離が (地上子 − ズレ)<地上子 && 地上子<(地上子 + ズレ)
//
////////////////////////////////////////////////////
//printf("distance_rest%f\n",distance_rest);
		if(mark2-3*gap2 < distance_rest && distance_rest <= mark2+gap2){	
//printf("ズレ以内\n");
			if(random1==0 && count1==0){
				rule2(mark2,fazzy_distance);
				count1++;
//printf("1\n");
			}
			else if((random1==1 || random1==2) && count1==0){
				if(distance_rest <= mark2+gap2*0.7){
					rule2(mark2,fazzy_distance);
					count1++;
//printf("2\n");
				}
				else{
					rule2(distance_rest,fazzy_distance);
//printf("2.1\n");
				}
			}
			else if((random1==3 || random1==4 || random1==5) && count1==0){
				if(distance_rest <= mark2+gap2*0.3){
					rule2(mark2,fazzy_distance);
					count1++;
//printf("3\n");
				}
				else{
					rule2(distance_rest,fazzy_distance);
//printf("3.1\n");
				}
			}
			else if((random1==6 || random1==7 || random1==8) && count1==0){
				if(distance_rest <= mark2-gap2*0.3){
					rule2(mark2,fazzy_distance);
					count1++;
//printf("4\n");
				}
				else{
					rule2(distance_rest,fazzy_distance);
//printf("4.1\n");
				}
			}
			else if((random1==9 || random1==10) && count1==0){
				if(distance_rest <= mark2+gap2*0.7){
					rule2(mark2,fazzy_distance);
					count1++;
//printf("5\n");
				}
				else{
					rule2(distance_rest,fazzy_distance);
//printf("5.1\n");
				}
			}
			else if((random1==11) && count1==0){
				if(distance_rest <= mark2-gap2){
					rule2(mark2,fazzy_distance);
					count1++;
//printf("6\n");
				}
				else{
					rule2(distance_rest,fazzy_distance);
//printf("6.1\n");
				}
			}
			else{
				rule2(distance_rest,fazzy_distance);
//printf("7\n");
			}
		}
		else if(distance_rest <= mark3+gap3){
//printf("ズレ以内2\n");
			if(random2==0 && count2==0){
				rule2(mark3,fazzy_distance);
				count2++;
//printf("0\n");
			}
			else if((random2==1 || random2==2) && count2==0){
				if(distance_rest <= mark3+gap3*0.7){
					rule2(mark3,fazzy_distance);
					count2++;
//printf("1\n");
				}
				else{
					rule2(distance_rest,fazzy_distance);
//printf("1.1\n");
				}
			}
			else if((random2==3 || random2==4 || random2==5) && count2==0){
				if(distance_rest <= mark3+gap3*0.3){
					rule2(mark3,fazzy_distance);						//ルール2実行
					count2++;
//printf("2\n");
				}
				else{
					rule2(distance_rest,fazzy_distance);
//printf("2.1\n");
				}
			}
			else if((random2==6 || random2==7 || random2==8) && count2==0){
				if(distance_rest <= mark3-gap3*0.3){
					rule2(mark3,fazzy_distance);						//ルール2実行
					count2++;
//printf("3\n");
				}
				else{
					rule2(distance_rest,fazzy_distance);
//printf("3.1\n");
				}
			}
			else if((random2==9 || random2==10) && count2==0){
				if(distance_rest <= mark3-gap3){
					rule2(mark3,fazzy_distance);
					count2++;
//printf("4\n");
				}
				else{
					rule2(distance_rest,fazzy_distance);
//printf("4.1\n");
				}
			}
			else if((random2==11) && count2==0){
				if(distance_rest <= mark3-gap3){
					rule2(mark3,fazzy_distance);
					count2++;
//printf("5\n");
				}
				else{
					rule2(distance_rest,fazzy_distance);
//printf("5.1\n");
				}
			}
			else{
				rule2(distance_rest,fazzy_distance);
//printf("!!!!!\n");
			}
		}
		else{
			rule2(distance_rest,fazzy_distance);
//printf("??????\n");
		}
		rule3(jerk,fazzy_jerk);
		alg_times(output,fazzy_jerk,fazzy_speed,fazzy_distance);		//ルール1とルール2とルール3を代数積にて結合
		down_rate=singleton(output);						//スピードダウンの割合を算出
		speed_next = speed * (1.0-down_rate);
//printf("\tspeed_past%f\tspeed_next%f\tspeed%f\n",speed_past,speed_next,speed);
		train_accel[1] = (speed_next-speed_past)/sample_time;				//加減速度算出[km/sec]
		l = speed_past * sample_time + (train_accel[1]*sample_time*sample_time)/2;	//移動距離算出
		distance = distance + l;
		speed_past=speed_next;
		distance_rest = first_distance - distance;
printf("\taccel[0]%f\taccel[1]%f\n",train_accel[0],train_accel[1]);
		if(train_time>0.0){
		jerk = (train_accel[1] - train_accel[0])/sample_time;				//ジャークの計算(加速度の微分)	
		train_accel[0] = train_accel[1];
		}
printf("\ttime%f\tjerk%f\tspeed%f\trest%f\n",train_time,jerk,speed,distance_rest);

////////////////////////////////////////////////
//
//出力用
//
////////////////////////////////////////////////
		//printf("%f\t%f\n",train_time,jerk);							//time & jerk
		//printf("%f\t%f\n",train_time,distance_rest);					//time & distance
		//printf("%f\t%f\n",train_time,speed);						//time & speed
		train_time = train_time + sample_time;
	}
printf("random1=%d\t",random1);
printf("random2=%d\n",random2);	
}

///////////////////////////////////////////////
//摩擦 
//入力は現在速度                             
//戻り値は摩擦により変化した速度,残走距離      
//摩擦によって変化した速度,移動距離
//////////////////////////////////////////////
float friction(float v_fri){
	float train_friction = weight*g*mu;					//摩擦=mgμ
	float fri_accel = -g*mu;						//摩擦による減速度
	float v_next;								//変化後の速度

	v_next = v_fri + fri_accel * sample_time;				//速度算出
	if(v_next<0.0)v_next=0.0;
	
	return v_next;
}
	
/////////////////////////////////////
//距離演算	                   //
//引数は現在速度                    //
//戻り値は摩擦により変化した速度     //
/////////////////////////////////////
/////////////////////////////////////
//ルール1	                   //
//スピードのファジィ命題	           //
//入力は,現在速度,speed[]	   //
/////////////////////////////////////
void rule1(float v_rule1,float speed_rule1[]){
	float a_low = 1.0/(sp_low - sp_mid);					//Lowメンバシップ関数の傾き
	float a_mid = -1.0 / (sp_mid - sp_low);					//Midメンバシップ関数の傾き
	float a_high = 1.0 / (sp_high - sp_mid);					//Lowメンバシップ関数の傾き
	float b_low = 1.0 - (sp_low * a_low);					//Lowメンバシップ関数の切片
	float b_high = 1.0 - (sp_high * a_high);					//Highメンバシップ関数の切片
	float aa = sp_low - sp_mid;
	a_low = 1 / aa;	
//printf("sp_low=%f\tsp_mid=%f\taa=%f\t\n",sp_low,sp_mid,aa);
//printf("a_low=%f\ta_mid=%f\ta_high=%f\t\n",a_low,a_mid,a_high);
//printf("b_low=%f\tb_high=%f\n",b_low,b_high);
	
	if(v_rule1 <= sp_low){
		speed_rule1[0] = 1;
		speed_rule1[1] = 0;
		speed_rule1[2] = 0;
	}
	else if(sp_low < v_rule1 && v_rule1 <= sp_mid){
		speed_rule1[0] = a_low * v_rule1 + b_low;
		speed_rule1[1] = a_mid * (-v_rule1 + sp_mid) + 1;
		speed_rule1[2] = 0;
	}
	else if(sp_mid < v_rule1 && v_rule1 <= sp_high){
		speed_rule1[0] = 0;
		speed_rule1[1] = a_mid * (v_rule1 - sp_mid) + 1;
		speed_rule1[2] = a_high * v_rule1 + b_high;
	}
	else if(sp_high < v_rule1){
		speed_rule1[0] = 0;
		speed_rule1[1] = 0;
		speed_rule1[2] = 1;
	}
//printf("speed_rule1[0]=%f\n",speed_rule1[0]);
//printf("speed_rule1[1]=%f\n",speed_rule1[1]);
//printf("speed_rule1[2]=%f\n",speed_rule1[2]);
}



/////////////////////////////////////
//ルール2                          //
//距離のファジィ命題                //
//入力は,現在残走距離,distance[]	　 //
/////////////////////////////////////
void rule2(float distance_rest2,float distance_rule2[]){
	float a_ne = 1.0 / (dis_ne - dis_zo);					//Neメンバシップ関数の傾き
	float a_zo = -1.0 / (dis_zo - dis_ne);					//Zoメンバシップ関数の傾き
	float a_po = 1.0 / (dis_po - dis_zo);					//Poメンバシップ関数の傾き
	float b_ne = 1.0 - (dis_ne * a_ne);					//Neメンバシップ関数の切片
	float b_po = 1.0 - (dis_po * a_po);					//Poメンバシップ関数の切片
//float distance_rest = first_distance - distance;			//残走距離=初期距離-移動距離

//printf("a_ne=%f\ta_zo=%f\ta_po=%f\t\n",a_ne,a_zo,a_po);
//printf("b_ne=%f\tb_po=%f\n",b_ne,b_po);
//printf("distance_rest2=%f",distance_rest2);
	if(distance_rest2 <= dis_ne){
		distance_rule2[0]=1;
		distance_rule2[1]=0;
		distance_rule2[2]=0;
	}
	else if(dis_ne < distance_rest2 && distance_rest2 <= dis_zo){
		distance_rule2[0]=a_ne * distance_rest2 + b_ne;
		distance_rule2[1]=a_zo * (-distance_rest2 + dis_zo) + 1;
		distance_rule2[2]=0;
	}
	else if(dis_zo < distance_rest2 && distance_rest2 <= dis_po){
		distance_rule2[0]=0;
		distance_rule2[1]=a_zo * (distance_rest2 - dis_zo) + 1;
		distance_rule2[2]=a_po * distance_rest2 + b_po;
	}
	else if(dis_po < distance_rest2){
		distance_rule2[0]=0;
		distance_rule2[1]=0;
		distance_rule2[2]=1;
	}
//printf("distance_rule2[0]=%f\n",distance_rule2[0]);
//printf("distance_rule2[1]=%f\n",distance_rule2[1]);
//printf("distance_rule2[2]=%f\n",distance_rule2[2]);
}

///////////////////////////////////////
//ルール3
//ジャークのファジィ命題
//入力は,現在のジャーク,jerk[]
///////////////////////////////////////
void rule3(float jerk3,float jerk_rule3[]){
	float a_go = 1.0 /(jerk_go - jerk_nor);				//goodメンバシップ関数の傾き
	float a_nor = -1.0 /(jerk_nor - jerk_go);			//normalメンバシップ関数の傾き
	float a_ba = 1.0 /(jerk_ba - jerk_nor);				//badメンバシップ関数の傾き	
	float b_go = 1.0 - (a_go * jerk_go);				//goodメンバシップ関数の切片
	float b_ba = 1.0 - (a_ba * jerk_ba);				//badメンバシップ関数の切片

//printf("a_go=%f\ta_nor=%f\ta_ba=%f\t\n",a_go,a_nor,a_ba);
//printf("b_go=%f\tb_ba=%f\n",b_go,b_ba);
//printf("jerk3=%f\n",jerk3);
//printf("jerk_go=%f\tjerk_nor=%f\tjerk_ba=%f\t\n",jerk_go,jerk_nor,jerk_ba);

	if(jerk3 >= jerk_go){
		jerk_rule3[0]=1;
		jerk_rule3[1]=0;
		jerk_rule3[2]=0;
	}
	else if(jerk_go > jerk3 && jerk3 >= jerk_nor){
		jerk_rule3[0]=a_go * jerk3 + b_go;
		jerk_rule3[1]=a_nor * (-jerk3 + jerk_nor) +1;
		jerk_rule3[2]=0;
	}
	else if(jerk_nor > jerk3 && jerk3 >= jerk_ba){
		jerk_rule3[0]=0;
		jerk_rule3[1]=a_nor * (jerk3 - jerk_nor) +1;
		jerk_rule3[2]=a_ba * jerk3 + b_ba;
	}
	else if(jerk_ba > jerk3){
		jerk_rule3[0]=0;
		jerk_rule3[1]=0;
		jerk_rule3[2]=1;
	}
//printf("jerk_rule3[0]=%f\n",jerk_rule3[0]);
//printf("jerk_rule3[1]=%f\n",jerk_rule3[1]);
//printf("jerk_rule3[2]=%f\n",jerk_rule3[2]);
}
/////////////////////////////////////
//ファジィ推論                      
//引数はoutput[][],jerk[],speed[],distance[]
//代数積にてrule1&rule2を処理       
/////////////////////////////////////
void alg_times(float output_times[][3][3],float jerk_times[],float speed_times[],float distance_times[]){
	output_times[0][0][0] = jerk_times[0] * speed_times[0] * distance_times[0];  
	output_times[0][0][1] = jerk_times[0] * speed_times[0] * distance_times[1];  
	output_times[0][0][2] = jerk_times[0] * speed_times[0] * distance_times[2];  
	output_times[0][1][0] = jerk_times[0] * speed_times[1] * distance_times[0];  
	output_times[0][1][1] = jerk_times[0] * speed_times[1] * distance_times[1];  
	output_times[0][1][2] = jerk_times[0] * speed_times[1] * distance_times[2];  
	output_times[0][2][0] = jerk_times[0] * speed_times[2] * distance_times[0];  
	output_times[0][2][1] = jerk_times[0] * speed_times[2] * distance_times[1];  
	output_times[0][2][2] = jerk_times[0] * speed_times[2] * distance_times[2];  
	output_times[1][0][0] = jerk_times[1] * speed_times[0] * distance_times[0];  
	output_times[1][0][1] = jerk_times[1] * speed_times[0] * distance_times[1];  
	output_times[1][0][2] = jerk_times[1] * speed_times[0] * distance_times[2];  
	output_times[1][1][0] = jerk_times[1] * speed_times[1] * distance_times[0];  
	output_times[1][1][1] = jerk_times[1] * speed_times[1] * distance_times[1];  
	output_times[1][1][2] = jerk_times[1] * speed_times[1] * distance_times[2];  
	output_times[1][2][0] = jerk_times[1] * speed_times[2] * distance_times[0];  
	output_times[1][2][1] = jerk_times[1] * speed_times[2] * distance_times[1];  
	output_times[1][2][2] = jerk_times[1] * speed_times[2] * distance_times[2];  
	output_times[2][0][0] = jerk_times[2] * speed_times[0] * distance_times[0];  
	output_times[2][0][1] = jerk_times[2] * speed_times[0] * distance_times[1];  
	output_times[2][0][2] = jerk_times[2] * speed_times[0] * distance_times[2];  
	output_times[2][1][0] = jerk_times[2] * speed_times[1] * distance_times[0];  
	output_times[2][1][1] = jerk_times[2] * speed_times[1] * distance_times[1];  
	output_times[2][1][2] = jerk_times[2] * speed_times[1] * distance_times[2];  
	output_times[2][2][0] = jerk_times[2] * speed_times[2] * distance_times[0];  
	output_times[2][2][1] = jerk_times[2] * speed_times[2] * distance_times[1];  
	output_times[2][2][2] = jerk_times[2] * speed_times[2] * distance_times[2];  
//printf("%f\t%f\t%f\n",output_times[0][0][0],output_times[0][0][1],output_times[0][0][2]);
//printf("%f\t%f\t%f\n",output_times[0][1][0],output_times[0][1][1],output_times[0][1][2]);
//printf("%f\t%f\t%f\n\n",output_times[0][2][0],output_times[0][2][1],output_times[0][2][2]);

//printf("%f\t%f\t%f\n",output_times[1][0][0],output_times[1][0][1],output_times[1][0][2]);
//printf("%f\t%f\t%f\n",output_times[1][1][0],output_times[1][1][1],output_times[1][1][2]);
//printf("%f\t%f\t%f\n\n",output_times[1][2][0],output_times[1][2][1],output_times[1][2][2]);
//
//printf("%f\t%f\t%f\n",output_times[2][0][0],output_times[2][0][1],output_times[2][0][2]);
//printf("%f\t%f\t%f\n",output_times[2][1][0],output_times[2][1][1],output_times[2][1][2]);
//printf("%f\t%f\t%f\n\n",output_times[2][2][0],output_times[2][2][1],output_times[2][2][2]);
}

//////////////////////////////////////
//代数和にてプラス.		　 　
//入力はoutput[][]	           
//出力は速度減速の％	           
//非ファジィ化には重心法を用いる. 
//
//////////////////////////////////////
float singleton(float output_plus[][3][3]){
	float x1_single_small=0.0;			//シングルトン1(0%)
	float x2_single_small=0.0;			//シングルトン2(3.6%)
	float x3_single_small=0.0;			//シングルトン3(7.3%)
	float x4_single_small=0.0;			//シングルトン4(10%)
	float x5_single_small=0.0;			//シングルトン5(45%)
	float sum_single = 0.0;				//面積の総和、重心法の分子
	float value_single =0.0;			//重心法の分母
	float defuzzy_single;					//非ファジィ化して得た値(出力値)
float test=0.0;	
float test2=0.0;	

/////////////////////////////////////
//で一番大きい値をx1_single_small(0%)とする.
/////////////////////////////////////
	if(output_plus[0][0][1] >= x1_single_small){
		x1_single_small = output_plus[0][0][1];
	}
	if(output_plus[0][0][2] >= x1_single_small){
		x1_single_small = output_plus[0][0][2];
	}
	if(output_plus[0][1][1] >= x1_single_small){
		x1_single_small = output_plus[0][1][1];
	}
	if(output_plus[0][1][2] >= x1_single_small){
		x1_single_small = output_plus[0][1][2];
	}
	if(output_plus[0][2][2] >= x1_single_small){
		x1_single_small = output_plus[0][2][2];
	}	
	
	if(output_plus[1][0][1] >= x1_single_small){
		x1_single_small = output_plus[1][0][1];
	}
	if(output_plus[1][0][2] >= x1_single_small){
		x1_single_small = output_plus[1][0][2];
	}
	if(output_plus[1][1][2] >= x1_single_small){
		x1_single_small = output_plus[1][1][2];
	}
	
	if(output_plus[2][0][1] >= x1_single_small){
		x1_single_small = output_plus[2][0][1];
	}
	if(output_plus[2][0][2] >= x1_single_small){
		x1_single_small = output_plus[2][0][2];
	}
	if(output_plus[2][1][2] >= x1_single_small){
		x1_single_small = output_plus[2][1][2];
	}
	
//printf("x1_single_small=%f\n",x1_single_small);

/////////////////////////////////////
//で一番大きい値をx2_single_small(3.6%)とする.	
/////////////////////////////////////
	if(output_plus[0][0][0] >= x2_single_small){
		x2_single_small = output_plus[0][0][0];
	}
	if(output_plus[0][1][0] >= x2_single_small){
		x2_single_small = output_plus[0][1][0];
	}
	if(output_plus[0][2][1] >= x2_single_small){
		x2_single_small = output_plus[0][2][1];
	}
	if(output_plus[1][1][1] >= x2_single_small){
		x2_single_small = output_plus[1][1][1];
	}
	if(output_plus[1][2][2] >= x2_single_small){
		x2_single_small = output_plus[1][2][2];
	}
//printf("x2_single_small=%f\n",x2_single_small);

/////////////////////////////////////
//で一番大きい値をx3_single_small(7.3%)とする.
/////////////////////////////////////
	if(output_plus[1][0][0] >= x3_single_small){
		x3_single_small = output_plus[1][0][0];
	}
	if(output_plus[1][2][1] >= x3_single_small){
		x3_single_small = output_plus[1][2][1];
	}
	if(output_plus[2][1][1] >= x3_single_small){
		x3_single_small = output_plus[2][1][1];
	}	
	if(output_plus[2][2][2] >= x3_single_small){
		x3_single_small = output_plus[2][2][2];
	}
//printf("x3_single_small=%f\n",x3_single_small);
		
/////////////////////////////////////
//で一番小さい値をx4_single_small(10%)とする.	  
/////////////////////////////////////
	if(output_plus[1][1][0] >= x4_single_small){
		x4_single_small = output_plus[1][1][0];
	}
	if(output_plus[2][0][0] >= x4_single_small){
		x4_single_small = output_plus[2][0][0];
	}	
	if(output_plus[2][1][0] >= x4_single_small){
		x4_single_small = output_plus[2][1][0];
	}	
/////////////////////////////////////
//で一番小さい値をx5_single_small(45%)とする.	  
/////////////////////////////////////
	if(output_plus[0][2][0] >= x5_single_small){
			x5_single_small = output_plus[0][2][0];
	}
	if(output_plus[1][2][0] >= x5_single_small){
			x5_single_small = output_plus[1][2][0];
	}
	if(output_plus[2][2][0] >= x5_single_small){
			x5_single_small = output_plus[2][2][0];
	}
	if(output_plus[2][2][1] >= x5_single_small){
		x5_single_small = output_plus[2][2][1];
	}	
//printf("x4_single_small=%f\n",x4_single_small);	
/////////////////////////////////////
//シングルトンで非ファジィ化
/////////////////////////////////////
	sum_single = x1_single_small * x0_singleton + x2_single_small * x1_singleton + x3_single_small * x2_singleton + x4_single_small *x3_singleton + x5_single_small * x4_singleton;
	value_single = x1_single_small + x2_single_small + x3_single_small + x4_single_small + x5_single_small;
//printf("sum_single = %f\n",sum_single);
//printf("value_single = %f\n",value_single);
	defuzzy_single = sum_single / value_single;
	
printf("defuzzy_single = %f\n",defuzzy_single);
	return(defuzzy_single);
}


////////////////////////////////////
//出力用(gnuplotにてepsファイル作成)
////////////////////////////////////
//set terminal postscript eps
//set output"6_15_no3.eps"
//plot"jerk.dat"with line,"speed.dat"with line,"distance_rest.dat"with line