#include "Sub.h"
//目标姿态

float maxspeed = 910.0;
float minspeed = 280.0;
float maxerrorspeed = 310.0;

//float turnspeed  = maxspeed - maxerror_f;

Quaternion   _attitude_target_quat;
extern mavlink_rov_state_monitoring_t rov_message;

bool Sub::clean_init()
{
	pos_control.set_alt_target(0);
	hal.rcout->write(10,1500);//毛刷停止
	motor1_speed_target =startval;//1500;
	motor2_speed_target =startval;//
	float _head =0;
	_head =  (float)ahrs.yaw_sensor/100;
	//track
	track_head_gd =  constrain_float(_head,0.0,360.0);
	
	last_roll = 0;
	last_pitch = 0;
	last_yaw = ahrs.yaw_sensor;

	//记录进入stable的YAW的当前值
	yaw_press = (int16_t)(ahrs.yaw_sensor/100);//记住方位
	last_input_ms = AP_HAL::millis();
	track_reset();
	

	return true;

}
void Sub::track_reset(void){
	autoclean_command = FALSE;//自动刷网
	clean_bottom_command =FALSE;
	clean_bottom_flag = FALSE;	
	autoclean_flag = FALSE;
	handclean = TRUE;
	
	turn_angle = 16.0;//20210622
			//----------------------
	track_motor_arm = 1;//履带停机标志
			//履带的初始方向位置 默认为水平方向的yaw值
	track_head_gd = 0;
			//履带停
	motor1_speed_target =startval;
	motor2_speed_target =startval;
	clean_mode =0;

    attitude_control.relax_attitude_controllers();//外置九轴YAW更新四元数
    last_roll = 0;
    last_pitch = 0;
    last_yaw = ahrs.yaw_sensor;

	yaw_press = (int16_t)ahrs.yaw_sensor/100;//记住方位

	ahrs.get_quat_body_to_ned(_attitude_target_quat);
	
}
extern mavlink_data64_t rov_message2;
extern mavlink_rov_control_t rov_control;


void Sub::clean_run()
{
 // if not armed set throttle to zero and exit immediately
		if (!motors.armed()) {
			motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
			attitude_control.set_throttle_out(0,true,g.throttle_filt);
			attitude_control.relax_attitude_controllers();//---------------------20210623
			
			PressLevel_f =5.0;//压力为0
			PressLevel = no;

			//-------------
			track_reset();
			return;
				
		}
		//推进器-------------------
		motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);


		motors.set_throttle(1.0-(float)PressLevel_f*0.1);//压力等级
		
		rov_message.pressure_level = int(PressLevel);
		
		if (clean_thruster_help==1)
		{
			float pitch_throttle = (0.5-channel_throttle->norm_input())*2;
			float yaw_throttle = channel_yaw->norm_input();
			if (fabsf(yaw_throttle)>fabsf(pitch_throttle))
			{
				pitch_throttle = 0.0;
			}else{
				yaw_throttle =0.0;
			}
			
			motors.set_forward(0.0);	//右侧摇杆，前推为+，后推为- 右履带的前进后退
			motors.set_yaw(0.0);//左侧摇杆左推- 右推+
			
			motors.set_pitch(pitch_throttle);
			
		}else{
			motors.set_forward(0.0);	//右侧摇杆，前推为+，后推为- 右履带的前进后退
			motors.set_yaw(0.0);//左侧摇杆左推- 右推+
			motors.set_pitch(0.0);
		}

	    motors.set_roll(0.0);//(channel_roll->norm_input());//左右移动改为roll
		motors.set_lateral(0.0);//(channel_lateral->norm_input());

	   static int j = 0;
		 j++;
		 if(j>800)
		 {
			 // gcs().send_text(MAV_SEVERITY_INFO, " track_limit %f\n",(float)rov_control.track_limit);
			  j=0;
		 }
		 
		 


		//turnspeed  = maxspeed - maxerror_f;
		 
		autoclean_flag_chose();//自动洗网的状态切换
		clean_net_joystick();//默认手动洗网
		
		clean_sidenet_auto();//按一下自动洗网使能，再按一下失能，回到手动洗网状态

}
void Sub::autoclean_flag_chose(void){

	if (autoclean_command == TRUE)
	{
		autoclean_flag = TRUE;

	}
	else{
		autoclean_flag = FALSE;
	}
	if (autoclean_flag == FALSE && clean_bottom_flag == FALSE)
	{
		handclean = TRUE;
		ahrs.get_quat_body_to_ned(_attitude_target_quat);
	}
	else{
		handclean = FALSE;
	}

}
float maxspeed_set=800.0;

