/*
 * bsp_FSM.c
 *
 *  Created on: Aug 2, 2024
 *      Author: akeguo
 */

#include <FSM.h>
#include "bsp_hardware_hash_table.h"
#include "MSP/msp_MK32.h"
void GF_Dispatch();

//Manual=0,
//	Auto,
//	Abnormal,
//	SetLaneChangeDistance,
//	SetBackwardsDistance,
//	LaneChangeMode,
//	Forwards,
//	Backwards,
//	TurnLeft,
//	TurnRight


ACTION_MAP_t actionMap[] =
{
	{ Manual, Manual_State_Entry, Manual_State_Do, Manual_State_Exit },
	{ Auto, Auto_State_Entry, Auto_State_Do, Auto_State_Exit },
	{ SetLaneChangeDistance, LaneChangeDistance_Setting_State_Entry, LaneChangeDistance_Setting_State_Do, LaneChangeDistance_Setting_State_Exit },
	{ SetBackwardsDistance, BackWardsDistance_Setting_State_Entry, BackWardsDistance_Setting_State_Do, BackWardsDistance_Setting_State_Exit },
	{ Forwards, Forwards_State_Entry, Forwards_State_Do, Forwards_State_Exit },
	{ Backwards, Backwards_State_Entry, Backwards_State_Do, Backwards_State_Exit },
	{ TurnLeft, TurnLeft_State_Entry, TurnLeft_State_Do, TurnLeft_State_Exit },
	{ TurnRight, TurnRight_State_Entry, TurnRight_State_Do, TurnRight_State_Exit },

};



EVENT_MAP_t eventMap[] =
{
 { EVENT_Manual, Manual, Manual },
{ EVENT_Auto, Auto, Auto },
{ EVENT_Abnormal, Abnormal, Abnormal },
{ EVENT_SetLaneChangeDistance, SetLaneChangeDistance, SetLaneChangeDistance },
{ EVENT_SetBackwardsDistance, SetBackwardsDistance, SetBackwardsDistance },
{ EVENT_LaneChangeMode, LaneChangeMode, LaneChangeMode },
{ EVENT_Forwards, Forwards, Forwards },
{ EVENT_Backwards, Backwards, Backwards },
{ EVENT_TurnLeft, TurnLeft, TurnLeft },
{ EVENT_TurnRight, TurnRight, TurnRight },
{ EVENT_MAP_END, 0, 0 }, };

FSM_t RobotFSM;	//定义状态机

void fsm_init(FSM_t *pFsm, EVENT_MAP_t *pEventMap, ACTION_MAP_t *pActionMap)
{
	//pFsm->stCurState = 0;
	pFsm->stCurState = EVENT_Abnormal;
	pFsm->stNextState = EVENT_MAP_END;
	pFsm->pEventMap = pEventMap;
	pFsm->pActionMap = pActionMap;
}
void fsm_state_transfer(FSM_t *pFsm, EVENT_t stEventID)
{
	char i = 0;

	for (i = 0; pFsm->pEventMap[i].stEventID < EVENT_MAP_END; i++)
	{
		if (stEventID == pFsm->pEventMap[i].stEventID) //MAKE SURE THIS STATE EXISTS
		{
			if (pFsm->stCurState != stEventID)
			{
				pFsm->stNextState = stEventID; //pFsm->pEventMap[i].stNextState;
			} else
			{
				pFsm->stNextState = EVENT_MAP_END; //pFsm->pEventMap[i].stNextState;
			}
			return;
		}

	}

}
void action_perfrom(FSM_t *pFsm)
{
	if (EVENT_MAP_END != pFsm->stNextState)
	{
		pFsm->pActionMap[pFsm->stCurState].ExitAct();
		pFsm->pActionMap[pFsm->stNextState].EnterAct();

		pFsm->stCurState = pFsm->stNextState;
		pFsm->stNextState = EVENT_MAP_END;
	} else
	{
		pFsm->pActionMap[pFsm->stCurState].RunningAct();
	}
}

void Fsm_Init()
{
	fsm_init(&RobotFSM, eventMap, actionMap);	//注册状态机
	GF_BSP_Interrupt_Add_CallBack(
			DF_BSP_InterCall_TIM8_2ms_PeriodElapsedCallback, GF_Dispatch);
}

bool IsAbnormal = false;

int GF_Dispacher_Counter = 0;
void GF_Dispatch()
{
	if(GV.DH_CAN.Work_Start==1)
	{
		fsm_state_transfer(&RobotFSM, EVENT_Auto);
	}else
	{
		// fsm_state_transfer(&RobotFSM, EVENT_Manual);

		if (GV.DH_CAN.Swing_Right == 1)
			{
				GV.SwingMotor.Target_Position -= 100;

			} else if (GV.DH_CAN.Swing_Left == 1)
			{
				GV.SwingMotor.Target_Position += 100;
			}

			if (GV.DH_CAN.Mode_Selection == 9) //换道距离设定
			{
				fsm_state_transfer(&RobotFSM, EVENT_SetLaneChangeDistance);
			} else if (GV.DH_CAN.Mode_Selection == 10) //后退距离设定
			{
				fsm_state_transfer(&RobotFSM, EVENT_SetBackwardsDistance);
			}
		    //前进
			if (GV.DH_CAN.Move_Forward == 1)
			{
				fsm_state_transfer(&RobotFSM, EVENT_Forwards);

			} else if (GV.DH_CAN.Move_Backward == 1) //后退左转右转
			{
				fsm_state_transfer(&RobotFSM, EVENT_Backwards);

			} else if (GV.DH_CAN.Move_Turn_Left == 1)
			{
				fsm_state_transfer(&RobotFSM, EVENT_TurnLeft); //左转


			} else if (GV.DH_CAN.Move_Turn_Right == 1)//右转
			{
				fsm_state_transfer(&RobotFSM, EVENT_TurnRight);

			}

	}
#if 0
		if (IsAbnornalStatus == 1)
		{
			IsAbnormal = true;
			//进入异常状态；
			fsm_state_transfer(&RobotFSM, EVENT_Abnormal);
		} else
		{
			//退出异常状态
			IsAbnormal = false;
		}
#endif 0
	//}


	 action_perfrom(&RobotFSM);
}