/*
 * 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 Robot_Move()
{
	CurrentMoveState = Move_HALT;
	if(!(P_U7->M2 == -1000 && P_U7->M1 == -1000
			&& P_U7->M6 == -1000 && P_U7->M5 == -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->M2 == -1000 && P_U7->M1 == -1000
				&& P_U7->M6 == -1000 && P_U7->M5 == -1000)	//自动作业与换道互斥
		{
		   if(P_U7->RU_Three == 1000)
		   {
				CurrentMoveState = Move_AutoForward;
				if(GV.AuTo_Flag == 0)
				{
					GV.AuTo_Flag=1;
				}
		   }
		   else if(P_U7->RU_Three == -1000)
		   {
				CurrentMoveState = Move_AutoBackward;
				if(GV.AuTo_Flag == 0)
				{
					GV.AuTo_Flag=3;
				}
		   }
		}
		else if(!(P_U7->M2 == -1000 && P_U7->M1 == -1000
				&& P_U7->M6 == -1000 && P_U7->M5 == -1000)
				&& P_U7->RU_Three == 0)
		{
			if(GV.Chg_Flag == 1 && (P_U7->M1 == 1000 || P_U7->M2 == 1000
					|| P_U7->M5 == 1000 || P_U7->M6 == 1000))
			{
				CurrentMoveState = Move_ChgFinish;
			}
			else if(P_U7->M1 == 1000
					&& P_U7->M2 == -1000 && P_U7->M6 == -1000 && P_U7->M5 == -1000)
			{
				CurrentMoveState = Move_ChgUp;
			}
			else if(P_U7->M2 == 1000
					&& P_U7->M1 == -1000 && P_U7->M6 == -1000 && P_U7->M5 == -1000)
			{
				CurrentMoveState = Move_ChgDown;
			}
			else if(P_U7->M5 == 1000
					&& P_U7->M1 == -1000 && P_U7->M2 == -1000 && P_U7->M6 == -1000)
			{
				CurrentMoveState = Move_ChgLeft;
			}
			else if(P_U7->M6 == 1000
					&& P_U7->M1 == -1000 && P_U7->M2 == -1000 && P_U7->M5 == -1000)
			{
				CurrentMoveState = Move_ChgRight;
			}
		}
	}
	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->M2 == -1000 && P_U7->M1 == -1000
			&& P_U7->M6 == -1000 && P_U7->M5 == -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
			{
				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;  //停车
	}
}
void Front_End()
{
	//	 版本二：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_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);
	}



	// 控制电磁阀喷枪（非自锁版本）
	// 功能：按一下开启，再按一下关闭，长按不会反复切换
	// 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;
	}

}
void Emergency()
{
	/*急停版本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_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;
	}



}

void GF_Dispatch()//2ms调用一次 给车体速度等赋值
{
	//	//运动部分 Move Region
	//左补偿、右补偿
	Lcompensation_control();
	Rcompensation_control();
	Robot_Move();
	Front_End();
	Emergency();
	/****************************************/
	//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_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_Compensation_Left = GV.Left_Compensation;
	IV.Robot_Compensation_Right = GV.Right_Compensation;

	IV.Robot_Error_Left = GV.LeftFrontMotor.ERROR_Flag;
	IV.Robot_Error_Right = GV.RightFrontMotor.ERROR_Flag;
	IV.Robot_CurrentState = CurrentMoveState;


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

}

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