/*
 * fsm.c
 *
 *  Created on: Oct 18, 2024
 *      Author: akeguo
 */
// 自动运行 换道 左补偿/右补偿 control
#include <bsp_tempature.h>
#include <math.h>

#include "BHBF_ROBOT.h"
#include "bsp_include.h"
#include "msp_PID.h"
#include "msp_MK32_1.h"

#include "fsm_state_control.h"

#define M_PI       3.14159265358979323846
#define Move_Horizontal_AngleThreshold_D 85

void PaintGunControl();
void IV_control();
void PV_Data_Reading();
void MoveControl();
void BlowersControl();	// 风机
void WireMotorControl();// 卷线电机
uint8_t MotorErrorDetect();

int Move_Halt_AngleError();
void paintrobot_backwards();
void paintrobot_forwards();
void paint_joysticker_manual_control();
int AbnormalDetect();//异常检测
char is_upper_computer_take_over_control=0;
int CH13_S2_Value ;


void Fsm_Init()
{
	blowers_Control_Init();	// 风机

	//机器人运动状态初始化
	fsm_state_init(&current_robot_move_state, &robot_halt_state);

//	//喷枪电磁阀供电状态初始化(IO-1)
//	fsm_state_init(&current_paintgun_state, &paintgun_off_state);
//	//机器人推杆供电状态初始化（IO-0、推杆）
//	fsm_state_init(&current_motor_power_state, &motor_power_off_state);

	// 风机PWM初始化
	fsm_state_init(&current_blowers_state, &blowers_off_state);// 风机

	GF_BSP_Interrupt_Add_CallBack(
	        DF_BSP_InterCall_TIM8_2ms_PeriodElapsedCallback, GF_Dispatch);
}

/*
 * 主逻辑函数
 * */
void GF_Dispatch()
{
	fsm_state_run(&current_robot_move_state);
	fsm_state_run(&current_blowers_state);// 风机

//	fsm_state_run(&current_motor_power_state);
//	fsm_state_run(&current_paintgun_state);
//	if (current_robot_move_state.p_state != &robot_halt_state)
//	{
//		IsRobotHaltSateChangedFlag = 1; //机器人动了就是1(用于先停喷枪再停车)
//	}

	PV_Data_Reading();	//PV数据更新//按钮复位时 app数据生效
	GV.LaneChangeDistance = GV.PV.LaneChangeDistance;
	IV_control();		//IV数据更新//串口发送到遥控数据

//	/*包含上电按钮检测 急停 SBUS 串口 等*/
//	if(AbnormalDetect()==1) {return;}


//	/***上电检测通过 推杆电机上电**/
//	fsm_state_set(&current_motor_power_state, &motor_power_on_state); /* 上电关 此时设为开 */
//	if (GV.PV.RunMode == Move_Manual) //app 无 的时候才能使用测试喷枪开启按钮
//	{
//		PaintGunControl(); //测试喷枪开启关闭
//	}
//	else if (GV.PV.RunMode >= Move_Vertical_Move_To_Left
//	        && GV.PV.RunMode <= Move_Vertical_Move_To_Right) //不是app无 时 控制开喷枪
//	{
//		if (P_MK32->CH13_S2 != CH13_S2_Value)
//		{
//			fsm_state_set(&current_paintgun_state, &paintgun_on_state); /*打开喷枪电磁阀*/
//			CH13_S2_Value = P_MK32->CH13_S2;
//		}
//	}
//	if(Move_Halt_AngleError()==1)
//	{
//		return;
//	}

	MoveControl();
	BlowersControl();	//风机
//	WireMotorControl();	//卷线电机

}

