%%
clear;

%% Jacobian
load('mat/jacobian.mat', 'J');

Jinv = pinv(J);

save('sim_data/J.mat', 'J', 'Jinv');

%% Saved HAC
% load('mat/Khac_iff_struts.mat', 'Khac_iff_struts');

%% Developed HAC
s = zpk('s');

a  = 5;  % Amount of phase lead / width of the phase lead / high frequency gain
wc = 2*pi*110; % Frequency with the maximum phase lead [rad/s]

H_lead = (1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
H_lpf = 1/(1 + s/2/pi/300);

gm = 0.02;
xi = 0.5;
wn = 2*pi*700;

H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);

Khac_iff_struts = -2.2e4 * ... % Gain
                  H_lead * ...       % Lead
                  H_lpf * ...       % Lead
                  H_notch * ...      % Notch
                  (2*pi*100/s) * ... % Integrator
                  eye(6);            % 6x6 Diagonal

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

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

%% Loop Gain
load('mat/damped_plant_enc_plates.mat', 'f', 'G_enc_iff_opt')

L_frf = zeros(size(G_enc_iff_opt));

for i = 1:size(G_enc_iff_opt, 1)
    L_frf(i, :, :) = squeeze(G_enc_iff_opt(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]);