%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Patrick LeGresley
% Stability & Control
% Last modified: April 26, 1999
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Inputs:
%  b_wing = wing span (ft)
%  S_ref = wing reference area (ft^2)
%  taper_wing = wing taper ratio
%  sweep_wing_25c = wing quarter chord sweep angle (deg)
%  x_wing = x-position of wing root leading edge (ft)
%  b_ht = horizontal tail span (ft)
%  Sht_ref = horizontal tail reference area (ft^2)
%  taper_ht = horizontal tail taper ratio
%  sweep_ht_25c = horizontal tail quarter chord sweep angle (deg)
%  x_ht = x-position of horizontal tail root leading edge (ft)
%  b_vt = vertical tail span (ft)
%  Svt_ref = vertical tail reference area (ft^2)
%  taper_vt = vertical tail taper ratio
%  sweep_vt_25c = vertical tail quarter chord sweep angle (deg)
%  x_vt = x-position of vertical tail root leading edge (ft)
%  h = cruise altitude (ft)
%  V = cruise speed (KTAS)
%  y_eng = distance from centerline to outboard engine (ft)
%  N_e = number of engines
%  x_maingear = x-position of main gear (ft)
%  CL_max_L = maximum lift coefficient in landing configuration
%  CL_max_to = maximum lift coefficient in takeoff configuration
%  W_to = takeoff weight (lbs)
%  x_cg_aft = x-postion of aftmost center of gravity (ft)
%  x_cg_forward = x-position of forwardmost center of gravity (ft)
%  z_cg = z-position of highest center of gravity (ft)
%  I_yy = pitching moment of inertia (slug-ft^2)
%  T_to = total takeoff installed thrust (lbs)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Outputs:
%  cg_max = maximum c.g. excursion (% chord)
%  SM_min = minimum static margin (% chord)
%  Sht_reqd = minimum h. tail size for airplane to be able to rotate (ft^2)
%  V_mc = minimum control speed (ft/sec)
%  V_s_L = stall speed in landing configuration (ft/sec)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function [cg_max, SM_min, Sht_reqd, V_mc, V_s_L] = stabcont(tiltwing,b_wing, S_ref,...
   taper_wing, sweep_wing_25c, x_wing, b_ht, Sht_ref, taper_ht, sweep_ht_25c,...
   x_ht, b_vt, Svt_ref, taper_vt, sweep_vt_25c, x_vt, h, V, y_eng, N_e, x_maingear,...
   CL_data_to, CL_data_land, W_to, x_cg_aft, x_cg_forward, z_cg, I_yy, T_to)


