/*
 * 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 Out_Lift_Count = 0;
int Spray_Flag=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_ChgFinish, 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;

double Speed_Judge()
{
	if(DHRougheningContrller.Vehicle_Speed1 == 1)
	{
		return 0.1;
	}
	if(DHRougheningContrller.Vehicle_Speed2 == 1)
	{
		return 0.3;
	}
	if(DHRougheningContrller.Vehicle_Speed3 == 1)
	{
		return 0.5;
	}
	if(DHRougheningContrller.Vehicle_Speed4 == 1)
	{
		return 0.8;
	}
	if(DHRougheningContrller.Vehicle_Speed5 == 1)
	{
		return 1;
	}
	if(DHRougheningContrller.Vehicle_Speed6 == 1)
	{

		return 3;
	}
	if(DHRougheningContrller.Vehicle_Speed7 == 1)
	{

		return 5;
	}
	if(DHRougheningContrller.Vehicle_Speed8 == 1)
	{
		return 8;
	}
	if(DHRougheningContrller.Vehicle_Speed9 == 1)
	{
		return 10;
	}
	if(DHRougheningContrller.Vehicle_Speed10 == 1)
	{
		return 15;
	}
	return 0;
}

void Robot_Halt(MoveSTATE_t STATE)
{

	if(DHRougheningContrller.Horizontal_Task == 1
			|| DHRougheningContrller.Vertical_Task == 1)
	{
		GV.AuTo_Flag = 99;
	}
	else
	{
		GV.AuTo_Flag = 0;
	}


	if((DHRougheningContrller.LaneChange_Horizontal_Left == 1
			|| DHRougheningContrller.LaneChange_Horizontal_Right == 1
			|| DHRougheningContrller.LaneChange_Vertical_Down == 1
			|| DHRougheningContrller.LaneChange_Vertical_Up == 1))
	{
		GV.Chg_Flag = 1;
		Chg_Pa.change_road_finish_flag = 1;
	}
	else
	{
		GV.Chg_Flag = 0;
		Chg_Pa.change_road_finish_flag = 0;
	}

	CurrentMoveState = STATE;  //停车
}

void Robot_ResetJudge()
{
	if(DHRougheningContrller.Wireless_State == 0)
	{
		return;
	}
	if((DHRougheningContrller.LaneChange_Horizontal_Left == 0
			&& DHRougheningContrller.LaneChange_Horizontal_Right == 0
			&& DHRougheningContrller.LaneChange_Vertical_Down == 0
			&& DHRougheningContrller.LaneChange_Vertical_Up == 0
			&& DHRougheningContrller.Vertical_Task == 0
			&& DHRougheningContrller.Horizontal_Task ==0)
			&& DHRougheningContrller.Forwards == 0
			&& DHRougheningContrller.Backwards == 0
			&& DHRougheningContrller.Left_Turn == 0
			&& DHRougheningContrller.Right_Turn == 0
			&& DHRougheningContrller.Roughening_Start == 0
			&& DHRougheningContrller.End_Down == 0
			&& DHRougheningContrller.End_Up == 0)
	{
		GV.Robot_Move_Step = 1;
	}
}

void Robot_Move()
{
	//		急停版本一
		if(DHRougheningContrller.Emergency_Stop == 1)
		{
			GF_BSP_GPIO_SetIO(Out_Vacucm,1);
			GF_BSP_GPIO_SetIO(Out_Lift_1,1);
			GF_BSP_GPIO_SetIO(Out_Lift_2,1);

			if(Polish_Count == 100 && polish_Flag == 1)
			{
				GF_BSP_GPIO_SetIO(Out_Polish,1);
				polish_Flag = 99;
				Polish_Count = 0;
			}
			else if(polish_Flag == 1)
			{
				GF_BSP_GPIO_SetIO(Out_Polish,0);
				Polish_Count++;
			}
			Robot_Halt(Move_Emergency);

			return;
		}
		else if(DHRougheningContrller.Emergency_Stop == 0)
		{
			if(DHRougheningContrller.Roughening_Stop && polish_Flag == 99)
			{
				polish_Flag = 0;
			}
		}


	//运动部分 Move Region
	if((DHRougheningContrller.LaneChange_Horizontal_Left == 1
			|| DHRougheningContrller.LaneChange_Horizontal_Right == 1
			|| DHRougheningContrller.LaneChange_Vertical_Down == 1
			|| DHRougheningContrller.LaneChange_Vertical_Up == 1
			|| DHRougheningContrller.Vertical_Task == 1
			|| DHRougheningContrller.Horizontal_Task ==1)
			&& DHRougheningContrller.Forwards == 0
			&& DHRougheningContrller.Backwards == 0
			&& DHRougheningContrller.Left_Turn == 0
			&& DHRougheningContrller.Right_Turn == 0)
	{
		if(DHRougheningContrller.LaneChange_Horizontal_Left == 0
				&& DHRougheningContrller.LaneChange_Horizontal_Right == 0
				&& DHRougheningContrller.LaneChange_Vertical_Down == 0
				&& DHRougheningContrller.LaneChange_Vertical_Up == 0
				&& (DHRougheningContrller.Vertical_Task == 1
				|| DHRougheningContrller.Horizontal_Task == 1))	//自动作业与换道互斥
		{
		   if(DHRougheningContrller.Horizontal_Task == 1)
		   {
				CurrentMoveState = Move_AutoForward;
				if(GV.AuTo_Flag == 0)
				{
					GV.AuTo_Flag=1;
				}
		   }
		   else if(DHRougheningContrller.Vertical_Task == 1)
		   {
				CurrentMoveState = Move_AutoBackward;
				if(GV.AuTo_Flag == 0)
				{
					GV.AuTo_Flag=3;
				}
		   }
		   else
		   {
				CurrentMoveState = Move_HALT;//停车
		   }
		}
		else if((DHRougheningContrller.LaneChange_Horizontal_Left == 1
				|| DHRougheningContrller.LaneChange_Horizontal_Right == 1
				|| DHRougheningContrller.LaneChange_Vertical_Down == 1
				|| DHRougheningContrller.LaneChange_Vertical_Up == 1)
				&& (DHRougheningContrller.Vertical_Task == 0
				&& DHRougheningContrller.Horizontal_Task == 0))
		{
			if(GV.Chg_Flag == 1
					&& (DHRougheningContrller.LaneChange_Horizontal_Left == 1
					|| DHRougheningContrller.LaneChange_Horizontal_Right == 1
					|| DHRougheningContrller.LaneChange_Vertical_Down == 1
					|| DHRougheningContrller.LaneChange_Vertical_Up == 1))
			{
				CurrentMoveState = Move_ChgFinish;
			}
			else if(DHRougheningContrller.LaneChange_Horizontal_Left == 0
					&& DHRougheningContrller.LaneChange_Horizontal_Right == 0
					&& DHRougheningContrller.LaneChange_Vertical_Down == 0
					&& DHRougheningContrller.LaneChange_Vertical_Up == 1 )
			{
				CurrentMoveState = Move_ChgUp;
			}
			else if(DHRougheningContrller.LaneChange_Horizontal_Left == 0
					&& DHRougheningContrller.LaneChange_Horizontal_Right == 0
					&& DHRougheningContrller.LaneChange_Vertical_Down == 1
					&& DHRougheningContrller.LaneChange_Vertical_Up == 0 )
			{
				CurrentMoveState = Move_ChgDown;
			}
			else if(DHRougheningContrller.LaneChange_Horizontal_Left == 1
					&& DHRougheningContrller.LaneChange_Horizontal_Right == 0
					&& DHRougheningContrller.LaneChange_Vertical_Down == 0
					&& DHRougheningContrller.LaneChange_Vertical_Up == 0 )
			{
				CurrentMoveState = Move_ChgLeft;
			}
			else if(DHRougheningContrller.LaneChange_Horizontal_Left == 0
					&& DHRougheningContrller.LaneChange_Horizontal_Right == 1
					&& DHRougheningContrller.LaneChange_Vertical_Down == 0
					&& DHRougheningContrller.LaneChange_Vertical_Up == 0 )
			{
				CurrentMoveState = Move_ChgRight;
			}
			else
			{
				CurrentMoveState = Move_HALT;//停车
			}
		}
		else
		{
			CurrentMoveState = Move_HALT;//停车
		}
	}
	else if((DHRougheningContrller.LaneChange_Horizontal_Left == 0
			&& DHRougheningContrller.LaneChange_Horizontal_Right == 0
			&& DHRougheningContrller.LaneChange_Vertical_Down == 0
			&& DHRougheningContrller.LaneChange_Vertical_Up == 0
			&& DHRougheningContrller.Vertical_Task == 0
			&& DHRougheningContrller.Horizontal_Task == 0)

			&& (DHRougheningContrller.Forwards == 1
			|| DHRougheningContrller.Backwards == 1
			|| DHRougheningContrller.Left_Turn == 1
			|| DHRougheningContrller.Right_Turn == 1))
	{
		if (DHRougheningContrller.Left_Turn == 0
				&& DHRougheningContrller.Right_Turn == 0)//前进周围300范围内才继续检测前进是否按下
		{
			if (DHRougheningContrller.Forwards == 1)//前进
			{
				CurrentMoveState = Move_Forwards;
			}
			else if (DHRougheningContrller.Backwards == 1)
			{
				CurrentMoveState = Move_Backwards;  //后退
			}
			else
			{
				CurrentMoveState = Move_HALT;//停车
			}
		}
		else if (DHRougheningContrller.Forwards == 0
				&& DHRougheningContrller.Backwards == 0)
		{
			if (DHRougheningContrller.Right_Turn == 1)
			{
				CurrentMoveState = Move_TurnRight;  //右转
			}
			else if (DHRougheningContrller.Left_Turn == 1)
			{
				CurrentMoveState = Move_TurnLeft;  //左转
			}
			else
			{
				CurrentMoveState = Move_HALT;  //停车
			}
		}
		else
		{
			CurrentMoveState = Move_HALT;  //停车
		}
	}
	else
	{
		Robot_Halt(Move_HALT);  //停车
	}
	/****************************************/
	//LU_Single 控制打磨电机
	if(DHRougheningContrller.Roughening_Start && polish_Flag == 0)
	{
		GF_BSP_GPIO_SetIO(Out_Vacucm,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(DHRougheningContrller.Roughening_Stop && polish_Flag == 1)
	{
		GF_BSP_GPIO_SetIO(Out_Vacucm,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_Three控制打磨升降
	if(DHRougheningContrller.End_Up)
	{
		GF_BSP_GPIO_SetIO(Out_Lift_1,0);
		GF_BSP_GPIO_SetIO(Out_Lift_2,1);
	}
	else if(DHRougheningContrller.End_Down)
	{
		GF_BSP_GPIO_SetIO(Out_Lift_1,1);
		GF_BSP_GPIO_SetIO(Out_Lift_2,0);
	}
	else
	{
		GF_BSP_GPIO_SetIO(Out_Lift_1,1);
		GF_BSP_GPIO_SetIO(Out_Lift_2,1);
	}
}
void GF_Dispatch()//2ms调用一次 给车体速度等赋值
{
	switch(GV.Robot_Move_Step)
	{
		case 0:
			Robot_ResetJudge();
			break;
		case 1:
			Robot_Move();
		default:
			break;
	}

	IV.Robot_CurrentState = CurrentMoveState;

	//Robot Speed 指向的是GV.movespeed
	GV.Robot_Speed_Mpm = Speed_Judge();
	GV.Robot_ManualSpeed = abs(GV.Robot_Speed_Mpm * 186.9 );//RobotSpeed指向GV的移动车体速度 这是整体车速 不是单个轮的
	CV.TurnLeftRightSpeed = abs(2 * 186.9 );//RobotSpeed指向GV的移动车体速度 这是整体车速 不是单个轮的


	GV.Robot_AutoSpeed = GV.Robot_Speed_Mpm * 186.9;
	// magic data：65，控制换道距离，对应10cm左右
	GV.Left_Compensation = DHRougheningContrller.Left_Compensation / 13056.0;
	GV.Right_Compensation = 65 - DHRougheningContrller.Right_Compensation / 10043.0 * 10;


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