%function flight_sim = flightsim(time)



%	conventions:	Large arrays are in all caps
%						Vectors and other variables are in mixed case, as appropriate
%
%						Initial condition assume that the Earth Frame is aligned with the Body Frame
%						x: North
%						y: West
%						z: Up
%
%
%

clear all;


% LOAD AND WORK WITH INITIAL CONDITIONS
cd 'D:\Development\Websites\rlpotter\www\ryan\projects\6DOF';

load initial.dat


% Create the global data array
global data;
global index;
global e;

% make it long enough
data_input = initial(:,1);
data = zeros(1, max(data_input));

% load 'data'
for i=1:length(data_input)
   data(data_input(i)) = initial(i,2);
end


%these go away soon
%global runstate;
%global enDTo;
%global enDEo;
%global enERATE;
%global eoPOX;
%global etPC;
%global eoMOX;
%global ecMFUEL;
%global cpropep;




% CREATE AND POPULATE THE 'IC' MATRIX
IC = zeros(max(initial(:,1)),3);
for i=1:length(initial)
    IC(initial(i,1),:) = [ initial(i,2) initial(i,3) initial(i,4) ];
end
clear initial


% SET THE INITIAL CONDITIONS FOR THE FIRST ITTERATION CYCLE
%enDTo = IC(20,1)*.0254;
%enDEo = IC(21,1)*.0254;
%enERATE = IC(23,1)*.0254;
%eoPOX = IC(33,1)*6.8929e+003; % Pa
%etPC(1) = IC(73,1)*6.8929e+003;  % Pa
%eoMOX(1) = IC(36,1);
%ecMFUEL(1) = IC(40,1);


%runstate = 0;			% 0 = initialize, 1 = run
index = 1;
dtout = data(3);
endtime = data(4);
d2r = 0.017453;		% conversion for degrees to radians


% initial orientation: Euler -> Quaternians
phi =   IC(25) *d2r; % roll
theta = IC(26) *d2r; % pitch
psi =   IC(27) *d2r; % yaw

% convert orientations (phi, theta, psi) from euler angels to quaternians
e0 = cos(phi/2)*cos(theta/2)*cos(psi/2) + sin(phi/2)*sin(theta/2)*sin(psi/2);
e1 = sin(phi/2)*cos(theta/2)*cos(psi/2) - cos(phi/2)*sin(theta/2)*sin(psi/2);
e2 = cos(phi/2)*sin(theta/2)*cos(psi/2) + sin(phi/2)*cos(theta/2)*sin(psi/2);
e3 = cos(phi/2)*cos(theta/2)*sin(psi/2) - sin(phi/2)*sin(theta/2)*cos(psi/2);





% INITIALS FOR ODExx:	
Y=[IC(10,1) IC(11,1) IC(12,1) ...		% Position: x,y,z (Earth Frame)
   IC(13,1) IC(14,1) IC(15,1) ...		% Velocity: u,v,w (Earth Frame)
   e0 e1 e2 e3  ...							% Orientation: phi, theta, psi (Body Frame)
   IC(35,1) IC(36,1) IC(37,1) ];			% Angular rates: p, q, r (Body Frame)



OPTIONS = odeset('RelTol',1e-4, 'OutputFcn','odeplot');
TIME = [0 : dtout : endtime];




% SOLVE THE SIMULATION

% ode113 is pretty good
[t x] = ode113('kinematics', TIME, Y, OPTIONS);
%flight = [t x]; 


% cluge
%time(index) = max(t);


%	PLOTS

EFLK = 1;
BFLK = 1;
EFRK = 1;
BFRK = 1;


if EFLK == 1
figure(1)
subplot(331)
plot(data(:,1),data(:,10))
ylabel('position (m)')
title('Earth Frame Linear Kinematics')
subplot(332)
plot(data(:,1),data(:,11))
subplot(333)
plot(data(:,1),data(:,12))
subplot(334)
plot(data(:,1),data(:,13))
ylabel('velocity_E (m/s)')
subplot(335)
plot(data(:,1),data(:,14))
subplot(336)
plot(data(:,1),data(:,15))
subplot(337)
plot(data(:,1),data(:,19))
xlabel('x')

ylabel('accelerations (m/s2)')
subplot(338)
plot(data(:,1),data(:,20))
xlabel('y')
subplot(339)
plot(data(:,1),data(:,21))
xlabel('z')
end


if BFLK == 1
figure(2)
subplot(331)
plot(data(:,1),data(:,10))
ylabel('position (m)')
title('Body Frame Linear Kinematics')
subplot(332)
plot(data(:,1),data(:,11))
subplot(333)
plot(data(:,1),data(:,12))
subplot(334)
plot(data(:,1),data(:,16))
ylabel('velocity_b (m/s)')
subplot(335)
plot(data(:,1),data(:,17))
subplot(336)
plot(data(:,1),data(:,18))
subplot(337)
plot(data(:,1),data(:,22))
xlabel('x')

ylabel('accelerations (m/s2)')
subplot(338)
plot(data(:,1),data(:,23))
xlabel('y')
subplot(339)
plot(data(:,1),data(:,24))
xlabel('z')
end


if EFRK == 1
figure(3)
subplot(331)
plot(data(:,1),data(:,25))
ylabel('attitude (deg)')
title('Earth Frame Rotational Kinematics')
subplot(332)
plot(data(:,1),data(:,26))
subplot(333)
plot(data(:,1),data(:,27))
subplot(334)
plot(data(:,1),data(:,32))
ylabel('att rates (deg/s)')
subplot(335)
plot(data(:,1),data(:,33))
subplot(336)
plot(data(:,1),data(:,34))
subplot(337)
plot(data(:,1),data(:,38))
xlabel('p')

ylabel('att accel (deg/s2)')
subplot(338)
plot(data(:,1),data(:,39))
xlabel('q')
subplot(339)
plot(data(:,1),data(:,40))
xlabel('r')
end


if BFRK == 1
figure(4)
subplot(331)
plot(data(:,1),data(:,25))
ylabel('attitude (deg)')
title('Body Frame Rotational Kinematics')
subplot(332)
plot(data(:,1),data(:,26))
subplot(333)
plot(data(:,1),data(:,27))
subplot(334)
plot(data(:,1),data(:,35))
ylabel('att rates (deg/s)')
subplot(335)
plot(data(:,1),data(:,36))
subplot(336)
plot(data(:,1),data(:,37))
subplot(337)
plot(data(:,1),data(:,41))
xlabel('p')

ylabel('att accel (deg/s2)')
subplot(338)
plot(data(:,1),data(:,42))
xlabel('q')
subplot(339)
plot(data(:,1),data(:,43))
xlabel('r')
end


figure(5)
plot(data(:,10), data(:,12))
xlabel('downrange')
ylabel('altitude')
title('flight profile')