%% Clear Workspace and Close figures
clear; close all; clc;

%% Intialize Laplace variable
s = zpk('s');

addpath('./src/');

% Test with one APA

%% Load measurement data for APA number 1
strut_number = 1;
% load(sprintf('mat/frf_data_exc_strut_%i_noise_lf.mat', strut_number), 't', 'Va', 'Vs', 'de');
load(sprintf('mat/frf_data_exc_strut_%i_noise_hf.mat', strut_number), 't', 'Va', 'Vs', 'de');

% Compute transfer functions:

Ts = (t(end) - t(1))/(length(t)-1);
Fs = 1/Ts;

win = hanning(ceil(1*Fs)); % Hannning Windows

%% DVF
[G_dvf, f] = tfestimate(Va, de, win, [], [], 1/Ts);

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

ax1 = nexttile([2,1]);
hold on;
for i =1:6
    plot(f, abs(G_dvf(:,i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_{out}/V_{in}$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;

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

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

%% IFF
[G_iff, f] = tfestimate(Va, Vs, win, [], [], 1/Ts);

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

ax1 = nexttile([2,1]);
hold on;
for i =1:6
    plot(f, abs(G_iff(:,i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_{out}/V_{in}$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;

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

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

%%
[coh_dvf, ~] = mscohere(Va, de, win, [], [], 1/Ts);
[coh_iff, ~] = mscohere(Va, Vs, win, [], [], 1/Ts);

%%
figure;
hold on;
for i =1:6
    plot(f, coh_dvf(:,i));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Coherence');
ylim([0,1]);

%%
figure;
hold on;
for i =1:6
    plot(f, coh_iff(:,i));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Coherence');
ylim([0,1]);