/* 卷线电机手动控制 */
void WireMotorControl()//卷线电机
{
	int32_t target_velcity = 180 * 8;//RPM，卷线电机速度,减速比180

	if (P_MK32->CH0_RY_H >= -300 && P_MK32->CH0_RY_H <= 300)//水平拨杆300范围内才继续检测 竖直拨杆前进是否按下
	{
		if (P_MK32->CH1_RY_V > 300)//下
		{
			GV.WireMotor.Target_Velcity = target_velcity;
		}
		else if (P_MK32->CH1_RY_V < -300)//上
		{
			GV.WireMotor.Target_Velcity = -1*target_velcity;
		}
		else
		{
			GV.WireMotor.Target_Velcity = 0;
		}
	}
}

//uint8_t blowers_pwm_base = 0;//风机调试用
/* 风机运动状态调度 */
void BlowersControl()// 风机
{
//	uint8_t blowers_pwm_base = 0;//增加默认值，否则遥控连接之前，下式输出为50
//	if(Get_BIT(SystemErrorCode, ComError_MK32_SBus) == CONNECTED)
//	{
//		blowers_pwm_base = (uint8_t)(abs(P_MK32->CH10_LD1 + 1000)  / 200.0f * 10);//0-100/*获取旋钮速度*/
//	}
//	GV.speed_blowers_pwm = blowers_pwm_base;//pwm > 90, 风速最大

	GV.speed_blowers_pwm = SetPWM_Auto();

	// 根据转速切换状态机
	if (GV.speed_blowers_pwm < 10)//<10就停
	{
		fsm_state_set(&current_blowers_state, &blowers_off_state);
	} else
	{
		fsm_state_set(&current_blowers_state, &blowers_on_state);
	}
}

//int8_t speed_selection = 0;//电机速度调试用
/* 机器人运动状态调度 */
void MoveControl()
{
//	if (GV.PV.RunMode == 0)  //app发0 无法行走（app重启时）
//	{
//		fsm_state_set(&current_robot_move_state, &robot_halt_state); /*停止机器人*/
//		return;
//	}
	/* 遥控通讯中断*/
	if (Get_BIT(SystemErrorCode, ComError_MK32_SBus) == DISCONNECTED)
	{
//		P_MK32->IsOnline = 0;//GV.P_MK32.IsOnline = 0;
		fsm_state_set(&current_robot_move_state, &robot_halt_state); /*停止机器人*/
		return;
	}

	/* 速度选择索引 */
	uint8_t speed_selection =  (uint8_t)(abs(P_MK32->CH11_RD1 + 1000)  / 200.0f * 1);//0-10/*获取旋钮速度*/

	GV.Robot_Move_Speed = speed_selection;
//	GV.Robot_Move_Speed   = speed_mpmin_to_rpmin(speed_selection);/*换算速度脉冲*/

	CV.LeftTurnSpeed  = GV.Robot_Move_Speed/2.0;
	CV.RightTurnSpeed = GV.Robot_Move_Speed/2.0;

//	//换道优先级优于普通的运行;
//		if (LaneChangeControl_Paint() == 1)
//		{
//			return;
//		}

	if (P_MK32->CH8_SE == -1000 && P_MK32->CH9_SF == -1000 )//急停
	{
		fsm_state_set(&current_robot_move_state, &robot_halt_state); /*停止机器人*/
		return;
	}
	if (P_MK32->CH7_SD == -1000)//自动作业
	{
//		Automatic_task_Control();
		fsm_state_set(&current_robot_move_state, &robot_Automatic_task_state); /*自动作业*/
		return;
	}
	if (P_MK32->CH4_SA == -1000)//换道（后退）
	{
//		Auto_ChangeLine_Control(1);
		fsm_state_set(&current_robot_move_state, &robot_Auto_ChangeLine_state); /*换道*/
		return;
	}
	if (P_MK32->CH4_SA == 0)//停止换道
	{
		Refresh_MK32_State();//刷新换道状态
	}

	if (P_MK32->CH5_SB == -1000) //自动巡航前进模式 不是换道和自动就能用
	{
		paintrobot_forwards();
		return;
	}
	if (P_MK32->CH5_SB == 1000)	 //自动巡航后退模式,根据app，模式自动选择纠偏与否
	{
		paintrobot_backwards();
		return;
	}

	paint_joysticker_manual_control();	 //摇杆手动控制
}

 /* @brief: 电机出现报警返回1，电机正常返回0 */
