/*
 * Function_Booth.c
 *
 *  Created on: Mar 21, 2025
 *      Author: bihon
 */

#include "BHBF_ROBOT.h"
#include "Function_Booth.h"
#include "Timer.h"

uint8_t flow_step;				//用于控制自动喷漆程序流程
uint8_t stop_flag;				//用于标志自动喷漆流程是否结束
void Elite_Motion_CMD(start_position_dir dir, uint8_t Order);
uint8_t Elite_Crash_Detect(uint8_t switchFlag, double Position);
uint8_t ElevatedCrashMark = 0;
/*
 * @brief 检查是否通过设定的检查点
 * @param current_Pos 当前位置值
 * @param previous_Pos 初始位置值
 * @param check_Pos 检查点位置值
 * @return 通过检查点返回 1，未通过返回 0
 */
int Position_Check(
		double current_Pos, 	//当前位置
		double previous_Pos, 	//初始位置
		double check_Pos		//检查点
		)
{
	return ((current_Pos >= check_Pos && previous_Pos < check_Pos) ||
	            (current_Pos < check_Pos && previous_Pos >= check_Pos));
}

void VehicleConnectDetect()
{
	static uint16_t TimeCount = 0;
	static uint8_t LastState = 0xFF;
	static uint8_t BreakCount = 0;
	TimeCount++;
	if(TimeCount % 50 == 0)
	{//每100毫秒读取大臂车心跳数据
		if(TimeCount >= 1500)
		{//3S以上无异常复位
			TimeCount = 0;//时间计数清零
			if(BreakCount < 20)BreakCount = 0;//异常计数清零
		}

		if(LastState == VehicleHeartReceive)
		{//心跳值未发生变化
			BreakCount++;//异常计数增加
			if(BreakCount > 20)
			{//连续20次无变化（2S）报警
				if(BreakCount % 20 == 0)
				{
					ElevatedCrashMark++;//记录通讯异常次数
				}
				Raspberry_Command(Stop_Painting);//向树莓派发送停止
			}
		}else
		{//心跳值发生变化
			BreakCount = 0;//异常计数清零
		}
		LastState = VehicleHeartReceive;
	}
}

void OrbitOffsetSend()
{
	static int OffsetSendCount = 0;
	/*👇👇👇发送偏移量👇👇👇*/
	OffsetSendCount++;
	if(OffsetSendCount >= 1000)
	{
		OffsetSendCount = 0;
		SendOffset2Raspberry();
	}
	/*👆👆👆发送偏移量👆👆👆*/
}
/**
 * @brief 超声波测距
 */
void Position_Detect()
{
	//读取四个传感器数据，
	static uint8_t ID = 0x00;
	static uint8_t state;
	static uint16_t count = 0;
	switch(state)
	{
	case 0://循环测距1
		count++;
		if(count >= 25)
		{
			ID++;		//超声测距传感器ID
			count = 0;	//重新计数
			state++;
		}
		break;
	case 1://循环测距2
		GF_Sonic_Inquiry(ID);
		switch(ID)
		{
		//更新距离信息
		case 0x01:
			GV.Vehicle_Cmd.Ranging_Sensor_Data_1 = GV.SonicData.Sonic_1_Data;
			break;
		case 0x02:
			GV.Vehicle_Cmd.Ranging_Sensor_Data_2 = GV.SonicData.Sonic_2_Data;
			break;
		case 0x03:
			GV.Vehicle_Cmd.Ranging_Sensor_Data_3 = GV.SonicData.Sonic_3_Data;
			break;
		case 0x04:
			GV.Vehicle_Cmd.Ranging_Sensor_Data_4 = GV.SonicData.Sonic_4_Data;
			ID = 0;
			break;
		default:
			ID = 0;
			break;
		}
		state = 0;
		break;
	default:
		break;
	}
}

