#include "common.h"
#include "data.h"

/* ״̬ */
void fsm(void){
	static float max_sum_weight_normalization_yaw = 0;			// һȨͺ
	static float box_fxxk_finsh_distance = 0;		// ʱ·
	static int8 box_dir = 0;	// 뷽
	
	// ֵ
	dog_cv.threshold(&dog_cv, mt9v03x_image);
	// ȡ
	dog_path.side_extract(&dog_path, dog_cv.image_OTSU);
	// ߵж
	dog_path.side_point_kind_judge(&dog_path);
	// ·ȡ
	dog_path.path_extract(&dog_path, dog_cv.image_OTSU);
	// ״̬ж
	path_state = path_state_judge(dog_cv.image_OTSU);
	
	/************************************* ״̬л *************************************/
	switch(path_state){
		// Բ뻷
		case L_circle_in:{ 
			/* ־λ */
			circle_euler_angle_solve.solve_flag = True;	// Բŷǽ
			/* ˶ */
			control_kind = CircleAngle2Inv2Speed; 	// ÿ
			move_solve_kind = XY_SPEED_SOLVE;		// ý
			x_speed_target = 0;
			y_speed_slow_change(circle_y_speed_target[plan_idx], 0.008, False);
			circle_rotation_yaw_target = -circle_angle_target[0];
			// жǷתָǶ
			if(circle_rotate_finsh_flag == True)
			{
				/* ־λ */
				circle_euler_angle_solve.solve_flag = False;		// رԲŷǽ
				circle_in_distance = displacement_solve.distance;	// ȡ뻷ʼ·
				circle_rotate_finsh_flag = False;					// ʼԲתɱ־λ
				/* ͣ */
				control_kind = Speed; 		// ÿ
				wheel_speed_target[0] = 0;
				wheel_speed_target[1] = 0;
				wheel_speed_target[2] = 0;
				/* ״̬л */
				path_state = L_circle;
				break;
			}
			break; 
		}
		// Բ
		case L_circle_out:{ 
			/* ־λ */
			circle_euler_angle_solve.solve_flag = True;	// Բŷǽ
			/* ˶ */
			control_kind = CircleAngle2Inv2Speed; 	// ÿ
			move_solve_kind = XY_SPEED_SOLVE;		// ý
			x_speed_target = 0;
			y_speed_slow_change(circle_y_speed_target[plan_idx], 0.008, False);
			circle_rotation_yaw_target = -circle_angle_target[1];
			// жǷתָǶ
			if(circle_rotate_finsh_flag == True)
			{
				/* ־λ */
				circle_euler_angle_solve.solve_flag = False;		// رԲŷǽ
				circle_out_distance = displacement_solve.distance;	// ȡʼ·
				circle_rotate_finsh_flag = False;					// ʼԲתɱ־λ
				/* ͣ */
				control_kind = Speed; 		// ÿ
				wheel_speed_target[0] = 0;
				wheel_speed_target[1] = 0;
				wheel_speed_target[2] = 0;
				/* ״̬л */
				path_state = common_path;
				break;
			}
			break; 
		}
		// Բ뻷
		case R_circle_in:{ 
			/* ־λ */
			circle_euler_angle_solve.solve_flag = True;	// Բŷǽ
			/* ˶ */
			control_kind = CircleAngle2Inv2Speed; 	// ÿ
			move_solve_kind = XY_SPEED_SOLVE;		// ý
			x_speed_target = 0;
			y_speed_slow_change(circle_y_speed_target[plan_idx], 0.008, False);
			circle_rotation_yaw_target = circle_angle_target[0];
			// жǷתָǶ
			if(circle_rotate_finsh_flag == True)
			{
				/* ־λ */
				circle_euler_angle_solve.solve_flag = False;		// رԲŷǽ
				circle_in_distance = displacement_solve.distance;	// ȡ뻷ʼ·
				circle_rotate_finsh_flag = False;					// ʼԲתɱ־λ
				/* ͣ */
				control_kind = Speed; 		// ÿ
				wheel_speed_target[0] = 0;
				wheel_speed_target[1] = 0;
				wheel_speed_target[2] = 0;
				/* ״̬л */
				path_state = R_circle;
				break;
			}
			break; 
		}
		// Բ
		case R_circle_out:{ 
			/* ־λ */
			circle_euler_angle_solve.solve_flag = True;	// Բŷǽ
			/* ˶ */
			control_kind = CircleAngle2Inv2Speed; 	// ÿ
			move_solve_kind = XY_SPEED_SOLVE;		// ý
			x_speed_target = 0;
			y_speed_slow_change(circle_y_speed_target[plan_idx], 0.008, False);
			circle_rotation_yaw_target = circle_angle_target[1];
			// жǷתָǶ
			if(circle_rotate_finsh_flag == True)
			{
				/* ־λ */
				circle_euler_angle_solve.solve_flag = False;		// رԲŷǽ
				circle_out_distance = displacement_solve.distance;	// ȡʼ·
				circle_rotate_finsh_flag = False;					// ʼԲתɱ־λ
				/* ͣ */
				control_kind = Speed; 		// ÿ
				wheel_speed_target[0] = 0;
				wheel_speed_target[1] = 0;
				wheel_speed_target[2] = 0;
				/* ״̬л */
				path_state = common_path;
				break;
			}
			break; 
		}
		// 
		case zebra_path:{
			if(fabsf(displacement_solve.distance-zebra_stop_distance) >= 50){
				/* ͣ */
				control_kind = Speed; 		// ÿ
				wheel_speed_target[0] = 0;
				wheel_speed_target[1] = 0;
				wheel_speed_target[2] = 0;
				
				system_delay_ms(500);
	
				control_kind = Stop; 		// ÿ
				/* ʾʶбҳ */
				menu_detection_list();
				break;
			}
			break;
		}
		// һζλ
		case box_first_track:{
			/* ˶ */
			control_kind = Track_XY2Inv2Speed;	// ÿģʽ
			angular_speed_target = 0;
			/* Ч */
			dog_rwr.radar_scanning_enable_flag = False;
			dog_rwr.radar_tracking_enable_flag = True;
			dog_rwr.missile_launch_enable_flag = False;			
			if(box_XY_finsh_flag == True){
				/* ־λ */
				box_XY_finsh_flag = False;
				/*
					˴ԲлͣģʽҪ֤ box_XY_finsh_flagΪFalseٽ XY2Inv2SpeedģʽƣڵǰλֵΧڣ box_XY_finsh_flagֱΪTrueµڶβжλ
				*/
				/* ͣ */
				control_kind = Speed;		// ÿģʽ
				wheel_speed_target[0] = 0;
				wheel_speed_target[1] = 0;
				wheel_speed_target[2] = 0;
				
				x_speed_target = 0;	// ûٳʼֵ
				
				/* ʼʶ */
				detection_result.tool_detection_finsh_flag = False;
				detection_result.num_detection_finsh_flag = False;
				
				/* ״̬л */
				path_state = box_calibration;	// ӽ״̬
				break;
			}
			break;
		}
		// ӽ
		case box_calibration:{
			static float max_sum_weight_normalization = 0;				// һȨ
			/* Ч */
			dog_rwr.radar_scanning_enable_flag = False;
			dog_rwr.radar_tracking_enable_flag = True;
			dog_rwr.missile_launch_enable_flag = False;		
			// ûյǰһֱͿʼʶ𴮿ڣֹAIͷûյ
			supplement_lamp(0);		// /ز
			if( (ai_camera_1_enable_flag == True && ai_camera_2_enable_flag == True && detection_result.tool_detection_finsh_flag == False && detection_result.num_detection_finsh_flag == False) ||
				(ai_camera_1_enable_flag == True && ai_camera_2_enable_flag == False && detection_result.tool_detection_finsh_flag == False && detection_result.num_detection_finsh_flag == False) ||
				(ai_camera_1_enable_flag == False && ai_camera_2_enable_flag == True && detection_result.num_detection_finsh_flag == False && detection_result.tool_detection_finsh_flag == False)){
				/* Ϳʼʶ𴮿 */
				uint8 detection_start[5] = {0x4f, 0x16, 0x08, 0x76, 0x76};
//				system_delay_ms(50);
				uart_write_buffer(AI_CAMERA_1_UART_INDEX, detection_start, sizeof(detection_start));
				#ifndef AI_CAMERA_MERGE
				uart_write_buffer(AI_CAMERA_2_UART_INDEX, detection_start, sizeof(detection_start));
				#endif
			}
			// ѯʽȴͷʶ
			if(	(ai_camera_1_enable_flag == True && ai_camera_2_enable_flag == True && detection_result.tool_detection_finsh_flag == True && detection_result.num_detection_finsh_flag == True) ||
				(ai_camera_1_enable_flag == True && ai_camera_2_enable_flag == False && detection_result.tool_detection_finsh_flag == True && detection_result.num_detection_finsh_flag == False) ||
				(ai_camera_1_enable_flag == False && ai_camera_2_enable_flag == True && detection_result.num_detection_finsh_flag == True && detection_result.tool_detection_finsh_flag == False) ||
				(ai_camera_1_enable_flag == False && ai_camera_2_enable_flag == False)){
				supplement_lamp(1);		// /
				/* ˶ */
				control_kind = Inv2Speed; 			// ÿ
				move_solve_kind = XY_SPEED_SOLVE;	// ý
				// ŷǽ
				box_euler_angle_solve.solve_flag = True;
				// Ŀٶȣdir1ƣdir-1ƣ
				if((detection_result.tool >= 0X01 && detection_result.tool <= 0X08) || ((detection_result.num&0X01) == 0 && detection_result.tool == 0X10)) box_dir = -1;	
				else if((detection_result.tool >= 0X09 && detection_result.tool <= 0X0F) || ((detection_result.num&0X01) == 1 && detection_result.tool == 0X10)) box_dir = 1;
				else box_dir = 1;
				// ֹͷʧʱұ䣨Ҳ֪Ϊɶݻ
				if(ai_camera_1_enable_flag == False && ai_camera_2_enable_flag == False)
					box_dir = 1;
				x_speed_slow_change(box_dir*box_x_speed_target, 0.012, True);	// X
				y_speed_target = 0;
				angular_speed_target = -box_dir*fabsf(x_speed_target)*box_x_angular_speed_rate;	// ʵֵᵼתٶȺXٶȲƥ䣬Ӷת
				// Գƶ
				symmetry_rectificate(dog_cv.image_OTSU);
				// һȨӦǣȡȨͶӦĽǶȽϴǸ
				// 㲻
				if(frame_white_num_normalization[0] >= frame_white_num_normalization_limit && frame_white_num_normalization[1] >= frame_white_num_normalization_limit && sum_weight_normalization >= sum_weight_normalization_limit[1]){					
					if(sum_weight_normalization >= max_sum_weight_normalization){	// Ȩʹ֮ǰ
						max_sum_weight_normalization = sum_weight_normalization;
						max_sum_weight_normalization_yaw = box_euler_angle_solve.yaw;
					}
				}
				// жǷ
				if(sum_weight_normalization >= sum_weight_normalization_limit[0] && frame_white_num_normalization[0] >= frame_white_num_normalization_limit && frame_white_num_normalization[1] >= frame_white_num_normalization_limit)
				{
					/* ־λ */
					box_euler_angle_solve.solve_flag = False;	// رŷǽ
					/* м */ 
					max_sum_weight_normalization_yaw = 0;		// һȨͶӦĺ
					max_sum_weight_normalization = 0;			// һȨ
					/* ͣ */
					control_kind = Speed;			// ÿģʽ
					wheel_speed_target[0] = 0;
					wheel_speed_target[1] = 0;
					wheel_speed_target[2] = 0;
					/* ״̬л */
					path_state = box_second_track;	// ״̬
					/* 洢ʶ */
					detection_result_list[detection_result_num] = detection_result;
					detection_result_num++;
					break;
				}
				// ת180ûﵽֵͽ״̬
				if(fabsf(box_euler_angle_solve.yaw) > 180)
				{
					/* м */ 
					max_sum_weight_normalization = 0;		// һȨ
					/* ˶ */
					control_kind = Inv2Speed; 				// ÿ
					move_solve_kind = XY_SPEED_SOLVE;		// ý
					x_speed_target = 0;	// ûٳʼֵ
					angular_speed_target = 0;
					/* ״̬л */
					path_state = box_inv_calibration;		// ״̬
					/* 洢ʶ */
					detection_result_list[detection_result_num] = detection_result;
					detection_result_num++;
					break;
				}
			}
			break;
		}
		// 
		case box_inv_calibration:{
			/* Ч */
			dog_rwr.radar_scanning_enable_flag = False;
			dog_rwr.radar_tracking_enable_flag = True;
			dog_rwr.missile_launch_enable_flag = False;	
			
			/* ˶ */
			control_kind = Inv2Speed; 				// ÿ
			move_solve_kind = XY_SPEED_SOLVE;		// ý
			x_speed_slow_change(-box_dir*box_x_speed_target, 0.012, True);	// X
			y_speed_target = 0;
			angular_speed_target = box_dir*fabsf(x_speed_target)*box_x_angular_speed_rate;
			
			// ֹʱΪ0Ĭת90
			if(max_sum_weight_normalization_yaw == 0){
				max_sum_weight_normalization_yaw = 90;
			}
			if(	(box_euler_angle_solve.yaw >= max_sum_weight_normalization_yaw && max_sum_weight_normalization_yaw < 0) || /* þֵһֱת⣬Ϊֵʧȥ˸ᣬʹyawСʱһ˲ֱ */
				(box_euler_angle_solve.yaw <= max_sum_weight_normalization_yaw && max_sum_weight_normalization_yaw > 0))
			{
				box_euler_angle_solve.solve_flag = False;	// رŷǽ
				/* м */ 
				max_sum_weight_normalization_yaw = 0;	// һȨͶӦĺ
				/* ͣ */
				control_kind = Speed;			// ÿģʽ
				wheel_speed_target[0] = 0;
				wheel_speed_target[1] = 0;
				wheel_speed_target[2] = 0;
				/* ״̬л */
				path_state = box_second_track;	// ״̬
				/* ־λ */
				box_X_finsh_flag = False;		// Xλɱ־λ
				break;
			}
			break;
		}
		// Ӷζλ
		case box_second_track:{
			/* Ч */
			dog_rwr.radar_scanning_enable_flag = False;
			dog_rwr.radar_tracking_enable_flag = False;
			dog_rwr.missile_launch_enable_flag = True;		
			/* ˶ */
			control_kind = Track_X2Inv2Speed;
			angular_speed_target = 0;
			if(box_X_finsh_flag == True){
				box_X_finsh_flag = False;
				/* ͣ */
				control_kind = Speed;		// ÿģʽ
				wheel_speed_target[0] = 0;
				wheel_speed_target[1] = 0;
				wheel_speed_target[2] = 0;
				path_state = box_fxxk;		// ״̬
				
				y_speed_target = 30;			// ӻٶ
				break;
			}
			break;
		}
		// ״̬
		case box_fxxk:{	// Ϊֹ˲ƫת˼
			static int8 num = 0;
			/* Ч */
			dog_rwr.radar_scanning_enable_flag = False;
			dog_rwr.radar_tracking_enable_flag = False;
			dog_rwr.missile_launch_enable_flag = True;	
			/* ־λ */
			static _bool_ is_in_track = False;	// Ƿ־λ
			rotate_euler_angle_solve.solve_flag = True;	// תŷǽ
			/* Xλ */
			float detection_box_center_err = detection_box_center_x-AI_CAMERA_0_IMAGE_WIDTH/2; 
			float X_Kp[4] = { BOX_TRACK_X_PID[0][0], BOX_TRACK_X_PID[1][0], BOX_TRACK_X_PID[2][0], BOX_TRACK_X_PID[3][0] };
			float X_Ki[4] = { BOX_TRACK_X_PID[0][1], BOX_TRACK_X_PID[1][1], BOX_TRACK_X_PID[2][1], BOX_TRACK_X_PID[3][1] };
			float X_Kd[4] = { BOX_TRACK_X_PID[0][2], BOX_TRACK_X_PID[1][2], BOX_TRACK_X_PID[2][2], BOX_TRACK_X_PID[3][2] };
			float X_i_limit[4] = { BOX_TRACK_X_PID[0][3], BOX_TRACK_X_PID[1][3], BOX_TRACK_X_PID[2][3], BOX_TRACK_X_PID[3][3] };
			float X_output_limit[4] = { BOX_TRACK_X_PID[0][4], BOX_TRACK_X_PID[1][4], BOX_TRACK_X_PID[2][4], BOX_TRACK_X_PID[3][4] };
			
			/* ˶ */
			control_kind = Fxxk_X2Inv2Speed;		// ÿģʽ
//			y_speed_target = box_fxxk_y_speed_target[plan_idx];
			angular_speed_target = -PATH_PID[plan_idx][0][5]*imu660ra.gyro_z;
			y_speed_slow_change(box_fxxk_y_speed_target[plan_idx], 0.012, True);		// Ŀٶ  
			/* жǷ */
			if(gray_sensor.voltage < 0.3 && num < 22)	// ڼ
				num++;
			if(gray_sensor.voltage > 0.6 && num < 22){	// ڼԼ
				num--;
				if(num < 2)
					num = 2;
			}
			if(num > 20)
				is_in_track = True;
			if(is_in_track == True && gray_sensor.voltage > 0.6)
			{
				/* ͣ */
				control_kind = Speed;		// ÿģʽ
				wheel_speed_target[0] = 0;
				wheel_speed_target[1] = 0;
				wheel_speed_target[2] = 0;
				
				y_speed_target = 0;		// ѭYٶȻٶ
				/*  */
				is_in_track = False;	
				num = 0;
				// ¼ʱ
				last_box_world_x[box_num] = displacement_solve.world_x_displacement;
				last_box_world_y[box_num] = displacement_solve.world_y_displacement;
				box_num++;
				box_fxxk_finsh_distance = displacement_solve.distance;	// ¼ʱ·
				/* ־λ */
				box_X_finsh_flag = False;
				rotate_euler_angle_solve.solve_flag = False;	// رŷǽ
				/* ״̬л */
				path_state = path_back;	//   ״̬
				break;
			}
			break;
		}
		// ״̬
		case path_back:{
			if(fabsf(displacement_solve.distance-box_fxxk_finsh_distance) >= 30){
				/* Ч */
				dog_rwr.radar_scanning_enable_flag = False;
				dog_rwr.radar_tracking_enable_flag = False;
				dog_rwr.missile_launch_enable_flag = True;	
				/* ־λ */
				rotate_euler_angle_solve.solve_flag = True;	// תŷǽ
				
				/* ˶ */
				control_kind = Angle2Inv2Speed;		// ÿģʽ
				move_solve_kind = XY_SPEED_SOLVE;	// ý
				x_speed_target = 0;
				y_speed_target = 0;
				angle_rotation_yaw_target = box_dir*95;
				
				if(angle_rotate_finsh_flag == True){
					rotate_euler_angle_solve.solve_flag = False;	// رŷǽ
					angle_rotate_finsh_flag = False;				// ʼתɱ־λ
					/* ͣ */
					control_kind = Speed;		// ÿģʽ
					wheel_speed_target[0] = 0;
					wheel_speed_target[1] = 0;
					wheel_speed_target[2] = 0;
					
					y_speed_target = 60;		// ѭYٶȻٶ
					/*  */
					dog_path.mid_x = MT9V03X_W/2;						// ѭʼе㣨һҪȻԭصͷ
					// 
					dog_path.longest_white_col(&dog_path, dog_cv.image_OTSU, control_point[1]);
					// ·ȡ
					dog_path.path_extract(&dog_path, dog_cv.image_OTSU);
					/* ״̬л */
					path_state = last_path_state;
					break;
				}
				break;
			}
			/* ˶ */
			control_kind = Inv2Speed;		// ÿģʽ
			x_speed_target = 0;
			y_speed_slow_change(-box_fxxk_y_speed_target[plan_idx], 0.012, True);		// ûĿٶ  
			angular_speed_target = -PATH_PID[plan_idx][0][5]*imu660ra.gyro_z;	// ע⼫
			angle_rotate_finsh_flag = False;				// ʼתɱ־λ
			break;
		}
		default: break;
	}
	/***********************************************************************************/
	
	// ѭ߿
	if(path_state == common_path || path_state == L_circle || path_state == R_circle || path_state == zebra_path){
		supplement_lamp(1);		// /
		if(out_last_box_circle_flag == True){
			dog_rwr.radar_scanning_enable_flag = True;
			dog_rwr.radar_tracking_enable_flag = False;
		}
		else{
			dog_rwr.radar_scanning_enable_flag = False;
			dog_rwr.radar_tracking_enable_flag = True;
		}
		dog_rwr.missile_launch_enable_flag = False;
		
		// Բ
		if(path_state == L_circle || path_state == R_circle){
			// ѭ
			path_err = dog_path.path[control_point[3]][0]-MT9V03X_W/2;	
			path_pid_calc();
			if(displacement_solve.y_speed >= circle_y_speed_target[plan_idx]*circle_x_speed_enable_y_speed_rate){
				x_speed_target = angular_speed_target*x_speed_rate;
			}
			else{
				x_speed_target = 0;
			}
			y_speed_slow_change(circle_y_speed_target[plan_idx], 0.011, True);				// Y
		}
		// ͨ/
		else{
			y_speed_slow_change(path_y_speed_target[plan_idx], 0.013, True);					// Y
			if(displacement_solve.y_speed >= path_y_speed_target[plan_idx]*path_x_speed_enable_y_speed_rate){
				// ̬ǰհ
				auto_control_point();
				// 
				dog_path.longest_white_col(&dog_path, dog_cv.image_OTSU, longest_white_control_point);
				// ѭ
				path_err = dog_path.longest_white_col_x-MT9V03X_W/2;
				path_pid_calc();
				x_speed_target = angular_speed_target*x_speed_rate;
				
			}
			else{	// ڻٹʹöǰհ
				// 
				dog_path.longest_white_col(&dog_path, dog_cv.image_OTSU, control_point[0]);
				// ѭ
				path_err = dog_path.longest_white_col_x-MT9V03X_W/2;
				path_pid_calc();
				x_speed_target = 0;
			}
		}
	}
}