uint8_t MotorErrorDetect()
{
	if (GV.LeftMotor.ERROR_Flag != 0 || GV.RightMotor.ERROR_Flag != 0)
	{
		return 1;
	}
	return 0;
}

/**
 * @brief       手动模式下自动巡航前进，
 * @function    前进
 * @param       无
 * @retval      无
 */
void paintrobot_forwards()
{

	/*   竖直向左 向右  */
	if (GV.PV.RunMode == Move_Vertical_Move_To_Left  )
	{
		fsm_state_set(&current_robot_move_state, &robot_move_vertical_task_forwards_state);/* 前进纠偏 */
		return;
	}

	/* 水平 */
	if (GV.PV.RunMode == Move_Vertical_Move_To_Right)
	{
		fsm_state_set(&current_robot_move_state, &robot_move_horizontal_task_forwards_state);/* 前进纠偏 */
		return;
	}


	fsm_state_set(&current_robot_move_state, &robot_forwards_state);/* app 无 前进纠偏 */
	return;
}

/**
 * @brief       手动模式下自动巡航后退，
 * @function    自动后退： 竖直模式下
 * @param       无
 * @retval      无
 */
void paintrobot_backwards()
{

	/*竖直*/
	if (GV.PV.RunMode == Move_Vertical_Move_To_Left)
	{
		fsm_state_set(&current_robot_move_state, &robot_move_vertical_task_backwards_state);/* 后退纠偏 */
		return;
	}

	/* 水平 */
	if (GV.PV.RunMode == Move_Vertical_Move_To_Right)
	{
		fsm_state_set(&current_robot_move_state, &robot_move_horizontal_task_backwards_state);/* 后退纠偏 */
		return;
	}


	fsm_state_set(&current_robot_move_state, &robot_backwards_state);/* 后退 */
	return;
}


//int anangle;	//调试用
uint8_t Index_Joy;	//调试用
/**
 * @brief       手动模式下运动，根据左侧控制按钮的值进行控制
 * @detail		前进，后退;水平模式下的前进后退，需要有校准，竖直的前进后退也需要有校准
 */
void paint_joysticker_manual_control()
{
//	paintrobot_forwards();//调试用
//	return;//调试用

	/*停止*/
	if (abs(P_MK32->CH2_LY_V) <= CV.Joy_Sticker_Value_Allowance && abs(P_MK32->CH3_LY_H) <= CV.Joy_Sticker_Value_Allowance)
	{
		Index_Joy = 1;
		fsm_state_set(&current_robot_move_state, &robot_halt_state); /*停止机器人*/
		return;
	}
	//double atan2(double y, double x) //返回以弧度表示的 y/x 的反正切(-pi,pi)
	int angle = atan2(P_MK32->CH2_LY_V, P_MK32->CH3_LY_H) * 180 / M_PI;
	//anangle = angle;
	/*前进*/
	if (abs(angle - 90) <= CV.Joy_Sticker_Angle_Allowance)
	{
		Index_Joy = 2;
		paintrobot_forwards();  //根据app模式选择纠偏与不纠偏
		return;
	}
	/*后退*/
	if (abs(angle - (-90)) <= CV.Joy_Sticker_Angle_Allowance)
	{
		Index_Joy = 3;
		paintrobot_backwards();
		return;
	}
	/*右转*/
	if (abs(angle - 0) <= CV.Joy_Sticker_Angle_Allowance)
	{
		Index_Joy = 4;
		fsm_state_set(&current_robot_move_state, &robot_turn_right_state); /*右转*/
		return;
	}
	/*左转*/
	//if (abs(angle - 180) <= CV.Joy_Sticker_Angle_Allowance || abs(angle - (-180)) <= CV.Joy_Sticker_Angle_Allowance)
	if (180 - abs(angle) <= CV.Joy_Sticker_Angle_Allowance)
	{
		Index_Joy = 5;
		fsm_state_set(&current_robot_move_state, &robot_turn_left_state); /*左转*/
		return;
	}

}