if tiltwing ~=1
   
   n_ht = 0.95;
   L_fus = 60;
   w_fus = 13;
   h_fus = 9.5;
   A_fus = 3.14159*(w_fus/2)*(h_fus/2);
   D_eff_fus = 2*sqrt(A_fus/3.14159);
   Kf_data = polyfit([.1 .2 .3 .4 .5 .6 .7],[.115 .172 .344 .487 .688 .888 1.146],3);
   
   % find beta
   [rho, Temp, a, mu] = atm(h);
   M=1.6878*V/a;
   kappa = 0.97;
   beta = sqrt(1-M^2);
   
   % find wing geometry
   AR_wing = b_wing^2/S_ref;
   c_r_wing = 2*S_ref/(b_wing*(1+taper_wing));
   c_t_wing = taper_wing*c_r_wing;
   mac_wing = (2/3)*c_r_wing*((1+taper_wing+taper_wing^2)/(1+taper_wing));
   y_mac_wing = b_wing*(1+2*taper_wing)/6/(1+taper_wing);
   sweep_wing_0c = atan(tan(sweep_wing_25c/57.3) + (1-taper_wing)/AR_wing/(1+taper_wing));
   sweep_wing_50c = tan(sweep_wing_0c) - 4*(0.5)*(1-taper_wing)/AR_wing/(1+taper_wing);
   n_mac_wing = tan(sweep_wing_0c)*y_mac_wing;
   x_ac_wing = x_wing + n_mac_wing + 0.25*mac_wing;
   C_L_alpha_wing = 2*pi*AR_wing/(2+sqrt(AR_wing^2*beta^2/kappa^2*(1+tan(sweep_wing_50c/57.3)^2/beta^2)+4));
   
   % find fuselage induced shift in aerodynamic center
   Kf = polyval(Kf_data,(x_wing+0.25*c_r_wing)/L_fus);
   dCm_CL_fus = Kf*w_fus^2*L_fus/S_ref/mac_wing/C_L_alpha_wing;
   x_ac_wf = x_ac_wing - mac_wing*dCm_CL_fus;
   
   % find horizontal tail geometry
   AR_ht = b_ht^2/Sht_ref;
   c_r_ht = 2*Sht_ref/(b_ht*(1+taper_ht));
   c_t_ht = taper_ht*c_r_ht;
   mac_ht = (2/3)*c_r_ht*((1+taper_ht+taper_ht^2)/(1+taper_ht));
   y_mac_ht = b_ht*(1+2*taper_ht)/6/(1+taper_ht);
   sweep_ht_0c = atan(tan(sweep_ht_25c/57.3) + (1-taper_ht)/AR_ht/(1+taper_ht));
   sweep_ht_50c = tan(sweep_ht_0c) - 4*(0.5)*(1-taper_ht)/AR_ht/(1+taper_ht);
   n_mac_ht = tan(sweep_ht_0c)*y_mac_ht;
   x_ac_ht = x_ht + n_mac_ht + 0.25*mac_ht;
   C_L_alpha_ht_o = 2*pi*AR_ht/(2+sqrt(AR_ht^2*beta^2/kappa^2*(1+tan(sweep_ht_50c/57.3)^2/beta^2)+4));
   deps_dalpha = 2*C_L_alpha_wing/(3.14159*AR_wing);
   C_L_alpha_ht = C_L_alpha_ht_o*(1-deps_dalpha)*n_ht;
   
   % find vertical tail geometry (assume there are 2 and that the reference
   % area is the total reference area for both of them)
   Svt_ref = 0.5*Svt_ref;
   AR_vt = b_vt^2/Svt_ref;
   c_r_vt = 2*Svt_ref/(b_vt*(1+taper_vt));
   c_t_vt = taper_vt*c_r_vt;
   mac_vt = (2/3)*c_r_vt*((1+taper_vt+taper_vt^2)/(1+taper_vt));
   z_mac_vt = b_vt*(1+2*taper_vt)/6/(1+taper_vt);
   sweep_vt_0c = atan(tan(sweep_vt_25c/57.3) + (1-taper_vt)/AR_vt/(1+taper_vt));
   sweep_vt_50c = tan(sweep_vt_0c) - 4*(0.5)*(1-taper_vt)/AR_vt/(1+taper_vt);
   n_mac_vt = tan(sweep_vt_0c)*z_mac_vt;
   x_ac_vt = x_vt + n_mac_vt + 0.25*mac_vt;
   
   
   % maximum c.g. excursion
   cg_max = 100*(x_cg_aft - x_cg_forward)/mac_wing;
   
   
   % minimum static margin
   SM_min = 100*(-(x_cg_aft-x_ac_wing)/mac_wing + (x_ac_ht-x_cg_aft)*Sht_ref*C_L_alpha_ht/(mac_wing*S_ref*C_L_alpha_wing) - dCm_CL_fus);
   
   
   % takeoff rotation
   
   [rho, Temp, a, mu] = atm(0);
   mu_g = 0.02;
   theta_ddot = 6/57.3;
   CLmax_ht = -1.2;
   n_ht = 0.95;
   
   CD_g = 0.5;
   Cm_ac_wf_g = -0.15;
   z_D = 9;
   z_T = 14;
   
   I_yy_mg = I_yy - (W_to/32.2)*(x_maingear)^2;
   
   [rho, Temp, a, mu]=atm(0);
   CL_max_to = max(CL_data_to);
   CL_wf_g = 0.85*CL_max_to;
   V_rotate = 1.1*sqrt(2*W_to/(rho*S_ref*CL_max_to));
   qbar_rotate = 0.5*rho*V_rotate^2;
   
   D_g = CD_g*qbar_rotate*S_ref;
   L_wf_g = CL_wf_g*qbar_rotate*S_ref;
   M_ac_wf_g = Cm_ac_wf_g*qbar_rotate*S_ref*mac_wing;
   
   Sht_reqd = ( W_to*(x_cg_forward - x_maingear - mu_g*z_cg) + D_g*(z_D - z_T) +...
      T_to*(z_cg - z_T) + L_wf_g*(x_maingear - x_ac_wf + mu_g*z_cg) + M_ac_wf_g -...
      I_yy_mg*theta_ddot )/(CLmax_ht*n_ht*qbar_rotate*(x_ac_ht - x_maingear - mu_g*z_cg));
   
   
   % minimum control speed
   
   CLmax_vt = 1.5;
   n_vt = 1;
   
   V_mc = sqrt((1.2*(T_to/N_e)*y_eng)/((x_ac_vt - x_cg_aft)*0.5*rho*(2*Svt_ref)*CLmax_vt*n_vt));
   
   
   % stall speed in landing configuration
   
   W_land = 0.9*W_to;
   CL_max_L = 8/1.44;
   V_s_L = sqrt(2*W_land/(rho*S_ref*CL_max_L));
   
   
   
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   
else
   
   n_ht = 0.95;
   L_fus = 60;
   w_fus = 13;
   h_fus = 9.5;
   A_fus = 3.14159*(w_fus/2)*(h_fus/2);
   D_eff_fus = 2*sqrt(A_fus/3.14159);
   Kf_data = polyfit([.1 .2 .3 .4 .5 .6 .7],[.115 .172 .344 .487 .688 .888 1.146],3);
   
   % find beta
   [rho, Temp, a, mu] = atm(h);
   M=1.6878*V/a;
   kappa = 0.97;
   beta = sqrt(1-M^2);
   
   % find wing geometry
   AR_wing = b_wing^2/S_ref;
   c_r_wing = 2*S_ref/(b_wing*(1+taper_wing));
   c_t_wing = taper_wing*c_r_wing;
   mac_wing = (2/3)*c_r_wing*((1+taper_wing+taper_wing^2)/(1+taper_wing));
   y_mac_wing = b_wing*(1+2*taper_wing)/6/(1+taper_wing);
   sweep_wing_0c = atan(tan(sweep_wing_25c/57.3) + (1-taper_wing)/AR_wing/(1+taper_wing));
   sweep_wing_50c = tan(sweep_wing_0c) - 4*(0.5)*(1-taper_wing)/AR_wing/(1+taper_wing);
   n_mac_wing = tan(sweep_wing_0c)*y_mac_wing;
   x_ac_wing = x_wing + n_mac_wing + 0.25*mac_wing;
   C_L_alpha_wing = 2*pi*AR_wing/(2+sqrt(AR_wing^2*beta^2/kappa^2*(1+tan(sweep_wing_50c/57.3)^2/beta^2)+4));
   
   % find fuselage induced shift in aerodynamic center
   Kf = polyval(Kf_data,(x_wing+0.25*c_r_wing)/L_fus);
   dCm_CL_fus = Kf*w_fus^2*L_fus/S_ref/mac_wing/C_L_alpha_wing;
   x_ac_wf = x_ac_wing - mac_wing*dCm_CL_fus;
   
   % find horizontal tail geometry
   AR_ht = b_ht^2/Sht_ref;
   c_r_ht = 2*Sht_ref/(b_ht*(1+taper_ht));
   c_t_ht = taper_ht*c_r_ht;
   mac_ht = (2/3)*c_r_ht*((1+taper_ht+taper_ht^2)/(1+taper_ht));
   y_mac_ht = b_ht*(1+2*taper_ht)/6/(1+taper_ht);
   sweep_ht_0c = atan(tan(sweep_ht_25c/57.3) + (1-taper_ht)/AR_ht/(1+taper_ht));
   sweep_ht_50c = tan(sweep_ht_0c) - 4*(0.5)*(1-taper_ht)/AR_ht/(1+taper_ht);
   n_mac_ht = tan(sweep_ht_0c)*y_mac_ht;
   x_ac_ht = x_ht + n_mac_ht + 0.25*mac_ht;
   C_L_alpha_ht_o = 2*pi*AR_ht/(2+sqrt(AR_ht^2*beta^2/kappa^2*(1+tan(sweep_ht_50c/57.3)^2/beta^2)+4));
   deps_dalpha = 2*C_L_alpha_wing/(3.14159*AR_wing);
   C_L_alpha_ht = C_L_alpha_ht_o*(1-deps_dalpha)*n_ht;
   
   % find vertical tail geometry (assume there are 2 and that the reference
   % area is the total reference area for both of them)
   Svt_ref = 0.5*Svt_ref;
   AR_vt = b_vt^2/Svt_ref;
   c_r_vt = 2*Svt_ref/(b_vt*(1+taper_vt));
   c_t_vt = taper_vt*c_r_vt;
   mac_vt = (2/3)*c_r_vt*((1+taper_vt+taper_vt^2)/(1+taper_vt));
   z_mac_vt = b_vt*(1+2*taper_vt)/6/(1+taper_vt);
   sweep_vt_0c = atan(tan(sweep_vt_25c/57.3) + (1-taper_vt)/AR_vt/(1+taper_vt));
   sweep_vt_50c = tan(sweep_vt_0c) - 4*(0.5)*(1-taper_vt)/AR_vt/(1+taper_vt);
   n_mac_vt = tan(sweep_vt_0c)*z_mac_vt;
   x_ac_vt = x_vt + n_mac_vt + 0.25*mac_vt;
   
   
   % maximum c.g. excursion
   cg_max = 100*(x_cg_aft - x_cg_forward)/mac_wing;
   
   
   % minimum static margin
   SM_min = 100*(-(x_cg_aft-x_ac_wing)/mac_wing + (x_ac_ht-x_cg_aft)*Sht_ref*C_L_alpha_ht/(mac_wing*S_ref*C_L_alpha_wing) - dCm_CL_fus);
   
   % not computing these for the tiltwing
   Sht_reqd = 0;
   V_mc = 0;
   V_s_L = 0;
   
end
