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

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

colors = colororder;

addpath('./mat/');
addpath('./src/');

added_mass = 6.4; % Added mass [kg]

apa_nums = [1 2 4 5 6 7 8];

%% Load Data
apa_mass = {};
for i = 1:length(apa_nums)
    apa_mass(i) = {load(sprintf('frf_data_%i_add_mass_closed_circuit.mat', apa_nums(i)), 't', 'de')};
    % The initial displacement is set to zero
    apa_mass{i}.de = apa_mass{i}.de - mean(apa_mass{i}.de(apa_mass{i}.t<11));
end

%% Plot the time domain measured deflection
figure;
hold on;
for i = 1:length(apa_nums)
  plot(apa_mass{i}.t, apa_mass{i}.de, 'DisplayName', sprintf('APA %i', apa_nums(i)));
end
hold off;
xlabel('Time [s]'); ylabel('Displacement $d_e$ [m]');
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);

%% Compute the stiffness
apa_k = zeros(length(apa_nums), 1);
for i = 1:length(apa_nums)
    apa_k(i) = 9.8 * added_mass / (mean(apa_mass{i}.de(apa_mass{i}.t > 12 & apa_mass{i}.t < 12.5)) - mean(apa_mass{i}.de(apa_mass{i}.t > 20 & apa_mass{i}.t < 20.5)));
end

%% Second identification
apa_sweep = {};
for i = 1:length(apa_nums)
    apa_sweep(i) = {load(sprintf('frf_data_%i_sweep.mat', apa_nums(i)), 't', 'Va', 'Vs', 'de', 'da')};
end

%% Third identification
apa_noise_hf = {};
for i = 1:length(apa_nums)
    apa_noise_hf(i) = {load(sprintf('frf_data_%i_noise_hf.mat', apa_nums(i)), 't', 'Va', 'Vs', 'de', 'da')};
end

%% Time vector
t = apa_sweep{1}.t - apa_sweep{1}.t(1) ; % Time vector [s]

%% Sampling
Ts = (t(end) - t(1))/(length(t)-1); % Sampling Time [s]
Fs = 1/Ts; % Sampling Frequency [Hz]

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

% Only used to have the frequency vector "f"
[~, f] = tfestimate(apa_sweep{1}.Va, apa_sweep{1}.de, win, [], [], 1/Ts);
i_lf = f <= 350;
i_hf = f >  350;

%% Coherence computation
coh_enc = zeros(length(f), length(apa_nums));
for i = 1:length(apa_nums)
    [coh_lf, ~] = mscohere(apa_sweep{i}.Va,    apa_sweep{i}.de,    win, [], [], 1/Ts);
    [coh_hf, ~] = mscohere(apa_noise_hf{i}.Va, apa_noise_hf{i}.de, win, [], [], 1/Ts);
    coh_enc(:, i) = [coh_lf(i_lf); coh_hf(i_hf)];
end

figure;
hold on;
for i = 1:length(apa_nums)
    plot(f, coh_enc(:, i));
end;
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
xlim([5, 5e3]); ylim([0, 1]);

%% Transfer function estimation
enc_frf = zeros(length(f), length(apa_nums));
for i = 1:length(apa_nums)
    [frf_lf, ~] = tfestimate(apa_sweep{i}.Va,    apa_sweep{i}.de,    win, [], [], 1/Ts);
    [frf_hf, ~] = tfestimate(apa_noise_hf{i}.Va, apa_noise_hf{i}.de, win, [], [], 1/Ts);
    enc_frf(:, i) = [frf_lf(i_lf); frf_hf(i_hf)];
end

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

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(apa_nums)
    plot(f, abs(enc_frf(:, i)), ...
         'DisplayName', sprintf('APA %i', apa_nums(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
ylim([1e-9, 1e-3]);

ax2 = nexttile;
hold on;
for i = 1:length(apa_nums)
    plot(f, 180/pi*angle(enc_frf(:, 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([10, 2e3]);

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

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(apa_nums)
    plot(f, abs(enc_frf(:, i)), ...
         'DisplayName', sprintf('APA %i', apa_nums(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
ylim([2e-5, 4e-4]);

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

linkaxes([ax1,ax2],'x');
xlim([80, 120]);

%% Compute the Coherence
coh_iff = zeros(length(f), length(apa_nums));
for i = 1:length(apa_nums)
    [coh_lf, ~] = mscohere(apa_sweep{i}.Va,    apa_sweep{i}.Vs,    win, [], [], 1/Ts);
    [coh_hf, ~] = mscohere(apa_noise_hf{i}.Va, apa_noise_hf{i}.Vs, win, [], [], 1/Ts);
    coh_iff(:, i) = [coh_lf(i_lf); coh_hf(i_hf)];
end

%% Plot the coherence
figure;
hold on;
for i = 1:length(apa_nums)
    plot(f, coh_iff(:, i));
end;
hold off;
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlim([5, 5e3]); ylim([0, 1]);

%% FRF estimation of the transfer function from Va to Vs
iff_frf = zeros(length(f), length(apa_nums));
for i = 1:length(apa_nums)
    [frf_lf, ~] = tfestimate(apa_sweep{i}.Va,    apa_sweep{i}.Vs,    win, [], [], 1/Ts);
    [frf_hf, ~] = tfestimate(apa_noise_hf{i}.Va, apa_noise_hf{i}.Vs, win, [], [], 1/Ts);
    iff_frf(:, i) = [frf_lf(i_lf); frf_hf(i_hf)];
end

%% Plot the FRF from Va to Vs
figure;
tiledlayout(2, 1, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile;
hold on;
for i = 1:length(apa_nums)
    plot(f, abs(iff_frf(:, i)), ...
         'DisplayName', sprintf('APA %i', apa_nums(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-2, 1e2]);
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);

ax2 = nexttile;
hold on;
for i = 1:length(apa_nums)
    plot(f, 180/pi*angle(iff_frf(:, i)));
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]);

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

%% Remove the APA 7 (6 in the list) from measurements
apa_nums(6) = [];
enc_frf(:,6) = [];
iff_frf(:,6) = [];

%% Save the measured FRF
save('mat/meas_apa_frf.mat', 'f', 'Ts', 'enc_frf', 'iff_frf', 'apa_nums');