/*
 * @brief 手动操作模式
 * @note 对于需机械臂完成的动作，直接向机械臂发送相应指令
 * @note 开关喷枪命令调用开关喷枪函数完成
 * */
void Manual_Control_Mode()
{
	static int16_t last_mode;//保存上一次的操作指令
	static int16_t mode;	//用于读取当前操作指令
	flow_step = 0;		//自动喷漆模式流程置0
	last_mode = mode;
	mode = GV.Vehicle_Cmd.Robot_Mode;//读取当前操作指令
	if(mode != last_mode)//操作指令发生变化
	{
		if(mode >= 1 && mode <= 18)
		{//向机械臂发送对应指令
			Elite_Command((BOperationState)mode);
		}else if(mode == 40)
		{//手动开喷枪
			Paint_Gun_Control(paint_gun_open);//开启喷枪
		}else if(mode == 41)
		{//手动关喷枪
			Paint_Gun_Control(paint_gun_close);//关闭喷枪
		}
	}
}

/*
 * @brief:	机械臂伺服上电
 */
void Elite_Power_On()
{
	Elite_Command(SERVO_POWER_ON_3);
}

/*
 * @brief:	机械臂清除报警
 */
void Elite_Clear_Alarm()
{
	Elite_Command(CLEAR_ALARM_1);
}

/**
 * @brief 正常作业模式
 * @param dir 1：举升车上升；0：举升车下降
 * @return 	0:正在作业
 * 			1:作业停止
 * 			2:意外停机
 */
uint8_t StartPainting(start_position_dir dir)
{
	static uint8_t Count = 0;
	uint8_t ret = 0;

	switch(flow_step)
	{
	case 0:
		//喷涂开始
		Raspberry_Command(Start_Painting);
		flow_step++;
		break;
	case 1:
		//喷涂结束
		if(StopPainting())
		{
			Raspberry_Command(Stop_Painting);
			flow_step = 2;
		}else
		{
			OrbitOffsetSend();
		}
		break;
	case 2:
		Count++;
		if(Count >= 25)
		{
			Count = 0;
			Raspberry_Command(Stop_Painting);
			flow_step = 3;
		}
		break;
	case 3:
		if(NextCmdPermission())
		{
			flow_step = 0;//流程置零
			Raspberry_Echo_Received();
			ret = 1;
		}else
		{
			Count++;
			if(Count >= 50)
			{
				Count = 0;
				Raspberry_Command(Stop_Painting);
			}
		}

		if(EliteArmCrashed())
		{
			EliteArmCrash_Reset();
			flow_step = 6;
		}
		break;
	case 6:
		//意外停机
		Raspberry_Command(Painting_Gun_Close);//关喷枪
		flow_step = 0;//流程置零
		ret = 2;
		break;
	default:
		break;
	}
	return ret;
}

/*
 * @brief	上电试运行
 * @return	1：试运行结束
 * 			0：尚未完成试运行
 */
uint8_t TrialOperation()
{
	static uint8_t state = 0;
	uint8_t ret = 0;
	switch(state)
	{
	case 0:
		state++;
		break;
	case 1:
		Raspberry_Command(Trial_Position);
		state++;
		break;
	case 2:
		if(NextCmdPermission() == 1)
		{
			Raspberry_Echo_Received();
			ret = 1;
			state = 0;
		}
		break;
	default:
		break;
	}
	return ret;
}

/**
 * @brief 	前往初始位置
 * @return  到达位置（计时结束）返回1,
 * 			否则返回0。
 */
uint8_t GoToStartPosition()
{
	static uint8_t state = 0;
	uint8_t ret = 0;
	switch(state)
	{
	case 0:
		state++;
		break;
	case 1:
		Raspberry_Command(Start_Position);//前往初始位置(洗枪位置)
		state++;
		//前往初始上电位置
		break;
	case 2:
		if(NextCmdPermission() == 1)
		{
			Raspberry_Echo_Received();
			ret = 1;
			state = 0;
		}
		break;
	default:
		break;
	}
	return ret;
}

