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

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

colors = colororder;
Fs = 1e4; % Sampling Frequency [Hz]
Ts = 1/Fs; % Sampling Time [s]

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

%% Load data
apa_sweep = load(sprintf('mat/frf_data_%i_sweep.mat', 1), 't', 'Va', 'Vs', 'da', 'de');

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

%% Plot the excitation signal
figure;
plot(t, apa_sweep.Va)
xlabel('Time [s]'); ylabel('Excitation Voltage $V_a$ [V]');

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

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

% Only used to have the frequency vector "f"
[~, f] = tfestimate(apa_sweep.Va, apa_sweep.de, win, [], [], 1/Ts);

%% Compute the coherence
[enc_coh, ~] = mscohere(apa_sweep.Va, apa_sweep.de, win, [], [], 1/Ts);
[int_coh, ~] = mscohere(apa_sweep.Va, apa_sweep.da, win, [], [], 1/Ts);

%% Plot the coherence
figure;
hold on;
plot(f, enc_coh, 'DisplayName', '$d_e/V_a$');
plot(f, int_coh, 'DisplayName', '$d_a/V_a$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
xlim([5, 5e3]); ylim([0, 1]);

%% TF - Encoder and interferometer
[frf_enc, ~] = tfestimate(apa_sweep.Va, apa_sweep.de, win, [], [], 1/Ts);
[frf_int, ~] = tfestimate(apa_sweep.Va, apa_sweep.da, win, [], [], 1/Ts);

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

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(frf_enc), 'color', colors(1, :), ...
     'DisplayName', 'Encoder');
plot(f, abs(frf_int), 'color', colors(2, :), ...
     'DisplayName', 'Interferometer');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'northeast');
ylim([1e-9, 1e-3]);

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(frf_enc), 'color', colors(1, :));
plot(f, 180/pi*angle(frf_int), 'color', colors(2, :));
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]);

%% Compute the coherence from Va to Vs
[iff_coh, ~] = mscohere(apa_sweep.Va, apa_sweep.Vs, win, [], [], 1/Ts);

%% Plot the coherence
figure;
hold on;
plot(f, iff_coh, 'k-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
xlim([5, 5e3]); ylim([0, 1]);

%% Compute the TF from Va to Vs
[iff_sweep, ~] = tfestimate(apa_sweep.Va, apa_sweep.Vs, win, [], [], 1/Ts);

%% Plot the TF
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(iff_sweep), 'k-');
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]);

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(iff_sweep), 'k-');
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]);

%% Load measured data - hysteresis
apa_hyst = load('frf_data_1_hysteresis.mat', 't', 'Va', 'de');
% Initial time set to zero
apa_hyst.t = apa_hyst.t - apa_hyst.t(1);

ampls = [0.1, 0.2, 0.4, 1, 2, 4]; % Excitation voltage amplitudes

