// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-

//****************************************************************
// Function that controls aileron/rudder, elevator, rudder (if 4 channel control) and throttle to produce desired attitude and airspeed.
//****************************************************************

void stabilize()
{
	float ch1_inf = 1.0f;
	float ch2_inf = 1.0f;
	float ch4_inf = 1.0f;
#if AIRSPEED_SENSOR == ENABLED
	float speed_scaler = STANDARD_SPEED_SQUARED / (airspeed * airspeed);
	speed_scaler = constrain(speed_scaler, 0.11f, 9.0f);
#endif
#if AIRSPEED_SENSOR == DISABLED
	float speed_scaler;
	if (servo_out[CH_THROTTLE] > 0) 
		speed_scaler = 0.5f + (THROTTLE_CRUISE / servo_out[CH_THROTTLE] / 2.0f);	// First order taylor expansion of square root
																				// Should maybe be to the 2/7 power, but we aren't goint to implement that...
	else
		speed_scaler = 1.67f;
	speed_scaler = constrain(speed_scaler, 0.6f, 1.67f);		// This case is constrained tighter as we don't have real speed info
#endif
	
	
	if(crash_timer > 0){
		nav_roll = 0;
	}

	// For Testing Only
	// roll_sensor = (radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 10;
	// Serial.print(" roll_sensor ");
	// Serial.print(roll_sensor,DEC);

	// Calculate dersired servo output for the roll 
	// ---------------------------------------------
	servo_out[CH_ROLL]	= pidServoRoll.get_pid((nav_roll - dcm.roll_sensor), deltaMiliSeconds, speed_scaler);
	servo_out[CH_PITCH] = pidServoPitch.get_pid((nav_pitch + fabsf(dcm.roll_sensor * get(PARAM_KFF_PTCHCOMP)) - (dcm.pitch_sensor - get(PARAM_TRIM_PITCH))), deltaMiliSeconds, speed_scaler);
	//Serial.print(" servo_out[CH_ROLL] ");
	//Serial.print(servo_out[CH_ROLL],DEC);

	// Mix Stick input to allow users to override control surfaces
	// -----------------------------------------------------------
	if ((control_mode < FLY_BY_WIRE_A) || (ENABLE_STICK_MIXING == 1 && control_mode > FLY_BY_WIRE_B)) {
	
		ch1_inf = (float)radio_in[CH_ROLL] - (float)radio_trim(CH_ROLL);
		ch1_inf = fabsf(ch1_inf);
		ch1_inf = min(ch1_inf, 400.0);
		ch1_inf = ((400.0 - ch1_inf) /400.0);
		
		ch2_inf = (float)radio_in[CH_PITCH] - radio_trim(CH_PITCH);
		ch2_inf = fabsf(ch2_inf);									
		ch2_inf = min(ch2_inf, 400.0);							
		ch2_inf = ((400.0 - ch2_inf) /400.0);
		
		// scale the sensor input based on the stick input
		// -----------------------------------------------
		servo_out[CH_ROLL]		*= ch1_inf;
		servo_out[CH_PITCH]		*= ch2_inf;
	
		// Mix in stick inputs 
		// -------------------
		servo_out[CH_ROLL]		+=	reverse_roll * 	(radio_in[CH_ROLL]  - radio_trim(CH_ROLL)) * 9;
		servo_out[CH_PITCH]		+=	reverse_pitch * (radio_in[CH_PITCH] - radio_trim(CH_PITCH)) * 9;

		//Serial.print(" servo_out[CH_ROLL] ");
		//Serial.println(servo_out[CH_ROLL],DEC);
	}

	// stick mixing performed for rudder for all cases including FBW unless disabled for higher modes
	// important for steering on the ground during landing
	// -----------------------------------------------
	if (control_mode <= FLY_BY_WIRE_B || ENABLE_STICK_MIXING == 1) {
		ch4_inf = (float)radio_in[CH_RUDDER] - (float)radio_trim(CH_RUDDER);
		ch4_inf = fabsf(ch4_inf);									
		ch4_inf = min(ch4_inf, 400.0);							
		ch4_inf = ((400.0 - ch4_inf) /400.0);
	}
	
	// Apply output to Rudder
	// ----------------------
	calc_nav_yaw(speed_scaler);
	servo_out[CH_RUDDER] *= ch4_inf;
	servo_out[CH_RUDDER] += reverse_rudder * (radio_in[CH_RUDDER] - radio_trim(CH_RUDDER)) * 9;
	
	// Call slew rate limiter if used
	// ------------------------------
	//#if(ROLL_SLEW_LIMIT != 0)
	//	servo_out[CH_ROLL] = roll_slew_limit(servo_out[CH_ROLL]);
	//#endif	
}

