#include "Sub.h"
//const AP_HAL::HAL& hal = AP_HAL::get_HAL();
extern const AP_HAL::HAL& hal;
#define PI	3.141592653589793238462643383279502884


extern mavlink_rov_state_monitoring_t rov_message;


#ifdef USERHOOK_INIT
void Sub::userhook_init()
{
    // put your initialisation code here
    // this will be called once at start-up
}
#endif

#ifdef USERHOOK_FASTLOOP
void Sub::userhook_FastLoop()
{
    // put your 100Hz code here
}
#endif

#ifdef USERHOOK_50HZLOOP
extern mavlink_rov_control_t rov_control;

void Sub::userhook_50Hz()
{
    USBL_PowerSwitch();
	getgain();
	getyaw();

	 float v  = chan_adc->voltage_average();

	 
	  chan_adc->set_pin(14);
	 float temprature =0.0;
	 if (v>2.58)
	{
		 temprature = -44.017*v + 112.64;
	}else if(v>1.0){
		temprature =-28.635*v + 72.616;
	}else if(v>0.518){
		temprature =-48.078*v + 91.882;
	}else if(v>0.28){
		temprature =-90.07*v + 113.38;
	}else if(v>0.12){
		temprature = -195.98*v + 141.46;
	}
	else{
		temprature = 120;
	}

	rov_message.temp = temprature;



     
}
#endif

#ifdef USERHOOK_MEDIUMLOOP
void Sub::userhook_MediumLoop()
{
    // put your 10Hz code here
}
#endif

#ifdef USERHOOK_SLOWLOOP
void Sub::userhook_SlowLoop()
{
    // put your 3.3Hz code here
}
#endif

#ifdef USERHOOK_SUPERSLOWLOOP
void Sub::userhook_SuperSlowLoop()
{
    // put your 1Hz code here
}
#endif

void Sub::USBL_PowerSwitch(void)
{
	uint32_t tnow = AP_HAL::millis();
	RC_Channel* chan = RC_Channels::rc_channel(8);
	uint16_t min = chan->get_radio_min();
	uint16_t max = chan->get_radio_max();

	   	if(!motors.armed()){	
	   		lights1 = constrain_float(min, min, max);
	   	}
	   if (lights1>1000)
		  {
		   lights = 1;
		  	}else{
	   		lights =0;
		}
	   
		RC_Channels::set_override(8, lights1, tnow);
		if(motors.armed() && (rov_control.USBL_flag==1 || usblpoweroff == 1))
		{
			
			 RC_Channels::set_override(9, 20000, tnow);       // lights 2
		
			
		}else{
		 	
			   RC_Channels::set_override(9, 0, tnow);		// lights 1
			 
		}
		

}
extern mavlink_rov_state_monitoring_t rov_message;

