/*
 * 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 Polish_lock = 0;        // 打磨锁定标志（非自锁）
int Spray_Flag=0;//喷漆标志
int Spray_Count=0;
int Spray_lock = 0;// 喷漆打磨锁定标志（非自锁）
//int Vacuum_Flag = 0;
//int Vacuum_Count = 0;
int Vacuum_Delay_Count = 0;  // Polish和Vacuum之间的延迟计数器
int emergency_lock_count = 0;// 急停：锁定计数器
int Operation_lock = 0;  // 通用锁定标志

int process_state = 0;       // 后台处理状态（0:空闲 1:开启中 2:关闭中）

//手动下 机器人运动
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},

};

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;

void GF_Dispatch()//2ms调用一次 给车体速度等赋值
{
	//	//运动部分 Move Region

	int b=Get_BIT(SystemErrorCode, u7_sbus) ;
	if (Get_BIT(SystemErrorCode, u7_sbus) == 1)
	{


	}


	CurrentMoveState = Move_HALT;
	if(!(P_U7->R1 == -1000 && P_U7->L1 == -1000
			&& P_U7->R2 == -1000 && P_U7->L2 == -1000
			&& P_U7->RU_Three == 0)
			&& (P_U7->CH1_LYB_H >= -300 && P_U7->CH1_LYB_H <= 300)
			&& (P_U7->CH0_LYB_V >= -300 && P_U7->CH0_LYB_V <= 300))
	{
		if(P_U7->RU_Three != 0
				&& P_U7->R1 == -1000 && P_U7->L1 == -1000
				&& P_U7->R2 == -1000 && P_U7->L2 == -1000)	//自动作业与换道互斥
		{
		   if(P_U7->RU_Three == 1000)
		   {
				CurrentMoveState = Move_AutoForward;
				if(GV.AuTo_Flag == 0)
				{
					GV.AuTo_Flag=1;
				}
//				if(Spray_Flag == 0
//						&& GV.LeftFrontMotor.Velcity >= GV.Robot_AutoSpeed - 100
//						&& GV.RightFrontMotor.Velcity <= -GV.Robot_AutoSpeed + 100)
//				{
//				    GF_BSP_GPIO_SetIO(Out_Spray,0);
//					Spray_Flag = 1;
//				}

		   }
		   else if(P_U7->RU_Three == -1000)
		   {
				CurrentMoveState = Move_AutoBackward;
				if(GV.AuTo_Flag == 0)
				{
					GV.AuTo_Flag=3;
				}
//				if(Spray_Flag == 0
//						&& GV.RightFrontMotor.Velcity >= GV.Robot_AutoSpeed - 100
//						&& GV.LeftFrontMotor.Velcity <= -GV.Robot_AutoSpeed + 100)
//				{
//				    GF_BSP_GPIO_SetIO(Out_Spray,0);
//					Spray_Flag = 1;
//				}
		   }
		   else
		   {
				CurrentMoveState = Move_HALT;//停车
		   }
		}
		else if(!(P_U7->R1 == -1000 && P_U7->L1 == -1000
				&& P_U7->R2 == -1000 && P_U7->L2 == -1000)
				&& P_U7->RU_Three == 0)
		{
			if(GV.Chg_Flag == 1 && (P_U7->L1 == 1000 || P_U7->R1 == 1000
					|| P_U7->L2 == 1000 || P_U7->R2 == 1000))
			{
				CurrentMoveState = Move_ChgFinish;
			}
			else if(P_U7->L1 == 1000
					&& P_U7->R1 == -1000 && P_U7->R2 == -1000 && P_U7->L2 == -1000)
			{
				CurrentMoveState = Move_ChgUp;
			}
			else if(P_U7->R1 == 1000
					&& P_U7->L1 == -1000 && P_U7->R2 == -1000 && P_U7->L2 == -1000)
			{
				CurrentMoveState = Move_ChgDown;
			}
			else if(P_U7->L2 == 1000
					&& P_U7->L1 == -1000 && P_U7->R1 == -1000 && P_U7->R2 == -1000)
			{
				CurrentMoveState = Move_ChgLeft;
			}
			else if(P_U7->R2 == 1000
					&& P_U7->L1 == -1000 && P_U7->R1 == -1000 && P_U7->L2 == -1000)
			{
				CurrentMoveState = Move_ChgRight;
			}
			else
			{
				CurrentMoveState = Move_HALT;//停车
			}
		}
		else
		{
			CurrentMoveState = Move_HALT;//停车
		}
	}
	else if( !((P_U7->CH1_LYB_H >= -300 && P_U7->CH1_LYB_H <= 300)
			&&(P_U7->CH0_LYB_V >= -300 && P_U7->CH0_LYB_V <= 300))
			&& P_U7->R1 == -1000 && P_U7->L1 == -1000
			&& P_U7->R2 == -1000 && P_U7->L2 == -1000
			&& P_U7->RU_Three == 0)
	{
		if (P_U7->CH1_LYB_H >= -300 && P_U7->CH1_LYB_H <= 300)//前进周围300范围内才继续检测前进是否按下
		{
			if (P_U7->CH0_LYB_V > 300)//前进
			{
				CurrentMoveState = Move_Forwards;
			}
			else if (P_U7->CH0_LYB_V < -300)
			{
				CurrentMoveState = Move_Backwards;  //后退
			}
			else
			{
//				if(CurrentMoveState == Move_AutoForward
//						|| CurrentMoveState == Move_AutoBackward)
//				{
//					if(Spray_Flag < 8)
//					{
//					    GF_BSP_GPIO_SetIO(Out_Spray,1);
//						Spray_Flag++;
//					}
//					else if(Spray_Flag >= 8)
//					{
//						Spray_Flag = 0;
//						CurrentMoveState = Move_HALT;//停车
//					}
//				}
//				else
//				{
					CurrentMoveState = Move_HALT;//停车
//				}

			}
		}
		else if (P_U7->CH0_LYB_V >= -300 && P_U7->CH0_LYB_V <= 300)
		{
			if (P_U7->CH1_LYB_H > 300)
			{
				CurrentMoveState = Move_TurnRight;  //右转
			}
			else if (P_U7->CH1_LYB_H < -300)
			{
				CurrentMoveState = Move_TurnLeft;  //左转
			}
			else
			{
				CurrentMoveState = Move_HALT;  //停车
			}
		}
		else
		{
			CurrentMoveState = Move_HALT;  //停车
		}
	}
	else
	{
		CurrentMoveState = Move_HALT;  //停车
	}


	/****************************************/

	//LU_Single 控制打磨电机:旧
