/*
 * 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

#define Horizontal_Head_To_Left_Flag 0
#define Horizontal_Head_To_Right_Flag 1
int LaneChangeWaittime_ms = 0;
/*
 * return: ms
 */
int32_t RunTime_DistanceCm_SpeedE_2MPMin();
int LaneChangeWaittime = 0;

char LaneChangeFlage = -1;

Lane_Horizontal_ChangeState CurrentHorizontal_ChangeState; /*当前换道处于哪一步*/
Lane_Vertical_ChangeState Current_Vertical_ChangeState;

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


/* 手动换道
 * 换道依据当前车头朝向 */
int LaneChangeControl_Rough()
{
//机器人SA按键处于中间状态
	if (P_MK32->CH4_SA == 0)
	{
		HorizontalLaneChangeState = Lane_Change_Start; //start and  stop
		CurrentHorizontal_ChangeState = HorizontalChange_StateZero;  //设定初始方向

		VerticalLaneChangeState = Lane_Change_Start;
		Current_Vertical_ChangeState = VerticalChange_StateZero;
		if (abs(GV.Robot_Angle - CV.RobotLeftAngleValue)  //头朝左
		<= 45 * 100)
		{
			// 头朝左，换道意味着头
			LaneChangeFlage = Horizontal_Head_To_Right_Flag;
			return 0;
		}
		if (abs(GV.Robot_Angle - CV.RobotRightAngleValue)  //头朝右
		<= 45 * 100)
		{
			LaneChangeFlage = Horizontal_Head_To_Left_Flag;
			return 0;
		}

		if (abs(GV.Robot_Angle - 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水平换道*/
	{
		// 防止无模式下，SB按下后，之后按下SA，SA抢占SB控制权
		if (GV.PV.RunMode == Move_Manual)
		{
			return 0;
		}
//		if (GV.PV.RunMode != Move_Vertical_Move_To_Left && GV.PV.RunMode != Move_Vertical_Move_To_Right)/*竖直换道*/
//		{
//			return 0;
//		}

		if (GV.PV.RunMode == Move_Horizontal_Move)/*水平换道*/
		{
			if (HorizontalLaneChangeState != Lane_Change_Start)
			{
				fsm_state_set(&current_robot_move_state, &robot_halt_state);
				return 1;
			}

			if (LaneChangeFlage == Horizontal_Head_To_Left_Flag) /*需要换到头朝左*/
			{
				Horizontal_Lane_Change_Turn_To_Left_Control(); /*水平左换道*/
				return 1;
			}
			if (LaneChangeFlage == Horizontal_Head_To_Right_Flag)
			{
				Horizontal_Lane_Change_Turn_To_Right_Control(); /*水平右换道*/
				return 1;
			}
			return 1;
		}
		/*************/



//		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) //竖直下换道
	{

		// 防止无模式下，SB按下后，之后按下SA，SA抢占SB控制权
		if (GV.PV.RunMode == Move_Manual)
		{
			return 0;
		}
//		if (GV.PV.RunMode != Move_Vertical_Move_To_Left && GV.PV.RunMode != Move_Vertical_Move_To_Right)/*竖直换道*/
//		{
//			return 0;
//		}

		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 Horizontal_Lane_Change_Turn_To_Right_Control()
{

	GV.Robot_Move_Speed = CV.Lane_Change_Speed_m_per_min*10;
	switch (CurrentHorizontal_ChangeState)
	{
		case HorizontalChange_StateZero:
		{
			CurrentHorizontal_ChangeState = HorizontalChange_TurnToUP;
			break;
		}
		case HorizontalChange_TurnToUP:
		{
			if (abs(GV.Robot_Angle - CV.RobotUpAngleValue)
			        >= CV.Allowable_Error_For_Angle_Tracking)
			{

				fsm_state_set(&current_robot_move_state,
				        &robot_move_head_to_up_enum_state); /*转到朝上*/
			}
			else
			{

				fsm_state_set(&current_robot_move_state,
				        &robot_move_vertical_task_backwards_state); /*转到朝左*/
				//开启计时
				timer_handler_1.start_timer = 1;
				CurrentHorizontal_ChangeState = HorizontalChange_DelayMove;
			}
			break;
		}
		case HorizontalChange_DelayMove:
		{
			//m/min 100cm/60s  100cm/(60*1000)mm 1cm/600ms
			LaneChangeWaittime = RunTime_DistanceCm_SpeedE_2MPMin();
			if (CompareTimer(LaneChangeWaittime, &timer_handler_1))
			{
				CurrentHorizontal_ChangeState = HorizontalChange_TurnToRight;
//				fsm_state_set(&current_robot_move_state,
//				        &robot_move_head_to_right_enum_state); /*转到朝上*/
				fsm_state_set(&current_robot_move_state, &robot_move_head_to_right_add_adjust_enum_state); /*转到朝右+竖直微调项*/

			}
			break;
		}
		case HorizontalChange_TurnToRight:
		{
//			fsm_state_set(&current_robot_move_state,
//			        &robot_move_head_to_right_enum_state); /*转到朝右*/
			fsm_state_set(&current_robot_move_state, &robot_move_head_to_right_add_adjust_enum_state); /*转到朝右+竖直微调项*/
			if (abs(GV.Robot_Angle - (CV.RobotRightAngleValue + GV.PV.Vertical_Calibration))
			        <= CV.Allowable_Error_For_Angle_Tracking)
			{
				CurrentHorizontal_ChangeState = HorizontalChange_End;
			}
			break;
		}
		case HorizontalChange_End:
		{
			HorizontalLaneChangeState = Lane_Change_Stop;
			break;
		}

		default:
			break;
	}

}

/* 水平  向下换道 最终头朝左
 * */
void Horizontal_Lane_Change_Turn_To_Left_Control()
{

	GV.Robot_Move_Speed = speed_M_min_toE01_M_min( CV.Lane_Change_Speed_m_per_min);

	switch (CurrentHorizontal_ChangeState)
	{
		case HorizontalChange_StateZero:
		{
			CurrentHorizontal_ChangeState = HorizontalChange_TurnToUP;
			break;
		}
		case HorizontalChange_TurnToUP:
		{
			if (abs(GV.Robot_Angle - CV.RobotUpAngleValue)
			        >= CV.Allowable_Error_For_Angle_Tracking)
			{

				fsm_state_set(&current_robot_move_state,
				        &robot_move_head_to_up_enum_state); /*转到朝上*/
			}
			else
			{

				fsm_state_set(&current_robot_move_state,
				        &robot_move_vertical_task_backwards_state); /*后退纠偏*/
				//开启计时
				timer_handler_1.start_timer = 1;
				CurrentHorizontal_ChangeState = HorizontalChange_DelayMove;
			}
			break;
		}
		case HorizontalChange_DelayMove:
		{
			LaneChangeWaittime = RunTime_DistanceCm_SpeedE_2MPMin();
			if (CompareTimer(LaneChangeWaittime, &timer_handler_1))		//计时结束
			{
				CurrentHorizontal_ChangeState = HorizontalChange_TurnToLeft;
//				fsm_state_set(&current_robot_move_state,
//				        &robot_move_head_to_left_enum_state); /*转到朝左*/
				fsm_state_set(&current_robot_move_state, &robot_move_head_to_left_add_adjust_enum_state); /*转到朝左+竖直微调项*/


			}
			break;
		}
		case HorizontalChange_TurnToLeft:
		{
//			fsm_state_set(&current_robot_move_state,
//			        &robot_move_head_to_left_enum_state); /*转到朝左*/
			fsm_state_set(&current_robot_move_state, &robot_move_head_to_left_add_adjust_enum_state); /*转到朝左+竖直微调项*/

			if (abs(GV.Robot_Angle - (CV.RobotLeftAngleValue + GV.PV.Vertical_Calibration))
			        <= CV.Allowable_Error_For_Angle_Tracking)
			{
				CurrentHorizontal_ChangeState = HorizontalChange_End;
			}
			break;
		}
		case HorizontalChange_End:
		{
			HorizontalLaneChangeState = Lane_Change_Stop;
			break;
		}
		default:
			break;
	}

}

/*
 * 以下4个：竖直左、竖直右模式下，换道函数
 * 已改进项：将竖直微调数据同步应用于换道参数配置。
 *
 * */

/*********************************************************************/
/* 竖直从左往右作业 上端 向左换道 最终头朝上
 * */
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 - 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 - (CV.RobotUpAngleValue + GV.PV.Vertical_Calibration))
			        >= CV.Allowable_Error_For_Angle_Tracking)
			{
//				fsm_state_set(&current_robot_move_state,
//				        &robot_move_head_to_up_enum_state); /* 移动至头朝上  */

				fsm_state_set(&current_robot_move_state, &robot_move_head_to_up_add_adjust_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 - 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:
		{
			// 这里判断条件从原来的 “当前值-9000” 改成了 “当前值-9000-竖直微调值”，否则回正时来回抖动
			if (abs(GV.Robot_Angle - (CV.RobotUpAngleValue + GV.PV.Vertical_Calibration))
			        >= CV.Allowable_Error_For_Angle_Tracking) //误差在1度内
			{
//				fsm_state_set(&current_robot_move_state,
//				        &robot_move_head_to_up_enum_state); /* 移动至头朝上  */
				fsm_state_set(&current_robot_move_state, &robot_move_head_to_up_add_adjust_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 - 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 - (CV.RobotUpAngleValue + GV.PV.Vertical_Calibration))
			        >= CV.Allowable_Error_For_Angle_Tracking)
			{
//				fsm_state_set(&current_robot_move_state,
//				        &robot_move_head_to_up_enum_state); /* 移动至头朝上  */
				fsm_state_set(&current_robot_move_state, &robot_move_head_to_up_add_adjust_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);
	switch (Current_Vertical_ChangeState)
	{
		case VerticalChange_StateZero:
		{
			Current_Vertical_ChangeState = VerticalChange_TurnToLeft;

			break;
		}
		case VerticalChange_TurnToLeft:
		{
			if (abs(GV.Robot_Angle - 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 - (CV.RobotUpAngleValue + GV.PV.Vertical_Calibration))
			        >= CV.Allowable_Error_For_Angle_Tracking) //误差在1度内
			{
//				fsm_state_set(&current_robot_move_state,
//				        &robot_move_head_to_up_enum_state); /* 移动至头朝上  */
				fsm_state_set(&current_robot_move_state, &robot_move_head_to_up_add_adjust_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()
{

	if (CurrentHorizontal_ChangeState==HorizontalChange_DelayMove)
	{

		//VerticalLaneChangeDistanceCalibrationCM(6cm)是下降距离的误差    1min =60*1000ms
		LaneChangeWaittime_ms = 600
		        * (GV.PV.LaneChangeDistance+ CV.Horizontal_ChangeLane_Compensation)
		        / CV.Lane_Change_Speed_m_per_min;
	}
	else
	{
		//m/min  5/3    cm/ms

		LaneChangeWaittime_ms = 600 * (GV.PV.LaneChangeDistance+CV.Vertical_ChangeLane_Compensation)
		        / CV.Lane_Change_Speed_m_per_min;
	}
	return LaneChangeWaittime_ms;

}