%% Plot the excitation voltages and measured displacements
figure;
tiledlayout(1, 2, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile;
plot(apa_hyst.t, apa_hyst.Va)
xlabel('Time [s]'); ylabel('Output Voltage [V]');

ax2 = nexttile;
plot(apa_hyst.t, apa_hyst.de)
xlabel('Time [s]'); ylabel('Measured Displacement [m]');

%% Measured displacement as a function of the output voltage
figure;
tiledlayout(1, 3, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile([1,2]);
hold on;
for i = flip(1:6)
    i_lim = apa_hyst.t > i*5-1 & apa_hyst.t < i*5;
    plot(apa_hyst.Va(i_lim) - mean(apa_hyst.Va(i_lim)), apa_hyst.de(i_lim) - mean(apa_hyst.de(i_lim)), ...
         'DisplayName', sprintf('$V_a = %.1f [V]$', ampls(i)))
end
hold off;
xlabel('Output Voltage [V]'); ylabel('Measured Displacement [m]');
legend('location', 'northeast');
xlim([-4, 4]); ylim([-1.2e-4, 1.2e-4]);

ax2 = nexttile;
hold on;
for i = flip(1:6)
    i_lim = apa_hyst.t > i*5-1 & apa_hyst.t < i*5;
    plot(apa_hyst.Va(i_lim) - mean(apa_hyst.Va(i_lim)), apa_hyst.de(i_lim) - mean(apa_hyst.de(i_lim)))
end
hold off;
xlim([-0.4, 0.4]); ylim([-0.8e-5, 0.8e-5]);

%% Load data for stiffness measurement
apa_mass = load(sprintf('frf_data_%i_add_mass_closed_circuit.mat', 1), 't', 'de');
apa_mass.de = apa_mass.de - mean(apa_mass.de(apa_mass.t<11));

%% Plot the deflection at a function of time
figure;
plot(apa_mass.t, apa_mass.de, 'k-');
xlabel('Time [s]'); ylabel('Displacement $d_e$ [m]');

added_mass = 6.4; % Added mass [kg]

k = 9.8 * added_mass / (mean(apa_mass.de(apa_mass.t > 12 & apa_mass.t < 12.5)) - mean(apa_mass.de(apa_mass.t > 20 & apa_mass.t < 20.5)));

wz = 2*pi*94; % [rad/s]
msus = 5.7; % [kg]
k = msus * wz^2;

%% Load Data
add_mass_oc = load(sprintf('frf_data_%i_add_mass_open_circuit.mat',   1), 't', 'de');
add_mass_cc = load(sprintf('frf_data_%i_add_mass_closed_circuit.mat', 1), 't', 'de');

%% Zero displacement at initial time
add_mass_oc.de = add_mass_oc.de - mean(add_mass_oc.de(add_mass_oc.t<11));
add_mass_cc.de = add_mass_cc.de - mean(add_mass_cc.de(add_mass_cc.t<11));

figure;
hold on;
plot(add_mass_oc.t, add_mass_oc.de, 'DisplayName', 'Not connected');
plot(add_mass_cc.t, add_mass_cc.de, 'DisplayName', 'Connected');
hold off;
xlabel('Time [s]'); ylabel('Displacement $d_e$ [m]');
legend('location', 'northeast');

apa_k_oc = 9.8 * added_mass / (mean(add_mass_oc.de(add_mass_oc.t > 12 & add_mass_oc.t < 12.5)) - mean(add_mass_oc.de(add_mass_oc.t > 20 & add_mass_oc.t < 20.5)));
apa_k_cc = 9.8 * added_mass / (mean(add_mass_cc.de(add_mass_cc.t > 12 & add_mass_cc.t < 12.5)) - mean(add_mass_cc.de(add_mass_cc.t > 20 & add_mass_cc.t < 20.5)));

%% Load the data
wi_k = load('frf_data_1_sweep_lf_with_R.mat', 't', 'Vs', 'Va'); % With the resistor
wo_k = load('frf_data_1_sweep_lf.mat',        't', 'Vs', 'Va'); % Without the resistor

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

%% Compute the transfer functions from Va to Vs
[frf_wo_k, f] = tfestimate(wo_k.Va, wo_k.Vs, win, [], [], 1/Ts);
[frf_wi_k, ~] = tfestimate(wi_k.Va, wi_k.Vs, win, [], [], 1/Ts);

%% Model for the high pass filter
C = 5.1e-6; % Sensor Stack capacitance [F]
R = 80.6e3; % Parallel Resistor [Ohm]

f0 = 1/(2*pi*R*C); % Crossover frequency of RC HPF [Hz]

G_hpf = 0.6*(s/2*pi*f0)/(1 + s/2*pi*f0);

%% Compare the HPF model and the measured FRF
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(frf_wo_k), 'DisplayName', 'Without $k$');
plot(f, abs(frf_wi_k), 'DisplayName', 'With $k$');
plot(f, abs(squeeze(freqresp(G_hpf, f, 'Hz'))), 'k--', 'DisplayName', sprintf('HPF $f_o = %.2f [Hz]$', f0));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_{out}/V_{in}$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-1, 1e0]);
legend('location', 'southeast')

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(frf_wo_k));
plot(f, 180/pi*angle(frf_wi_k));
plot(f, 180/pi*angle(squeeze(freqresp(G_hpf, f, 'Hz'))), 'k--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:45:360); ylim([-45, 90]);

linkaxes([ax1,ax2],'x');
xlim([0.2, 8]);