% dX = getDerivs(time, X)
% Function to get the state derivatives for 16.07 Lab 3, circular loop
% Fall 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 lab3.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 = getDerivs(time, 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     = 21000;                  % aircraft mass [kg]
    K1    = 5;                      % CL vs alpha slope []
    CD0   = 0.06;                   % profile drag coefficient []

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

    % steady-state angle of attack [rad]    
    alpha  = m*g/(0.5*K1*rho*185.0^2*S);   
        
    % unpack the state vector (follow the definition!)
    x     = X(1);                   % horizontal position [m]
    y     = X(2);                   % vertical position [m]
    x_dot = X(3);                   % horizontal speed [m/s]
    y_dot = X(4);                   % vertical speed [m/s]

    % get velocity magnitude [m/s]
    vel = sqrt(x_dot^2 + y_dot^2);

    % get dynamic pressure [kg/m/s^2]
    q = 0.5*rho*vel^2;

    % get flight direction
    theta = atan2(y_dot, x_dot);
    
    % set alpha for loop if after 20 seconds flight time
    % do exactly one complete loop
    if (time > 20) && (time < 66.1)
        alpha = (m*vel^2/r + m*g*x_dot/vel)/(q*S*K1);
    end
    
    % get the aerodynamic data
    CL = K1*alpha;                  % coefficient of lift []
    CD = CD0 + CL^2/(pi*AR*e);      % coefficient of drag []
       
    % 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
    x_dot_dot = (1/m)*( -L*sin(theta) );    % horizontal accel [m/s^2]
    y_dot_dot = (1/m)*(  L*cos(theta) );    % vertical accel [m/s^2]
    
    % add in gravity acceleration
    y_dot_dot = y_dot_dot - g;
    
        
    % pack up the vector of state derivatives (follow the definition)
    % key note: x_dot and y_dot are taken right from the state vector!
    dX = [ x_dot
           y_dot
           x_dot_dot
           y_dot_dot ];