/*
 * @brief 机械臂前往洗枪位置
 * @return 	0:未到达位置
 * 			1:到达位置
 */
uint8_t GoToWashGunPosition()
{
	static uint8_t state = 0;
	uint8_t ret = 0;
	switch(state)
	{
	case 0:
		state++;
		break;
	case 1:
		Raspberry_Command(Wash_Gun_Position);//前往初始位置(洗枪位置)
		state++;
		//前往初始上电位置
		break;
	case 2:
		if(NextCmdPermission() == 1)
		{
			Raspberry_Echo_Received();
			ret = 1;
			state = 0;
		}
		break;
	default:
		break;
	}
	return ret;
}

/*
 * @brief 机械臂前往初始位置
 * @param dir:decend前往初始正位置，ascend前往初始逆位置
 * @return 	0:未到达位置
 * 			1:到达位置
 */
uint8_t GoToPaintPosition(start_position_dir dir)
{
	static uint8_t state = 0;
	uint8_t ret = 0;
	switch(state)
	{
	case 0:
		if(dir == decend)
		{
			//前往下降喷涂起始位置
			if(InverseStartFlag())
			{//逆位置
				Raspberry_Command(Decline_Painting_3);
			}else
			{//正位置
				Raspberry_Command(Decline_Painting_1);
			}
		}else if(dir == ascend)
		{
			//前往初始逆位置
			if(InverseStartFlag())
			{//逆位置
				Raspberry_Command(Rising_Painting_3);
			}else
			{//正位置
				Raspberry_Command(Rising_Painting_1);
			}
		}
		state++;
		break;
	case 1:
		if(NextCmdPermission() == 1)
		{
			Raspberry_Echo_Received();
			ret = 1;
			state = 0;
		}
		break;
	default:
		break;
	}
	return ret;
}

/*
 * @brief 机械臂前往停机位置
 * @return 	0:未到达位置
 * 			1:到达位置
 */
uint8_t GoToStopPosition()
{
	static uint8_t state;
	uint8_t ret = 0;
	switch(state)
	{
	case 0:
		Raspberry_Command(Stopping_Position);
		state++;
		break;
	case 1:
		if(NextCmdPermission() == 1)
		{
			Raspberry_Echo_Received();
			ret = 1;
			state = 0;
		}
		break;
	default:
		break;
	}
	return ret;
}

/**
 * @brief 向机械臂发送指令
 * @param dir 1：举升车上升；0：举升车下降
 */
void Elite_Motion_CMD(start_position_dir dir, uint8_t Order)
{
	if(dir == decend)//大臂车下降
	{
		if(Order == 0)
		{
			//第一段正位置
			Raspberry_Command(Decline_Painting_2);
		}else if(Order == 1)
		{
			//第二段正位置
			Raspberry_Command(Decline_Painting_3);
		}else if(Order == 2)
		{
			Raspberry_Command(Decline_Painting_4);
		}else if(Order == 3)
		{
			Raspberry_Command(Decline_Painting_1);
		}
	}else if(dir == ascend)//大臂车上升
	{
		if(Order == 0)
		{
			//第一段逆位置
			Raspberry_Command(Rising_Painting_2);
		}else if(Order == 1)
		{
			//第二段逆位置
			Raspberry_Command(Rising_Painting_3);
		}else if(Order == 2)
		{
			Raspberry_Command(Rising_Painting_4);
		}else if(Order == 3)
		{
			Raspberry_Command(Rising_Painting_1);
		}
	}
}

/**
 * @brief 控制喷枪开关
 * @param state 喷枪开关状态，paint_gun_close表示关闭，paint_gun_open表示开启
 */