/* ״̬ж */
static _path_state_ path_state_judge(uint8 input[MT9V03X_H][MT9V03X_W]){
	_path_state_ path_state_return = common_path;
	
	// ֤һζλ״̬˳ܹлԭ״̬
	if(path_state == box_first_track)
		path_state_return = last_path_state;
	
	// Բж
	if(circle_enable_flag == True){
		uint16 circle_check[2] = {0};	// Բ
		uint16 L_side_X_delta_max = 0;	// Xֵ
		uint16 R_side_X_delta_max = 0;	// ұXֵ

		// Բ
		// 
		for(uint16 X = dog_path.mid_x;X >= 0;X--)
		{
			if(input[circle_check_y-1][X] == 0)    // ɫ
			{
				// 洢ɫ
				circle_check[0] = X+1;
				break;
			}
			if(X == 0)
			{
				// 洢ɫ
				circle_check[0] = 0;
				break;
			}
		}
		// ұ
		for(int X = dog_path.mid_x;X <= MT9V03X_W-1;X++)
		{
			if(input[circle_check_y-1][X] == 0)    // ɫ
			{
				// 洢ɫ
				circle_check[1] = X-1;
				break;
			}
			if(X == MT9V03X_W-1)
			{
				// 洢ɫ
				circle_check[1] = MT9V03X_W-1;
				break;
			}
		}

		// ұֵ
		for(int num = 1;num < path_start-path_end;num++)
		{
			// 
			if(abs((dog_path.path[num+1][0]+dog_path.path_width[num+1]/2)-(dog_path.path[num][0]+dog_path.path_width[num]/2)) >= R_side_X_delta_max)
			{
				R_side_X_delta_max = abs((dog_path.path[num+1][0]+dog_path.path_width[num+1]/2)-(dog_path.path[num][0]+dog_path.path_width[num]/2));
			}
			// 
			if(abs((dog_path.path[num+1][0]-dog_path.path_width[num+1]/2)-(dog_path.path[num][0]-dog_path.path_width[num]/2)) >= L_side_X_delta_max)
			{
				L_side_X_delta_max = abs((dog_path.path[num+1][0]-dog_path.path_width[num+1]/2)-(dog_path.path[num][0]-dog_path.path_width[num]/2));
			}
		}
		
		// Բ뻷
		// 1.Ѿͼ߽磻2.뻷ԲСһֵ3.뻷ԲͼԵ/СһֵֹʮУ4.뻷ԲڱXֵСһֵֹʮУ5.뻷ڱXֵһֵֹʮУ
		if(circle_check[1] == MT9V03X_W-1 && dog_path.L_bend_point_num <= 3 && ((float)dog_path.L_frame_point_num/(float)dog_path.L_side_point_num) <= 0.15 &&  L_side_X_delta_max <= side_x_delta_range[0] && R_side_X_delta_max >= side_x_delta_range[1] && path_state == common_path)
		{
			if(displacement_solve.distance-circle_out_distance >= circle_in_distance_limit){	// 󳬹1.5mٴ뻷
				path_state_return = R_circle_in;
			}
		}
		// Բ
		else if(circle_check[1] == MT9V03X_W-1 && path_state == R_circle)
		{
			if(displacement_solve.distance-circle_in_distance >= circle_out_distance_limit){	// 뻷󳬹1mٴγ
				path_state_return = R_circle_out;
			}
		}
		// Բ뻷
		// 1.Ѿͼ߽磻2.뻷ԲСһֵ3.뻷ԲͼԵ/СһֵֹʮУ4.뻷ԲڱXֵСһֵֹʮУ5.뻷ڱXֵһֵֹʮУ
		else if(circle_check[0] == 0 && dog_path.R_bend_point_num <= 3 && ((float)dog_path.R_frame_point_num/(float)dog_path.R_side_point_num) <= 0.15 && R_side_X_delta_max <= side_x_delta_range[0] && L_side_X_delta_max >= side_x_delta_range[1] && path_state == common_path)
		{
			if(displacement_solve.distance-circle_out_distance >= circle_in_distance_limit){	// 󳬹1.5mٴ뻷
				path_state_return = L_circle_in;
			}
		}
		// Բ
		else if(circle_check[0] == 0 && path_state == L_circle)
		{
			if(displacement_solve.distance-circle_in_distance >= circle_out_distance_limit){	// 뻷󳬹1mٴγ
				path_state_return = L_circle_out;
			}
		}
		
	}
	// ж
	if(zebra_enable_flag == True && path_state == common_path){
		zebra_path_timer.ticking_flag = True;
		uint16 black_white_point_num = 0;	// ڰ
		if(zebra_path_timer.time >= 5000)
		{
			for(int X = 0+25;X < MT9V03X_W-1-25;X++)
			{
				if((dog_cv.image_OTSU[MT9V03X_H-10][X] == 255 && dog_cv.image_OTSU[MT9V03X_H-10][X+1] == 0) || (dog_cv.image_OTSU[MT9V03X_H-10][X] == 0 && dog_cv.image_OTSU[MT9V03X_H-10][X+1] == 255))
				{
					black_white_point_num++;
				}
			}
			if(black_white_point_num >= 12)
			{
				path_state_return = zebra_path;
				zebra_stop_distance = displacement_solve.distance;
			}
		}
	}
	// ״̬ж
	// һζλȳֵڷΧʱ붨λ״̬  ߶ȳֵʱ״̬ ֹͼԵ޷붨λ״̬Ӷӹӣ
	if(ai_camera_0_enable_flag == True && ((detection_box_width >= detection_box_width_limit && abs(detection_box_center_x-AI_CAMERA_0_IMAGE_WIDTH/2) <= detection_box_center_x_limit) || detection_box_height >= detection_box_height_limit)){
		uint8 out_box_region_num = 0;	// ԲѾƹӶλ״̬
//		float now_last_yaw = 0;
//		float delta_x = 0;
//		float delta_y = 0;
		for(uint8 i = 0;i < box_num;i++){
			// XY
//			delta_x = displacement_solve.world_x_displacement-last_box_world_x[i];
//			delta_y = displacement_solve.world_y_displacement-last_box_world_y[i];
//			// һڵǰĽǶ
//			if(displacement_solve.world_y_displacement-last_box_world_y[i] != 0)
//				now_last_yaw = RAD2DEG(atan(delta_x/delta_y));
//			else if(delta_x >= 0 && delta_y == 0)
//				now_last_yaw = 0;
//			else if(delta_x < 0 && delta_y == 0)
//				now_last_yaw = -180;
//			// Ƕ
//			if(delta_x > 0 && delta_y <0)
//				now_last_yaw = now_last_yaw+180;
//			else if(delta_x < 0 && delta_y <0)
//				now_last_yaw = now_last_yaw-180;
			if( PYTHAGOREAN((displacement_solve.world_x_displacement-last_box_world_x[i]),(displacement_solve.world_y_displacement-last_box_world_y[i])) >= box_distance)	/* һƹӵһ뾶Բ */
				out_box_region_num++;
//			if( PYTHAGOREAN((displacement_solve.world_x_displacement-last_box_world_x[i]),(displacement_solve.world_y_displacement-last_box_world_y[i])) < box_distance &&	/* һƹӵһ뾶Բ */
//				/* ͷ90û */
//				((fabs(displacement_solve.world_yaw-now_last_yaw) >= 90 && fabsf(now_last_yaw) <= 90) ||		/* ϸϵڳһ */
//				((fabs(displacement_solve.world_yaw-now_last_yaw) >= 90 || displacement_solve.world_yaw < (270+now_last_yaw)) && now_last_yaw < -90) || 	/* ϸϵڳĵ */
//				((fabs(displacement_solve.world_yaw-now_last_yaw) >= 90 || displacement_solve.world_yaw > (-270+now_last_yaw)) && now_last_yaw > 90)))	/* ϸϵڳĵ */
//				out_box_region_num++;		
		}
		if(out_box_region_num == box_num){
			// ʶȻ߶ȳֵӶλ״̬
			path_state_return = box_first_track;
			out_last_box_circle_flag = False;
			// ֻҪһζλ״̬У״ֻ̬Ҫ֣״̬лϸ״̬
		}
		else
			out_last_box_circle_flag = True;
	}

	// ״̬
	switch(path_state){
		case R_circle_in:{ path_state_return = R_circle_in; break; }
		case L_circle_in:{ path_state_return = L_circle_in; break; }
		case R_circle:{ if(path_state_return != R_circle_out && path_state_return != box_first_track)path_state_return = R_circle; break; }	// ڱ R_circle ״̬ᵼ޷л R_circle_out ״̬Ҫ
		case L_circle:{ if(path_state_return != L_circle_out && path_state_return != box_first_track)path_state_return = L_circle; break; }	// ڱ L_circle ״̬ᵼ޷л L_circle_out ״̬Ҫ
		case R_circle_out:{ path_state_return = R_circle_out; break; }
		case L_circle_out:{ path_state_return = L_circle_out; break; }
		case zebra_path:{ path_state_return = zebra_path; break; }						//  zebra_path ״ֱ̬·ֵ̳ͣл zebra_path_stop ״̬
		case box_first_track:{ if(path_state_return == box_first_track)path_state_return = box_first_track; break; }			//  box_first_track ״̬ǰ᣺жϲcommon_path/R_circle/L_circleԴ˱֤;Ӻܼѭ߻Ĵֵڣ򱣳һζλ״̬
		case box_calibration:{ path_state_return = box_calibration; break; }			//  box_calibration ״̬
		case box_inv_calibration:{ path_state_return = box_inv_calibration; break; }	//  box_inv_calibration ״̬
		case box_second_track:{ path_state_return = box_second_track; break; }			//  box_second_track ״̬
		case box_fxxk:{ path_state_return = box_fxxk; break; }							//  box_fxxk ״̬				
		case path_back:{ path_state_return = path_back; break; }						//  path_back ״̬
		default: break;
	}
	// ¼״̬ǰһ״̬
	if(path_state == common_path || path_state == R_circle || path_state == L_circle){
		last_path_state = path_state;
	}
	
	return path_state_return;
}