//	if(P_U7->LU_Single==1000 && Polish_Flag == 0)
//	{
//		GF_BSP_GPIO_SetIO(Out_Polish,0);
//		if(Polish_Count == 100)
//		{
//		   GF_BSP_GPIO_SetIO(Out_Polish,1);
//		   Polish_Flag = 1;
//		}
//		else
//		{
//		   Polish_Count++;
//		}
//	}
//	else if(P_U7->LU_Single==-1000 && Polish_Flag == 1)
//	{
//		GF_BSP_GPIO_SetIO(Out_Polish,0);
//		if(Polish_Count == 100)
//		{
//		   GF_BSP_GPIO_SetIO(Out_Polish,1);
//		   Polish_Flag = 0;
//		}
//		else
//		{
//		   Polish_Count++;
//		}
//	}
//	else
//	{
//		Polish_Count=0;
//	}





//	 版本二：LU_Single 控制打磨电机和吸尘器（非自锁版本）
		if (P_U7->LU_Single == 1000 && Polish_lock == 0 && process_state == 0)
		{
		    Polish_lock = 1;  // 立即锁定，防止重复触发

		    if (Polish_Flag == 0)
		    {
		        process_state = 1;  // 准备开俩设备
		        Vacuum_Delay_Count = 0;            // 开吸尘器前，提前重置上一轮计数
		    }
		    else
		    {
		        process_state = 2;  // 准备关闭俩设备
		        Polish_Count = 100; // 从100开始倒计时
		    }
		}
		else if (P_U7->LU_Single == -1000)
		{
		    Polish_lock = 0;
		}

		if (process_state == 1)
		{
			GF_BSP_GPIO_SetIO(Out_Vacuum, 0);
		    if (Vacuum_Delay_Count < 200)
		    {
		        Vacuum_Delay_Count++;
		    }
		    else
		    {
		        GF_BSP_GPIO_SetIO(Out_Polish, 0);

		        if (Polish_Count >= 100)
		        {
		            GF_BSP_GPIO_SetIO(Out_Polish, 1);
		            Polish_Flag = 1;
		            process_state = 0;  // 跑完耗时操作就退回初始状态
		            Polish_Count = 0;   // 重置计数
		        }
		        else
		        {
		            Polish_Count++;
		        }
		    }
		}

		else if (process_state == 2)
		{
			GF_BSP_GPIO_SetIO(Out_Vacuum, 1);
		    GF_BSP_GPIO_SetIO(Out_Polish, 0);

		    if (Polish_Count <= 0)
		    {
		        GF_BSP_GPIO_SetIO(Out_Polish, 1);
		        Polish_Flag = 0;
//		        GF_BSP_GPIO_SetIO(Out_Vacuum, 1);
		        process_state = 0;  // 跑完退回
		    }
		    else
		    {
		        Polish_Count--;
		    }
		}



