% dX = getDerivsPt3(t, X)
% Function to get the state derivatives for 16.07 Lab 1 Part 3
% 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 = getDerivsPt3(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
    thrust = 170000;                % engine thrust force [N]
    alpha  = 0.04;                  % angle of attack [rad]
    
    
    % 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  ];
