/*
 * fsm.c
 *
 *  Created on: Oct 18, 2024
 *      Author: akeguo
 */

#include "fsm.h"

#include "BHBF_ROBOT.h"
#include "bsp_include.h"
#include "robot_state.h"

int32_t *RobotSpeed;

//int32_t JontSwingSpeed;
//int32_t JontTiltSpeed;
int32_t JontSwingSpeed=400;
int32_t JontTiltSpeed=400;
void action_perfrom(transition_t transitions[], int length, int state);
void GF_Dispatch();
void action_perfrom();
MoveSTATE_t CurrentMoveState;
SwingSTATE_t CurrentSwingState;
TiltSTATE_t CurrentTiltState; //点头，抬头
SetSTATE_t CurrentSetState; //设置
int index_counter = 0;
int polish_Flag=0;//打磨标志
int Polish_Count = 0;//打磨启停上升沿计数
int AutoSpray_Stop_Count = 0; //自动喷漆结束计数
int AutoSpray_Flag=0;//喷枪标志

int USValue_Flag1 = 0; //超声波探边计数
int USValue_Flag2 = 0;
//手动下 机器人运动
transition_t MoveTransitions[] =
{
{ Move_Forwards, Forwards_State_Do },
{ Move_Backwards, Backwards_State_Do },
{ Move_TurnLeft, TurnLeft_State_Do },
{ Move_TurnRight, TurnRight_State_Do },
{ Move_HALT, HALT_State_Do },
{ Move_AutoForward, Auto},
{ Move_AutoBackward, Auto},
{ Move_ChgLeft, ChgLeft},
{ Move_ChgRight, ChgRight},
{ Move_ChgUp, ChgUp},
{ Move_ChgDown, ChgDown},
{ Move_ChgFinish, ChgFinish},
{ Move_Emergency, HALT_State_Do},
};

transition_t SwingTransitions[] =
{

{ SwingHALT, SwingHALT_State_Do },
{ SwingLeft, SwingLeft_State_Do },
{ SwingRight, SwingRight_State_Do },
{ SwingHome, SwingHome_Do }, };

transition_t SetTransitions[] =
{

{ OtherMode, OtherMode_State_Do },
{ SetLaneChangeDistance, LaneChangeDistance_Setting_State_Do },
{ SetBackwardsDistance, BackWardsDistance_Setting_State_Do },

};

transition_t TiltTransitions[] =
{

{ TiltHALT, TiltHalt_Do },
{ TiltUP, TiltUp_Do },
{ TiltDown, TiltDown_Do },
{ TiltHome, TiltHome_Do },
{ TiltCurrentModeDown, TiltCurrentModeDown_Do },
};

void Fsm_Init()
{
	GF_BSP_Interrupt_Add_CallBack(
			DF_BSP_InterCall_TIM8_2ms_PeriodElapsedCallback, GF_Dispatch);
}
//状态迁移

//double Y_Value_1;
//double X_Value_1;
//double Rocker_angle;

