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

%% User Defined Values
% Initial Conditions
Kp = -8;
Ki = -0.25;
Kd = -1.6818;

v0          = 0;                                                            % Initial Velocity (z)          [m/s]
M0          = 1.2;                                                          % Initial Mass                  [kg]
yaw0        = 75;                                                            % Initial Yaw                   [deg]
pitch0      = 0;                                                            % 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    = 1/100;                                                        % Simulation Step Size          [s]
maxServo    = 7.5;                                                           % 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
h0 = 21;
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 = 0; % 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