/*
 * robot_move_actions.c
 *
 *  Created on: 2025年7月14日
 *      Author: akeguo
 */
#include "robot_move_actions.h"
#include "fsm_state.h"
#include "fsm_state_control.h"
#include "paint_gun_action.h"
#include "MSP/msp_PID.h"

transition_t   current_robot_move_state;
transition_t   current_robot_automove_state;

transition_state_t   robot_halt_state	={HALT_State_Enter, HALT_State_Do, HALT_State_Exit}; /* 停 */
transition_state_t   robot_forwards_state	={NULL, Forwards_State_Do, NULL};	/* 前 */
transition_state_t   robot_backwards_state	={NULL, Backwards_State_Do, NULL};	/* 后 */
transition_state_t   robot_turn_left_state	={NULL, TurnLeft_State_Do, NULL};	/* 左 */
transition_state_t   robot_turn_right_state	={NULL, TurnRight_State_Do, NULL};	/* 右 */


transition_state_t   robot_move_vertical_task_forwards_state ={NULL, Move_Vertical_Task_Forwards_Do, NULL};	/* 竖直纠偏前进 */
transition_state_t   robot_move_vertical_task_backwards_state={NULL, Move_Vertical_Task_Backwards_Do, NULL};/* 竖直纠偏后退 */

transition_state_t   robot_move_horizontal_task_forwards_left_state={NULL,Move_Horizontal_Task_Forwards_Left_Do,NULL};	/* 水平纠偏前进 头朝左*/
transition_state_t   robot_move_horizontal_task_backwards_left_state={NULL,Move_Horizontal_Task_Backwards_Left_Do,NULL};/* 水平纠偏后退 头朝左*/
transition_state_t   robot_move_horizontal_task_forwards_right_state={NULL,Move_Horizontal_Task_Forwards_Right_Do,NULL};/* 水平纠偏前进 头朝右*/
transition_state_t   robot_move_horizontal_task_backwards_right_state={NULL,Move_Horizontal_Task_Backwards_Right_Do,NULL};/* 水平纠偏后退 头朝右*/

transition_state_t   robot_move_head_to_left_enum_state={NULL,Move_Head_To_Left_Do,NULL};	/* 移动至头朝左 */
transition_state_t   robot_move_head_to_up_enum_state={NULL,Move_Head_To_UP_Do,NULL};	    /* 移动至头朝上  */
transition_state_t   robot_move_head_to_right_enum_state={NULL,Move_Head_To_Right_Do,NULL};	/* 移动至头朝右  */
transition_state_t   robot_move_head_to_down_enum_state={NULL,Move_Head_To_Down_Do,NULL};	/* 移动至头朝下  */

transition_state_t	robot_Automatic_task_state = {NULL, Move_Automatic_task_Do, NULL};//自动作业
transition_state_t	robot_Auto_ChangeLine_state = {NULL, Move_Auto_ChangeLine_Do, NULL};//换道
transition_state_t  robot_move_horizontal_task_forwards_state  ={NULL,Move_Horizontal_Task_Forwards_Do,NULL};/* 水平纠偏前进 */
transition_state_t  robot_move_horizontal_task_backwards_state ={NULL,Move_Horizontal_Task_Backwards_Do,NULL};/* 水平纠偏后退 */

/* 声明最底层函数 */

void Move_Forwards_Do(int32_t Target_Angle);
void Move_Backwards_Do(int32_t Target_Angle);
void Calbrate_Robot_Positon(int Target_Angle);


char IsRobotHaltSateChangedFlag = 0;

void Move_Automatic_task_Do(transition_t *p_this)
{
	Automatic_task_Control();
}
void Move_Auto_ChangeLine_Do(transition_t *p_this)
{
	Auto_ChangeLine_Control(1);
}

void Move_Horizontal_Task_Forwards_Do(transition_t *p_this)
{
	if(GV.Robot_Angle >= 0)//头朝右
	{
		Auto_steer_moveforward_do(90 - GV.PV.Vertical_Calibration/100);
	}
	else//头朝左
	{
		Auto_steer_moveforward_do(-90 + GV.PV.Vertical_Calibration/100);
	}

}
void Move_Horizontal_Task_Backwards_Do(transition_t *p_this)
{
	if(GV.Robot_Angle >= 0)//头朝右
	{
		Auto_steer_moveback_do(90 + GV.PV.Vertical_Calibration/100);
	}
	else//头朝左
	{
		Auto_steer_moveback_do(-90 - GV.PV.Vertical_Calibration/100);
	}
}

