/*
 * 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 "MSP/msp_PID.h"
transition_t   current_robot_move_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_horizontal_task_forwards_left_paint_state={NULL,Move_Horizontal_Task_Forwards_Left_Paint_Do,NULL};	/* 水平纠偏前进 头朝左*/
transition_state_t   robot_move_horizontal_task_backwards_left_paint_state={NULL,Move_Horizontal_Task_Backwards_Left_Paint_Do,NULL};/* 水平纠偏后退 头朝左*/
transition_state_t   robot_move_horizontal_task_forwards_right_paint_state={NULL,Move_Horizontal_Task_Forwards_Right_Paint_Do,NULL};	/* 水平纠偏前进 头朝右*/
transition_state_t   robot_move_horizontal_task_backwards_right_paint_state={NULL,Move_Horizontal_Task_Backwards_Right_Paint_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_move_head_to_up_add_adjust_enum_state={NULL,Move_Head_To_UP_Add_Adjust_Do,NULL};//移动到头朝上+竖直微调角度
transition_state_t   robot_move_head_to_left_add_adjust_enum_state={NULL,Move_Head_To_Left_Add_Adjust_Do,NULL};	/* 移动至头朝左+竖直微调角度 */
transition_state_t   robot_move_head_to_right_add_adjust_enum_state={NULL,Move_Head_To_Right_Add_Adjust_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);

int32_t* DHRoughening_Speed[10];
static double dletAngle = 0;
char IsRobotHaltSateChangedFlag = 0;

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.RightTurnSpeed;
}

/* 向右转
 *
 * */
void TurnRight_State_Do(transition_t *p_this)
{

	GV.LeftMotor.Target_Velcity = CV.LeftTurnSpeed;
	GV.RightMotor.Target_Velcity = -CV.RightTurnSpeed;
}

/* 停止
 * 零速停止，不关闭抱闸
 * */
void HALT_State_Do(transition_t *p_this)
{

		GV.LeftMotor.Target_Velcity = 0;
		GV.RightMotor.Target_Velcity = 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);
	// 修改项V1.0：水平移动+左右补偿+水平微调
//	Move_Forwards_Do(CV.RobotRightAngleValue - GV.Right_Compensation + GV.PV.Horizontal_Calibration); //+补偿+水平微调
// 修改项V2.0：水平移动，共用竖直微调这一补偿项，即删除替换原来的水平微调。水平移动+左右补偿+竖直微调
	Move_Forwards_Do(CV.RobotRightAngleValue - GV.Right_Compensation + GV.PV.Vertical_Calibration); //+补偿+竖直微调

}
void Move_Horizontal_Task_Forwards_Right_Paint_Do(transition_t *p_this)
{
	Move_Forwards_Do(CV.RobotRightAngleValue);
}
/* 水平头朝右后退 ，向左退 左补偿生效
 * 带补偿
 * */
void Move_Horizontal_Task_Backwards_Right_Do(transition_t *p_this)
{
//	Move_Backwards_Do(CV.RobotRightAngleValue + GV.Left_Compensation);
//	Move_Backwards_Do(CV.RobotRightAngleValue + GV.Left_Compensation + GV.PV.Horizontal_Calibration);
	// 修改项V2.0：
	Move_Backwards_Do(CV.RobotRightAngleValue + GV.Left_Compensation + GV.PV.Vertical_Calibration);

}
/**和拉毛区分开 防止左右补偿加到喷漆上**/
void Move_Horizontal_Task_Backwards_Right_Paint_Do(transition_t *p_this)
{
	Move_Backwards_Do(CV.RobotRightAngleValue);
}
/* 水平头朝左前进
 * 带补偿
 * */
void Move_Horizontal_Task_Forwards_Left_Do(transition_t *p_this)
{
//	Move_Forwards_Do(CV.RobotLeftAngleValue + GV.Left_Compensation);
//	Move_Forwards_Do(CV.RobotLeftAngleValue + GV.Left_Compensation + GV.PV.Horizontal_Calibration);
	// 修改项V2.0：
	Move_Forwards_Do(CV.RobotLeftAngleValue + GV.Left_Compensation + GV.PV.Vertical_Calibration);

}
/**和拉毛区分开 防止左右补偿加到喷漆上**/
void Move_Horizontal_Task_Forwards_Left_Paint_Do(transition_t *p_this)
{
	Move_Forwards_Do(CV.RobotLeftAngleValue);
}

/* 水平头朝左后退  向右退 右补偿
 * 带补偿
 * */
void Move_Horizontal_Task_Backwards_Left_Do(transition_t *p_this)
{
//	Move_Backwards_Do(CV.RobotLeftAngleValue - GV.Right_Compensation);
//	Move_Backwards_Do(CV.RobotLeftAngleValue - GV.Right_Compensation + GV.PV.Horizontal_Calibration);
	// 修改项V2.0：
	Move_Backwards_Do(CV.RobotLeftAngleValue - GV.Right_Compensation + GV.PV.Vertical_Calibration);

}
void Move_Horizontal_Task_Backwards_Left_Paint_Do(transition_t *p_this)
{
	Move_Backwards_Do(CV.RobotLeftAngleValue);
}


