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

#include "fsm.h"
#include "msp_DAM0404D.h"
#include "BHBF_ROBOT.h"
#include "BSP/bsp_include.h"
#include "msp_DH_Roughening.h"
#include "MSP/msp_PID.h"
#include "robot_state.h"
void Vertical_Left_Right_Lane_Change_Control(int *LaneChangeCmd, int left_or_right);
void Horizontal_Up_Down_Lane_Change_Control(int *LaneChangeCmd, int up_or_down,
        int head_to_left_or_right);

int32_t *RobotSpeed;

int32_t *RobotLaneChangeDistance;

int LaneChangeCommand;
int LastLaneChangeCommand;
int LaneChangeCommandState;
int var0;
int var1;

MoveSTATE_t CurrentMoveState;
SwingSTATE_t CurrentSwingState;
TiltSTATE_t CurrentTiltState; //点头，抬头

LaneChangeSTATE_t CurrentLaneChangeSTATE;

RougheningEndState_t CurrentRougheningEndState; //拉毛运动或者不动
SetSTATE_t CurrentSetState; //设置
int index_counter = 0;

void LaneChangeControl();
void DHRougheningControl();
void TiltControl();

//手动下 机器人运动
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_Vertical_Task_Forwards, Move_Vertical_Task_Forwards_Do },
{ Move_Vertical_Task_Backwards, Move_Vertical_Task_Backwards_Do },
{ Move_Horizontal_Task_Forwards, Move_Horizontal_Task_Forwards_Do },
{ Move_Horizontal_Task_Backwards, Move_Horizontal_Task_Backwards_Do },

{ Move_Head_To_Left_Enum, Move_Head_To_Left_Do },
{ Move_Head_To_UP_Enum, Move_Head_To_UP_Do },
{ Move_Head_To_Right_Enum, Move_Head_To_Right_Do },
{ Move_Head_To_Down_Enum, Move_Head_To_Down_Do },

};

transition_with_state_t LaneChangeTransitions[] =
        {

        { Lane_HALT, Lane_HALT_Do },
        { Lane_Turn_To_Straight_Up, Lane_Turn_To_Straight_Up_Do },
        { Lane_Turn_To_Straight_Down, Lane_Turn_To_Straight_Down_Do },
        { Lane_Turn_To_Horizontal_Left, Lane_Turn_To_Horizontal_Left_Do },
        { Lane_Turn_To_Horizontal_Right, Lane_Turn_To_Horizontal_Right_Do },

                { Lane_Vertical_Task_Move_Forwards,
                        Lane_Vertical_Task_Move_Forwards_Do },
                { Lane_Vertical_Task_Move_Backwards,
                        Lane_Vertical_Task_Move_Backwards_Do },
                { Lane_Horizontal_Task_Move_Forwards,
                        Lane_Horizontal_Task_Move_Forwards_Do },
                { Lane_Horizontal_Task_Move_Backwards,
                        Lane_Horizontal_Task_Move_Backwards_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 },

};

transition_t RougheningEndTransitions[] =
{
{ RougheningEndHALT, RougheningEndHALT_Do },
{ RougheningEndSwing, RougheningEndSwing_Do },

};

void Fsm_Init()
{
	GF_BSP_Interrupt_Add_CallBack(
	        DF_BSP_InterCall_TIM8_2ms_PeriodElapsedCallback, GF_Dispatch);
}

