/*
 * 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 - 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 =  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)
			        >= 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 = 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:
		{
			if (abs(GV.Robot_Angle - 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 = 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)
			        >= 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 = 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)
			        >= 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;

}