void Sub::getgain(void){
	//上位机设置油门
	//uint16_t _gain = SRV_Channels::srv_channel(9)->get_output_min();//上位机设置油门
	
	gain = 1.0;
	

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

void Sub::getyaw(void){
	if (!motors.armed()){
		//yaw_press = (int16_t)(ahrs.yaw_sensor/100);//记住方位
		 return;
	}else{
	
	}
}

float Sub::getgainf(float data){
	float temp =0;

	if (data>=-0.6 && data <-0.08)
	{
		temp = -0.2;
	}else if(data>=-0.8 && data<-0.6 )
	{
		temp = -0.5;
	}else if(data>=-1.0 && data<-0.8 )
	{
		temp = -0.8;
	}
	else if (data<=0.6 && data >0.08)
	{
		temp = 0.2;
	}else if(data<=0.8 && data>0.6 )
	{
		temp = 0.5;
	}else if(data<=1.0 && data>0.8 )
	{
		temp = 0.8;
	}else{
		temp = 0.0;
	}
	return temp;
}
float Sub::sign_in(float f){
	return (f<0)? -1 :1;
}
float Sub::gaindelay(float gain1){

	if(throttle_continuous(gain1)){
		
		bool vel_stationary = velocity_z_filer > -5.0 && velocity_z_filer < 5.0;//cm
		float	result = (updowmgain-0.5)*2;

		if (vel_stationary){
			last_continuous = 1;
			 continuous_count++;
			 if (continuous_count>=MAIN_LOOP_RATE && continuous_count<=4*MAIN_LOOP_RATE){
				result = result + sign_in(result)*0.2*(continuous_count/MAIN_LOOP_RATE);//步长是0.2
				result = constrain_float(result,-1.0,1.0);
			}else if(continuous_count>4*MAIN_LOOP_RATE){
				result = result +sign_in(result)*0.2*(continuous_count/MAIN_LOOP_RATE);//步长是0.2
				result = constrain_float(result,-1.0,1.0);
				continuous_count = 4*MAIN_LOOP_RATE+1;
			}
		}else{
			if (last_continuous == 1)
			{
				if (continuous_countdec<MAIN_LOOP_RATE){
					continuous_countdec++;
					result = result + sign_in(result)*0.2*(continuous_count/MAIN_LOOP_RATE);//步长是0.2
					result = constrain_float(result,-1.0,1.0);
				}else{
					result = result + sign_in(result)*0.2*(continuous_count/MAIN_LOOP_RATE);//步长是0.2
					result = constrain_float(result,-1.0,1.0);
				}
			}else{
					result = (updowmgain-0.5)*2;
					continuous_count = 0;
					last_continuous = 0;
					continuous_countdec = 0;
			}
		
		}
		 static uint16_t count = 0;
	 count++;
	 if (count>400)
		 {
		 count = 0;
		 gcs().send_text(MAV_SEVERITY_INFO, "conti %d %d %d \n",(int)continuous_count,(int)last_continuous,(int)continuous_countdec);

	 }

		return constrain_float(result/2.0+0.5,0.0,1.0);
	}else{

		 	
			continuous_count = 0;
			last_continuous = 0;
			continuous_countdec = 0;
		return gain1;//有压力分级的时候返回压力分级 没有压力分级的时候返回按键
	}
    
}
bool Sub::throttle_continuous(float gain1){
	static float last_gain = updowmgain;
	if (fabsf(gain1 -0.5)<0.05)//没有压力分级
	{
		delaygain++;
		if (delaygain>400)//1秒
		{
			delaygain=401;
			updowmgain = 0.5;
			last_gain = updowmgain;
			return FALSE;//1秒内没有持续按键
		}else{
			
			if ((last_gain>0.5 && updowmgain<0.5) || (last_gain<0.5 && updowmgain>0.5))//防止突然反向
			{
				last_gain = updowmgain;
				return FALSE;
			}
			last_gain = updowmgain;
			return TRUE;//持续按键
		}
		
    }else{
    	delaygain=401;
    //有压力分级
    	updowmgain = 0.5;
		last_gain = updowmgain;
		continuous_count = 0;
		last_continuous = 0;
		continuous_countdec = 0;
    	return FALSE;//1秒内没有持续按键
	}
}

/*float Sub::gaindelay(float gain1){
	 float result=0.5;
	 static uint16_t count=0;
	 static uint16_t countdec=0;
	 static uint8_t last = 0;
	if (fabsf(gain1 -0.5)<0.05)
	{
		delaygain++;
		if (delaygain>300)//1秒
		{
			delaygain=0;
			
			result = 0.5;
			updowmgain = 0.5;
			count = 0;
		}else{
			float velocity_z = pos_control.alt_rate.get()/100;// m/s
		    bool vel_stationary = velocity_z > -0.05 && velocity_z < 0.05;
			if (vel_stationary && updowmgain>0.5)//上升
			{
				last = 1;
				countdec = 0;
				 count++;
				 if (count<MAIN_LOOP_RATE)
				{
					 result = updowmgain;
				}
				 else if (count>=MAIN_LOOP_RATE && count<=3*MAIN_LOOP_RATE)
				{
					 result = updowmgain+0.1*(count/MAIN_LOOP_RATE);
					 result = constrain_float(result,-1.0,1.0);
				}else if(count>3*MAIN_LOOP_RATE){
					result = 1.0;
					count = 3*MAIN_LOOP_RATE+1;
				}
				
			}else{
				if (last == 1)
				{
					if (countdec<MAIN_LOOP_RATE){
						countdec++;
						result = updowmgain+0.1*(count/MAIN_LOOP_RATE);
					    result = constrain_float(result,-1.0,1.0);
					}else{
						last = 0;
						result = updowmgain;
						count = 0;
						countdec = 0;
					}
				}else{
					result = updowmgain;
					count = 0;
					last = 0;
					countdec = 0;
				}
			
			}
		}
		return result;
    }else{
		count = 0;
		updowmgain = 0.5;
		last = 0;
		countdec = 0;
		return gain1;//有压力分级的时候返回压力分级 没有压力分级的时候返回按键
    } 
}*/
//得到手柄的轴
void Sub::get_heading_to_ned(Matrix3f &mat_half,Matrix3f mat_body){

	// Matrix3f mat_body = ahrs.get_rotation_body_to_ned();
	float beta = 0.0;  
	float dot=0.0;
	
	if(mat_body.c.x<0.98 && mat_body.c.x>-0.98){//大约78.5度认为Z轴指向方向，小于78.5进入X轴指向方向
		Vector3f Xbx = mat_body*Vector3f(1,0,0);
		Xbx.z = 0.0;
		Vector3f Xex = Vector3f(1,0,0);
	
	   dot = Xbx * Xex/Xbx.length();
		beta = acosf(dot );
		if (Xbx.y<0)
	   {
		   beta = -beta;
	   }
		Vector3f Xbz = mat_body*Vector3f(0,0,1);
		Vector3f Xez = Vector3f(0,0,1);
	   float dotz = Xbz * Xez/Xbz.length();
		if (dotz<0)
	   {
			beta = beta-PI;
			if (beta<-PI)
		   {
				beta = beta+2*PI;
		   }
	   }
	
		
		mat_half.a.x= cosf(beta);
		mat_half.a.y=-sinf(beta);
		mat_half.a.z=0.0;  
		mat_half.b.x= sinf(beta);
		mat_half.b.y= cosf(beta);
		mat_half.b.z=0.0;
		mat_half.c.x= 0.0;
		mat_half.c.y= 0.0;
		mat_half.c.z=1.0;
	
	
	   }
	else{
	   
	   Vector3f Xbz = mat_body*Vector3f(0,0,1);
	   Xbz.z = 0.0;
	   Vector3f Xex = Vector3f(1,0,0);
	   dot = Xbz * Xex/Xbz.length();
	   beta =  acosf(dot );
	   if (Xbz.y<0)
	   {
		   beta = -beta;
	   }
		if (mat_body.c.x>0)//头朝下
	   {
			beta = beta-PI;
			if (beta<-PI)
		   {
				beta = beta+2*PI;
		   }
	   }
		mat_half.a.x= cosf(beta);
		mat_half.a.y=-sinf(beta);
		mat_half.a.z=0.0;  
		mat_half.b.x= sinf(beta);
		mat_half.b.y= cosf(beta);
		mat_half.b.z=0.0;
		mat_half.c.x= 0.0;
		mat_half.c.y= 0.0;
		mat_half.c.z=1.0;
	
	
	}

		static uint16_t countx = 0;
	   countx++;
	   if (countx>400)
		  {
		   countx = 0;
		   
		  	//   gcs().send_text(MAV_SEVERITY_INFO, "ang %d %d  \n",(int)(beta*180/PI),(int)ahrs.yaw_sensor/100);

	   }

}
float Sub::get_pitch_to_ned(Matrix3f mat_body){

	float pitch_ang = acosf(mat_body.c.x);
	if (mat_body.c.z<0)
	{
		pitch_ang = -pitch_ang;
	}
	pitch_ang =pitch_ang-PI/2;
	if (pitch_ang<-PI)
	{
		pitch_ang =pitch_ang+2*PI;
	}
	return pitch_ang;
}