///* 作业过程中，角度偏差超过允许值时 关闭喷枪并停车
// * 扫描当前车体运动状态 及 当前车体角度
// * */
int Move_Halt_AngleError()
{

	/**********竖直方向**************/
	if (GV.PV.RunMode != Move_Vertical_Move_To_Right  && GV.PV.RunMode != Move_Vertical_Move_To_Left)
	{
		return 0;
	}

	if (current_robot_move_state.p_state != &robot_move_vertical_task_forwards_state
	        && (current_robot_move_state.p_state    !=  &robot_move_vertical_task_backwards_state))
	{
		return 0;
	}

	if (abs(GV.Robot_Angle - CV.RobotUpAngleValue)  >  CV.Robot_Permitted_Angler_Error_Value_E_2D)
	{
		if (current_paintgun_state.p_state == &paintgun_on_state)
		{
			fsm_state_set(&current_robot_move_state, &robot_halt_state); /*停止机器人*/
			fsm_state_set(&current_paintgun_state, &paintgun_off_state); /*关闭喷枪电磁阀*/
			return 1;
		}
	}
	/*app竖直，状态为竖直纠偏  但是在误差内 正常执行*/
	return 0;

}

/* 暂存App传输的PV数据，用于在业务执行过程中进行数据隔离
 */
void PV_Data_Reading() //按钮复位时 app数据生效
{
	if (P_MK32->CH4_SA == 0 && P_MK32->CH5_SB == 0 && P_MK32->CH7_SD == 0)
	{
		//只有在所有指定按钮都释放时，才更新过程变量
		GV.PV = decoded_PV;//通过串口从遥控接收数据
	}
}

/* IV 更新 */
void IV_control()
{
	IV.CurrentAngle 	= GV.Robot_Angle;
	IV.RobotMoveSpeed	= GV.Robot_Move_Speed;//GV.Robot_Move_Speed;
	IV.SBUS_State		= GV.P_MK32.IsOnline;//P_MK32->IsOnline is But_Value[17]
	IV.SystemError		= GV.SystemErrorData.Com_Error_Code; /* SystemErrorCode = &GV.SystemErrorData.Com_Error_Code; */
	IV.Left_Motor_Err 	= GV.LeftMotor.ERROR_Flag;
	IV.Right_Motor_Err 	= GV.RightMotor.ERROR_Flag;
//	IV.IsWorking		= ;
	IV.blowers_pwm		= GV.speed_blowers_pwm;
	IV.ForceValue		= GV.ForceValue;
	IV.CurrentDirection = GV.RobotAngle_DDM;
	IV.CurrentHeight	= GV.Wire_lenth;

}

/* 测试喷枪控制* */
void PaintGunControl()
{
	if (P_MK32->CH6_SC == -1000)
	{
		if (Paint_Gun_ButtonReset_Flag == 0)
		{
			fsm_state_set(&current_paintgun_state, &paintgun_on_state); /*开启喷枪电磁阀*/
			Paint_Gun_ButtonReset_Flag = 1;
		}
	}
	else if (P_MK32->CH6_SC == 0)
	{
		fsm_state_set(&current_paintgun_state, &paintgun_off_state); /*关闭喷枪电磁阀*/
		Paint_Gun_ButtonReset_Flag = 0;
	}
	else if (P_MK32->CH6_SC == 1000)
	{
		fsm_state_set(&current_paintgun_state, &paintgun_off_state); /*关闭喷枪电磁阀*/
		Paint_Gun_ButtonReset_Flag = 0;
	}
}

