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

#include "fsm.h"
#include "BHBF_ROBOT.h"
#include "BSP/bsp_include.h"
#include "robot_state.h"
void action_perfrom(transition_t transitions[], int length ,int state);
void GF_Dispatch();
void action_perfrom();
MoveSTATE_t CurrentMoveState;
SwingSTATE_t CurrentSwingState;
SetSTATE_t CurrentSetState;
int index_counter = 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 }, };

transition_t SwingTransitions[] =
{

{ SwingHALT, SwingHALT_State_Do },
{ SwingLeft, SwingLeft_State_Do },
{ SwingRight, SwingRight_State_Do },

};

transition_t SetTransitions[] =
{

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

};

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

void GF_Dispatch()
{
	if (GV.DH_CAN.Work_Start == 1)
	{




	} else
	{
		//Swing 部分
		if (GV.DH_CAN.Swing_Right == 1)
		{
			CurrentSwingState = SwingRight;

		} else if (GV.DH_CAN.Swing_Left == 1)
		{
			CurrentSwingState = SwingLeft;

		} else
		{
			CurrentSwingState = SwingHALT;
		}
		//设定部分 setting region
		if (GV.DH_CAN.Mode_Selection == 9) //换道距离设定
		{
			CurrentSetState = SetLaneChangeDistance;

		} else if (GV.DH_CAN.Mode_Selection == 10) //后退距离设定
		{
			CurrentSetState = SetBackwardsDistance;
		} else
		{
			CurrentSetState = OtherMode;
		}

		//运动部分 Move Region
		if (GV.DH_CAN.Move_Forward == 1)
		{
			CurrentMoveState = Move_Forwards;
		} else if (GV.DH_CAN.Move_Backward == 1) //后退左转右转
		{
			CurrentMoveState = Move_Backwards;
		} else if (GV.DH_CAN.Move_Turn_Left == 1)
		{
			CurrentMoveState = Move_TurnLeft;

		} else if (GV.DH_CAN.Move_Turn_Right == 1) //右转
		{
			CurrentMoveState = Move_TurnRight;
		} else
		{
			CurrentMoveState = Move_HALT;
			// change the parameter under the conditon of Halt
			action_perfrom(SetTransitions,sizeof(SetTransitions)/sizeof(transition_t), CurrentSetState);
		}

		action_perfrom(MoveTransitions,sizeof(MoveTransitions)/sizeof(transition_t), CurrentMoveState);
		action_perfrom(SwingTransitions, sizeof(SwingTransitions)/sizeof(transition_t),CurrentSwingState);

	}
	//左补偿，右补偿 摆臂角度，摆臂速度
	GV.Swing_Speed = CV.SwingSpeedRange * GV.DH_CAN.Swing_Speed_Value / 255;
	GV.Swing_Angle = CV.SwingAngleRange * GV.DH_CAN.Swing_Angle_Value / 255;
	GV.Left_Compensation = CV.LeftCompensationRange
			* GV.DH_CAN.Left_Compensation_Value / 255;
	GV.Right_Compensation = CV.RightCompensationRange
			* GV.DH_CAN.Right_Compensation_Value / 255;


}

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