int JJ_Flag;
int Speed_Judge(T_F4_Value* T_F4)
{
	T_F4_Value* m = T_F4;
	if (m->Speed_Level_1 == 1)
		return 1;
	else if (m->Speed_Level_2 == 1)
		return 3;
	else if (m->Speed_Level_3 == 1)
		return 5;
	else if (m->Speed_Level_4 == 1)
		return 8;
	else if (m->Speed_Level_5 == 1)
		return 12;
	else if (m->Speed_Level_6 == 1)
		return 14;
	else if (m->Speed_Level_7 == 1)
		return 16;
	else if (m->Speed_Level_8 == 1)
		return 18;
	else if (m->Speed_Level_9 == 1)
		return 20;
	else if (m->Speed_Level_10 == 1)
		return 22;
}
//--喷漆自动开启
void AutoForSpray_On()
{
	CurrentMoveState = Move_AutoForward;
	if(GV.AuTo_Flag == 0)
	{
		GV.AuTo_Flag=1;
	}

	if(AutoSpray_Flag == 0
			&& Current_T_F4_Value.Spray_On
			&& GV.LeftFrontMotor.Velcity >= GV.Robot_AutoSpeed - 100
			&& GV.RightFrontMotor.Velcity <= -GV.Robot_AutoSpeed + 100)
	{
	    GF_BSP_GPIO_SetIO(Out_Spray,0);
		AutoSpray_Flag = 1;
	}
}
void AutoBackSpray_On()
{
	CurrentMoveState = Move_AutoBackward;
	if(GV.AuTo_Flag == 0)
	{
		GV.AuTo_Flag=3;
	}

	if(AutoSpray_Flag == 0
			&& Current_T_F4_Value.Spray_On
			&& GV.RightFrontMotor.Velcity >= GV.Robot_AutoSpeed - 100
			&& GV.LeftFrontMotor.Velcity <= -GV.Robot_AutoSpeed + 100)
	{
	    GF_BSP_GPIO_SetIO(Out_Spray,0);
		AutoSpray_Flag = 1;
	}
}
//--
//--车体停
void Off_AutoSpray()
{
	//喷漆自动关闭
	if(AutoSpray_Flag == 1)
	{

		GF_BSP_GPIO_SetIO(Out_Spray,1);
		AutoSpray_Stop_Count++;
	}
	//喷漆程序结束

	//若自动喷漆启动，计数AutoSpray_Stop_Count*2ms后再停车
	if(AutoSpray_Flag == 1)
	{
		if(AutoSpray_Stop_Count <= 300)
		{
			AutoSpray_Flag = 0;
			AutoSpray_Stop_Count = 0;
			CurrentMoveState = Move_HALT;  //停车
		}
	}
	else
	{
		CurrentMoveState = Move_HALT;  //停车
	}

	//停车，禁止其它置位
	if(Current_T_F4_Value.AutoForWard == 1
			|| Current_T_F4_Value.AutoBackWard == 1)
	{
		GV.AuTo_Flag = 99;
	}
	else
	{
		GV.AuTo_Flag = 0;
	}

	if(Current_T_F4_Value.ChgDown_Ver == 1 || Current_T_F4_Value.ChgUp_Ver == 1
		 || Current_T_F4_Value.ChgLeft_Hor == 1 || Current_T_F4_Value.ChgRight_Hor == 1)
	{
		GV.Chg_Flag = 1;
		Chg_Pa.change_road_finish_flag = 1;

	}
	else
	{
		GV.Chg_Flag = 0;
		Chg_Pa.change_road_finish_flag = 0;
	}

}
//--
//--探边检测
void Dangerous_Test()
{
	//计数，置位标志
	//前进探边计数
	if(GV.Robot_Ultrasonic.USValue_1 >= 5000 || GV.Robot_Ultrasonic.USValue_1 == 0)
	{
		GV.Robot_Ultrasonic.USValue_Count_1 ++;
		if(GV.Robot_Ultrasonic.USValue_Count_1 >= 1)
		{
			GV.Robot_Ultrasonic.USValue_Count_1 = 1;
			GV.Robot_Ultrasonic.USValue_Flag_1 = 1;
		}
	}
	else
	{
		GV.Robot_Ultrasonic.USValue_Count_1  = 0;
	}
	//后退探边计数
	if(GV.Robot_Ultrasonic.USValue_2 >= 3000 || GV.Robot_Ultrasonic.USValue_2 == 0)
	{
		GV.Robot_Ultrasonic.USValue_Count_2 ++;
		if(GV.Robot_Ultrasonic.USValue_Count_2 >= 1)
		{
			GV.Robot_Ultrasonic.USValue_Count_2 = 1;
			GV.Robot_Ultrasonic.USValue_Flag_2 = 1;
		}
	}
	else
	{
		GV.Robot_Ultrasonic.USValue_Count_2  = 0;
	}

	//前进避障计数
	if(GV.Robot_Ultrasonic.USValue_4 <= 5000
			&& GV.Robot_Ultrasonic.USValue_4 != 0)
	{
		GV.Robot_Ultrasonic.USValue_Count_4 ++;
		if(GV.Robot_Ultrasonic.USValue_Count_4 >= 1)
		{
			GV.Robot_Ultrasonic.USValue_Count_4 = 1;
			GV.Robot_Ultrasonic.USValue_Flag_4 = 1;
		}
	}
	else
	{
		GV.Robot_Ultrasonic.USValue_Count_4  = 0;
	}

	//后退避障计数
	if(GV.Robot_Ultrasonic.USValue_3 <= 4000
			&& GV.Robot_Ultrasonic.USValue_3 != 0)
	{
		GV.Robot_Ultrasonic.USValue_Count_3 ++;
		if(GV.Robot_Ultrasonic.USValue_Count_3 >= 1)
		{
			GV.Robot_Ultrasonic.USValue_Count_3 = 1;
			GV.Robot_Ultrasonic.USValue_Flag_3 = 1;
		}
	}
	else
	{
		GV.Robot_Ultrasonic.USValue_Count_3  = 0;
	}

	//根据置位标志，置零遥控器
	if(GV.Robot_Ultrasonic.USValue_Flag_1 >= 1
			|| GV.Robot_Ultrasonic.USValue_Flag_4 >= 1)
	{
		Current_T_F4_Value.AutoForWard = 0;
	}
	if(GV.Robot_Ultrasonic.USValue_Flag_2 >= 1
			|| GV.Robot_Ultrasonic.USValue_Flag_3 >= 1)
	{
		Current_T_F4_Value.AutoBackWard = 0;
	}

}
//--
//--气压检测
void Dym_Test()
{
	if(Current_T_F4_Value.Test_On == 1)
	{
		//计数，置位标志
		//前进探边计数
		if(GV.Robot_Ultrasonic.Dym_Value <= 10300 ||  GV.Robot_Ultrasonic.Dym_Value >= 11000)
		{
			GV.Robot_Ultrasonic.Dym_Value_Count ++;
			if(GV.Robot_Ultrasonic.Dym_Value_Count >= 3)
			{
				GV.Robot_Ultrasonic.Dym_Value_Count = 3;
				GV.Robot_Ultrasonic.Dym_Value_Flag = 1;
				GF_BSP_GPIO_SetIO(Light, 0);
			}
		}
		else
		{
			GV.Robot_Ultrasonic.Dym_Value_Count = 0;
			GF_BSP_GPIO_SetIO(Light, 1);
		}
	}
	else
	{
		GV.Robot_Ultrasonic.Dym_Value_Count = 0;
		GF_BSP_GPIO_SetIO(Light, 1);
	}

	//报警后，等自动程序都复位才能继续
	if(GV.Robot_Ultrasonic.Dym_Value_Count < 3
			&& Current_T_F4_Value.AutoForWard == 0
			&& Current_T_F4_Value.AutoBackWard == 0
			&& Current_T_F4_Value.ChgDown_Ver == 0
			&& Current_T_F4_Value.ChgUp_Ver == 0
			&& Current_T_F4_Value.ChgLeft_Hor == 0
			&& Current_T_F4_Value.ChgRight_Hor == 0)
	{
		GV.Robot_Ultrasonic.Dym_Value_Flag = 0;
	}


}

