#include "Sub.h"
#define PI	3.141592653589793238462643383279502884

// stabilize_init - initialise stabilize controller
bool Sub::stabilize_init()
{
    // set target altitude to zero for reporting
    pos_control.relax_alt_hold_controllers();//目标高度为当前高度，目标速度和上次速度 当前速度
    
    last_roll_s = 0;
    last_pitch_s = 0;
    updowmgain =0.5;
	delaygain = 401;

	continuous_count=0;
	continuous_countdec=0;
	last_continuous = 0;
    last_yaw_s = ahrs.yaw_sensor;
    //last_input_ms = AP_HAL::millis();
	last_input_ms_stable = AP_HAL::millis();

	pitch_input_inc=0;
	
	prepare_state = Horizontal;
    return true;
}

// stabilize_run - runs the main stabilize controller
// should be called at 100hz or more
extern mavlink_rov_state_monitoring_t rov_message;

void Sub::handle_attitude_stablize(){
	
	uint32_t tnow = AP_HAL::millis();

	motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);

	// get pilot desired lean angles
	float target_roll, target_pitch, target_yaw;

	// Check if set_attitude_target_no_gps is valid
	if (tnow - sub.set_attitude_target_no_gps.last_message_ms < 5000) {
		Quaternion(
			set_attitude_target_no_gps.packet.q
		).to_euler(
			target_roll,
			target_pitch,
			target_yaw
		);
		target_roll = 100 * degrees(target_roll);
		target_pitch = 100 * degrees(target_pitch);
		target_yaw = 100 * degrees(target_yaw);
		last_roll_s = target_roll;
		last_pitch_s = target_pitch;
		last_yaw_s = target_yaw;
		
		attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, target_yaw, true);
	} else {
		

		int32_t pitch_input   = 0;
		

		if (prepare_state == Horizontal)
	   {
		   pitch_input = 0;
		   target_roll = 0;
		   last_roll_s = 0;
		   target_yaw = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()*0.4);

		   
	   }else if(prepare_state == Vertical)
		{
		   pitch_input = 8000;
		   //target_roll = ((float)channel_yaw->get_control_in()*0.4);
		   target_roll = 0;
		   target_yaw = ((float)channel_yaw->get_control_in()*0.4);
	   }
	   else{
			pitch_input = 0;
		}
	   pitch_input_inc = constrain_int32(pitch_input,-9000,9000);
	   //-------------------------------------
	 
	  if (abs(target_roll) > 300 ){
		attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, 0, 0);
		last_yaw_s = ahrs.yaw_sensor;
		last_roll_s = ahrs.roll_sensor;
		last_input_ms_stable = tnow;
		
		}else if (abs(target_yaw) > 300) {
			// if only yaw is being controlled, don't update pitch and roll
			attitude_control.input_rate_bf_roll_pitch_yaw(0, 0, target_yaw);
			last_yaw_s = ahrs.yaw_sensor;
			
			last_input_ms_stable = tnow;
		} else if (tnow < last_input_ms_stable + 250) {
			// just brake for a few mooments so we don't bounce
			last_yaw_s = ahrs.yaw_sensor;
			
			attitude_control.input_rate_bf_roll_pitch_yaw(0, 0, 0);
		} else {
			// Lock attitude
			attitude_control.input_euler_angle_roll_pitch_yaw(last_roll_s, (float)pitch_input_inc, last_yaw_s, true);
			
		}//--------------------------------------------------------------------------------------------------------------
	   static int f = 0;
		f++;
		if(f>800)
		{
	

			//gcs().send_text(MAV_SEVERITY_INFO, " stableinput %f %f,%f ",(float)last_roll_s,(float)pitch_input_inc,(float)last_yaw_s);
			f=0;
		}
		
	}
}

void Sub::stabilize_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();
		pos_control.relax_alt_hold_controllers();//目标高度为当前高度，目标速度和上次速度 当前速度
        last_roll_s = 0;
        last_pitch_s = 0;
		updowmgain =0.5;
		delaygain = 401;
	
		continuous_count=0;
		continuous_countdec=0;
		last_continuous = 0;
        last_yaw_s = ahrs.yaw_sensor;

		PressLevel_f =5.0;//压力分级复位
		PressLevel = no;
		//yaw_press = (int16_t)(ahrs.yaw_sensor/100);//记住方位
		pitch_input_inc=0;
		prepare_state = Horizontal;
        return;
    }
    motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);

    handle_attitude_stablize();


    if(stable_alt_control){
			altcontrol();
	}
	else{
		//防止交叉
		 float _forward = channel_forward->norm_input();
		 float _lateral =channel_lateral->norm_input();
		if (fabsf(_forward)>=fabsf(_lateral))
		 {
		   _lateral = 0.0;
		  }else{
			_forward = 0.0;
		  }
    	motors.set_forward(constrain_float(getgainf(_forward),-0.8,0.8));
    	motors.set_lateral(constrain_float(getgainf(_lateral),-0.8,0.8));
		
		attitude_control.set_throttle_out(gaindelay(1.0-(float)PressLevel_f*0.1), false, g.throttle_filt);//压力分级
		 static uint16_t count = 0;
		count++;
		if (count>400)
		{
			count = 0;
			gcs().send_text(MAV_SEVERITY_INFO, "set_throttle_out %f %f \n",gaindelay(1.0-(float)PressLevel_f*0.1),motors.get_throttle());
	
		}

	
	rov_message.pressure_level = int(PressLevel);
	}
}