void Paint_Gun_Control(paint_gun_state state)
{
	switch(state)
	{
	case paint_gun_open:
		//喷枪开启
		Raspberry_Command(Painting_Gun_Open);
		break;
	case paint_gun_close:
		//喷枪关闭
		Raspberry_Command(Painting_Gun_Close);
		break;
	}
}
/*
 * @brief:	远程启动函数
 * @note：	1.伺服上电
 * 			2.清除报警
 * 			3.“精确”
 */
void Elite_Remote_PowerOn()
{
	static uint8_t state;
	static uint8_t cnt;
	HAL_GPIO_WritePin(OUT_1_GPIO_Port, OUT_1_Pin, GPIO_PIN_RESET);//RESET 远程启动继电器上电
//	HAL_GPIO_WritePin(OUT_1_GPIO_Port, OUT_1_Pin, GPIO_PIN_SET);//SET 远程启动继电器断电
	switch(state)
	{
	case 0:
		Elite_Command(SERVO_POWER_ON_3);//伺服上电
		Timer_Set(10000, 0);
		state++;
		break;
	case 1:
		if(Timer_Start_Count() == 1)
		{
			Elite_Command(CLEAR_ALARM_1);//清除报警
			cnt++;
			if(cnt >= 3)
			{
				cnt = 0;
				state = 2;
			}else
			{
				Timer_Set(2000, 0);
				state = 1;
			}
		}
		break;
	case 2:

		break;
	default:
		break;
	}
}
void Elite_Remote_PowerOn2()
{
	HAL_GPIO_WritePin(OUT_1_GPIO_Port, OUT_1_Pin, GPIO_PIN_RESET);
	HAL_Delay(3000);
	HAL_GPIO_WritePin(OUT_1_GPIO_Port, OUT_1_Pin, GPIO_PIN_SET);
	HAL_Delay(30000);
	for(int i = 0; i < 3; i++)
	{
		Elite_Command(CLEAR_ALARM_1);	//清除报警
		HAL_Delay(500);
	}
	for(int i = 0; i < 3; i++)
	{
		Elite_Command(SERVO_POWER_ON_3);//伺服上电
		HAL_Delay(500);
	}
	Elite_Command(ARM_PRECISE_9);	//"精确"
	HAL_Delay(1000);
}

/*
 * @brief 机械臂意外停止检测函数
 * @param switchFlag：开启1/关闭0标志
 * @return 0:未发生意外停止或未在检测
 * 		   1：机械臂意外停止
 * @note 通过检测机械臂关节电机1位置信息来判断机械臂是否发生碰撞报警或急停
 * @note 仅可以检测机械臂运动过程中的意外停止
 * @note 机械臂停止运动时会给出信号，也可以检测运动过程是否结束
 */
uint8_t Elite_Crash_Detect(uint8_t switchFlag, double Position)
{
	// 计数变量，每调用20次函数进行一次位置比较
	static int cycleCount = 0;
	// 记录当前位置与前一次位置相等的次数
	static int equalPositionCount = 0;
	 // 记录前一次的位置信息
	static double previousPosition = 0.0;
	// 记录当前的位置信息
	static double currentPosition = 0.0;
	if(switchFlag == 1)
	{
		//开启检测
		cycleCount++;
		if(cycleCount >= 20)
		{
			cycleCount = 0;//重新计数
			// 每20次调用函数时(40ms)，对比关节位置信息
			previousPosition = currentPosition;
			currentPosition = Position;

			// 定义一个位置变化的阈值，避免因微小误差导致误判
			const double positionThreshold = 0.001;
			if(fabs(currentPosition - previousPosition) < positionThreshold)
			{
				equalPositionCount++;
			}else
			{
				equalPositionCount = 0;
			}
			if(equalPositionCount == 5)
			{
				 // 连续5次位置未发生变化，判定为意外停止
				equalPositionCount = 0;
				return 1;
			}else
			{
				return 0;
			}
		}
	}else
	{
		//停止检测
		cycleCount = 0;
		equalPositionCount = 0;
		return 0;
	}
	return 0;
}