void Sub::clean_net_joystick(void)
{
	//左右履带和左右电机 左右反了

	float left =startval;
	float _turn =startval;
	int16_t motors1 =startval;
	int16_t motors2 =startval;
	
	minspeed =(float)SRV_Channels::srv_channel(15)->get_output_max()/10;// 暂时用来做最小速度   -----最小水深
	maxerrorspeed =(float)SRV_Channels::srv_channel(15)->get_output_min()/10;//两个履带最大差速 ----------最深距离 厘米 
	maxspeed_set = (float)SRV_Channels::srv_channel(15)->get_trim()/10;

	maxspeed = 910.0;

	if(maxspeed_set<maxspeed)
	{
		maxspeed = maxspeed_set;
	}
	if (maxspeed<280.0)
	{
		 maxspeed = 280.0;
	}
	 if(maxspeed>910.0)
	{
		 maxspeed = 910.0;
	}

	if(handclean == TRUE ){
		
		//计算电机的 手动转速
		float pitch_throttle = (0.5-channel_throttle->norm_input())*2;
		float yaw_throttle = channel_yaw->norm_input();
		if (fabsf(yaw_throttle)>fabsf(pitch_throttle))
		{
				//pitch_throttle = 0.0;
				
			_turn = Constrate1(channel_yaw->norm_input());//转向 右+左-，+右转 左履带增，-左转 右lvdai+
		}else{
				_turn =0.0;
		}

		left  = Constrate1(channel_forward->norm_input());//   
		
		motors1 = (int16_t)((left*maxspeed +_turn*maxerrorspeed));//右履带  //第一台高压电机 
		motors2 = (int16_t)((left*maxspeed -_turn*maxerrorspeed));//左履带
		if(_turn>0.1)
		{
			_turn =1.0;
			rov_message.turn    = 3;

		}else if(_turn<-0.1){
			_turn =-1.0;
			rov_message.turn    = 2;

		}else{
			rov_message.turn    = 1;
			
			}
		int16_t minspeed_int = (int16_t)minspeed;
		if (left>0.1)
		{
			//前进

			if (motors1<minspeed_int)
			{
				motors1 =minspeed_int;
			}
			if (motors2<minspeed_int)
			{
				motors2 =minspeed_int;
			}
		}
		else if (left<-0.1)
		{
			//后退
			
			if (motors1>-minspeed_int)
			{
				motors1 =-minspeed_int;
			}
			if (motors2>-minspeed_int)
			{
				motors2 =-minspeed_int;
			}
		}
		else{
			if(fabsf(_turn)<0.1)
			{
				motors1 =0;
				motors2 =0;
			}
			else{

				motors1 = (int16_t)(_turn*maxerrorspeed);////第一台高压电机 
				motors2 = (int16_t)(-_turn*maxerrorspeed);//
			}
		}

		int16_t minspeedlimit_back =(int16_t)(maxspeed*0.6);
		motors1=constrain_int16(motors1,-(int16_t)minspeedlimit_back,(int16_t)maxspeed);
		motors2=constrain_int16(motors2,-(int16_t)minspeedlimit_back,(int16_t)maxspeed);
		



		  //左摇杆前进后退的指示---------------

		if(left>0.1){
			track_motor_arm = 2;//前进
		}
		else if(left<-0.1){
			track_motor_arm = 0;
		}
		else{
			track_motor_arm=1;
			

		}
		rov_message.forward = track_motor_arm;
		
		 
		//---------------------------
		//右摇杆 转向的控制
		if(fabsf(_turn)>0.1 )//  防抖
		{
		//转弯中 手动控制
			track_head_gd = (float)ahrs.yaw_sensor/100;//to show 20210611
			
			
			slowly_speed1(motor1_speed_target,motors1,4,1);
			slowly_speed2(motor2_speed_target,motors2,4,1);
			track_motor_arm =3;//left or right

			
		}
		else{
			//纯手动控制
			if(clean_mode == 0){
				slowly_speed1(motor1_speed_target,motors1,4,1);
				slowly_speed2(motor2_speed_target,motors2,4,1);


	
			}else {		
					
			}

		}

	}

}

