%%
clear; close all; clc;

%% Load the transfer functions
load('./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');

%% Plant
figure;
% Amplitude
ax1 = subaxis(2,1,1);
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_cart('Dx', 'Fnx'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Amplitude [m/N]');
hold off;

% Phase
ax2 = subaxis(2,1,2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_vc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_pz.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
set(gca,'ColorOrderIndex',1)
plot(freqs, 180/pi*angle(squeeze(freqresp(G_heavy_vc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_heavy_pz.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
set(gca,'xscale','log');
yticks(-1800:90:1800);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
legend('Location', 'southwest');
hold off;

linkaxes([ax1,ax2],'x');

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

%%
figure;
% Amplitude
ax1 = subaxis(2,1,1);
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_cart('Dz', 'Fnz'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_cart('Dz', 'Fnz'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Amplitude [m/N]');
hold off;

% Phase
ax2 = subaxis(2,1,2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_vc.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_pz.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
set(gca,'ColorOrderIndex',1)
plot(freqs, 180/pi*angle(squeeze(freqresp(G_heavy_vc.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_heavy_pz.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
set(gca,'xscale','log');
yticks(-1800:90:1800);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
legend('Location', 'southwest');
hold off;

linkaxes([ax1,ax2],'x');

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

%% Plot all the coupling
figure;

for i_input = 1:3
    for i_output = 1:3
        subaxis(3,3,3*(i_input-1)+i_output);
        hold on;
        plot(freqs, abs(squeeze(freqresp(G_light_vc.G_cart(i_output, i_input), freqs, 'Hz'))));
        plot(freqs, abs(squeeze(freqresp(G_light_pz.G_cart(i_output, i_input), freqs, 'Hz'))));
        set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
        xlim([freqs(1) freqs(end)]); ylim([1e-12, 1e-2]);
        yticks([1e-12, 1e-8, 1e-4]); xticks([0.1 1 10 100 1000]);
        if i_output > 1; set(gca,'yticklabel',[]); end
        if i_input < 3; set(gca,'xticklabel',[]); end
        hold off;
    end
end

if save_fig; exportFig('comp_models_plant_coupling_all', 'full-tall', struct('path', 'identification')); end

%% Plot some coupling
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', 'VC - Light - $Fx \to Dx$');
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light - $Fx \to Dx$');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_cart('Dy', 'Fnx'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy - $Fx \to Dy$');
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_cart('Dy', 'Fnx'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy - $Fx \to Dy$');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_light_vc.G_cart('Dz', 'Fnx'), freqs, 'Hz'))), '-.', 'DisplayName', 'VC - Heavy - $Fx \to Dz$');
plot(freqs, abs(squeeze(freqresp(G_light_pz.G_cart('Dz', 'Fnx'), freqs, 'Hz'))), '-.', 'DisplayName', 'PZ - Heavy - $Fx \to Dz$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/m]');
legend('Location', 'southwest');
xticks('manual'); xlim([freqs(1) freqs(end)]);
hold off;

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