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

%% User Defined Values
% Initial Conditions
v0          = 0;                                                            % Initial Velocity (z)          [m/s]
M0          = 1.2;                                                          % Initial Mass                  [kg]
yaw0        = 0;                                                            % Initial Yaw                   [deg]
pitch0      = 5;                                                            % Initial Pitch                 [deg]
roll0       = 0;                                                            % Initial Roll                  [deg]
p0          = 0;                                                            % Initial Angular Velocity (x)  [deg/s]
q0          = 0;                                                            % Initial Angular Velocity (y)  [deg/s]
r0          = 0;                                                            % Initial Angular Velocity (z)  [deg/s]

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

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

% Moment of Inertia / Mass
I11         = (1/12) * 0.5318^2 + 0.25 * 0.05105^2;                         % (1/12) * h^2 + 0.25 * r^2
I22         = (1/12) * 0.5318^2 + 0.25 * 0.05105^2;                         % (1/12) * h^2 + 0.25 * r^2
I33         = 0.5 * 0.05105^2;                                              % 0.5 * r^2
I           = [I11 0 0; 0 I22 0; 0 0 I33];                                  % I divided by Mass... this is taken care of in Simulink since our mass isn't constant

%% Pre-Sim Calcs
K = calcLQR(I*M0);                                                          % LQR Gain Calcs
[h0, vb, burnStartTime] = burnStartTimeCalc(Tcurve, tb, M0, mdot, Mb, v0);  % Burn Start Time Calc
simTime = burnStartTime + tb;                                               % Simulation Time               [s]

yaw0        = yaw0   * pi / 180;                                            % Initial Yaw                   [rad]
pitch0      = pitch0 * pi / 180;                                            % Initial Pitch                 [rad]
roll0       = roll0  * pi / 180;                                            % Initial Roll                  [rad]
p0          = p0     * pi / 180;                                            % Initial Angular Velocity (x)  [rad/s]
q0          = q0     * pi / 180;                                            % Initial Angular Velocity (y)  [rad/s]
r0          = r0     * pi / 180;                                            % Initial Angular Velocity (z)  [rad/s]

%% Simulink
tic

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

toc
%% Outputs
figure(1)

% Acceleration
subplot(3, 1, 1)
plot(simOut.a.Data(:, 3))
title('Acceleration vs Time')
xlabel('Time (s)')
ylabel('Acceleration (g''s)')

% Velocity
subplot(3, 1, 2)
plot(simOut.v.Data(:, 3))
title('Velocity vs Time')
xlabel('Time (s)')
ylabel('Velocity (m/s)')

% Altitude
subplot(3, 1, 3)
plot(simOut.h.Time, simOut.h.Data(:,3))
title('Altitude vs Time')
xlabel('Time (s)')
ylabel('Altitude (m)')
ylim([0 h0+5])
saveas(gcf,'outputs/Accel-Vel-Alt vs Time.png')

figure(2)

% Euler Angles
subplot(2, 1, 1)
plot(simOut.YPR.Data)
title('Euler Angles vs Time')
xlabel('Time (ms)')
ylabel('Euler Angles (deg)')
legend('Yaw', 'Pitch', 'Roll')

% Angular Velocity
subplot(2, 1, 2)
plot(simOut.YPRdot.Data)
title('Angular Velocity vs Time')
xlabel('Time (ms)')
ylabel('Angular Velocity (deg/s)')
legend('X', 'Y', 'Z')
saveas(gcf,'outputs/Euler Angles vs Time.png')

figure(3)

% Servo 1 Position
subplot(2, 1, 1)
plot(simOut.servo1.Data)
title('Servo 1 Position vs Time')
xlabel('Time (ms)')
ylabel('Servo 1 Position (rad)')

% Servo 2 Position
subplot(2, 1, 2)
plot(simOut.servo2.Data)
title('Servo 2 Position vs Time')
xlabel('Time (ms)')
ylabel('Servo 2 Position (rad)')
saveas(gcf,'outputs/Servo Position vs Time.png')

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

for i = 1 : length(simOut.h.Data)
    clearpoints(K);
    addpoints(K, simOut.h.Data(i, 1), simOut.h.Data(i, 3));
    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 = 50; % 50 is a nice default
    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