void crash_checker()
{
	if(dcm.pitch_sensor < -4500){
		crash_timer = 255;
	}
	if(crash_timer > 0)
		crash_timer--;
}


#if AIRSPEED_SENSOR == DISABLED
void calc_throttle()
{
	int throttle_target = get(PARAM_TRIM_THROTTLE) + throttle_nudge;
	
	// no airspeed sensor, we use nav pitch to determine the proper throttle output
	// AUTO, RTL, etc
	// ---------------------------------------------------------------------------
	if (nav_pitch >= 0) {
		servo_out[CH_THROTTLE] = throttle_target + (get(PARAM_THR_MAX) - throttle_target) * nav_pitch / get(PARAM_LIM_PITCH_MAX);
	} else {
		servo_out[CH_THROTTLE] = throttle_target - (throttle_target - get(PARAM_THR_MIN)) * nav_pitch / get(PARAM_LIM_PITCH_MIN);
	}
	
	servo_out[CH_THROTTLE] = constrain(servo_out[CH_THROTTLE], get(PARAM_THR_MIN), get(PARAM_THR_MAX));
}
#endif

#if AIRSPEED_SENSOR == ENABLED
void calc_throttle()
{
	// throttle control with airspeed compensation	 
	// -------------------------------------------
	energy_error = airspeed_energy_error + (float)altitude_error * 0.098f;
	
	// positive energy errors make the throttle go higher
	servo_out[CH_THROTTLE] = get(PARAM_TRIM_THROTTLE) + pidTeThrottle.get_pid(energy_error, dTnav);
	servo_out[CH_THROTTLE] = max(servo_out[CH_THROTTLE], 0);

	// are we going too slow? up the throttle to get some more groundspeed
	// move into PID loop in the future
	// lower the contstant value to limit the effect : 50 = default
	
	//JASON - We really should change this to act on airspeed in this case.  Let's visit about it....
	/*int gs_boost = 30 * (1.0 - ((float)gps.ground_speed / (float)airspeed_cruise));
	gs_boost = max(0,gs_boost);
	servo_out[CH_THROTTLE] += gs_boost;*/
	servo_out[CH_THROTTLE] = constrain(servo_out[CH_THROTTLE], 
			get(PARAM_THR_MIN), get(PARAM_THR_MAX));
}
#endif



/*****************************************
 * Calculate desired roll/pitch/yaw angles (in medium freq loop)
 *****************************************/

//  Yaw is separated into a function for future implementation of heading hold on rolling take-off
// ----------------------------------------------------------------------------------------
void calc_nav_yaw(float speed_scaler)
{
#if HIL_MODE != HIL_MODE_ATTITUDE
	Vector3f temp = imu.get_accel();
	long error = -temp.y;

	// Control is a feedforward from the aileron control + a PID to coordinate the turn (drive y axis accel to zero)
	servo_out[CH_RUDDER] = get(PARAM_KFF_RDDRMIX) * servo_out[CH_ROLL] + pidServoRudder.get_pid(error, deltaMiliSeconds, speed_scaler);
#else
	servo_out[CH_RUDDER] = get(PARAM_KFF_RDDRMIX) * servo_out[CH_ROLL];
	// XXX probably need something here based on heading
#endif
}


void calc_nav_pitch()
{
	// Calculate the Pitch of the plane
	// --------------------------------
#if AIRSPEED_SENSOR == ENABLED
	nav_pitch = -pidNavPitchAirspeed.get_pid(airspeed_error, dTnav);
#else
	nav_pitch = pidNavPitchAltitude.get_pid(altitude_error, dTnav);
#endif
	nav_pitch = constrain(nav_pitch, get(PARAM_LIM_PITCH_MIN), get(PARAM_LIM_PITCH_MAX));
}


