/*
 * Automatic_task_Control.c
 *
 *  Created on: 2026年4月13日
 *      Author: SHI
 */
#include "Automatic_task_Control.h"

static uint32_t height_storage = 0;	//总高多少cm？
static const uint32_t height_ending = 100;	//作业下限多少cm？

static uint8_t last_state_DDM = 0;
static uint8_t current_state_DDM = 0;//电子罗盘4个方向状态
static uint8_t count_DDM = 0;//方向状态 变化计数
static uint8_t flag_ChangeLine_complete = 1;//换道完成标志
static float current_level = 0;//开始换道时的作业高度

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

static int get_state_DDM();
static int Auto_steer_do(float target_angle);

/*
 * 自动作业（圆罐环境，从上往下，从下看 头朝左）
 * 1.根据电子罗盘状态，经历东南西北4个方向状态 判断为一圈作业循环
 * 2.走完一圈执行 换道（换道完重置方向状态）
 * 3.根据罐体高度和拉线长度，判断是否到达圆罐底部，作业完成
 */
void Automatic_task_Control()
{
	height_storage = GV.PV.Height_Storage * 100;
	if (Get_BIT(SystemErrorCode, ComError_TL720D) == DISCONNECTED
			||Get_BIT(SystemErrorCode, ComError_DDM360B) == DISCONNECTED
			||Get_BIT(SystemErrorCode, ComError_wire_sensor) == DISCONNECTED)
	{
		fsm_state_set(&current_robot_move_state, &robot_halt_state);
		return;//安卓屏幕 模式选择错误
	}
	if (GV.PV.RunMode == Move_Manual)
	{
		fsm_state_set(&current_robot_move_state, &robot_halt_state);
		return;
	}

	if( GV.Wire_lenth > (height_storage - height_ending))//到达底部
	{
		fsm_state_set(&current_robot_move_state, &robot_halt_state);
		return;
	}

	if(current_state_DDM !=0) {last_state_DDM = current_state_DDM;}// 只有上次状态有效时才保存
	current_state_DDM = get_state_DDM();// 更新状态
	if(current_state_DDM != last_state_DDM)	{count_DDM ++;} // 状态变化时计数

	if(count_DDM >= 5)//换道
	{
		Auto_ChangeLine_Control(0);
	}
	else//绕圈
	{
		Auto_steer_moveforward_do(-90 + GV.PV.Vertical_Calibration/100);//垂直校准，正抬头，负低头
	}
}

//电子罗盘 北东南西 对应4个状态
static int get_state_DDM()
{
	if(GV.RobotAngle_DDM >5*100 && GV.RobotAngle_DDM <90*100)			//[5,90]
		{return 1;}
	else if(GV.RobotAngle_DDM >95*100 && GV.RobotAngle_DDM <180*100)	//[95,180]
		{return 2;}
	else if(GV.RobotAngle_DDM >185*100 && GV.RobotAngle_DDM <270*100)	//[185,270]
		{return 3;}
	else if(GV.RobotAngle_DDM >275*100 && GV.RobotAngle_DDM <360*100)	//[275,360]
		{return 4;}
	return 0;
}

void Refresh_MK32_State()
{
	if (P_MK32->CH4_SA == 0)//更新换道状态
	{
		HorizontalLaneChangeState = Lane_Change_Start;//准备开始换道
		return;
	}
}

/*
 * 后退换道（从下看 头朝左->头朝上->头朝左，可修改）
 * 1.距离差大于5，先调整方向至0°，再后退
 * 2.距离差大于2小于5，慢速后退
 * 3.调整方向至90°
 */