//--探边置位消除
void Dangerous_Flag_Delete()
{
	if(GV.Robot_Ultrasonic.USValue_Flag_1 >= 1
			&& (CurrentMoveState == Move_Backwards
					|| CurrentMoveState == Move_AutoBackward))
	{
		GV.Robot_Ultrasonic.USValue_Flag_1 ++;
		if(GV.Robot_Ultrasonic.USValue_Flag_1 >= 5)
		{
			GV.Robot_Ultrasonic.USValue_Flag_1 = 0;
		}
	}

	if(GV.Robot_Ultrasonic.USValue_Flag_2 >= 1
			&& (CurrentMoveState == Move_Forwards
					|| CurrentMoveState == Move_AutoForward))
	{
		GV.Robot_Ultrasonic.USValue_Flag_2 ++;
		if(GV.Robot_Ultrasonic.USValue_Flag_2 >= 5)
		{
			GV.Robot_Ultrasonic.USValue_Flag_2 = 0;
		}
	}

	if(GV.Robot_Ultrasonic.USValue_Flag_3 >= 1
			&& (CurrentMoveState == Move_Forwards
					|| CurrentMoveState == Move_AutoForward))
	{
		GV.Robot_Ultrasonic.USValue_Flag_3 ++;
		if(GV.Robot_Ultrasonic.USValue_Flag_3 >= 5)
		{
			GV.Robot_Ultrasonic.USValue_Flag_3 = 0;
		}
	}

	if(GV.Robot_Ultrasonic.USValue_Flag_4 >= 1
			&& (CurrentMoveState == Move_Backwards
					|| CurrentMoveState == Move_AutoBackward))
	{
		GV.Robot_Ultrasonic.USValue_Flag_4 ++;
		if(GV.Robot_Ultrasonic.USValue_Flag_4 >= 5)
		{
			GV.Robot_Ultrasonic.USValue_Flag_4 = 0;
		}
	}

}

//--遥控器急停判断
void RC_Emergency()
{
	if(Current_T_F4_Value.emergency_stop == 1)
	{
		GF_BSP_GPIO_SetIO(Out_Spray,1);
		GF_BSP_GPIO_SetIO(0,1);
		CurrentMoveState = Move_Emergency;
	}
	else
	{
		GF_BSP_GPIO_SetIO(0,0);
	}
}

