% 16.07 Lab 2 solution code
% Chris Sequeira (csequeir@mit.edu), Fall 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_dot
%        y_dot
%        x
%        y         ]
% X' = [ x_dot_dot
%        y_dot_dot
%        x_dot
%        y_dot     ]

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

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

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


% simulate the first loop
[t, X] = ode45(@getDerivsLoop1, tspan, X0, options);

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

% plot velocity magnitude vs. time
velocitymag = sqrt(X(:,1).^2 + X(:,2).^2);
subplot(2,1,2); plot(t, velocitymag);
xlabel('time [s]');
ylabel('vel [m/s]');
title('Loop 1: Aircraft Velocity Magnitude vs. Time');
grid on;

% get accel vectors and convert to body coordinates
accelx  = derivative(t, X(:,1));
accely  = derivative(t, X(:,2));
accelvi = [accelx, accely];         % accel vector in inertial space
thetas  = atan2(X(:,2), X(:,1));    % flight path direction

accelvb = zeros(length(t),2);       % prepare to store accel in body coords
for conv = 1:length(t)
    % direction cosine matrix
    T = [  cos(thetas(conv))  sin(thetas(conv))
          -sin(thetas(conv))  cos(thetas(conv)) ];
    accelvb(conv,:) = transpose(T*transpose(accelvi(conv,:)));
end

% plot tangential acceleration vs. time
figure;
subplot(3,1,1); plot(t, accelvb(:,1));
xlabel('time [s]');
ylabel('accel [m/s^2]');
title('Loop 1: Aircraft Tangential Accel vs. Time');
grid on;

% plot normal acceleration vs. time
subplot(3,1,2); plot(t, accelvb(:,2));
xlabel('time [s]');
ylabel('accel [m/s^2]');
title('Loop 1: Aircraft Normal Accel vs. Time');
grid on;

% plot radius of curvature
subplot(3,1,3); plot(t, velocitymag.^2./accelvb(:,2));
%axis([21 66 0 2000]);
axis([0 100 0 2000]);
xlabel('time [s]');
ylabel('radius [m]');
title('Loop 1: Radius of Curvature vs. Time');
grid on;


% simulate the second loop
[t, X] = ode45(@getDerivsLoop2, tspan, X0, options);

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

% plot velocity vs. time
velocitymag = sqrt(X(:,1).^2 + X(:,2).^2);
subplot(2,1,2); plot(t, velocitymag);
xlabel('time [s]');
ylabel('vel [m/s]');
title('Loop 2: Aircraft Velocity Magnitude vs. Time');
grid on;

% get accel vectors and convert to body coordinates
accelx  = derivative(t, X(:,1));
accely  = derivative(t, X(:,2));
accelvi = [accelx, accely];         % accel vector in inertial space
thetas  = atan2(X(:,2), X(:,1));    % flight path direction

accelvb = zeros(length(t),2);       % prepare to store accel in body coords
for conv = 1:length(t)
    % direction cosine matrix
    T = [  cos(thetas(conv))  sin(thetas(conv))
          -sin(thetas(conv))  cos(thetas(conv)) ];
    accelvb(conv,:) = transpose(T*transpose(accelvi(conv,:)));
end

% plot tangential acceleration vs. time
figure;
subplot(3,1,1); plot(t, accelvb(:,1));
xlabel('time [s]');
ylabel('accel [m/s^2]');
title('Loop 2: Aircraft Tangential Accel vs. Time');
grid on;

% plot normal acceleration vs. time
subplot(3,1,2); plot(t, accelvb(:,2));
xlabel('time [s]');
ylabel('accel [m/s^2]');
title('Loop 2: Aircraft Normal Accel vs. Time');
grid on;

% plot radius of curvature
subplot(3,1,3); plot(t, velocitymag.^2./accelvb(:,2));
%axis([21 59 0 2000]);
axis([0 100 0 2000]);
xlabel('time [s]');
ylabel('radius [m]');
title('Loop 2: Radius of Curvature vs. Time');
grid on;