%%
clear; close all; clc;

%% Saved HAC
% load('mat/Khac_iff_struts.mat', 'Khac_iff_struts');
% load('mat/Khac_iff_struts_rob.mat', 'Kd');
load('mat/Khac_iff_struts_svd.mat', 'Kd');
% load('mat/Khac_iff_struts_jacobian_cok.mat', 'Kd');

%% Save Controller
load('sim_data/data_sim.mat', 'Ts')
Khac_iff_struts = c2d(Kd, Ts, 'Tustin');

save('sim_data/Khac_iff_struts.mat', 'Khac_iff_struts');

%% Loop Gain
load('mat/frf_iff_vib_table_m.mat', 'f', 'G_dL');

L_frf = zeros(size(G_dL{3}));

for i = 1:length(f)
    L_frf(i, :, :) = squeeze(G_dL{3}(i,:,:))*freqresp(Khac_iff_struts, f(i), 'Hz');
end

colors = colororder;

freqs = 2*logspace(1, 3, 1000);

figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
% Diagonal Elements FRF
plot(f, abs(L_frf(:,1,1)), 'color', colors(1,:), ...
     'DisplayName', 'Diagonal');
for i = 2:6
    plot(f, abs(L_frf(:,i,i)), 'color', colors(1,:), ...
         'HandleVisibility', 'off');
end
plot(f, abs(L_frf(:,1,2)), 'color', [colors(2,:), 0.2], ...
     'DisplayName', 'Off-Diag');
for i = 1:5
    for j = i+1:6
        plot(f, abs(L_frf(:,i,j)), 'color', [colors(2,:), 0.2], ...
             'HandleVisibility', 'off');
    end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gain [-]'); set(gca, 'XTickLabel',[]);
ylim([1e-3, 1e2]);
legend('location', 'northeast');
grid;

ax2 = nexttile;
hold on;
for i =1:6
    plot(f, 180/pi*angle(L_frf(:,i,i)), 'color', colors(1,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-180, 180]);
grid;

linkaxes([ax1,ax2],'x');
xlim([1, 2e3]);