void calc_nav_roll()
{

	// Adjust gain based on ground speed - We need lower nav gain going in to a headwind, etc.
	// This does not make provisions for wind speed in excess of airframe speed
	nav_gain_scaler = (float)gps.ground_speed / (STANDARD_SPEED * 100.0);
	nav_gain_scaler = constrain(nav_gain_scaler, 0.2, 1.4);
	
	// negative error = left turn
	// positive error = right turn
	// Calculate the required roll of the plane
	// ----------------------------------------
	nav_roll = pidNavRoll.get_pid(bearing_error, dTnav, nav_gain_scaler);	//returns desired bank angle in degrees*100
	nav_roll = constrain(nav_roll,-get(PARAM_LIM_ROLL), get(PARAM_LIM_ROLL));
	
}


/*****************************************
 * Roll servo slew limit
 *****************************************/
/*
float roll_slew_limit(float servo)
{
	static float last;
	float temp = constrain(servo, last-ROLL_SLEW_LIMIT * deltaMiliSeconds/1000.f, last + ROLL_SLEW_LIMIT * deltaMiliSeconds/1000.f);
	last = servo;
	return temp;
}*/

/*****************************************
 * Throttle slew limit
 *****************************************/
/*float throttle_slew_limit(float throttle)
{
	static float last;
	float temp = constrain(throttle, last-THROTTLE_SLEW_LIMIT * deltaMiliSeconds/1000.f, last + THROTTLE_SLEW_LIMIT * deltaMiliSeconds/1000.f);
	last = throttle;
	return temp;
}
*/

// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
// Keeps outdated data out of our calculations
void reset_I(void)
{
	pidNavRoll.reset_I();
	pidNavPitchAirspeed.reset_I();
	pidNavPitchAltitude.reset_I();
	pidTeThrottle.reset_I();
	pidAltitudeThrottle.reset_I();
}