void GF_Dispatch()
{
	TiltControl();
	DHRougheningControl();

	if (Get_BIT(SystemErrorCode, DHRougheningController) == 1)
	{
		CurrentMoveState = Move_HALT;

		action_perfrom(MoveTransitions,
		        sizeof(MoveTransitions) / sizeof(transition_t),
		        CurrentMoveState);
		return;
	}
	LaneChangeControl();
	//获取电机速度及状态  m/min

	//电机直径280mm/101传动比  发送的数据单位是0.1 rpm
//	1 rpm 意思是1分钟转1圈，英文是 1rotation per minite。一圈的弧度是2π rad，一分钟60秒，那么
//	1 rpm = 1(r)/1(min) = 1(r)/60(s) = 2π (rad)/60(s) =π/30 (rad/s)
//	所以，1 rpm = π/30 (rad/s), 即 1 rpm = 0.1047 （rad/s）
//   v=ω×r

	*RobotSpeed = (int32_t) (DHRougheningGetSpeed(DHRougheningContrller) / 0.880
	        * 101 * 10);

	*RobotLaneChangeDistance = DHRougheningGetLaneChangeDistance(
	        DHRougheningContrller);

	LaneChangeControl();



	if (DHRougheningContrller.Forward_Backward_Speed > 1000
	        || DHRougheningContrller.Left_Right_Turn_Speed > 1000)
	{

		if (DHRougheningContrller.Forwards + DHRougheningContrller.Backwards
		        + DHRougheningContrller.Left_Turn
		        + DHRougheningContrller.Right_Turn == 1)
		{

			if (DHRougheningContrller.Forwards == 1)
			{

				if (DHRougheningContrller.Horizontal_Task == 1)
				{
					CurrentMoveState = Move_Horizontal_Task_Forwards;
				}
				else if (DHRougheningContrller.Vertical_Task == 1)
				{
					CurrentMoveState = Move_Vertical_Task_Forwards;
				}
				else
				{
					CurrentMoveState = Move_Forwards;
				}

			}

			if (DHRougheningContrller.Backwards == 1)
			{

				if (DHRougheningContrller.Horizontal_Task == 1)
				{
					CurrentMoveState = Move_Horizontal_Task_Backwards;
				}
				else if (DHRougheningContrller.Vertical_Task == 1)
				{
					CurrentMoveState = Move_Vertical_Task_Backwards;
				}
				else
				{
					CurrentMoveState = Move_Forwards;
				}

			}
			if (DHRougheningContrller.Left_Turn == 1)
			{
				CurrentMoveState = Move_TurnLeft;
			}
			if (DHRougheningContrller.Right_Turn == 1)
			{
				CurrentMoveState = Move_TurnRight;
			}
		}
		else
		{
			CurrentMoveState = Move_HALT;
		}
	}
	else
	{
		CurrentMoveState = Move_HALT;
	}

	//参数设置

	//手动模式
	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;
		}
	}
}

//int* LaneChangeCommand, int* var0,int* var1)
void action_perfrom_with_state(transition_with_state_t transitions[],
        int length, int *state, int *LaneChangeCommand, int *var0, int *var1)
{

	for (index_counter = 0; index_counter < length; index_counter++)
	{
		if (transitions[index_counter].State == *state)
		{
			if (transitions[index_counter].robotRunWithState != NULL)
			{
				transitions[index_counter].robotRunWithState(state,
				        LaneChangeCommand, var0, var1);
			}

			break;
			//return;
		}
	}
}

//超过某个角度，则一直向左转，在误差允许范围内，进行水平左移动，然后计算时间，随后停止

void TiltControl()
{
	//Tilt 部分 前端上升、前端下降
	if (DHRougheningContrller.End_Up == 1)
	{
		CurrentTiltState = TiltUP;

	}
	else if (DHRougheningContrller.End_Down == 1)
	{
		CurrentTiltState = TiltDown;
	}
	else
	{
		CurrentTiltState = TiltHALT;
	}

	//点头、抬头 上下移动
	action_perfrom(TiltTransitions,
	        sizeof(TiltTransitions) / sizeof(transition_t), CurrentTiltState);
}

void DHRougheningControl()
{
	//拉毛部分
	if (DHRougheningContrller.Roughening_Start == 1)
	{
		CurrentRougheningEndState = RougheningEndSwing; //拉毛开启
	}
	else
	{
		CurrentRougheningEndState = RougheningEndHALT; //拉毛关闭
	}

	//拉毛
	action_perfrom(RougheningEndTransitions,
	        sizeof(RougheningEndTransitions) / sizeof(transition_t),
	        CurrentRougheningEndState);
}
void LaneChangeControl()
{
	//换道
	if (DHRougheningContrller.LaneChange_Horizontal_Left == 1)
	{
		LaneChangeCommand = 1;
	}
	else if (DHRougheningContrller.LaneChange_Vertical_Up == 1)
	{
		LaneChangeCommand = 2;
	}
	else if (DHRougheningContrller.LaneChange_Horizontal_Right == 1)
	{
		LaneChangeCommand = 3;
	}
	else if (DHRougheningContrller.LaneChange_Vertical_Down == 1)
	{
		LaneChangeCommand = 4;
	}
	else
	{
		LaneChangeCommand = 0;
	}

	if (LastLaneChangeCommand != LaneChangeCommand)
	{
		LaneChangeCommandState=1;
		switch (LaneChangeCommand)
		{

			case 0:
			{
				//CurrentAutoMoveSTATE = Auto_Move_HALT;
			}
				break;
			case 1:
			{
				//CurrentLaneChangeSTATE = Lane_Turn_To_Horizontal_Left;

				Vertical_Left_Right_Lane_Change_Control(&LaneChangeCommandState,0);
			}
				break;
			case 2:
			{
					//up
				  Horizontal_Up_Down_Lane_Change_Control(&LaneChangeCommandState,   0,
				          0);
			}
				break;
			case 3:
			{
				//CurrentLaneChangeSTATE = Lane_Turn_To_Horizontal_Right;
				Vertical_Left_Right_Lane_Change_Control(&LaneChangeCommandState,1);
			}
				break;
			case 4:
			{
				//CurrentLaneChangeSTATE = Lane_Turn_To_Straight_Down;
				//down
				 Horizontal_Up_Down_Lane_Change_Control(&LaneChangeCommandState,   1,
								          0);
			}
				break;
		}
	}
	else
	{

	}
	LastLaneChangeCommand = LaneChangeCommand;
}
static int32_t timeCount = 0;

