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

#include "fsm.h"
#include "BHBF_ROBOT.h"
#include "bsp_include.h"
#include "robot_state.h"
#include "msp_Steering_Engine.h"

int32_t *RobotSpeed;
//int32_t *JontSwingSpeed;
//int32_t *JontTiltSpeed;

#define M_PI		3.14159265358979323846

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;                              //点头，抬头
int index_counter = 0;
int32_t speed_selection;
uint32_t rotational_speed = 5;//°/s

//手动下 机器人运动

transition_t MoveTransitions[] =
{

{ Manual_State, Manual_State_Do },
{ Move_HALT, HALT_State_Do },
{ Auto_Back_State,Auto_Back_State_Do}

};


transition_t SwingTransitions[] =
{

{ SwingHALT, SwingHALT_State_Do },
{ SwingAuto, SwingAuto_Do },
{ SwingManual, SwingManual_Do },
{ SwingHome, SwingHome_Do}

};


transition_t TiltTransitions[] =
{

{ TiltHALT, TiltHalt_Do },
{ TiltAuto, TiltAuto_Do },
{ TiltManual, TiltManual_Do },
{ TiltHome,TiltHome_Do}

};

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


void GF_Dispatch()
{
	if(GV.MK32_Key.CH4_SA == -1000)                                  //SA上拨
	{
		Steering_0_Damping_1_working();
	}
	else if(GV.MK32_Key.CH4_SA == 1000)                              //SA下拨
	{
		Steering_0_working_1_Damping();
	}
	else {

		Steering_0_working_1_working();                          //1、2舵机负责行驶，3、4舵机负责转向
	}

	IV.Robot_Move_Speed = speed_selection*32*3.14/3/1000;
	IV.Steer_Angle = abs(GV.Steering_Real_Angle_3 - 93);
	IV.Steer_Angle_back = abs(GV.Steering_Real_Angle_4 - 93);

	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 Steering_0_working_1_Damping()
{
	speed_selection = (GV.MK32_Key.CH10_LD1 + 1000) / 20;            //最快速度100°/s

	int angle;
	angle = atan2(P_MK32->CH2_LY_V, P_MK32->CH3_LY_H) * 180 / M_PI;

	if((angle >= 45) && (angle <= 135))                              //前进
	{
		GV.Servo_Speed_1 = speed_selection;
		GV.Servo_Speed_2 = 0;
		GV.Servo_Power_2 = 10;
		return;
	}
	else if(P_MK32->CH3_LY_H > 100)
	{
		if((angle >= -45) && (angle <=45))                           //右转
		{
			GV.Servo_Speed_4 = -rotational_speed;
		return;
		}
	}

	else if((angle >= -135) && (angle < -45))                        //后退
	{
		GV.Servo_Speed_1 = -speed_selection;
		GV.Servo_Speed_2 = 0;
		GV.Servo_Power_2 = 10;
		return;
	}
	else if(((angle > 135) && (angle <= 180))
			|| (angle < -135) && (angle >= -179))                    //左转
	{
		GV.Servo_Speed_4 = rotational_speed;
		return;
	}
	else {
		GV.Servo_Speed_1 = 0;
		GV.Servo_Power_1 = 1000;
		GV.Servo_Speed_2 = 0;
		GV.Servo_Power_2 = 10;
		GV.Servo_Speed_4 = 0;
//		update_steering_state();
	}
}

void Steering_0_Damping_1_working()
{
	speed_selection = (GV.MK32_Key.CH10_LD1 + 1000) / 20;            //最快速度100°/s

	int angle;
	angle = atan2(P_MK32->CH2_LY_V, P_MK32->CH3_LY_H) * 180 / M_PI;

	if((angle >= 45) && (angle <= 135))                              //前进
	{
		GV.Servo_Speed_2 = speed_selection;
		GV.Servo_Speed_1 = 0;
		GV.Servo_Power_1 = 10;
		return;
	}
	else if(P_MK32->CH3_LY_H > 100)
	{
		if((angle >= -45) && (angle <=45))                           //右转
		{
			GV.Servo_Speed_3 = -rotational_speed;
		return;
		}
	}

	else if((angle >= -135) && (angle < -45))                        //后退
	{
		GV.Servo_Speed_2 = -speed_selection;
		GV.Servo_Speed_1 = 0;
		GV.Servo_Power_1 = 10;
		return;
	}
	else if(((angle > 135) && (angle <= 180))
			|| (angle < -135) && (angle >= -179))                    //左转
	{
		GV.Servo_Speed_3 = rotational_speed;
		return;
	}
	else {
		GV.Servo_Speed_2 = 0;
		GV.Servo_Power_2 = 1000;
		GV.Servo_Speed_1 = 0;
		GV.Servo_Power_1 = 10;
		GV.Servo_Speed_3 = 0;
//		update_steering_state();
	}
}
void Steering_0_working_1_working()
{
	speed_selection = (GV.MK32_Key.CH10_LD1 + 1000) / 20;            //最快速度100°/s

	int angle;
	angle = atan2(P_MK32->CH2_LY_V, P_MK32->CH3_LY_H) * 180 / M_PI;

	if((angle >= 45) && (angle <= 135))                              //前进
	{
		GV.Servo_Speed_1 = speed_selection;
		GV.Servo_Speed_2 = speed_selection;
		return;
	}
	else if(P_MK32->CH3_LY_H > 100)
	{
		if((angle >= -45) && (angle <=45))                           //右转
		{
			GV.Servo_Speed_3 = -rotational_speed;
			GV.Servo_Speed_4 = -rotational_speed;
		return;
		}
	}

	else if((angle >= -135) && (angle < -45))                        //后退
	{
		GV.Servo_Speed_1 = -speed_selection;
		GV.Servo_Speed_2 = -speed_selection;
		return;
	}
	else if(((angle > 135) && (angle <= 180))
			|| (angle < -135) && (angle >= -179))                    //左转
	{
		GV.Servo_Speed_3 = rotational_speed;
		GV.Servo_Speed_4 = rotational_speed;
		return;
	}
	else {
		GV.Servo_Speed_1 = 0;
		GV.Servo_Speed_2 = 0;
		GV.Servo_Speed_3 = 0;
		GV.Servo_Speed_4 = 0;
		GV.Servo_Power_1 = 1000;
		GV.Servo_Power_2 = 1000;

//		update_steering_state();
	}
}

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