%%
clear; close all; clc;

%%
exp_ol   = load('./data/exp_open_loop.mat', 'Dmeas');
exp_cl   = load('./data/exp_close_loop_xyz.mat', 'Dmeas');
exp_without_nass = load('./data/exp_whitout_nass.mat', 'Dmeas');
load('./mat/sim_conf.mat', 'sim_conf');

exp_without_nass.Dmeas.Data(:, 3) = exp_without_nass.Dmeas.Data(:, 3) - exp_without_nass.Dmeas.Data(end, 3);

Fs = ceil((length(exp_without_nass.Dmeas.Time(:))-1)/exp_without_nass.Dmeas.Time(end));

%%
N = length(exp_without_nass.Dmeas.Data(:, 1));
figure;
hold on;
% plot(exp_without_nass.Dmeas.Data(end-6300:end, 1),exp_without_nass.Dmeas.Data(end-6300:end, 2))
plot(exp_without_nass.Dmeas.Data(N-6600:N-6200, 1),exp_without_nass.Dmeas.Data(N-6600:N-6200, 2))
plot(exp_cl.Dmeas.Data(6:end, 1),exp_cl.Dmeas.Data(6:end, 2))
xlim([-1e-6, 1e-6]);
ylim([-1e-6, 1e-6]);
hold off;
xlabel('Displacement - $x$ [m]'); ylabel('Displacement - $y$ [m]');


%% With and without NASS
steady_i = ceil(length(exp_ol.Dmeas.Time)/2);
steady_i = 6;

figure;
hold on;
plot(exp_without_nass.Dmeas.Data(steady_i:end, 1),exp_without_nass.Dmeas.Data(steady_i:end, 2))
plot(exp_cl.Dmeas.Data(steady_i:end, 1),exp_cl.Dmeas.Data(steady_i:end, 2))
xlim([-1e-6, 1e-6]);
ylim([-1e-6, 1e-6]);
hold off;
xlabel('Displacement - $x$ [m]'); ylabel('Displacement - $y$ [m]');

exportFig('exp_w_wo_nass_xy', 'half-short')

%% Video of the simulation
close all;

figure(1);
hold on;
grid;
xlim([-1e-6, 1e-6]);
ylim([-1e-6, 1e-6]);
xlabel('Displacement - $x$ [m]'); ylabel('Displacement - $y$ [m]');

% Set up the movie.
writerObj = VideoWriter('open_loop.avi'); % Name it.
writerObj.FrameRate = 30; % How many frames per second.
open(writerObj); 

% Open Loop
N = length(exp_without_nass.Dmeas.Data(:, 1))-6300;
step_i = ceil(Fs/writerObj.FrameRate);
for i=1:step_i:N
    % We just use pause but pretend you have some really complicated thing here...
    pause(0.01);
    figure(1);
    plot(exp_without_nass.Dmeas.Data(i:min(i+step_i, N), 1),exp_without_nass.Dmeas.Data(i:min(i+step_i, N), 2), 'color', [0 0.4470 0.7410])
    frame = getframe(gcf); % 'gcf' can handle if you zoom in to take a movie.
    writeVideo(writerObj, frame);
end

% Close Loop
N = length(exp_cl.Dmeas.Data(:, 1));
step_i = ceil(Fs/writerObj.FrameRate);
first_i = 6;
for i=first_i:step_i:N
    % We just use pause but pretend you have some really complicated thing here...
    pause(0.01);
    figure(1);
    plot(exp_cl.Dmeas.Data(i:min(i+step_i, N), 1),exp_cl.Dmeas.Data(i:min(i+step_i, N), 2), 'color', [0.8500 0.3250 0.0980])
    frame = getframe(gcf); % 'gcf' can handle if you zoom in to take a movie.
    writeVideo(writerObj, frame);
end

hold off
close(writerObj); % Saves the movie.


%% Video of the simulation
close all;

figure(1);
hold on;
xlim([-40, 40]);
ylim([-40, 40]);
yticks([-40 -20 0 20 40])
xticks([-40 -20 0 20 40])
grid on;
xlabel('Displacement - $x$ [nm]'); ylabel('Displacement - $y$ [nm]');

set(gcf, 'pos', [20 20 300 300]);

% Set up the movie.
writerObj = VideoWriter('close_loop_zoom.avi'); % Name it.
writerObj.FrameRate = 30; % How many frames per second.
open(writerObj);

% Open Loop
N = length(exp_without_nass.Dmeas.Data(:, 1))-6300;
step_i = ceil(Fs/writerObj.FrameRate);
for i=1:step_i:N
    % We just use pause but pretend you have some really complicated thing here...
    pause(0.01);
    figure(1);
    plot(1e9*exp_without_nass.Dmeas.Data(i:min(i+step_i, N), 1),1e9*exp_without_nass.Dmeas.Data(i:min(i+step_i, N), 2), 'color', [0 0.4470 0.7410])
    frame = getframe(gcf); % 'gcf' can handle if you zoom in to take a movie.
    writeVideo(writerObj, frame);
