%% LANDER SIM PROTOTYPE
close all; clear all; clc;

%% User Defined Values
% Initial Conditions
v_0 = 0;                % Initial Velocity              [m/s]

% Thrust Curve Data from Text File                      [t, N]
Tcurve = readmatrix('F15_thrustCurve.txt');

% Constants
g = -9.81;              % Gravitational Acceleration    [m/s2]
M0 = 0.8;               % Initial Mass                  [kg]
Mp = 0.06;              % Propellant Mass               [kg]
Mb = M0 - Mp;           % Burnout Mass                  [kg]
tb = Tcurve(end, 1);    % Burn Time                     [s]
mdot = Mp / tb;         % Mass Flow Rate                [kg/s]
D = 0;                  % Drag                          [N]
stepSize = 0.001;       % Simulation Step Size          [s]

[h_0, vb, burnStartTime] = burnStartTimeCalc(Tcurve, tb, M0, mdot, Mb)
 
%% Simulink
tic
simTime = burnStartTime + tb; % Simulation Time         [s]

model = 'simProtoype';
load_system(model);
simOut = sim(model);

toc
%% Output    
% Acceleration
figure(1)
plot(simOut.a)
title('Acceleration vs Time')
xlabel('Time (s)')
ylabel('Acceleration (g''s)')
legend('X','Y')
saveas(gcf,'outputs/Acceleration vs Time.png')

% Velocity
figure(2)
plot(simOut.v)
title('Velocity vs Time')
xlabel('Time (s)')
ylabel('Velocity (m/s)')
legend('X','Y')
saveas(gcf,'outputs/Velocity vs Time.png')

% Altitude
figure(3)%k)
plot(sqrt(simOut.h.Data(:,2).^2 + simOut.h.Data(:,1).^2), simOut.h.Time)
title('Altitude vs Time')
%     title(['burnStart = ',num2str(burnStart),' s'])
xlabel('Time (s)')
ylabel('Altitude (m)')
saveas(gcf,'outputs/Altitude vs Time.png')

% Animation
h = figure(4);
K = animatedline('Marker', 'o');
axis([0, 2, 0, h_0])
xlabel('X-Position (m)')
ylabel('Altitude (m)')
title('Altitude')
grid on


for i = 1 : length(simOut.h.Data)
    clearpoints(K);
    addpoints(K, 1, simOut.h.Data(i, 2));
    title(sprintf('Altitude at T = %f', simOut.h.Time(i)))
    drawnow limitrate
    
    
    % Write Animation to gif, set to zero when testing since its slow to render. 
    outframes = 0;
    if outframes
        % Write to the GIF File
        if i == 1
            
            % Capture the plot as an image
            frame = getframe(h);
            im = frame2im(frame);
            [imind,cm] = rgb2ind(im,256);
            
            %initalize plot
            imwrite(imind,cm,'outputs/Altitude.gif','gif', 'Loopcount',inf);
            
        elseif mod(i,floor(length(simOut.h.Data)/outframes)) == 0
            % Capture the plot as an image
            frame = getframe(h);
            im = frame2im(frame);
            [imind,cm] = rgb2ind(im,256);
            
            % Append to plot
            imwrite(imind,cm,'outputs/Altitude.gif','gif','WriteMode','append', 'DelayTime', .2);
        end
    end
end