//Move_Head_To_UP_Enum,

//Move_Head_To_Down_Enum,
void Vertical_Left_Right_Lane_Change_Control(int *LaneChangeCmd, int left_or_right)
{
//1. 转到方向朝左，2. 前进距离，3.方向朝上
	switch (*LaneChangeCmd)
	{
		case 1:
		{

			if (left_or_right == 0) //left
			{
				if (abs(GV.Robot_Angle - CV.RobotHorizontalDirectionAngle)
				        >= 20 * 100)
				{
					CurrentMoveState = Move_Head_To_Left_Enum; //转到左边，前进
				}
				else
				{
					//开始计时
					timeCount = 0;
					*LaneChangeCmd += 1; //
				}
			}
			else if (left_or_right == 1) //right
			{
				if (abs(GV.Robot_Angle - CV.RobotHorizontalDirectionAngle)
				        >= 20 * 100)
				{
					CurrentMoveState = Move_Head_To_Right_Enum; //转到左边，前进
				}
				else
				{
					//开始计时
					timeCount = 0;
					*LaneChangeCmd += 1; //
				}
			}

		}
			break;
		case 2:
		{
			timeCount++;
			CurrentMoveState = Move_Horizontal_Task_Forwards; //转到左边，前进

			if (timeCount
			        >= 60 * (*RobotLaneChangeDistance)
			                / (DHRougheningGetSpeed(DHRougheningContrller))) //传输距离除以速度
			{
				*LaneChangeCmd += 1; //
			}
		}
			break;
		case 3:
		{
			if (abs(GV.Robot_Angle - CV.RobotVerticalDirectionAngle)
			        >= 10 * 100) //45度
			{
				CurrentMoveState = Move_Head_To_UP_Enum; //转到左边，前进
			}
			else
			{
				*LaneChangeCmd += 1; //
			}
		}
			break;
		case 4:
		{
			CurrentMoveState = Move_HALT; //转到左边，前进
		}
			break;

	}
}

void Horizontal_Up_Down_Lane_Change_Control(int *LaneChangeCmd, int up_or_down,
        int head_to_left_or_right)
{
//1. 转到方向朝左，2. 前进距离，3.方向朝上
	switch (*LaneChangeCmd)
	{
		case 1:

			if (abs(GV.Robot_Angle - CV.RobotVerticalDirectionAngle)
			        >= 20 * 100)
			{
				CurrentMoveState = Move_Head_To_UP_Enum; //
			}
			else
			{
				//开始计时
				timeCount = 0;
				*LaneChangeCmd += 1; //
			}

			break;
		case 2:
		{

			if (up_or_down == 0)
			{
				CurrentMoveState = Move_Vertical_Task_Forwards;
			}
			else if (up_or_down == 1)
			{
				CurrentMoveState = Move_Vertical_Task_Backwards;
			}
			timeCount++;
			if (timeCount
			        >= 60 * (*RobotLaneChangeDistance)
			                / (DHRougheningGetSpeed(DHRougheningContrller))) //传输距离除以速度
			{
				*LaneChangeCmd += 1; //
			}
		}
			break;
		case 3:
		{

			if (head_to_left_or_right == 0) //head to left
			{
				if (abs(GV.Robot_Angle - CV.RobotHorizontalDirectionAngle)
				        >= 10 * 100) //45度
				{
					CurrentMoveState = Move_Head_To_Left_Enum; //转到左边，前进
				}
				else
				{
					*LaneChangeCmd += 1; //
				}

			}
			else if (head_to_left_or_right == 1)
			{
				if (abs(GV.Robot_Angle - CV.RobotHorizontalOppositeDirectionAngle)
				        >= 10 * 100) //45度
				{
					CurrentMoveState = Move_Head_To_Right_Enum; //转到左边，前进
				}
				else
				{
					*LaneChangeCmd += 1; //
				}
			}

		}
			break;
		case 4:
		{
			CurrentMoveState = Move_HALT; //转到左边，前进
		}
			break;

	}
}