/* 原地PID转向至目标角度 CV.RobotUpAngleValue
 * Target_Angle： 目标角度 （单位 0.01 度）
 * */
void Move_Head_To_UP_Do(transition_t *p_this)
{
	Calbrate_Robot_Positon(CV.RobotUpAngleValue);  // 原始版本，转到固定的0/90度那种
//	Calbrate_Robot_Positon((CV.RobotUpAngleValue + GV.PV.Vertical_Calibration));  // 加入竖直微调项的转动角度
}

void Move_Head_To_UP_Add_Adjust_Do(transition_t *p_this)
{
	Calbrate_Robot_Positon((CV.RobotUpAngleValue + GV.PV.Vertical_Calibration));  // 加入竖直微调项的转动角度
}



/* 原地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);
//	Calbrate_Robot_Positon(CV.RobotLeftAngleValue + GV.PV.Vertical_Calibration);// 加入竖直微调项的转动角度,用于水平

}

void Move_Head_To_Left_Add_Adjust_Do(transition_t *p_this)
{
	Calbrate_Robot_Positon(CV.RobotLeftAngleValue + GV.PV.Vertical_Calibration);// 加入竖直微调项的转动角度,用于水平

}



/* 原地PID转向至目标角度 CV.RobotRightAngleValue
 * Target_Angle： 目标角度 （单位 0.01 度）
 * */
void Move_Head_To_Right_Do(transition_t *p_this)
{
	Calbrate_Robot_Positon(CV.RobotRightAngleValue);
//	Calbrate_Robot_Positon(CV.RobotLeftAngleValue + GV.PV.Vertical_Calibration);// 加入竖直微调项的转动角度,用于水平

}

void Move_Head_To_Right_Add_Adjust_Do(transition_t *p_this)
{
	Calbrate_Robot_Positon(CV.RobotRightAngleValue + GV.PV.Vertical_Calibration);// 加入竖直微调项的转动角度,用于水平

}

/*******************以下纠偏底层函数**********************************/
/*
 * 底层前进，保持目标角度  纠偏  被调用
 * Target_Angle: 0.1度
 * */
int aaa=0;
int delt=0;
void Move_Forwards_Do(int32_t Target_Angle)
{

	aaa=abs(GV.Robot_Angle - Target_Angle);
	if (abs(GV.Robot_Angle - Target_Angle) <= CV.PID_mid.PID_Angle) //// 误差在正负x度内
	{
		if (abs(GV.Robot_Angle - Target_Angle) < CV.PID_low.PID_Angle) //// 误差在正负0.3度内
		{
			dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle,
					CV.PID_low.Kp, CV.PID_low.Ki, CV.PID_low.Kd, 5);
			GV.LeftMotor.Target_Velcity = GV.Robot_Move_Speed - dletAngle ;
			GV.RightMotor.Target_Velcity =GV.Robot_Move_Speed + dletAngle ;
		}
		else  //中角度 low-mid 0.3-x
		{
			dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle,
					CV.PID_mid.Kp, CV.PID_mid.Ki, CV.PID_mid.Kd, 10);

			GV.LeftMotor.Target_Velcity = -dletAngle ;
			GV.RightMotor.Target_Velcity = dletAngle ;

		}



	}
	else //>1
	{
		dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle,
				CV.PID_high.Kp, CV.PID_high.Ki, CV.PID_high.Kd,40);

		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) <= CV.PID_mid.PID_Angle) //// 误差在正负4度内
	{
		if (abs(GV.Robot_Angle - Target_Angle) < CV.PID_low.PID_Angle) //// 误差在正负1度内
		{
			dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle,
					CV.PID_low.Kp, CV.PID_low.Ki, CV.PID_low.Kd, 5);
			GV.LeftMotor.Target_Velcity = -GV.Robot_Move_Speed - dletAngle  ;
			GV.RightMotor.Target_Velcity = -GV.Robot_Move_Speed + dletAngle ;
		}
		else  /*1-4*/
		{
			dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle,
					CV.PID_mid.Kp, CV.PID_mid.Ki, CV.PID_mid.Kd, 10);
			GV.LeftMotor.Target_Velcity = -dletAngle ;
			GV.RightMotor.Target_Velcity = dletAngle ;
		}

	}
	else /*大于4*/
	{
		dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle,
				CV.PID_high.Kp, CV.PID_high.Ki, CV.PID_high.Kd, 40);
		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) <= 1000) //误差在正负1度内  1-5°
	{
		dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 1, 0,0.5, 10);
		GV.LeftMotor.Target_Velcity = -dletAngle ;  /* 换道回正or转向时，最后的5度内，转速降低 */
		GV.RightMotor.Target_Velcity = dletAngle;
	}
	else//大于2°
	{

		dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 2, 0, 0.5, 50);
		GV.LeftMotor.Target_Velcity = -dletAngle;/* 换道回正or转向时，最后的5度内，转速降低 */
		GV.RightMotor.Target_Velcity = dletAngle ;
	}
}


/* 车体速度选择  具体车速在配置结构体CV 0.01m/min  */
double GetVehicleSpeed(double speed_selection)
{

	if(speed_selection==0)
	{
		return 1.0;
	}
	return CV.MAXSpeed_m_per_min*speed_selection/20.0;

}