%%
clear; close all; clc;

%% Load Configuration file
load('./mat/config.mat', 'save_fig', 'freqs');

%% Load Simulation Results
sim_light_vc_ol = load('./mat/sim_light_vc_ol.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
sim_light_vc_cl = load('./mat/sim_light_vc_cl.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
sim_light_pz_ol = load('./mat/sim_light_pz_ol.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
sim_light_pz_cl = load('./mat/sim_light_pz_cl.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');

%% Start after few seconds
T_init = 1;

%% Plot X against Y in OL - Piezo and Voice Coil
figure;
hold on;
plot(1e9*sim_light_vc_ol.Dx, 1e9*sim_light_vc_ol.Dy);
plot(1e9*sim_light_pz_ol.Dx, 1e9*sim_light_pz_ol.Dy);
hold off;
xlabel('X Displacement [nm]'); ylabel('Y Displacement [nm]');
xlim([-1000 1000]); ylim([-1000 1000]);
xticks(-1000:200:1000); yticks(-1000:200:1000);
legend({'VC - Light - OL', 'PZ - Light - OL'});

if save_fig; exportFig('xy_ol_vc_pz', 'normal-normal', struct('path', 'control')); end

%% Plot X against Y in CL - Piezo and Voice Coil
figure;
hold on;
plot(1e9*sim_light_vc_cl.Dx, 1e9*sim_light_vc_cl.Dy);
plot(1e9*sim_light_pz_cl.Dx, 1e9*sim_light_pz_cl.Dy);
hold off;
xlabel('X Displacement [nm]'); ylabel('Y Displacement [nm]');
xlim([-500 500]); ylim([-500 500]);
xticks(-500:100:500); yticks(-500:100:500);
legend({'VC - Light - CL', 'PZ - Light - CL'});

if save_fig; exportFig('xy_cl_vc_pz', 'normal-normal', struct('path', 'control')); end

%% Compute the RMS Values
i_init = find(sim_light_vc_ol.time > T_init, 1);

rms_light_vc_ol = rms(sqrt(sim_light_vc_ol.Dx(i_init:end).^2+sim_light_vc_ol.Dy(i_init:end).^2));
rms_light_pz_ol = rms(sqrt(sim_light_pz_ol.Dx(i_init:end).^2+sim_light_pz_ol.Dy(i_init:end).^2));
rms_light_vc_cl = rms(sqrt(sim_light_vc_cl.Dx(i_init:end).^2+sim_light_vc_cl.Dy(i_init:end).^2));
rms_light_pz_cl = rms(sqrt(sim_light_pz_cl.Dx(i_init:end).^2+sim_light_pz_cl.Dy(i_init:end).^2));

fprintf('   \t OL   \t CL   [nm RMS]\n');
fprintf('PZ \t %.0f \t %.0f \n',   1e9*rms_light_pz_ol, 1e9*rms_light_pz_cl);
fprintf('VC \t %.0f \t %.0f \n\n', 1e9*rms_light_vc_ol, 1e9*rms_light_vc_cl);

%% Compute the PSD
sim_light_vc_ol.psd = computePsdDispl(sim_light_vc_ol, 1, 2);
sim_light_pz_ol.psd = computePsdDispl(sim_light_pz_ol, 1, 2);
sim_light_vc_cl.psd = computePsdDispl(sim_light_vc_cl, 1, 2);
sim_light_pz_cl.psd = computePsdDispl(sim_light_pz_cl, 1, 2);

%% PSD Open Loop and Close Loop for the X direction
figure;
hold on;
plot(sim_light_vc_ol.psd.f, sim_light_vc_ol.psd.dx, 'DisplayName', 'VC - $T_x$ - OL');
plot(sim_light_pz_ol.psd.f, sim_light_pz_ol.psd.dx, 'DisplayName', 'PZ - $T_x$ - OL');
set(gca,'ColorOrderIndex',1);
plot(sim_light_vc_cl.psd.f, sim_light_vc_cl.psd.dx, '--', 'DisplayName', 'VC - $T_x$ - CL');
plot(sim_light_pz_cl.psd.f, sim_light_pz_cl.psd.dx, '--', 'DisplayName', 'PZ - $T_x$ - CL');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [$m^2/Hz$]'); xlabel('Frequency [Hz]');
xlim([sim_light_vc_ol.psd.f(1), sim_light_vc_ol.psd.f(end)])
hold off;
legend('Location', 'southwest');

if save_fig; exportFig('psd_ol_cl_pz_vc_light_tx', 'normal-normal', struct('path', 'control')); end

%% PSD Open Loop and Close Loop for the Z direction
figure;
hold on;
plot(sim_light_vc_ol.psd.f, sim_light_vc_ol.psd.dz, 'DisplayName', 'VC - $T_z$ - OL');
plot(sim_light_pz_ol.psd.f, sim_light_pz_ol.psd.dz, 'DisplayName', 'PZ - $T_z$ - OL');
set(gca,'ColorOrderIndex',1);
plot(sim_light_vc_cl.psd.f, sim_light_vc_cl.psd.dz, '--', 'DisplayName', 'VC - $T_z$ - CL');
plot(sim_light_pz_cl.psd.f, sim_light_pz_cl.psd.dz, '--', 'DisplayName', 'PZ - $T_z$ - CL');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [$m^2/Hz$]'); xlabel('Frequency [Hz]');
xlim([sim_light_vc_ol.psd.f(1), sim_light_vc_ol.psd.f(end)])
hold off;
legend('Location', 'southwest');

if save_fig; exportFig('psd_ol_cl_pz_vc_light_tz', 'normal-normal', struct('path', 'control')); end