%%
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_pz_ol = load('./mat/sim_light_pz_ol.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
sim_heavy_vc_ol = load('./mat/sim_heavy_vc_ol.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
sim_heavy_pz_ol = load('./mat/sim_heavy_pz_ol.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');

%%
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_heavy_vc_ol.psd = computePsdDispl(sim_heavy_vc_ol, 1, 2);
sim_heavy_pz_ol.psd = computePsdDispl(sim_heavy_pz_ol, 1, 2);

%% PSD Plot of translations
figure;
hold on;
plot(sim_light_vc_ol.psd.f, sim_light_vc_ol.psd.dx);
plot(sim_light_vc_ol.psd.f, sim_light_vc_ol.psd.dy);
plot(sim_light_vc_ol.psd.f, sim_light_vc_ol.psd.dz);
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [$m^2/Hz$]'); xlabel('Frequency [Hz]');
hold off;
legend({'PSD $Tx$', 'PSD $Tz$', 'PSD $Tz$'})

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

%% PSD Plot of rotations
figure;
hold on;
plot(sim_light_vc_ol.psd.f, sim_light_vc_ol.psd.rx);
plot(sim_light_vc_ol.psd.f, sim_light_vc_ol.psd.ry);
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [$rad^2/Hz$]'); xlabel('Frequency [Hz]');
hold off;
legend({'PSD $Rx$', 'PSD $Rz$'})

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

%% PSD Plot of translations
figure;
hold on;
plot(sim_light_pz_ol.psd.f, sim_light_pz_ol.psd.dx);
plot(sim_light_pz_ol.psd.f, sim_light_pz_ol.psd.dy);
plot(sim_light_pz_ol.psd.f, sim_light_pz_ol.psd.dz);
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [$m^2/Hz$]'); xlabel('Frequency [Hz]');
hold off;
legend({'PSD $Tx$', 'PSD $Tz$', 'PSD $Tz$'})

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

%% PSD Plot of rotations
figure;
hold on;
plot(sim_light_pz_ol.psd.f, sim_light_pz_ol.psd.rx);
plot(sim_light_pz_ol.psd.f, sim_light_pz_ol.psd.ry);
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [$rad^2/Hz$]'); xlabel('Frequency [Hz]');
hold off;
legend({'PSD $Rx$', 'PSD $Rz$'})

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


%% PSD Plot of translations
figure;
hold on;
plot(sim_light_vc_ol.psd.f, sim_light_vc_ol.psd.dx);
plot(sim_light_vc_ol.psd.f, sim_light_vc_ol.psd.dy);
plot(sim_light_vc_ol.psd.f, sim_light_vc_ol.psd.dz);
set(gca,'ColorOrderIndex',1);
plot(sim_light_pz_ol.psd.f, sim_light_pz_ol.psd.dx, '--');
plot(sim_light_pz_ol.psd.f, sim_light_pz_ol.psd.dy, '--');
plot(sim_light_pz_ol.psd.f, sim_light_pz_ol.psd.dz, '--');
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({'PSD $Tx$ - VC', 'PSD $Tz$ - VC', 'PSD $Tz$ - VC', 'PSD $Tx$ - PZ', 'PSD $Tz$ - PZ', 'PSD $Tz$ - PZ'})

if save_fig; exportFig('psd_ol_pz_vc_light_trans', 'wide-tall', struct('path', 'control')); end