int AbnormalDetect()
{
//	//上位机此时，其他设备全部处于不可操作状态，喷漆停止、摆臂上抬，只保持轮子的基本操作（为处理遥控器无法使用的情况）
//	if (is_upper_computer_take_over_control == Taken_Over)
//	{
//		GV.PV.RunMode = Move_Manual;
//		fsm_state_set(&current_paintgun_state, &paintgun_off_state);
//		fsm_state_set(&current_motor_power_state, &motor_power_on_state); //电机上电，上电后，激活电机，
//		return 1;
//	}
	/*急停*/
	if (P_MK32->CH8_SE == -1000 && P_MK32->CH9_SF == -1000)
	{
//		fsm_state_set(&current_motor_power_state, &motor_power_off_state); /*关闭电机电磁阀*/
		fsm_state_set(&current_robot_move_state, &robot_halt_state); /*停止机器人*/
//		fsm_state_set(&current_paintgun_state, &paintgun_off_state); /*关闭喷枪电磁阀*/
		return 1;
	}

	/* 按钮未复位 第一次上电  SystemErrorCode不为0 计算值为1 未复位 */
	//按钮未复位  SystemErrorCode 右移ComError_Remote_Button_Reset_State位
	if (Get_BIT(SystemErrorCode, ComError_Remote_Button_Reset_State) == Has_Not_Reset)//Has_Reset 0 复位 1未复位
	{
		CH13_S2_Value = P_MK32->CH13_S2;

//		fsm_state_set(&current_paintgun_state, &paintgun_off_state); /*关闭喷枪电磁阀*/
//		fsm_state_set(&current_motor_power_state, &motor_power_off_state); /*关闭电机电磁阀*/
		fsm_state_set(&current_robot_move_state, &robot_halt_state); /*停止机器人*/
		return 1;
	}
	/* SBUS出错 */
	if (Get_BIT(SystemErrorCode, ComError_MK32_SBus) == DISCONNECTED)
	{
		fsm_state_set(&current_robot_move_state, &robot_halt_state); /*停止机器人*/
//		fsm_state_set(&current_paintgun_state, &paintgun_off_state); /*关闭喷枪电磁阀*/
		return 1;
	}

	if (P_MK32->IsOnline == 0)	//等于0时 sbus有数，但是遥控器关机了，或者失联
	{
		fsm_state_set(&current_robot_move_state, &robot_halt_state); /*停止机器人*/
//		fsm_state_set(&current_paintgun_state, &paintgun_off_state); /*关闭喷枪电磁阀*/
		Is_All_Button_Reset = 0;	//遥控关机
		return 1;
	}

	//陀螺仪无信号
	if (Get_BIT(SystemErrorCode, ComError_TL720D) == DISCONNECTED)
	{
		fsm_state_set(&current_robot_move_state, &robot_halt_state); /*停止机器人*/
//		fsm_state_set(&current_paintgun_state, &paintgun_off_state); /*关闭喷枪电磁阀*/
		return 1;
	}
	//电子罗盘无信号
	if (Get_BIT(SystemErrorCode, ComError_DDM360B) == DISCONNECTED)
	{
		fsm_state_set(&current_robot_move_state, &robot_halt_state); /*停止机器人*/
		return 1;
	}
	//左电机无信号
	if (Get_BIT(SystemErrorCode, ComError_LS_LeftMotor) == DISCONNECTED)
	{
		fsm_state_set(&current_robot_move_state, &robot_halt_state); /*停止机器人*/
		return 1;
	}
	//右电机无信号
	if (Get_BIT(SystemErrorCode, ComError_LS_RightMotor) == DISCONNECTED)
	{
		fsm_state_set(&current_robot_move_state, &robot_halt_state); /*停止机器人*/
		return 1;
	}

	return 0;
}