void Forwards_State_Do(transition_t *p_this)
{
	GV.LeftMotor.Target_Velcity = GV.Robot_Move_Speed;
	GV.RightMotor.Target_Velcity = -GV.Robot_Move_Speed;
}

void Backwards_State_Do(transition_t *p_this)
{
	GV.LeftMotor.Target_Velcity = -GV.Robot_Move_Speed;
	GV.RightMotor.Target_Velcity = GV.Robot_Move_Speed;
}
/* 向左转 */
void TurnLeft_State_Do(transition_t *p_this)
{
	GV.LeftMotor.Target_Velcity =  -CV.LeftTurnSpeed;
	GV.RightMotor.Target_Velcity = -CV.LeftTurnSpeed;
}

/* 向右转 */
void TurnRight_State_Do(transition_t *p_this)
{
	GV.LeftMotor.Target_Velcity =CV.RightTurnSpeed;
	GV.RightMotor.Target_Velcity = CV.RightTurnSpeed;
}

/* 停止
 * 零速停止，不关闭抱闸
 * */
void HALT_State_Do(transition_t *p_this)
{
	GV.LeftMotor.Target_Velcity = 0;
	GV.RightMotor.Target_Velcity = 0;

//	if (IsRobotHaltSateChangedFlag == 1)//机器人为运动状态
//	{
//		if (current_paintgun_state.p_state == &paintgun_on_state) //停车看喷枪状态  开了关喷枪计时停车
//		{
//			fsm_state_set(&current_paintgun_state, &paintgun_off_state);
//			timer_handler_3.start_timer = 1; //开始计时
//			return;
//		}
//		if (CompareTimer(
//				600 * CV.Paint_Gun_Shutdown_Distance / GetVehicleSpeed( speed_selection),
//				&timer_handler_3))  //喷枪开 计时停  关 直接停
//		{
//			IsRobotHaltSateChangedFlag = 0;
//			GV.LeftMotor.Target_Velcity = 0;
//			GV.RightMotor.Target_Velcity = 0;
//		}
//	}else
//	{
//		GV.LeftMotor.Target_Velcity = 0;
//		GV.RightMotor.Target_Velcity = 0;
//		IsRobotHaltSateChangedFlag=0;
//	}
}


/* 竖直前进，保持目标角度 CV.RobotUpAngleValue
 * Target_Angle: 0.01度
 * */
void Move_Vertical_Task_Forwards_Do(transition_t *p_this)
{
	Move_Forwards_Do(CV.RobotUpAngleValue + GV.PV.Vertical_Calibration);
}

/* 竖直后退，保持目标角度 CV.RobotUpAngleValue
 * Target_Angle: 0.01度
 * */
void Move_Vertical_Task_Backwards_Do(transition_t *p_this)
{
	Move_Backwards_Do(CV.RobotUpAngleValue + GV.PV.Vertical_Calibration);
}


void HALT_State_Enter(transition_t *p_this)
{

}
void HALT_State_Exit(transition_t *p_this)
{


}

/* 水平头朝右前进
 * 带补偿
 * */
void Move_Horizontal_Task_Forwards_Right_Do(transition_t *p_this)
{
	Move_Forwards_Do(CV.RobotRightAngleValue - GV.Right_Compensation);
}
/* 水平头朝右后退 ，向左退 左补偿生效
 * 带补偿
 * */
void Move_Horizontal_Task_Backwards_Right_Do(transition_t *p_this)
{
	Move_Backwards_Do(CV.RobotRightAngleValue + GV.Left_Compensation);
}
/* 水平头朝左前进
 * 带补偿
 * */
void Move_Horizontal_Task_Forwards_Left_Do(transition_t *p_this)
{
	Move_Forwards_Do(CV.RobotLeftAngleValue + GV.Left_Compensation);
}


/* 水平头朝左后退  向右退 右补偿
 * 带补偿
 * */
void Move_Horizontal_Task_Backwards_Left_Do(transition_t *p_this)
{
	Move_Backwards_Do(CV.RobotLeftAngleValue - GV.Right_Compensation);
}


/* 原地PID转向至目标角度 CV.RobotUpAngleValue
 * Target_Angle： 目标角度 （单位 0.01 度）
 * */
void Move_Head_To_UP_Do(transition_t *p_this)
{
	Calbrate_Robot_Positon(CV.RobotUpAngleValue);

}

/* 原地PID转向至目标角度 CV.RobotDownAngleValue
 * Target_Angle： 目标角度 （单位 0.01 度）
 * */
void Move_Head_To_Down_Do(transition_t *p_this)
{
	Calbrate_Robot_Positon(CV.RobotDownAngleValue);
}

