%%
clear; close all; clc;

%% Load the identified transfer functions
save('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');

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

%% Transfer function from ground displacement to measured displacement
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_dg('Dz', 'Dgz'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_dg('Dz', 'Dgz'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_dg('Dz', 'Dgz'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_dg('Dz', 'Dgz'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/m]');
hold off;
legend('Location', 'southwest');

if save_fig; exportFig('comp_models_xw_to_d', 'normal-normal', struct('path', 'identification')); end

%%
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_dg('Dx', 'Dgx'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_dg('Dx', 'Dgx'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_dg('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_dg('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/m]');
hold off;
legend('Location', 'southwest');

%% Transfer function from direct force to measured displacement
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]');
hold off;
legend('Location', 'southwest');

if save_fig; exportFig('comp_models_fi_to_d', 'normal-normal', struct('path', 'identification')); end

%%
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_fs('Ry', 'Fsx'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_fs('Ry', 'Fsx'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_fs('Ry', 'Fsx'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_fs('Ry', 'Fsx'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]');
hold off;
legend('Location', 'southwest');

%%
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_fs('Rz', 'Fsx'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_fs('Rz', 'Fsx'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_fs('Rz', 'Fsx'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_fs('Rz', 'Fsx'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]');
hold off;
legend('Location', 'southwest');