% dX = getDerivsPt4(t, X)
% Function to get the state derivatives for 16.07 Lab 1 Part 4
% Fall 2004
% Last updated September 7th, 2004
%
% x' = F(x): "state derivatives are a function of the current state"
%
% t: current time given by the integrator
% X: current state vector given by the integrator
% dX: vector of state derivatives
% See lab1.m for a definition of the state vectors
%
% Parameters and inputs are hardcoded inside; while bad coding practice, it avoids
%   the whole "function calling function function with nested function"
%   nonsense that would otherwise be necessary
%   ( http://www.mathworks.com/access/helpdesk/help/techdoc/math/funfun18.html#942684 )
function dX = getDerivsPt4(t, X)
    % aircraft parameters
    % loosely based on the Russian Federation Sukhoi Su-35
    % source: Jane's All The World's Aircraft 2004-2005
    b     = 15.16;                  % wingspan [m]
    S     = 65.66;                  % wing area [m^2]
    AR    = b^2/S;                  % wing aspect ratio []
    e     = 0.8;                    % Oswald's efficiency coefficient []
    m     = 27250;                  % aircraft mass [kg]
    K1    = 5;                      % CL vs alpha slope []
    CD0   = 0.06;                   % profile drag coefficient []

    % world parameters
    rho   = 1.225;                  % air density [kg/m^3]
    g     = 9.81;                   % gravity acceleration [m/s^2]

    % aircraft inputs
    if t < 30                       % get off the ground fast
        thrust = 196043;                % engine thrust force [N]
        alpha  = 0.10;                  % steep angle of attack [rad]
    elseif (t > 30) && (t < 40)     % gradual reduction in alpha
        thrust = 196043;
        alpha = 0.10 - (t - 30)*0.005;
    elseif (t > 40) && (t < 170)    % climb out at reduced alpha and power
        thrust = 190000;                
        alpha  = 0.05;                 
    else                            % settle into cruise at 178 m/s
        thrust = 82986;
        alpha  = 0.0419;
    end
    
    
    % pull states from the state vector (follow the definition!)
    x   = X(1);                     % horizontal position [m]
    y   = X(2);                     % vertical position [m]
    v_x = X(3);                     % horizontal speed [m/s]
    v_y = X(4);                     % vertical speed [m/s]
    
    % get velocity magnitude [m/s]
    vel = sqrt(v_x^2 + v_y^2);
    
    % get flight direction
    theta = atan2(v_y, v_x);
    
    % get the aerodynamic data
    CL = K1*alpha ;                 % coefficient of lift []
    CD = CD0 + CL^2/(pi*AR*e);      % coefficient of drag []
    
    q = 0.5*rho*vel^2;              % dynamic pressure [kg/m/s^2]
    
    % resolve to forces
    L = q*S*CL;                     % lift [N]
    D = q*S*CD;                     % drag [N]

    
    % Newton's 2nd: convert to accelerations in inertial reference frame
    v_x_dot = (1/m)*( -L*sin(theta) - D*cos(theta) + thrust*cos(theta) ); % horizontal accel [m/s^2]
    v_y_dot = (1/m)*(  L*cos(theta) - D*sin(theta) + thrust*sin(theta) ); % vertical accel [m/s^2]
    
    % add in gravity acceleration
    v_y_dot = v_y_dot - g;
    
    
    % are we at or below ground level?  don't "sink"
    if (y <= 0) && (v_y < 0)        % don't let vertical velocity go negative
        v_y = 0;
    end
    if (y <= 0) && (v_y_dot < 0)    % don't let vertical acceleration go negative
        v_y_dot = 0;
    end
    
    
    % pack up the vector of state derivatives (follow the definition)
    % key note: v_x and v_y are taken right from the state vector!
    dX = [ v_x;
           v_y;
           v_x_dot;
           v_y_dot  ];