end

% Close Loop
N = length(exp_cl.Dmeas.Data(:, 1));
step_i = ceil(Fs/writerObj.FrameRate);
first_i = 6;
for i=first_i:step_i:N
    % We just use pause but pretend you have some really complicated thing here...
    pause(0.01);
    figure(1);
    plot(1e9*exp_cl.Dmeas.Data(i:min(i+step_i, N), 1),1e9*exp_cl.Dmeas.Data(i:min(i+step_i, N), 2), 'color', [0.8500 0.3250 0.0980])
    frame = getframe(gcf); % 'gcf' can handle if you zoom in to take a movie.
    writeVideo(writerObj, frame);
end

hold off
close(writerObj); % Saves the movie.


%%
figure;
hold on;
plot(exp_without_nass.Dmeas.Time(steady_i:end), exp_without_nass.Dmeas.Data(steady_i:end, 2));
plot(exp_without_nass.Dmeas.Time(steady_i:end), exp_cl.Dmeas.Data(steady_i:end, 2));
legend({'$y$ - without NASS', '$y$ - with NASS'})
hold off;
xlabel('Time [s]'); ylabel('Displacement [m]');

exportFig('exp_w_wo_nass_y', 'half-small')



%% Compare OL and CL - Time
figure;
hold on;
plot(exp_ol.Dmeas.Time, exp_ol.Dmeas.Data(:, 1));
plot(exp_cl.Dmeas.Time, exp_cl.Dmeas.Data(:, 1));
legend({'x - OL', 'x - CL'})
hold off;
xlabel('Time [s]'); ylabel('Displacement [m]');

exportFig('exp_control_time_x', 'normal-normal')


figure;
hold on;
plot(exp_ol.Dmeas.Time, exp_ol.Dmeas.Data(:, 2));
plot(exp_cl.Dmeas.Time, exp_cl.Dmeas.Data(:, 2));
legend({'y - OL', 'y - CL'})
hold off;
xlabel('Time [s]'); ylabel('Displacement [m]');

exportFig('exp_control_time_y', 'normal-normal')


figure;
hold on;
plot(exp_ol.Dmeas.Time, exp_ol.Dmeas.Data(:, 3));
plot(exp_cl.Dmeas.Time, exp_cl.Dmeas.Data(:, 3));
legend({'z - OL', 'z - CL'})
hold off;
xlabel('Time [s]'); ylabel('Displacement [m]');

exportFig('exp_control_time_z', 'normal-normal')

%%
steady_i = ceil(length(exp_ol.Dmeas.Time)/2);
% steady_i = 1;

figure;
hold on;
plot(exp_ol.Dmeas.Data(steady_i:end, 1),exp_ol.Dmeas.Data(steady_i:end, 2))
plot(exp_cl.Dmeas.Data(steady_i:end, 1),exp_cl.Dmeas.Data(steady_i:end, 2))
legend({'OL', 'CL'})
hold off;
xlabel('Displacement - $x$ [s]'); ylabel('Displacement - $y$ [m]');


figure;
hold on;
plot3(exp_ol.Dmeas.Data(steady_i:end, 1),exp_ol.Dmeas.Data(steady_i:end, 2),exp_ol.Dmeas.Data(steady_i:end, 3))
plot3(exp_cl.Dmeas.Data(steady_i:end, 1),exp_cl.Dmeas.Data(steady_i:end, 2),exp_cl.Dmeas.Data(steady_i:end, 3))
legend({'OL', 'CL'})
hold off;
xlabel('Displacement - $x$ [s]'); ylabel('Displacement - $y$ [m]'); zlabel('Displacement - $z$ [m]');


%% Compare OL and CL - PSD
han_windows_ol = hanning(ceil(length(exp_ol.Dmeas.Time(steady_i:end))/2));
[psd_x_ol, freqs_x_ol] = pwelch(exp_ol.Dmeas.Data(steady_i:end, 1), han_windows_ol, 0, [], 1/sim_conf.Ts);

han_windows_cl = hanning(ceil(length(exp_cl.Dmeas.Time(steady_i:end))/2));
[psd_x, freqs_x] = pwelch(exp_cl.Dmeas.Data(steady_i:end, 1), han_windows_cl, 0, [], 1/sim_conf.Ts);

figure;
hold on;
plot(freqs_x_ol, sqrt(psd_x_ol));
plot(freqs_x, sqrt(psd_x));
set(gca,'xscale','log'); set(gca,'yscale','log');
xlabel('Frequency [Hz]'); ylabel('PSD [$m/\sqrt{Hz}$]');
legend({'x - OL', 'x - CL'})
hold off;

exportFig('exp_psd_x', 'normal-normal')