// 0-自动作业换道; 1-手动拨杆换道
void Auto_ChangeLine_Control(int flag)//换道(后退)
{
	if (Get_BIT(SystemErrorCode, ComError_TL720D) == DISCONNECTED
			||Get_BIT(SystemErrorCode, ComError_wire_sensor) == DISCONNECTED)
	{
		fsm_state_set(&current_robot_move_state, &robot_halt_state);
		return;//安卓屏幕 模式选择错误
	}
	if (GV.PV.RunMode == Move_Manual)
	{
		fsm_state_set(&current_robot_move_state, &robot_halt_state);
		return;//安卓屏幕 模式选择错误
	}
	if (flag == 1)
	{

		if (HorizontalLaneChangeState != Lane_Change_Start)
		{
			fsm_state_set(&current_robot_move_state, &robot_halt_state);
			return;//换道完 拨杆未复位
		}
	}

	height_storage = GV.PV.Height_Storage * 100;
	float current_pos = height_storage - GV.Wire_lenth;//当前实时高度
	if(flag == 1 && flag_ChangeLine_complete == 0)//手动拨杆换道时，上一个自动换道未完成
	{
		flag_ChangeLine_complete = 2;
		current_level = current_pos;
	}
	if(flag == 0 && flag_ChangeLine_complete == 2)//自动作业换道时，上一个手动拨杆换道未完成
	{
		flag_ChangeLine_complete = 0;
		current_level = current_pos;
	}
	if(flag_ChangeLine_complete == 1)//上一个换道完成
	{
		flag_ChangeLine_complete = 0;
		current_level = current_pos;//只记录一次,当前作业高度
	}

	float Target_pos = current_level - GV.LaneChangeDistance;// 目标后退高度
	float delta_pos = current_pos - Target_pos;

	if(delta_pos > 5)//1.大差距，先调整方向再后退
	{
		Auto_steer_moveback_do(0);
	}
	else if(delta_pos > 2 && delta_pos <= 5)//2.慢速后退
	{
		GV.LeftMotor.Target_Velcity  = -GV.Robot_Move_Speed/2 ;
		GV.RightMotor.Target_Velcity =  GV.Robot_Move_Speed/2 ;
	}
	else//3.调整方向
	{
		static uint32_t timeout_count = 0;
		if( Auto_steer_do(-90) == 1 )
		{
			if(flag == 1)
			{
				HorizontalLaneChangeState = Lane_Change_Stop;
			}
			count_DDM = 0;
		    flag_ChangeLine_complete = 1;
		    timeout_count = 0;
		}
//		else if( ++timeout_count > 5000 )//10s超时判断
//		{
//		    count_DDM = 0;
//		    flag_ChangeLine_complete = 1;
//		    timeout_count = 0;
//		}
	}
}

