% 16.07 Lab 1 solution code
% Chris Sequeira (csequeir@mit.edu), Fall 2004
% Last updated October 25th, 2004
%
% Coordinate systems: 
% Inertial:
%   +x: to the computer screen right
%   +y: to the computer screen top
% Aerodynamic: angle of attack = angle of incidence
%   +Lift:   to the aircraft top perpendicular to the flight direction
%   +Drag:   to the aircraft tail parallel to the flight direction
%   +Thrust: to the aircraft nose parallel to the flight direction
%
% State vector and state derivative vector:
% X' and X are vectors in --column format--!!!!!!
% X  = [ x
%        y
%        v_x
%        v_y     ]
% X' = [ v_x
%        v_y
%        v_x_dot
%        v_y_dot ]

% integrator options
options = odeset('RelTol', 1e-4, 'AbsTol', 1e-4, 'MaxStep', .05);

% set initial state vector
X0 = [ 0.0;                         % horizontal position [m]
       0.0;                         % vertical position [m]
       0.001;                       % horizontal speed [m/s]
       0.0    ];                    % vertical speed [m/s]

% how long do we run for?
tspan = [0, 300];                   % starting and ending time [s]

% run part 3
[t, X] = ode45(@getDerivsPt3, tspan, X0, options);

% plot y vs. x
figure;
plot(X(:, 1), X(:, 2));
xlabel('x [m]');
ylabel('y [m]');
title('Part 3: Aircraft Position');
grid on;

% plot hodograph
figure; 
plot(X(:, 3), X(:, 4));
xlabel('velocity x [m/s]');
ylabel('velocity y [m/s]');
title('Part 3: Aircraft Hodograph');
grid on;


% how long do we run for?
tspan = [0, 480];                   % starting and ending time [s]

% run part 4
[t, X] = ode45(@getDerivsPt4, tspan, X0, options);

% store thrust and alpha inputs for part 4
thrusts = zeros(1, length(t));
alphas  = zeros(1, length(t));
for count = 1:length(t)
    if t(count) < 30                                % get off the ground fast
        thrusts(count) = 196043;                        % engine thrust force [N]
        alphas(count)  = 0.10;                          % steep angle of attack [rad]
    elseif (t(count) > 30) && (t(count) < 40)       % gradual reduction in alpha
        thrusts(count) = 196043;
        alphas(count) = 0.10 - (t(count) - 30)*0.005;
    elseif (t(count) > 40) && (t(count) < 170)      % climb out at reduced alpha and power
        thrusts(count) = 190000;                
        alphas(count)  = 0.05;                 
    else                                            % settle into cruise at 178 m/s
        thrusts(count) = 82986;
        alphas(count)  = 0.0419;
    end
end 

% plot alphas vs. time
figure; 
subplot(2,1,1); plot(t, alphas*180/pi);
xlabel('time [s]');
ylabel('\alpha [deg]');
title('Part 4: Alpha Input vs. Time');
grid on;

% plot thrusts vs. time
subplot(2,1,2); plot(t, thrusts*180/pi);
xlabel('time [s]');
ylabel('thrust [N]');
title('Part 4: Thrust Input vs. Time');
grid on;

% plot y vs. x
figure;
plot(X(:, 1), X(:, 2));
xlabel('x [m]');
ylabel('y [m]');
title('Part 4: Aircraft Position');
grid on;

% plot hodograph
figure; 
plot(X(:, 3), X(:, 4));
xlabel('velocity x [m/s]');
ylabel('velocity y [m/s]');
title('Part 4: Aircraft Hodograph');
grid on;


% set new initial state vector and ending time
X0 = [     0.0;                     % horizontal position [m]
       10000.0;                     % vertical position [m]
         185.0;                     % horizontal speed [m/s]
           0.0  ];                  % vertical speed [m/s]
tspan = [0, 500];                   % starting and ending time [s]

% run part 5
[t, X] = ode45(@getDerivsPt5, tspan, X0, options);

% plot aircraft undamped phugoid mode
figure;
plot(X(:, 1), X(:, 2));
xlabel('x [m]');
ylabel('y [m]');
title('Part 5: Aircraft Undamped Phugoid Mode');
grid on;