/* 原地PID转向至目标角度 CV.RobotLeftAngleValue
 * Target_Angle： 目标角度 （单位 0.01 度）
 * */
void Move_Head_To_Left_Do(transition_t *p_this)
{
	Calbrate_Robot_Positon(CV.RobotLeftAngleValue);
}

/* 原地PID转向至目标角度 CV.RobotRightAngleValue
 * Target_Angle： 目标角度 （单位 0.01 度）
 * */
void Move_Head_To_Right_Do(transition_t *p_this)
{
	Calbrate_Robot_Positon(CV.RobotRightAngleValue);
}


/*******************以下纠偏底层函数**********************************/

double dletAngle;
double delt;

/* 竖直面 */
/* * 底层前进，保持目标角度  纠偏  被调用
 * Target_Angle: 0.01度
 * */

void Move_Forwards_Do(int32_t Target_Angle)
{
	if (abs(GV.Robot_Angle - Target_Angle) <= 400) //// 误差在正负4度内,行进中校正
	{
		if (abs(GV.Robot_Angle - Target_Angle) < 100) //// 误差在正负1度内
		{
//			dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 0.02, 0, 0.01, 2);
			dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 0.01, 0, 0.01, 1);
		}
		else  //1-4°
		{
//			dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 0.04, 0, 0.01, 4);
			dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 0.02, 0, 0.01, 2);

		}
		GV.LeftMotor.Target_Velcity = GV.Robot_Move_Speed - dletAngle ;
		GV.RightMotor.Target_Velcity = -(GV.Robot_Move_Speed + dletAngle) ;//注意电机方向
	}
	else //大于4°,原地转向校正
	{
//		dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 0.06, 0, 0.01, 6);
		dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 0.03, 0, 0.01, 3);
		GV.LeftMotor.Target_Velcity = -dletAngle ;
		GV.RightMotor.Target_Velcity = -(dletAngle) ;//注意电机方向
	}
	delt=dletAngle;
}

/* 后退，保持目标角度 纠偏
 * Target_Angle: 0.01度
 * */
void Move_Backwards_Do(int32_t Target_Angle)
{
	if (abs(GV.Robot_Angle - Target_Angle) <= 400) //// 误差在正负4度内
	{
		if (abs(GV.Robot_Angle - Target_Angle) < 100) //// 误差在正负1度内
		{
//			dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 0.02, 0, 0.01, 2);
			dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 0.01, 0, 0.01, 1);
		}
		else  /*1-4*/
		{
//			dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 0.04, 0,0.01, 4);
			dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 0.02, 0,0.01, 2);
		}
		GV.LeftMotor.Target_Velcity = -(GV.Robot_Move_Speed + dletAngle) ;
		GV.RightMotor.Target_Velcity = (GV.Robot_Move_Speed - dletAngle);//注意电机方向
	}
	else /*大于4*/
	{
//		dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 0.06, 0, 0.01, 8);
		dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 0.03, 0, 0.01, 3);
		GV.LeftMotor.Target_Velcity = -dletAngle ;
		GV.RightMotor.Target_Velcity = -(dletAngle) ;//注意电机方向
	}
}

/* 原地PID转向至目标角度
 * Target_Angle： 目标角度 （单位 0.01 度）
 * */
void Calbrate_Robot_Positon(int Target_Angle)
{
	if (abs(GV.Robot_Angle - Target_Angle) <= 50) //误差在正负1度内
	{
		GV.LeftMotor.Target_Velcity = 0;
		GV.RightMotor.Target_Velcity = 0;
	}
	else if (abs(GV.Robot_Angle - Target_Angle) <= 200) //误差在正负1度内  1-2°
	{
		dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 0.04, 0,0.01, 4);
		GV.LeftMotor.Target_Velcity = -dletAngle ;
		GV.RightMotor.Target_Velcity = dletAngle;
	}
	else//大于2°
	{
		dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 0.06, 0, 0.01, 8);
		GV.LeftMotor.Target_Velcity = -dletAngle ;
		GV.RightMotor.Target_Velcity = dletAngle ;
	}
}

int32_t GetVehicleSpeed(float speed_selection)
{
	//(1r/min)360*81*100/360/60=135 //减速比81
	// 1r=3.14*180mm/1000 =0.5652m
	// 1m/min=135/0.5652= 238.85
	//	return (int32_t)(238.85*speed_selection);

	float speed_m_per_min = speed_selection;
	return (int32_t) (speed_m_per_min *CV.pulse_Per_Circle * CV.wheel_Reduction_Ratio / 60.0f
			        / (3.14159265f * CV.wheel_Diameter_m));
}