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

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

colors = colororder;

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

%% Load Data
leg_sweep    = load(sprintf('frf_data_leg_%i_sweep.mat',    1), 't', 'Va', 'Vs', 'de', 'da');
leg_noise_hf = load(sprintf('frf_data_leg_%i_noise_hf.mat', 1), 't', 'Va', 'Vs', 'de', 'da');

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

%% Sampling frequency/time
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(leg_sweep.Va, leg_sweep.de, win, [], [], 1/Ts);
i_lf = f <= 350; % Indices used for the low frequency
i_hf = f >  350; % Indices used for the low frequency

%% Compute the coherence for both excitation signals
[int_coh_sweep, ~]    = mscohere(leg_sweep.Va,    leg_sweep.da,    win, [], [], 1/Ts);
[int_coh_noise_hf, ~] = mscohere(leg_noise_hf.Va, leg_noise_hf.da, win, [], [], 1/Ts);

%% Combine the coherence
int_coh = [int_coh_sweep(i_lf); int_coh_noise_hf(i_hf)];

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

%% Compute FRF function from Va to da
[frf_sweep, ~]    = tfestimate(leg_sweep.Va,    leg_sweep.da,    win, [], [], 1/Ts);
[frf_noise_hf, ~] = tfestimate(leg_noise_hf.Va, leg_noise_hf.da, win, [], [], 1/Ts);

%% Combine the FRF
int_frf = [frf_sweep(i_lf); frf_noise_hf(i_hf)];

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

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(int_frf), 'k-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-9, 1e-3]);

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

%% Compute the coherence for both excitation signals
[iff_coh_sweep, ~]    = mscohere(leg_sweep.Va,    leg_sweep.Vs,    win, [], [], 1/Ts);
[iff_coh_noise_hf, ~] = mscohere(leg_noise_hf.Va, leg_noise_hf.Vs, win, [], [], 1/Ts);

%% Combine the coherence
iff_coh = [iff_coh_sweep(i_lf); iff_coh_noise_hf(i_hf)];

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

%% Compute the FRF
[frf_sweep,    ~] = tfestimate(leg_sweep.Va,    leg_sweep.Vs,    win, [], [], 1/Ts);
[frf_noise_hf, ~] = tfestimate(leg_noise_hf.Va, leg_noise_hf.Vs, win, [], [], 1/Ts);

%% Combine the FRF
iff_frf = [frf_sweep(i_lf); frf_noise_hf(i_hf)];

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

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

%% Load data
leg_enc_sweep    = load(sprintf('frf_data_leg_coder_badly_align_%i_noise.mat',    1), 't', 'Va', 'Vs', 'de', 'da');
leg_enc_noise_hf = load(sprintf('frf_data_leg_coder_badly_align_%i_noise_hf.mat', 1), 't', 'Va', 'Vs', 'de', 'da');

%% Compute the coherence for both excitation signals
[int_coh_sweep, ~]    = mscohere(leg_enc_sweep.Va,    leg_enc_sweep.da,    win, [], [], 1/Ts);
[int_coh_noise_hf, ~] = mscohere(leg_enc_noise_hf.Va, leg_enc_noise_hf.da, win, [], [], 1/Ts);

%% Combine the coherinte
int_coh = [int_coh_sweep(i_lf); int_coh_noise_hf(i_hf)];

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

%% Compute FRF function from Va to da
[frf_sweep, ~]    = tfestimate(leg_enc_sweep.Va,    leg_enc_sweep.da,    win, [], [], 1/Ts);
[frf_noise_hf, ~] = tfestimate(leg_enc_noise_hf.Va, leg_enc_noise_hf.da, win, [], [], 1/Ts);

%% Combine the FRF
int_with_enc_frf = [frf_sweep(i_lf); frf_noise_hf(i_hf)];

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

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(int_with_enc_frf), 'k-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_a/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-7, 1e-3]);

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

%% Plot the FRF from Va to da with and without the encoder
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(int_with_enc_frf), '-', 'DisplayName', 'With encoder');
plot(f, abs(int_frf),          '-', 'DisplayName', 'Without encoder');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_a/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-7, 1e-3]);
legend('location', 'northeast')

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

%% Compute the coherence for both excitation signals
[enc_coh_sweep, ~]    = mscohere(leg_enc_sweep.Va,    leg_enc_sweep.de,    win, [], [], 1/Ts);
[enc_coh_noise_hf, ~] = mscohere(leg_enc_noise_hf.Va, leg_enc_noise_hf.de, win, [], [], 1/Ts);

%% Combine the coherence
enc_coh = [enc_coh_sweep(i_lf); enc_coh_noise_hf(i_hf)];

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

%% Compute FRF function from Va to da
[frf_sweep, ~]    = tfestimate(leg_enc_sweep.Va,    leg_enc_sweep.de,    win, [], [], 1/Ts);
[frf_noise_hf, ~] = tfestimate(leg_enc_noise_hf.Va, leg_enc_noise_hf.de, win, [], [], 1/Ts);

%% Combine the FRF
enc_frf = [frf_sweep(i_lf); frf_noise_hf(i_hf)];

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

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(enc_frf), 'k-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-7, 1e-3]);

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

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

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(enc_frf),          'DisplayName', 'Encoder');
plot(f, abs(int_with_enc_frf), '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', 'FontSize', 8, 'NumColumns', 2);
ylim([1e-8, 1e-3]);

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

%% Transfer function from Vs to de with indicated resonances
figure;
hold on;
plot(f, abs(enc_frf), 'k-');
text(93,  4e-4,  {'93Hz'}, 'VerticalAlignment','bottom','HorizontalAlignment','center')
text(200, 1.3e-4,{'197Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
text(300, 4e-6,  {'290Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
text(400, 1.4e-6,{'376Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); xlabel('Frequency [Hz]');
hold off;
ylim([1e-7, 1e-3]); xlim([10, 2e3]);

%% Compute the coherence for both excitation signals
[iff_coh_sweep, ~]    = mscohere(leg_enc_sweep.Va,    leg_enc_sweep.Vs,    win, [], [], 1/Ts);
[iff_coh_noise_hf, ~] = mscohere(leg_enc_noise_hf.Va, leg_enc_noise_hf.Vs, win, [], [], 1/Ts);

%% Combine the coherence
iff_coh = [iff_coh_sweep(i_lf); iff_coh_noise_hf(i_hf)];

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

%% Compute FRF function from Va to da
[frf_sweep, ~]    = tfestimate(leg_enc_sweep.Va,    leg_enc_sweep.Vs,    win, [], [], 1/Ts);
[frf_noise_hf, ~] = tfestimate(leg_enc_noise_hf.Va, leg_enc_noise_hf.Vs, win, [], [], 1/Ts);

%% Combine the FRF
iff_with_enc_frf = [frf_sweep(i_lf); frf_noise_hf(i_hf)];

%% Plot FRF of the transfer function from Va to Vs
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

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

%% Compare the IFF plant with and without the encoders
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(iff_with_enc_frf), 'DisplayName', 'With Encoder');
plot(f, abs(iff_frf), 'DisplayName', 'Without Encoder');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'northeast', 'FontSize', 8);
ylim([1e-2, 1e2]);

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