/*
 * change_line_control.c
 *
 *  Created on: 2025年8月12日
 *      Author: xsq
 */
#include "change_line_control.h"
#include "BHBF_ROBOT.h"

#define Veritical_To_Left_Flag 1
#define Veritical_To_Right_Flag 2


int LaneChangeWaittime_ms = 0;
/*
 * return: ms
 */
int32_t RunTime_DistanceCm_SpeedE_2MPMin();
int LaneChangeWaittime = 0;

char LaneChangeFlage = -1;


Lane_Vertical_ChangeState Current_Vertical_ChangeState;

LaneChangeControlSTATE  VerticalLaneChangeState; /*当前换道处于开始或者结束*/

/* 手动换道
 * 换道依据当前车头朝向
// * */
//int LaneChangeControl_Paint()
//{
////机器人SA按键处于中间状态
////	if (P_MK32->CH4_SA == 0)
////	{
////		VerticalLaneChangeState = Lane_Change_Start;
////		Current_Vertical_ChangeState = VerticalChange_StateZero;
////
////		if (abs(GV.Robot_Angle_Desire - CV.RobotUpAngleValue)  //头朝上
////		<= 45 * 100)
////		{
////			if (GV.PV.RunMode == Move_Vertical_Move_To_Left)
////			{
////				LaneChangeFlage = Veritical_To_Left_Flag;
////
////			}
////			if (GV.PV.RunMode == Move_Vertical_Move_To_Right)
////			{
////				LaneChangeFlage = Veritical_To_Right_Flag;
////
////			}
////			return 0;
////		}
////		LaneChangeFlage = -1;
////		return 0;
////	}
////
////	if (P_MK32->CH4_SA == -1000) /*竖直上or水平换道*/
////	{
////
////		if (GV.PV.RunMode != Move_Vertical_Move_To_Left
////		        && GV.PV.RunMode != Move_Vertical_Move_To_Right)/*竖直换道*/
////		{
////			return 1;
////		}
////
////
////		if (VerticalLaneChangeState != Lane_Change_Start)
////		{
////			fsm_state_set(&current_robot_move_state, &robot_halt_state);
////			return 1;
////		}
////
////		if (LaneChangeFlage == Veritical_To_Left_Flag) /*向左作业*/
////		{
////			Vertical_Lane_Change_From_Right_To_Left_UP_Control(); /*竖直上换道*/
////			return 1;
////		}
////		if (LaneChangeFlage == Veritical_To_Right_Flag) /*向右作业*/
////		{
////			Vertical_Lane_Change_From_Left_To_Right_UP_Control(); /*竖直上换道*/
////			return 1;
////		}
////		return 1;
////
////	}
////
////	if (P_MK32->CH4_SA == 1000) //竖直下换道
////	{
////		if (GV.PV.RunMode != Move_Vertical_Move_To_Left
////		        && GV.PV.RunMode != Move_Vertical_Move_To_Right)
////		{
////			return 1;
////		}
////
////		if (VerticalLaneChangeState != Lane_Change_Start) /*换道结束了*/
////		{
////			fsm_state_set(&current_robot_move_state, &robot_halt_state);
////			return 1;
////		}
////		if (LaneChangeFlage == Veritical_To_Left_Flag) /*向左作业*/
////		{
////			Vertical_Lane_Change_From_Right_To_Left_Down_Control(); /*竖直下换道*/
////			return 1;
////		}
////		if (LaneChangeFlage == Veritical_To_Right_Flag) /*向右作业*/
////		{
////			Vertical_Lane_Change_From_Left_To_Right_Down_Control(); /*竖直下换道*/
////			return 1;
////		}
////		return 1;
////	}
////
////	return 0;
//
//}
//
//
//
///* 竖直从左往右作业 上端 向右换道 最终头朝上
// * */
//void Vertical_Lane_Change_From_Left_To_Right_UP_Control()
//{
//
//	GV.Robot_Move_Speed = speed_M_min_toE01_M_min( CV.Lane_Change_Speed_m_per_min);
//	switch (Current_Vertical_ChangeState)
//	{
//		case VerticalChange_StateZero:
//		{
//			Current_Vertical_ChangeState = VerticalChange_TurnToLeft;
//			break;
//		}
//		case VerticalChange_TurnToLeft:
//		{
//			if (abs(GV.Robot_Angle_Desire - CV.RobotLeftAngleValue)
//			        >= CV.Allowable_Error_For_Angle_Tracking)
//			{
//				fsm_state_set(&current_robot_move_state,
//				        &robot_move_head_to_left_enum_state); /* 移动至头朝左  */
//			}
//			else
//			{
//
//				fsm_state_set(&current_robot_move_state,
//				        &robot_move_horizontal_task_backwards_left_state);/* 水平纠偏后退 头朝左*/
//				//开启计时
//				timer_handler_1.start_timer = 1;
//				Current_Vertical_ChangeState = VerticalChange_DelayMove;
//			}
//			break;
//		}
//		case VerticalChange_DelayMove:
//		{
//			LaneChangeWaittime = RunTime_DistanceCm_SpeedE_2MPMin();
//			if (CompareTimer(LaneChangeWaittime, &timer_handler_1))
//			{
//
//				Current_Vertical_ChangeState = VerticalChange_TurnToUP;
//			}
//			break;
//		}
//		case VerticalChange_TurnToUP:
//		{
//			if (abs(GV.Robot_Angle_Desire - CV.RobotUpAngleValue)
//			        >= CV.Allowable_Error_For_Angle_Tracking)
//			{
//				fsm_state_set(&current_robot_move_state,
//				        &robot_move_head_to_up_enum_state); /* 移动至头朝上  */
//			}
//			else
//			{
//				Current_Vertical_ChangeState = VerticalChange_End;
//			}
//
//			break;
//		}
//		case VerticalChange_End:
//		{
//			VerticalLaneChangeState = Lane_Change_Stop;
//			break;
//		}
//		default:
//			break;
//	}
//}
//
///* 竖直从左往右作业 下端 向右换道 最终头朝上
// * */
//void Vertical_Lane_Change_From_Left_To_Right_Down_Control()
//{
//
//	GV.Robot_Move_Speed = speed_M_min_toE01_M_min( CV.Lane_Change_Speed_m_per_min);
//	switch (Current_Vertical_ChangeState)
//	{
//		case VerticalChange_StateZero:
//		{
//			Current_Vertical_ChangeState = VerticalChange_TurnToRight;
//
//			break;
//		}
//		case VerticalChange_TurnToRight:
//		{
//			if (abs(GV.Robot_Angle_Desire - CV.RobotRightAngleValue)
//			        >= CV.Allowable_Error_For_Angle_Tracking)
//			{
//				fsm_state_set(&current_robot_move_state,
//				        &robot_move_head_to_right_enum_state); /* 移动至头朝右  */
//			}
//			else
//			{
//
//				fsm_state_set(&current_robot_move_state,
//				        &robot_move_horizontal_task_forwards_right_state);/* 水平纠偏前进 头朝右*/
//				//开启计时
//				timer_handler_1.start_timer = 1;
//				Current_Vertical_ChangeState = VerticalChange_DelayMove;
//			}
//			break;
//		}
//		case VerticalChange_DelayMove:
//		{
//			LaneChangeWaittime = RunTime_DistanceCm_SpeedE_2MPMin();
//			if (CompareTimer(LaneChangeWaittime, &timer_handler_1))
//			{
//
//				Current_Vertical_ChangeState = VerticalChange_TurnToUP;
//
//			}
//			break;
//		}
//		case VerticalChange_TurnToUP:
//		{
//			if (abs(GV.Robot_Angle_Desire - CV.RobotUpAngleValue)
//			        >= CV.Allowable_Error_For_Angle_Tracking) //误差在1度内
//			{
//				fsm_state_set(&current_robot_move_state,
//				        &robot_move_head_to_up_enum_state); /* 移动至头朝上  */
//			}
//			else
//			{
//				Current_Vertical_ChangeState = VerticalChange_End;
//			}
//			break;
//		}
//		case VerticalChange_End:
//		{
//			VerticalLaneChangeState = Lane_Change_Stop;
//			break;
//		}
//		default:
//			break;
//	}
//
//}
//
//
///* 竖直从右往左作业 上端 向左换道 最终头朝上
// * */
//void Vertical_Lane_Change_From_Right_To_Left_UP_Control()
//{
//
//	GV.Robot_Move_Speed = speed_M_min_toE01_M_min( CV.Lane_Change_Speed_m_per_min);
//	switch (Current_Vertical_ChangeState)
//	{
//		case VerticalChange_StateZero:
//		{
//			Current_Vertical_ChangeState = VerticalChange_TurnToRight;
//			break;
//		}
//		case VerticalChange_TurnToRight:
//		{
//			if (abs(GV.Robot_Angle_Desire - CV.RobotRightAngleValue)
//			        >= CV.Allowable_Error_For_Angle_Tracking)
//			{
//				fsm_state_set(&current_robot_move_state,
//				        &robot_move_head_to_right_enum_state); /* 移动至头朝右  */
//			}
//			else
//			{
//
//				fsm_state_set(&current_robot_move_state,
//				        &robot_move_horizontal_task_backwards_right_state);/* 水平纠偏后退 头朝右*/
//				//开启计时
//				timer_handler_1.start_timer = 1;
//				Current_Vertical_ChangeState = VerticalChange_DelayMove;
//			}
//			break;
//		}
//		case VerticalChange_DelayMove:
//		{
//			LaneChangeWaittime = RunTime_DistanceCm_SpeedE_2MPMin();
//			if (CompareTimer(LaneChangeWaittime, &timer_handler_1))
//			{
//				Current_Vertical_ChangeState = VerticalChange_TurnToUP;
//			}
//			break;
//		}
//		case VerticalChange_TurnToUP:
//		{
//			if (abs(GV.Robot_Angle_Desire - CV.RobotUpAngleValue)
//			        >= CV.Allowable_Error_For_Angle_Tracking)
//			{
//				fsm_state_set(&current_robot_move_state,
//				        &robot_move_head_to_up_enum_state); /* 移动至头朝上  */
//			}
//			else
//			{
//				Current_Vertical_ChangeState = VerticalChange_End;
//			}
//
//			break;
//		}
//		case VerticalChange_End:
//		{
//			VerticalLaneChangeState = Lane_Change_Stop;
//			break;
//		}
//		default:
//			break;
//	}
//
//}
//
///* 竖直从右往左作业 下端 向左换道 最终头朝上
// * */
//void Vertical_Lane_Change_From_Right_To_Left_Down_Control()
//{
//
//	GV.Robot_Move_Speed = speed_M_min_toE01_M_min( CV.Lane_Change_Speed_m_per_min);//转完 前进or后退得速度
//	switch (Current_Vertical_ChangeState)
//	{
//		case VerticalChange_StateZero:
//		{
//			Current_Vertical_ChangeState = VerticalChange_TurnToLeft;
//
//			break;
//		}
//		case VerticalChange_TurnToLeft:
//		{
//			if (abs(GV.Robot_Angle_Desire - CV.RobotLeftAngleValue)
//			        >= CV.Allowable_Error_For_Angle_Tracking)
//			{
//				fsm_state_set(&current_robot_move_state,
//				        &robot_move_head_to_left_enum_state); /* 移动至头朝左  */
//			}
//			else
//			{
//				fsm_state_set(&current_robot_move_state,
//				        &robot_move_horizontal_task_forwards_left_state);/* 水平纠偏前进 头朝左*/
//				//开启计时
//				timer_handler_1.start_timer = 1;
//				Current_Vertical_ChangeState = VerticalChange_DelayMove;
//			}
//			break;
//		}
//		case VerticalChange_DelayMove:
//		{
//			LaneChangeWaittime = RunTime_DistanceCm_SpeedE_2MPMin();
//			if (CompareTimer(LaneChangeWaittime, &timer_handler_1))
//			{
//
//				Current_Vertical_ChangeState = VerticalChange_TurnToUP;
//
//			}
//			break;
//		}
//		case VerticalChange_TurnToUP:
//		{
//			if (abs(GV.Robot_Angle_Desire - CV.RobotUpAngleValue)
//			        >= CV.Allowable_Error_For_Angle_Tracking) //误差在1度内
//			{
//				fsm_state_set(&current_robot_move_state,
//				        &robot_move_head_to_up_enum_state); /* 移动至头朝上  */
//			}
//			else
//			{
//				Current_Vertical_ChangeState = VerticalChange_End;
//			}
//			break;
//		}
//		case VerticalChange_End:
//		{
//			VerticalLaneChangeState = Lane_Change_Stop;
//			break;
//		}
//		default:
//			break;
//	}
//
//}
//
///*
// * return: ms
// */
//int32_t RunTime_DistanceCm_SpeedE_2MPMin()
//{
//
////		LaneChangeWaittime_ms = 600 * (GV.PV.LaneChangeDistance+CV.Vertical_ChangeLane_Compensation)
////		        / CV.Lane_Change_Speed_m_per_min;
////
////	return LaneChangeWaittime_ms;
//
//}