/* ԳƷʼ */
void symmetry_rectificate_init(void)
{
	sum_weight = 0;
	for(int X = MT9V03X_W/2;X >= 0;X--)
	{
		for(int Y = symmetry_rectificate_end_y;Y < symmetry_rectificate_start_y;Y++)
		{
			if(MT9V03X_W/2-X >= 0 && MT9V03X_W/2-X < MT9V03X_W/8)	
				sum_weight += rectificate_weight[0];
			else if(MT9V03X_W/2-X >= MT9V03X_W/8 && MT9V03X_W/2-X < 2*MT9V03X_W/8)	
				sum_weight += rectificate_weight[1];
			else if(MT9V03X_W/2-X >= 2*MT9V03X_W/8 && MT9V03X_W/2-X < 3*MT9V03X_W/8)	
				sum_weight += rectificate_weight[2];
			else 
				sum_weight += rectificate_weight[3];			
		}
	}
}

/* ԳƷ */
void symmetry_rectificate(uint8 input[MT9V03X_H][MT9V03X_W])
{
	// ԳƷȨغ
	uint32 sum_weight_rectificate = 0;
	// ͼ߿ɫ
	int16 L_white_num = 0;
	int16 R_white_num = 0;
	
	for(int X = MT9V03X_W/2;X >= 0;X--)
	{
		for(int Y = symmetry_rectificate_end_y;Y < symmetry_rectificate_start_y;Y++)
		{
			if(input[Y][X] == input[Y][MT9V03X_W-1-X])
			{
				if(MT9V03X_W/2-X >= 0 && MT9V03X_W/2-X < MT9V03X_W/8)	
					sum_weight_rectificate += rectificate_weight[0];
				else if(MT9V03X_W/2-X >= MT9V03X_W/8 && MT9V03X_W/2-X < 2*MT9V03X_W/8)	
					sum_weight_rectificate += rectificate_weight[1];
				else if(MT9V03X_W/2-X >= 2*MT9V03X_W/8 && MT9V03X_W/2-X < 3*MT9V03X_W/8)	
					sum_weight_rectificate += rectificate_weight[2];
				else 
					sum_weight_rectificate += rectificate_weight[3];			
			}
		}
	}
	for(int Y = symmetry_rectificate_end_y;Y < symmetry_rectificate_start_y;Y++)
	{
		if(input[Y][frame_offset] == 255)
			L_white_num++;
		if(input[Y][MT9V03X_W-1-frame_offset] == 255)
			R_white_num++;
	}
	sum_weight_normalization = (float)sum_weight_rectificate/(float)sum_weight;
	frame_white_num_normalization[0] = (float)L_white_num/(float)(symmetry_rectificate_start_y-symmetry_rectificate_end_y+1);
	frame_white_num_normalization[1] = (float)R_white_num/(float)(symmetry_rectificate_start_y-symmetry_rectificate_end_y+1);
}

/* ̬ǰհ */
uint16 auto_control_point(void)
{
	float gyro_z_normalization = 0;
	if(fabsf(imu660ra.gyro_z) > auto_control_point_normalize_range[0])
		gyro_z_normalization = (fabsf(imu660ra.gyro_z)-auto_control_point_normalize_range[0])/(auto_control_point_normalize_range[1]-auto_control_point_normalize_range[0]);
	if(gyro_z_normalization > 1)
		gyro_z_normalization = 1;
	longest_white_control_point = control_point[2]-(control_point[2]-control_point[1])*gyro_z_normalization;
}