//		/* 版本三：LU_Single状态机Switch版本（尚未验证） */
//		// 1. 按键触发逻辑（仅在空闲状态响应）
//		if (P_U7->LU_Single == 1000 && Polish_lock == 0 && process_state == 0)
//		{
//		    Polish_lock = 1;  // 锁定触发，防止重复触发
//
//		    if (Polish_Flag == 0)
//		    {
//		        process_state =21;  // 切换到开启流程
//		        Vacuum_Delay_Count = 0;          // 重置吸尘器延迟计数
//		    }
//		    else
//		    {
//		        process_state = 2;   // 切换到关闭流程
//		        Polish_Count = 100;              // 初始化关闭计数
//		    }
//		}
//		// 按键松开时解除锁定
//		else if (P_U7->LU_Single == -1000)
//		{
//		    Polish_lock = 0;
//		}
//
//		// 2. 状态机主体（使用switch-case实现）
//		switch (process_state)
//		{
//		    case 0:
//		        // 空闲状态：无需执行操作，等待触发
//		        break;
//
//		    case 1:
//		        // 开启流程：先开吸尘器，延迟后开打磨
//		        GF_BSP_GPIO_SetIO(Out_Vacuum, 0);  // 开启吸尘器
//
//		        if (Vacuum_Delay_Count < 200)
//		        {
//		            Vacuum_Delay_Count++;  // 吸尘器延迟计时
//		        }
//		        else
//		        {
//		            // 延迟结束，处理打磨开启
//		            GF_BSP_GPIO_SetIO(Out_Polish, 0);
//
//		            if (Polish_Count >= 100)
//		            {
//		                GF_BSP_GPIO_SetIO(Out_Polish, 1);
//		                Polish_Flag = 1;               // 更新状态标志
//		                process_state = 0;    // 回到空闲状态
//		                Polish_Count = 0;              // 重置计数
//		            }
//		            else
//		            {
//		                Polish_Count++;
//		            }
//		        }
//		        break;
//
//		    case 2:
//		        // 关闭流程：先处理打磨关闭，再关吸尘器
//		        GF_BSP_GPIO_SetIO(Out_Polish, 0);
//
//		        if (Polish_Count <= 0)
//		        {
//		            GF_BSP_GPIO_SetIO(Out_Polish, 1);
//		            Polish_Flag = 0;               // 更新状态标志
//		            GF_BSP_GPIO_SetIO(Out_Vacuum, 1);  // 关闭吸尘器
//		            process_state = 0;    // 回到空闲状态
//		        }
//		        else
//		        {
//		            Polish_Count--;
//		        }
//		        break;
//
//		    default:
//		        // 异常状态处理：回到初始状态
//		        process_state = 0;
//		        Polish_Count = 0;
//		        Vacuum_Delay_Count = 0;
//		        break;
//		}



	//LU_Three控制打磨推杆升降
	if(P_U7->LU_Three==1000)
	{
		GF_BSP_GPIO_SetIO(Out_Lift_2,0);
		GF_BSP_GPIO_SetIO(Out_Lift_1,1);
	}
	else if(P_U7->LU_Three==-1000)
	{
		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(P_U7->RU_Single==1000)  // && CurrentMoveState != Move_AutoForward&& CurrentMoveState != Move_AutoBackward)
//	{
//	   GF_BSP_GPIO_SetIO(Out_Spray,0);
//	}
//	else if(P_U7->RU_Single==-1000)  // && CurrentMoveState != Move_AutoForward  && CurrentMoveState != Move_AutoBackward)
//	{
//	   GF_BSP_GPIO_SetIO(Out_Spray,1);
//	}


	// 控制电磁阀喷枪（非自锁版本）
	// 功能：按一下开启，再按一下关闭，长按不会反复切换
	// Flag管当前喷枪状态、Lock管长短按问题
	if (P_U7->RU_Single == 1000)
	{
	    if (Spray_lock == 0) // 未锁定时才处理
	    {
	        if (Spray_Flag == 0)// 当前关闭，切换为开启
	        {
	            GF_BSP_GPIO_SetIO(Out_Spray, 0);
	            Spray_Flag = 1;
	            Spray_lock = 1; // 上锁。一旦按下实现状态切换就锁定。
	        }
	        else                 // 当前开启，切换为关闭
	        {
	            GF_BSP_GPIO_SetIO(Out_Spray, 1);
	            Spray_Flag = 0;
	            Spray_lock = 1; // 上锁关闭后锁定，防止长按再次触发
	        }
	    }
	}
	else
	{
	    // 松开时复位锁定，允许下次按键触发
		Spray_lock = 0;
	}




	/*急停版本2：手动复位按键版
	 * 急停：同时按下S3、S4则触发急停；
	 * 退出急停：极短的一段时间后，按下S3、S4任意键则解除
	 *
	 * 急停一旦触发即锁定，并关闭全部设备；退出急停后，需手动复位所有被按下按键才能恢复控制
	 *
	 * 优先级：
	 * 急停且自动锁定、急停退出但且自动锁定、急停退出且解除自动锁定
	 * */


	if(P_U7->S3 == 1000 && P_U7->S4 == 1000)
	{
	    GV.Emergency = 1;
	    Operation_lock = 1;  // 急停时自动锁定
	}

	// 2急停锁定计时（仅在急停状态下计数，解决松手不同时问题）
	if (GV.Emergency == 1)
	{
	    if (emergency_lock_count < 500)
	    {
	        emergency_lock_count++;
	    }
	}
	else
	{
	    emergency_lock_count = 0;  // 非急停状态重置
	}

	if (GV.Emergency == 1 && emergency_lock_count >= 500)
	{
	    if ((P_U7->S3 == 1000 && P_U7->S4 == -1000) ||
	        (P_U7->S3 == -1000 && P_U7->S4 == 1000))
	    {
	        GV.Emergency = 0;
	        emergency_lock_count = 0;
	        // 退出后保持锁定（Operation_lock仍为1），需手动解锁
	    }
	}

	// 解锁条件：按键全部复位：即，三键的推杆回中（0）且两键恢复默认状态（-1000）
	// 仅在已退出急停（GV.Emergency==0）但仍锁定时生效
	if (GV.Emergency == 0 && Operation_lock)
	{
//	    if (P_U7->LU_Three == 0 && P_U7->LU_Single == -1000 && P_U7->RU_Single == -1000)
		if (P_U7->LU_Three == 0)
	    {
	    	Operation_lock = 0;  // 满足条件，解锁
	    }
		GF_BSP_GPIO_SetIO(0,0);
	}

	// 急停
	if(GV.Emergency == 1
		|| (GV.Emergency == 0 && Operation_lock == 1))
	{
		Polish_Flag = 0;
		Spray_Flag = 0;
	    GF_BSP_GPIO_SetIO(Out_Spray,1);
//	    GF_BSP_GPIO_SetIO(Out_Polish,0);
//	    if(Polish_Count >= 100)
//	    {
//	        GF_BSP_GPIO_SetIO(Out_Polish,1);
//	        Polish_Flag = 0;
//		    // 重置相关状态变量
//		    Polish_Count = 0;
//	    }
//	    else
//	    {
//	        Polish_Count++;
//	    }
	    GF_BSP_GPIO_SetIO(Out_Vacuum, 1);
	    GF_BSP_GPIO_SetIO(Out_Lift_2,1);
	    GF_BSP_GPIO_SetIO(Out_Lift_1,1);
	    GF_BSP_GPIO_SetIO(0,1);


	    CurrentMoveState = Move_Emergency;
	}








//	if(P_U7->LU_pulley>=900 && JJ_Flag==0)
//	{
//		JJ_Flag=1;
//		GV.PV.Robot_AutoSpeedBase = GV.PV.Robot_AutoSpeedBase + 1;
//	}
//	else if(P_U7->LU_pulley<= -900 && JJ_Flag==0)
//	{
//		JJ_Flag=1;
//		GV.PV.Robot_AutoSpeedBase = GV.PV.Robot_AutoSpeedBase - 1;
//	}
//	else if(P_U7->LU_pulley<=100 && P_U7->LU_pulley>=-100  && JJ_Flag==1)
//	{
//		JJ_Flag=0;
//	}
//	if(GV.PV.Robot_AutoSpeedBase<=0)
//	{
//		GV.PV.Robot_AutoSpeedBase = 0;
//	}


	//Robot Speed 指向的是GV.movespeed
	GV.Robot_ManualSpeed = abs((GV.PV.Robot_ManualSpeedBase* 186.9 * (P_U7->CH0_LYB_V))/ 1000);//RobotSpeed指向GV的移动车体速度 这是整体车速 不是单个轮的
	CV.TurnLeftRightSpeed = abs((2 * 186.9 * (P_U7->CH1_LYB_H))/ 1000);//RobotSpeed指向GV的移动车体速度 这是整体车速 不是单个轮的

	GV.Robot_AutoSpeed = GV.PV.Robot_AutoSpeedBase* 186.9;
//
//	IV.Robot_Move_AutoSpeed = GV.PV.Robot_AutoSpeedBase;//好像是往安卓界面传数据的
//	IV.Robot_Move_ManualSpeed = GV.PV.Robot_ManualSpeedBase;//好像是往安卓界面传数据的

	//	IV.Robot_Swing_Speed=GV.SwingMotor.Velcity;
	//	IV.Robot_Tilt_Speed=GV.TiltMotor.Velcity;
	IV.Robot_AngleRoll = GV.Robot_Angle.RF_Angle_Roll;
	IV.Robot_ForceValue = GV.Robot_ForceValue;
	IV.Robot_DynamometerValue = GV.Robot_DynamometerValue;
	IV.Robot_Current_Left = GV.LeftFrontMotor.Current;
	IV.Robot_Current_Right = GV.RightFrontMotor.Current;

	IV.Robot_Error = 0;
	if(GV.LeftFrontMotor.ERROR_Flag != 0)
	{
		IV.Robot_Error = 100+GV.LeftFrontMotor.ERROR_Flag;
	}
	else if(GV.RightFrontMotor.ERROR_Flag != 0)
	{
		IV.Robot_Error = 200+GV.RightFrontMotor.ERROR_Flag;
	}
	else if(GV.LeftFrontMotor.ERROR_Flag != 0 && GV.RightFrontMotor.ERROR_Flag != 0)
	{
		IV.Robot_Error = 10000 + GV.LeftFrontMotor.ERROR_Flag * 100 + GV.RightFrontMotor.ERROR_Flag;
	}


	IV.Robot_CurrentState = CurrentMoveState;

//	GV.Robot_ChgLength = CV.PV.Robot_ChgLength - 10;
//	GV.Left_Compensation = CV.PV.Robot_LeftCompensation;
//	GV.Right_Compensation = CV.PV.Robot_RightCompensation;

	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;
		}
	}
}