% Set plane and get data
%   iplane = 1 is 747
%   iplane = 2 is F-104

iplane = 2;
airdata;

% Set numerical method, timestep
%   imethod = 1 is Forward Euler
%   imethod = 2 is Backward Euler
%   imethod = 3 is 2-stage Runge-Kutta
%   imethod = 4 is 2nd order Adams-Bashforth
%   imethod = 5 is 2nd order Adams-Moulton (trapezoidal)
%   imethod = 6 is 4-stage Runge-Kutta

imethod = 6;
dt = 1e-2;

% Set final time
Tf = 30.0;

% Set initial condition
uinit = [0.1; 0; 0; 0];

% Calculate M, Atilde, and A matrices

M = zeros(4,4);
Atilde = zeros(4,4);

M(1,1) = m*u0;
M(2,2) = Ix;
M(2,3) = -Ixz;
M(3,2) = -Ixz;
M(3,3) = Iz;
M(4,4) = 1.0;

Atilde(1,1) = Yb;
Atilde(1,2) = Yp;
Atilde(1,3) = -m*u0 + Yr;
Atilde(1,4) = m*g;

Atilde(2,1) = Lb;
Atilde(2,2) = Lp;
Atilde(2,3) = Lr;

Atilde(3,1) = Nb;
Atilde(3,2) = Np;
Atilde(3,3) = Nr;

Atilde(4,2) = 1.0;

% Calculate A matrix and its eigendecomposition

A = M\Atilde;
[R,D] = eig(A);
lambda = diag(D);
Ainv = inv(A);

% Calculate solution numerically using Forward Euler method

ct0 = cputime;
if (imethod == 1),
  linFE;
elseif (imethod == 2),
  linBE;
elseif (imethod == 3),
  linRK2;
elseif (imethod == 4),
  linAB2;
elseif (imethod == 5),
  linAM2;
elseif (imethod == 6),
  linRK4;
else
  fprintf('ERROR: unknown value of imethod = %d\n',imethod);
  break;
end

ctf = cputime;
fprintf('CPU seconds to run simulation: %e\n',ctf-ct0);

% Calculate analytic solution
alpha = R\uinit;

w = exp(lambda*t);
w(1,:) = alpha(1)*w(1,:);
w(2,:) = alpha(2)*w(2,:);
w(3,:) = alpha(3)*w(3,:);
w(4,:) = alpha(4)*w(4,:);

u = real(R*w);

% Calculate maximum error in beta
berror = max(abs(v(1,:)-u(1,:)));
fprintf('dt, Maximum beta error = %e, %e\n',dt,berror);

% Plot results
subplot(221);
plot(t,u(1,:));
hold on;plot(t,v(1,:),'ro');
title(sprintf('sideslip (beta) vs t for %s',stype));
xlabel('t (sec)');
ylabel('beta (rad)');

subplot(222);
plot(t,u(2,:));
hold on;plot(t,v(2,:),'ro');
title(sprintf('roll rate (p) vs t for %s',stype));
xlabel('t (sec)');
ylabel('p (rad/sec)');

subplot(223);
plot(t,u(3,:));
hold on;plot(t,v(3,:),'ro');
title(sprintf('yaw rate (r) vs t for %s',stype));
xlabel('t (sec)');
ylabel('r (rad/sec)');

subplot(224);
plot(t,u(4,:));
hold on;plot(t,v(4,:),'ro');
title(sprintf('roll angle (phi) vs t for %s',stype));
xlabel('t (sec)');
ylabel('phi (rad)');