void Sub::slowly_speed2(int16_t &p1, int16_t p2,int16_t step,int16_t per) 
{
	static int16_t countper = 0;
	countper++;
	if(countper>per){
		countper = 0;
		 if (p1 > p2)
		  {
		  	  p1 -=step;
		  }else if(p1 < p2){
			  p1 +=step;
		  }
		  if (p2==startval && fabsf(p1-p2)<=step )
		 {
			 p1 =startval;
		 }

	}
	else{
		
		}
	 
	 

	
	 
	  //p1 = constrain_int16(p1, -(Speedmax_hand+maxerror), Speedmax_hand+maxerror);
	  p1 = constrain_int16(p1, -((int16_t)maxspeed+maxerror), (int16_t)maxspeed+maxerror);

	  static int j = 0;
	  j++;
	  if(j>800)
	  {
		  
		 //  gcs().send_text(MAV_SEVERITY_WARNING, " thrust%d  %f ,%d,%d\n",i,thrust_in,thrust,last_thrust_pwm[i]);
		   j=0;
	  }
	 
	
	
}


void Sub::slowly_speed1(int16_t &p1, int16_t p2,int16_t step,int16_t per) 
{
	static int16_t countper = 0;
	countper++;
	if(countper>per){
		countper = 0;
		 if (p1 > p2)
		  {
		  	  p1 -=step;
		  }else if(p1 < p2){
			  p1 +=step;
		  }
		  if (p2==startval && fabsf(p1-p2)<=step )
		 {
			 p1 =startval;
		 }

	}
	else{
		
		}
	 
	 

	
	 
	  //p1 = constrain_int16(p1, -(Speedmax_hand+maxerror), Speedmax_hand+maxerror);
	  p1 = constrain_int16(p1, -((int16_t)maxspeed+maxerror), (int16_t)maxspeed+maxerror);

	  static int j = 0;
	  j++;
	  if(j>800)
	  {
		  
		 //  gcs().send_text(MAV_SEVERITY_WARNING, " thrust%d  %f ,%d,%d\n",i,thrust_in,thrust,last_thrust_pwm[i]);
		   j=0;
	  }
	 
	
	
}
float Sub::Constrate1(float d1)
{//摇杆死区设置 0.06= 30/500
	if (fabsf(d1)*100<6)
		{
		return 0.0;
		}
	else{
		return d1;
	}
}

//extern mavlink_motor_feedback_t motor_feedback;
extern mavlink_motor_speed_t mav_motor_speed;
extern mavlink_rov_state_monitoring_t rov_message;

void Sub::motor_toCan(void)
 {

	 
		  

   if(mav_motor_speed.motorTest == 1)//电机测试模式
   {
	   motors.motor_to_can[8]  = mav_motor_speed.Ltrack*4;//切换成上位机控制 左履带
	   motors.motor_to_can[9] = mav_motor_speed.Rtrack*4;//右履带
   }else{
	   if(control_mode == CLEAN){
		motors.motor_to_can[8]	 = motor1_speed_target;//飞控自己的履带
		motors.motor_to_can[9] = motor2_speed_target;
   	   }else{//非ARCO模式履带不工作  
		   motors.motor_to_can[8]	= 0;
		   motors.motor_to_can[9] = 0;
	   }
   }
		static int jsn = 0;
			jsn++;
			 if(jsn>400)
			{
					gcs().send_text(MAV_SEVERITY_INFO, " track %d %d \n",(int)motors.motor_to_can[8],motors.motor_to_can[9]);
					jsn=0;
			}  

 }
