% Clean Up
close all
clear variables
clc


s = tf('s');
tfinal = 400;
ylim = [-1.5 0];

referenceCommandSize = -1;
opt = stepDataOptions('StepAmplitude', referenceCommandSize');



% Colors
blue = [0, 0.4470, 0.7410];
red = [0.8500, 0.3250, 0.0980];
orange = [0.9290, 0.6940, 0.1250];
purple = [0.4940, 0.1840, 0.5560];
green = [0.4660, 0.6740, 0.1880];

% Open Video File Object
v = VideoWriter('all_together_now.mp4', 'MPEG-4');
v.Quality = 20;
open(v);

%{
% Define first trace
zeta = -0.1;
wn = 5;
K = 0.3*wn;

sys = K^2/(s^2 + 2*zeta*wn*s + wn^2);

[Y, T] = step(sys, tfinal);

figure(1);
curve = animatedline('Color', blue, 'linewidth', 2);
set(gca, 'Xlim', [0 tfinal], 'Ylim', ylim);
set(gcf, 'Units', 'pixels', 'Position', [1 41 960 540]);
grid on
hold on
xlabel('Time [sec]');
ylabel('Output Variable');
title('Goals of Feedback Control');
legend('Initially Unstable', 'location', 'southeast');

for k = 1:length(T)
    addpoints(curve, T(k), Y(k))
    drawnow limitrate
    frame = getframe(gcf);
    writeVideo(v, frame);
end


% Define second trace
zeta = 0.1;
wn = 5;
K = 0.7*wn;

sys = K^2/(s^2 + 2*zeta*wn*s + wn^2);

[Y, T] = step(sys, tfinal);

curve2 = animatedline('Color', red, 'linewidth', 2);
set(gca, 'Xlim', [0 tfinal], 'Ylim', ylim);
grid on
legend('Initially Unstable', 'Stabilization', 'location', 'southeast');

for k = 1:length(T)
    addpoints(curve2, T(k), Y(k))
    drawnow limitrate
    frame = getframe(gcf);
    writeVideo(v, frame);
end


% Define third trace
zeta = 1;
wn = 5;
K = 0.7*wn;

sys = K^2/(s^2 + 2*zeta*wn*s + wn^2);

[Y, T] = step(sys, tfinal);

curve3 = animatedline('Color', orange, 'linewidth', 2);
set(gca, 'Xlim', [0 tfinal], 'Ylim', ylim);
grid on
legend('Initially Unstable', 'Stabilization', 'No Oscillation', 'location', 'southeast');


for k = 1:length(T)
    addpoints(curve3, T(k), Y(k))
    drawnow limitrate
    frame = getframe(gcf);
    writeVideo(v, frame);
end


% Define fourth trace
zeta = 1;
wn = 5;
K = wn;

sys = K^2/(s^2 + 2*zeta*wn*s + wn^2);

[Y, T] = step(sys, tfinal);

curve4 = animatedline('Color', purple, 'linewidth', 2);
set(gca, 'Xlim', [0 tfinal], 'Ylim', ylim);
grid on
legend('Initially Unstable', 'Stabilization', 'No Oscillation', 'Gain Correction', 'location', 'southeast');


for k = 1:length(T)
    addpoints(curve4, T(k), Y(k))
    drawnow limitrate
    frame = getframe(gcf);
    writeVideo(v, frame);
end


% Define fifth trace
zeta = 1;
wn = 10;
K = wn;

sys = K^2/(s^2 + 2*zeta*wn*s + wn^2);

[Y, T] = step(sys, tfinal);

curve5 = animatedline('Color', green, 'linewidth', 2);
set(gca, 'Xlim', [0 tfinal], 'Ylim', ylim);
grid on
legend('Initially Unstable', 'Stabilization', 'No Oscillation', 'Gain Correction', 'Faster Response', 'location', 'southeast');


for k = 1:length(T)
    addpoints(curve5, T(k), Y(k))
    drawnow limitrate
    frame = getframe(gcf);
    writeVideo(v, frame);
end

% =========================================================================
% Define first trace
zeta = 0.25;
wn = 0.05;
K = wn;

sys = K^2/(s^2 + 2*zeta*wn*s + wn^2);

[Y, T] = step(sys, tfinal, opt);

figure(1);
plot(T, -1*ones(1, length(T)), 'color', 'black', 'linestyle', '--', 'linewidth', 2);
curve = animatedline('Color', blue, 'linewidth', 2);
set(gca, 'Xlim', [0 tfinal], 'Ylim', ylim);
set(gcf, 'Units', 'pixels', 'Position', [1 41 960 540]);
grid on
hold on
xlabel('Time [sec]', 'FontSize', 14);
ylabel('[meters]', 'FontSize', 14);
title('Possible Float 1 m Descent Trajectories', 'FontSize', 20);
legend({'Target Depth', 'Too Much Undershoot'}, 'location', 'southeast', 'FontSize', 12);
saveas(gcf, 'too_much_undershoot_pic.jpg')

for k = 1:length(T)
    addpoints(curve, T(k), Y(k))
    drawnow limitrate
    if mod(k,2) == 0
        frame = getframe(gcf);
        writeVideo(v, frame);
    end
end

close(v)

% Open Video File Object
v = VideoWriter('too_slow.mp4', 'MPEG-4');
v.Quality = 20;
open(v);

% Define second trace
zeta = 1.4;
wn = 0.05;
K = wn;

sys = K^2/(s^2 + 2*zeta*wn*s + wn^2);

[Y, T] = step(sys, tfinal, opt);

curve2 = animatedline('Color', red, 'linewidth', 2);
set(gca, 'Xlim', [0 tfinal], 'Ylim', ylim);
grid on
legend({'Target Depth', 'Too Much Undershoot', 'Too Slow'}, 'location', 'southeast', 'FontSize', 12);
saveas(gcf, 'too_slow_pic.jpg')

for k = 1:length(T)
    addpoints(curve2, T(k), Y(k))
    drawnow limitrate
    if mod(k,4) == 0
        frame = getframe(gcf);
        writeVideo(v, frame);
    end
end

close(v)

% Open Video File Object
v = VideoWriter('just_right.mp4', 'MPEG-4');
v.Quality = 20;
open(v);


% Define third trace
zeta = 0.7;
wn = 0.09;
K = wn;

sys = K^2/(s^2 + 2*zeta*wn*s + wn^2);

[Y, T] = step(sys, tfinal, opt);

curve3 = animatedline('Color', orange, 'linewidth', 2);
set(gca, 'Xlim', [0 tfinal], 'Ylim', ylim);
grid on
legend({'Target Depth', 'Too Much Undershoot', 'Too Slow', 'Just Right'}, 'location', 'southeast', 'FontSize', 12);
saveas(gcf, 'just_right_pic.jpg')


for k = 1:length(T)
    addpoints(curve3, T(k), Y(k))
    drawnow limitrate
    if mod(k,3) == 0
        frame = getframe(gcf);
        writeVideo(v, frame);
    end
end
%}


% =========================================================================
% ALL TOGETHER NOW

% Define first trace
zeta = 0.25;
wn = 0.05;
K = wn;
sys = K^2/(s^2 + 2*zeta*wn*s + wn^2);
[Y1, T1] = step(sys, tfinal, opt);


% Define second trace
zeta = 1.4;
wn = 0.05;
K = wn;
sys = K^2/(s^2 + 2*zeta*wn*s + wn^2);
[Y2, T2] = step(sys, tfinal, opt);

% Define third trace
zeta = 0.7;
wn = 0.09;
K = wn;
sys = K^2/(s^2 + 2*zeta*wn*s + wn^2);
[Y3, T3] = step(sys, tfinal, opt);


figure(1);
plot(T1, -1*ones(1, length(T1)), 'color', 'black', 'linestyle', '--', 'linewidth', 2);
hold on
curve1 = animatedline('Color', blue, 'linewidth', 2);
curve2 = animatedline('Color', red, 'linewidth', 2);
curve3 = animatedline('Color', orange, 'linewidth', 2);

set(gca, 'Xlim', [0 tfinal], 'Ylim', ylim);
set(gcf, 'Units', 'pixels', 'Position', [1 41 960 540]);
grid on
xlabel('Time [sec]', 'FontSize', 14);
ylabel('Depth [meters]', 'FontSize', 14);
title('Possible Float 1 m Descent Trajectories', 'FontSize', 20);
legend({'Target Depth', 'Too Much Overshoot', 'Too Slow', 'Just Right'}, 'location', 'southeast', 'FontSize', 12);
saveas(gcf, 'all_together_now_pic.jpg')


for k = 1:length(T2)
    try
        addpoints(curve1, T1(k), Y1(k))
    catch
    end
    
    try
        addpoints(curve2, T2(k), Y2(k))
    catch
    end
    
    try
       addpoints(curve3, T3(k), Y3(k))
    catch
    end
    
    drawnow limitrate
    if mod(k,6) == 0
        frame = getframe(gcf);
        writeVideo(v, frame);
    end
end


close(v);
saveas(gcf, 'all_together_now_after_pic.jpg')