//先原地转向，再前进// 目标角度 1°
void Auto_steer_moveforward_do(float TargetAngle)//1°
{
	float Target_Angle = TargetAngle * 100;//0.01°
	float current_Angle = GV.Robot_Angle;
	float delta_Angle = current_Angle - Target_Angle;

	double dletSpeed = Angle_Tune_PID( current_Angle, Target_Angle, 0.01, 0, 0.01, 2);
	if(abs(delta_Angle) > 2*100)
	{	//double copysign(double x, double y); //结果具有x绝对值大小和y的符号
		GV.LeftMotor.Target_Velcity  = -1*copysign(CV.LeftTurnSpeed,  delta_Angle);
		GV.RightMotor.Target_Velcity = -1*copysign(CV.RightTurnSpeed, delta_Angle);
	}
	else
	{
		GV.LeftMotor.Target_Velcity  =  (GV.Robot_Move_Speed - dletSpeed*1);
		GV.RightMotor.Target_Velcity = -(GV.Robot_Move_Speed + dletSpeed*1);
	}

}
//先原地转向，再后退// 目标角度 1°
void Auto_steer_moveback_do(float TargetAngle)//1°
{
	float Target_Angle = TargetAngle *100;//0.01°
	float current_Angle = GV.Robot_Angle;
	float delta_Angle = current_Angle - Target_Angle;

	double dletSpeed = Angle_Tune_PID( current_Angle, Target_Angle, 0.02, 0, 0.01, 2);
	if(abs(delta_Angle) > 5*100)
	{	//double copysign(double x, double y); //结果具有x绝对值大小和y的符号
		GV.LeftMotor.Target_Velcity  = -1*copysign(CV.LeftTurnSpeed,  delta_Angle);
		GV.RightMotor.Target_Velcity = -1*copysign(CV.RightTurnSpeed, delta_Angle);
	}
	else
	{
		GV.LeftMotor.Target_Velcity  = -(GV.Robot_Move_Speed + dletSpeed*1);
		GV.RightMotor.Target_Velcity =   GV.Robot_Move_Speed - dletSpeed*1;
	}

}
//原地转向// 目标角度 1°
static int Auto_steer_do(float TargetAngle)//1°
{
	float Target_Angle = TargetAngle *100;
	float current_Angle = GV.Robot_Angle;
	float delta_Angle = current_Angle - Target_Angle;

	if(abs(delta_Angle) > 10*100)	//大角度快速转
	{	//double copysign(double x, double y); //结果具有x绝对值大小和y的符号
		GV.LeftMotor.Target_Velcity  = -1*copysign(CV.LeftTurnSpeed,  delta_Angle);
		GV.RightMotor.Target_Velcity = -1*copysign(CV.RightTurnSpeed, delta_Angle);
	}
	else if(abs(delta_Angle) > 2*100)//小角度慢速转
	{
		GV.LeftMotor.Target_Velcity  = -1*copysign(CV.LeftTurnSpeed/2,  delta_Angle);
		GV.RightMotor.Target_Velcity = -1*copysign(CV.RightTurnSpeed/2, delta_Angle);
	}
	else // <小角度,停止
	{
		GV.LeftMotor.Target_Velcity  = 0;
		GV.RightMotor.Target_Velcity = 0;
		return 1;
	}
	return 0;
}

uint8_t blowers_pwm_last = 0;
int SetPWM_Auto()//
{
	uint8_t blowers_pwm_base = 0;//旋钮pwm，增加默认值，否则遥控连接之前，获取旋钮速度输出为50
	int8_t blowers_pwm_PID = 0;//自动pwm

	if (P_MK32->CH6_SC == -1000)//自动调整风速
	{
		//根据压强大小 线性调整PWM，风机转速
		//压强数值输出范围是 -100 ~ 100 Kpa,//压强减小，blowers_pwm_Kp增大（>0）

		float Target_force = 3.2;
		uint8_t base_pwm = 41;// 41对应3.2；45对应3.8Kpa；
		float current_force = -1*GV.ForceValue ;
		float deltForce = current_force - Target_force;
		int detapwm = 0;
		if(deltForce < 0)
		{
			detapwm = Angle_Tune_PID( current_force, Target_force, 20, 0, 16, 100);
		}
		else if(deltForce < 0.5)
		{
			detapwm = Angle_Tune_PID( current_force, Target_force, 6, 0, 10, 100);//
		}
		else
		{
			detapwm = Angle_Tune_PID( current_force, Target_force, 10, 0, 8, 100);
		}

		blowers_pwm_PID = base_pwm - detapwm;
		if(blowers_pwm_PID < 40) {blowers_pwm_PID = 40;}//下限
		if(blowers_pwm_PID > 100) {blowers_pwm_PID = 100;}//上限
//		blowers_pwm_base = 0;
	}
	else//手动调节
	{
		if(Get_BIT(SystemErrorCode, ComError_MK32_SBus) == CONNECTED)
		{
			blowers_pwm_base = (uint8_t)(abs(P_MK32->CH10_LD1 + 1000)  / 200.0f * 10);//0-100/*获取旋钮速度*/
//			blowers_pwm_PID = 0;
			blowers_pwm_last = blowers_pwm_base;
		}
		else//遥控通讯中断，风机不能停
		{
			blowers_pwm_base = blowers_pwm_last;
		}
	}

	int blowers_pwm_total = blowers_pwm_base + blowers_pwm_PID;
	return (blowers_pwm_total);//pwm > 90, 风速最大
}