void Sub::clean_sidenet_auto(void)
{


}

//提取朝向误差并返回
float Sub::get_yaw_error(float yaw_heading){
     //目标四元数
	Quaternion error_head;
	error_head.from_axis_angle(Vector3f(0, 0, yaw_heading*RAD_TO_DEG));
	_attitude_target_quat = _attitude_target_quat*error_head;

	 
	//当前姿态
	Quaternion attitude_vehicle_quat;
	ahrs.get_quat_body_to_ned(attitude_vehicle_quat);
	
	 
	//当前姿态Z轴
	 Matrix3f att_to_rot_matrix; // rotation from the target body frame to the inertial frame.
     attitude_vehicle_quat.rotation_matrix(att_to_rot_matrix);
     Vector3f att_to_thrust_vec = att_to_rot_matrix * Vector3f(0.0f, 0.0f, 1.0f);
	 
	 //目标姿态Z轴
     Matrix3f att_from_rot_matrix; // rotation from the current body frame to the inertial frame.
     _attitude_target_quat.rotation_matrix(att_from_rot_matrix);
     Vector3f att_from_thrust_vec = att_from_rot_matrix * Vector3f(0.0f, 0.0f, 1.0f);

	 Vector3f thrust_vec_cross = att_from_thrust_vec % att_to_thrust_vec;//得到轴
	 float thrust_vec_dot = acosf(constrain_float(att_from_thrust_vec * att_to_thrust_vec, -1.0f, 1.0f));//得到角

	 
	 float thrust_vector_length = thrust_vec_cross.length();
		 if (is_zero(thrust_vector_length) || is_zero(thrust_vec_dot)) {
			 thrust_vec_cross = Vector3f(0, 0, 1);
			 thrust_vec_dot = 0.0f;
		 } else {
			 thrust_vec_cross /= thrust_vector_length;
		 }
		 

	Quaternion error_lean;
	error_lean.from_axis_angle(thrust_vec_cross, thrust_vec_dot);// 地系下的从obj到half态的误差四元数
	
	Quaternion error_lean_body= attitude_vehicle_quat.inverse() * error_lean * attitude_vehicle_quat;//b系下的从obj到half态的误差四元数

	Quaternion lean_earth= attitude_vehicle_quat * error_lean_body ;//half态下的四元数

	Quaternion error_yaw= lean_earth.inverse() * _attitude_target_quat ;//yaw误差四元数
	
	Vector3f rotation;
    error_yaw.to_axis_angle(rotation);//得到轴角
    float yaw_error = rotation.z*RAD_TO_DEG;//得到yaw方位的误差
    return yaw_error;


}

void Sub::clean_sidenet_state(void){
	//-------------机器人头朝上-自动洗网-
		min_depth =SRV_Channels::srv_channel(11)->get_output_min();// 上端距离 厘米 
		max_depth = SRV_Channels::srv_channel(11)->get_output_max();//最深距离 厘米
		int16_t depth_now =fabsf((int16_t)barometer.get_altitude())*100;

		if(autoclean_flag == FALSE)
		{//没有自动洗网 保存深度值
			autoclean_orgin =depth_now;
			autoclean_step = Orign;
			
		}else{
					int16_t depth_down=min_depth;
					static int8_t delayCnt = 0;
					int16_t depth_up = max_depth;
					static int8_t delayCnt2 = 0;
			switch(autoclean_step){
				case Orign:
					track_head_gd = 0;
					//起始位置位于中上部，先往下走，如果起始位置在中下部 先往上走 假设头向上
					if(fabsf(autoclean_orgin-min_depth)>fabsf(autoclean_orgin-max_depth)){
						track_motor_arm =2;//向前走
						autoclean_step = foward;
					}else{
						track_motor_arm =0;//向后走
						autoclean_step = backward;
					}
				break;
				case foward:
					//向前走 2
					if (track_motor_arm !=0 && depth_now<=min_depth)
						{//转折处 前走 或者 停 但深度小于设置值
						track_head_gd = turn_angle;//10度 转10度
						track_motor_arm = 1;//先停
						delayCnt++;
						if (delayCnt>100)//停后延时，400 HZ 理论是0.25秒
							{
							track_motor_arm = 0;
							delayCnt =0;
							}
						}else{
						}
					 if (track_motor_arm == 0 && depth_now -depth_down>100)//相反方向走了xx cm	
						{//切换到向后走
						track_head_gd = 0.0;//0度
						autoclean_step =backward;
						}
				break;
				case backward:
					//向后走kk
					if (track_motor_arm !=2 && depth_now>=max_depth)
					{//转折处 向前走或者停 深度大于设置的最大深度
						track_head_gd = -turn_angle;//-10度
						track_motor_arm = 1;//2;
						delayCnt2++;
						if (delayCnt2>100)
						{
							delayCnt2=0;
							track_motor_arm=2;
						}
					}
					else{
					}
					if (track_motor_arm == 2 && depth_up-depth_now>100)////相反方向走了xx cm
					{//切换到向前走
						track_head_gd = 0.0;
						autoclean_step =foward;//切换状态
					}
					break;
				default:
					track_motor_arm =1;
					track_head_gd=0.0;
					break;
				}
		}

}