void GF_Dispatch()//2ms调用一次 给车体速度等赋值
{
	//--遥控器急停判断
	RC_Emergency();
	//--自动作业模式下的探边检测判断
	if(Current_T_F4_Value.AutoForWard == 1
		|| Current_T_F4_Value.AutoBackWard == 1)
	{
		Dangerous_Test();
	}
	//--
	//--气压判断
	Dym_Test();
	//--
	//运动部分 Move Region
		if((Current_T_F4_Value.ChgDown_Ver == 1 || Current_T_F4_Value.ChgUp_Ver == 1
				|| Current_T_F4_Value.ChgLeft_Hor == 1 || Current_T_F4_Value.ChgRight_Hor == 1
				|| Current_T_F4_Value.AutoForWard == 1 || Current_T_F4_Value.AutoBackWard == 1)
				&& (Current_T_F4_Value.robot_forward_backward_speed >= 0X7A
					&& Current_T_F4_Value.robot_forward_backward_speed <= 0X84)
				&& (Current_T_F4_Value.robot_left_right_turn_speed >= 0X7A
					&& Current_T_F4_Value.robot_left_right_turn_speed <= 0X84)
				&& GV.Robot_Ultrasonic.Dym_Value_Flag != 1)
			{
				if((Current_T_F4_Value.AutoForWard == 1 || Current_T_F4_Value.AutoBackWard == 1)
					&&(Current_T_F4_Value.ChgDown_Ver != 1 && Current_T_F4_Value.ChgUp_Ver != 1
						&& Current_T_F4_Value.ChgLeft_Hor != 1 && Current_T_F4_Value.ChgRight_Hor != 1))//自动作业与换道互斥
				{
				   if(Current_T_F4_Value.AutoForWard == 1)
				   {
						//如启停喷枪按下后判断前进速度到达后，喷漆自动开启
						AutoForSpray_On();
						//喷漆程序结束
				   }
				   else if(Current_T_F4_Value.AutoBackWard == 1)
				   {
						//如启停喷枪按下后判断前进速度到达后，喷漆自动开启
						AutoBackSpray_On();
						//喷漆程序结束
				   }
				}
				else if((Current_T_F4_Value.AutoForWard != 1 && Current_T_F4_Value.AutoBackWard != 1)
						&&(Current_T_F4_Value.ChgDown_Ver == 1 || Current_T_F4_Value.ChgUp_Ver == 1
							|| Current_T_F4_Value.ChgLeft_Hor == 1 || Current_T_F4_Value.ChgRight_Hor == 1))
				{
					if(GV.Chg_Flag == 1 && (Current_T_F4_Value.ChgDown_Ver == 1 || Current_T_F4_Value.ChgUp_Ver == 1
						&& Current_T_F4_Value.ChgLeft_Hor == 1 || Current_T_F4_Value.ChgRight_Hor == 1))
					{
						CurrentMoveState = Move_ChgFinish;
					}
					else if(Current_T_F4_Value.ChgUp_Ver == 1
							&& !(Current_T_F4_Value.ChgDown_Ver == 1
									|| Current_T_F4_Value.ChgLeft_Hor == 1
									|| Current_T_F4_Value.ChgRight_Hor == 1))
					{
						CurrentMoveState = Move_ChgUp;
					}
					else if(Current_T_F4_Value.ChgDown_Ver == 1
							&& !(Current_T_F4_Value.ChgUp_Ver == 1
									|| Current_T_F4_Value.ChgLeft_Hor == 1
									|| Current_T_F4_Value.ChgRight_Hor == 1))
					{
						CurrentMoveState = Move_ChgDown;
					}
					else if(Current_T_F4_Value.ChgLeft_Hor == 1
							&& !(Current_T_F4_Value.ChgUp_Ver == 1
									|| Current_T_F4_Value.ChgDown_Ver == 1
									|| Current_T_F4_Value.ChgRight_Hor == 1))
					{
						CurrentMoveState = Move_ChgLeft;
					}
					else if(Current_T_F4_Value.ChgRight_Hor == 1
							&& !(Current_T_F4_Value.ChgUp_Ver == 1
									|| Current_T_F4_Value.ChgDown_Ver == 1
									|| Current_T_F4_Value.ChgLeft_Hor == 1))
					{
						CurrentMoveState = Move_ChgRight;
					}
				}
			}
			else if((Current_T_F4_Value.ChgDown_Ver != 1 && Current_T_F4_Value.ChgUp_Ver != 1
					&& Current_T_F4_Value.ChgLeft_Hor!= 1 && Current_T_F4_Value.ChgRight_Hor != 1
					&& Current_T_F4_Value.AutoForWard != 1 && Current_T_F4_Value.AutoBackWard != 1)
					&&  (Current_T_F4_Value.robot_forward_backward_speed < 0X7A  	//7A 84
						|| Current_T_F4_Value.robot_forward_backward_speed > 0X84
						|| Current_T_F4_Value.robot_left_right_turn_speed < 0X7A
						|| Current_T_F4_Value.robot_left_right_turn_speed > 0X84))
			{
					if (Current_T_F4_Value.robot_forward_backward_speed < 0X7A)//前进
					{
						CurrentMoveState = Move_Forwards;
					}
					else if (Current_T_F4_Value.robot_forward_backward_speed > 0X84)
					{
						CurrentMoveState = Move_Backwards;  //后退
					}

					else if (Current_T_F4_Value.robot_left_right_turn_speed < 0X7A)
					{
						CurrentMoveState = Move_TurnLeft;  //右转
					}
					else if (Current_T_F4_Value.robot_left_right_turn_speed > 0X84)
					{
						CurrentMoveState = Move_TurnRight;  //左转
					}
			}
			else
			{
				Off_AutoSpray();
			}

		//--探边置位消除
		Dangerous_Flag_Delete();
		//--

		/****************************************/
		//LU_Three控制升降
		if(Current_T_F4_Value.Lift_Up == 1)
		{
			GF_BSP_GPIO_SetIO(Out_Lift_2,0);
			GF_BSP_GPIO_SetIO(Out_Lift_1,1);
		}
		else if(Current_T_F4_Value.Lift_Down == 1)
		{
			GF_BSP_GPIO_SetIO(Out_Lift_2,1);
			GF_BSP_GPIO_SetIO(Out_Lift_1,0);
		}
		else
		{
			GF_BSP_GPIO_SetIO(Out_Lift_2,1);
			GF_BSP_GPIO_SetIO(Out_Lift_1,1);
		}

		//控制电磁阀喷枪
		if(Current_T_F4_Value.FA && AutoSpray_Flag == 0)  // && CurrentMoveState != Move_AutoForward&& CurrentMoveState != Move_AutoBackward)
		{
		   GF_BSP_GPIO_SetIO(Out_Spray,0);
		}
		else if(!Current_T_F4_Value.FA && AutoSpray_Flag == 0)  // && CurrentMoveState != Move_AutoForward  && CurrentMoveState != Move_AutoBackward)
		{
		   GF_BSP_GPIO_SetIO(Out_Spray,1);
		}




	//计算速度档位m/min
	GV.Robot_Speed_mpm = Speed_Judge(&Current_T_F4_Value);
	//代入速度m/min * 186.9  最大下发4725 即最大速度25m/min
	GV.Robot_ManualSpeed = abs(GV.Robot_Speed_mpm * 186.9 * ((Current_T_F4_Value.robot_forward_backward_speed - 127)/ 127.0));//RobotSpeed指向GV的移动车体速度 这是整体车速 不是单个轮的
	CV.TurnLeftRightSpeed = abs(2 * 186.9 * (Current_T_F4_Value.robot_left_right_turn_speed - 127 )/ 127.0);//RobotSpeed指向GV的移动车体速度 这是整体车速 不是单个轮的

	GV.Robot_AutoSpeed = (GV.Robot_Speed_mpm)* 186.9;

	GV.Left_Compensation = Current_T_F4_Value.robot_Compensation_Left / 51.0;
	GV.Right_Compensation = Current_T_F4_Value.robot_Compensation_Right / 51.0;

	action_perfrom(MoveTransitions, sizeof(MoveTransitions) / sizeof(transition_t),  CurrentMoveState);//结构体数组/大小/当前状态
	//	action_perfrom(SwingTransitions,sizeof(SwingTransitions) / sizeof(transition_t), CurrentSwingState);
	//	action_perfrom(TiltTransitions, sizeof(TiltTransitions) / sizeof(transition_t),  CurrentTiltState);


}

void action_perfrom(transition_t transitions[], int length, int state)
{

	for (index_counter = 0; index_counter < length; index_counter++)
	{
		if (transitions[index_counter].State == state)
		{
			if (transitions[index_counter].robotRun != NULL)//函数不为空
			{
				transitions[index_counter].robotRun();
			}

			break;
			//return;
		}
	}
}