/*****************************************
* Set the flight control servos based on the current calculated values
*****************************************/
void set_servos_4(void)
{
	if(control_mode == MANUAL){
		// do a direct pass through of radio values
		for(int y=0; y<4; y++) {
			radio_out[y] = radio_in[y];
		}
		
	} else {
		// Patch Antenna Control Code
		float phi, theta; //,servo_phi, servo_theta;
		float x1,x2,y1,y2,x,y,r,z;
		
		y1 = 110600*current_loc.lat/t7;
		x1 = (PI/180)*6378137*(cosf(atanf(0.99664719f*tanf(current_loc.lat/t7*PI/180))))*(current_loc.lng/t7);

		y2 = 110600*trackVehicle_loc.lat/t7;
		x2 = (PI/180)*6378137*(cosf(atanf(0.99664719f*tanf(current_loc.lat/t7*PI/180))))*(trackVehicle_loc.lng/t7);

		x = abs(x2 - x1);
		y = abs(y2 - y1);

		r = sqrtf(x*x+y*y);
		z = trackVehicle_loc.alt/100.0f - current_loc.alt;

		phi   = (atanf(z/r)*180/PI);
		theta = (atanf(x/y)*180/PI);
		// Check to see which quadrant of the angle
		
		if (trackVehicle_loc.lat >= current_loc.lat && trackVehicle_loc.lng >= current_loc.lng)
		{
			theta = abs(theta);
		}
		else if (trackVehicle_loc.lat >= current_loc.lat && trackVehicle_loc.lng <= current_loc.lng)
		{
			theta = 360 - abs(theta);
		}
		else if (trackVehicle_loc.lat <= current_loc.lat && trackVehicle_loc.lng >= current_loc.lng)		
		{
			theta = 180 - abs(theta);
		}
		else if (trackVehicle_loc.lat <= current_loc.lat && trackVehicle_loc.lng <= current_loc.lng)
		{
			theta = 180 +  abs(theta);
		}
	
		// Calibration of the top servo
		//servo_phi   = (91*phi + 7650)/9;

		// Calibration of the bottom servo
		//servo_theta = (2*theta + 5940)/3;
		

		Serial.print("target lat: "); Serial.println(current_loc.lat);
		Serial.print("tracker lat: "); Serial.println(trackVehicle_loc.lat);
		Serial.print("phi: "); Serial.println(phi);
		Serial.print("theta: "); Serial.println(theta);
		
		// Outputing to the servos
		servo_out[CH_ROLL] = 10000*phi/90.0f;
		servo_out[CH_PITCH] = 10000*theta/360.0f;
		servo_out[CH_RUDDER] = 0;
		servo_out[CH_THROTTLE] = 0;

		radio_out[CH_ROLL] 		= radio_trim(CH_ROLL)   + (reverse_roll   * ((float)servo_out[CH_ROLL]   * 0.11111));
		radio_out[CH_PITCH] 	= radio_trim(CH_PITCH)  + (reverse_pitch  * ((float)servo_out[CH_PITCH]  * 0.11111));
		radio_out[CH_RUDDER] 	=  0;
		radio_out[CH_THROTTLE]  = 0;

		/*
		if (mix_mode == 0){
			radio_out[CH_ROLL] 		= radio_trim(CH_ROLL)   + (reverse_roll   * ((float)servo_out[CH_ROLL]   * 0.11111));
			radio_out[CH_PITCH] 	= radio_trim(CH_PITCH)  + (reverse_pitch  * ((float)servo_out[CH_PITCH]  * 0.11111));
			radio_out[CH_RUDDER] 	= radio_trim(CH_RUDDER) + (reverse_rudder * ((float)servo_out[CH_RUDDER] * 0.11111));
		}else{
			//Elevon mode
			float ch1;
			float ch2;
			ch1 = reverse_elevons * (servo_out[CH_PITCH] - servo_out[CH_ROLL]);	
			ch2 = servo_out[CH_PITCH] + servo_out[CH_ROLL];
			radio_out[CH_ROLL] =	elevon1_trim + (reverse_ch1_elevon * (ch1 * 0.11111));
			radio_out[CH_PITCH] =	elevon2_trim + (reverse_ch2_elevon * (ch2 * 0.11111));
		}
		
		#if THROTTLE_OUT == 0
			radio_out[CH_THROTTLE] = 0;
		#endif
		
	
		// convert 0 to 100% into PWM
		servo_out[CH_THROTTLE] = constrain(servo_out[CH_THROTTLE], 0, 100);
		radio_out[CH_THROTTLE] = servo_out[CH_THROTTLE] * ((radio_max(CH_THROTTLE) - radio_min(CH_THROTTLE)) / 100);
		radio_out[CH_THROTTLE] += radio_min(CH_THROTTLE);

		//Radio_in: 1763	PWM output: 2000	Throttle: 78.7695999145	PWM Min: 1110	PWM Max: 1938

		#if THROTTLE_REVERSE == 1
			radio_out[CH_THROTTLE] = radio_max(CH_THROTTLE) + radio_min(CH_THROTTLE) - radio_out[CH_THROTTLE];
		#endif
		*/
	}

	// send values to the PWM timers for output
	// ----------------------------------------
	for(int y=0; y<4; y++) { 
		APM_RC.OutputCh(y, radio_out[y]); // send to Servos
	}
 }

void demo_servos(byte i) {
	
	while(i > 0){
		gcs.send_text(SEVERITY_LOW,"Demo Servos!");
		APM_RC.OutputCh(1, 1400);
		delay(400);
		APM_RC.OutputCh(1, 1600);
		delay(200);
		APM_RC.OutputCh(1, 1500);
		delay(400);
		i--;
	}
}

int readOutputCh(unsigned char ch)
{
 int pwm;
 switch(ch)
	{
		case 0:	pwm = OCR5B; break;	// ch0
		case 1:	pwm = OCR5C; break;	// ch1
		case 2:	pwm = OCR1B; break;	// ch2
		case 3:	pwm = OCR1C; break;	// ch3
		case 4:	pwm = OCR4C; break;	// ch4
		case 5:	pwm = OCR4B; break;	// ch5
		case 6:	pwm = OCR3C; break;	// ch6
		case 7:	pwm = OCR3B; break;	// ch7
		case 8:	pwm = OCR5A; break;	// ch8,	PL3
		case 9:	pwm = OCR1A; break;	// ch9,	PB5
		case 10: pwm = OCR3A; break;	// ch10, PE3
	}
	pwm >>= 1;	 // pwm / 2;
	return pwm;
}