void Sub::clean_sidenet_run(void)//自动洗测网
{
//水平方向还没有自动洗网，没有测试

	static int16_t motors1=0;
	static int16_t motors2=0;
	if (autoclean_flag == FALSE)
	{//没有自动洗网返回
		motors1=0;
		motors2=0;
		return;
	}
		//PID设置
		trackpid.p1 =	 attitude_control._thr_mix_man;
		trackpid.p2 =	 attitude_control._thr_mix_man;
		trackpid.i1 =	 attitude_control._thr_mix_max;
		trackpid.i2 =	 attitude_control._thr_mix_max;
		trackpid.d1 =	 attitude_control._thr_mix_min;
		trackpid.d2 =	 attitude_control._thr_mix_min;
	//1000-1500向前走,左履带1，右履带2，方向 右歪增大
		static int8_t per = 0;//分频数
		if (per >3)
		{//50HZ控制频率
			per = 0;
			track_pidcontrol(track_head_gd,motors1,motors2);
		}
		per++;
		slowly_speed1(motor1_speed_target,motors1,1,3);
		slowly_speed2(motor2_speed_target,motors2,1,3);
}

//  不同方向上的履带的PID调用 以及缓加缓减 
 void Sub::track_pidcontrol(float _targethead,int16_t &_motor1,int16_t &_motor2){
	 int16_t motors1=startval;
	 int16_t motors2 =startval;
	 uint32_t nowtime=AP_HAL::micros();
	 static uint32_t lasttime = nowtime;//用于换向延时防止抱死
 	float error = get_yaw_error(_targethead);
	 if (track_motor_arm ==0)
	 {//后走
			 if (nowtime - lasttime <500000)//500ms T1
			 {//防抱死
				 motors1 = startval;
				 motors2 = startval;
			 }
			 else{
			 	
				 motors1 = constrain_int16(-Speedmax+trackpid.updatePID1(0,0,error,(float)Speedmax),-Speedmax,Speedmax);
				 motors2 = constrain_int16(-Speedmax+trackpid.updatePID2(0,0,error,(float)Speedmax),-Speedmax,Speedmax);
 
			 }
	 }
	 else if (track_motor_arm ==1)
	 {//停
			 lasttime = AP_HAL::micros();
			 motors1 = startval;
			 motors2 =startval;
			 
	 }
	 else if (track_motor_arm ==2)
	 {//前走
			 
			 if (nowtime - lasttime <500000)//500ms T1
			 {//防抱死
					 motors1 =startval;
					 motors2 = startval;
			 }
			 else{
				 motors1 = constrain_int16(Speedmax+trackpid.updatePID1(0,0,error,(float)Speedmax),-Speedmax,Speedmax);
				 motors2 = constrain_int16(Speedmax+trackpid.updatePID2(0,0,error,(float)Speedmax),-Speedmax,Speedmax);
 
				 }
 
	 }
 
	 _motor1 = motors1;
	 _motor2 = motors2;

 
 }