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

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

%% Path for functions, data and scripts
addpath('./mat/'); % Path for data
addpath('./src/'); % Path for functions

%% Colors for the figures
colors = colororder;

% Effect of the Encoder on the measured dynamics
% <<ssec:test_struts_effect_encoder>>


%% Parameters for Frequency Analysis
Ts = 1e-4; % Sampling Time [s]
Nfft = floor(1/Ts); % Number of points for the FFT computation
win = hanning(Nfft); % Hanning window
Noverlap = floor(Nfft/2); % Overlap between frequency analysis

%% Measure FRF for Strut 1 - No encoder
% Load Data
leg_sweep    = load('frf_data_leg_1_sweep.mat', 'u', 'Vs', 'de', 'da');
leg_noise_hf = load('frf_data_leg_1_noise_hf.mat', 'u', 'Vs', 'de', 'da');

% We get the frequency vector that will be the same for all the frequency domain analysis.
[~, f] = tfestimate(leg_sweep.u, leg_sweep.de, win, Noverlap, Nfft, 1/Ts);
i_lf = f <= 350; % Indices used for the low frequency
i_hf = f >  350; % Indices used for the high frequency

% Compute FRF function from u to da (interferometer)
[frf_sweep, ~]    = tfestimate(leg_sweep.u,    leg_sweep.da,    win, Noverlap, Nfft, 1/Ts);
[frf_noise_hf, ~] = tfestimate(leg_noise_hf.u, leg_noise_hf.da, win, Noverlap, Nfft, 1/Ts);
int_frf = [frf_sweep(i_lf); frf_noise_hf(i_hf)]; % Combine the FRF

% Compute FRF function from u to Vs (force sensor)
[frf_sweep,    ~] = tfestimate(leg_sweep.u,    leg_sweep.Vs,    win, Noverlap, Nfft, 1/Ts);
[frf_noise_hf, ~] = tfestimate(leg_noise_hf.u, leg_noise_hf.Vs, win, Noverlap, Nfft, 1/Ts);
iff_frf = [frf_sweep(i_lf); frf_noise_hf(i_hf)]; % Combine the FRF

%% Measure FRF for Strut 1 - With encoder
% Load Data
leg_enc_sweep    = load('frf_data_leg_coder_1_noise.mat',    'u', 'Vs', 'de', 'da');
leg_enc_noise_hf = load('frf_data_leg_coder_1_noise_hf.mat', 'u', 'Vs', 'de', 'da');

% Compute FRF function from u to da (interferometer)
[frf_sweep, ~]    = tfestimate(leg_enc_sweep.u,    leg_enc_sweep.da,    win, Noverlap, Nfft, 1/Ts);
[frf_noise_hf, ~] = tfestimate(leg_enc_noise_hf.u, leg_enc_noise_hf.da, win, Noverlap, Nfft, 1/Ts);
int_with_enc_frf = [frf_sweep(i_lf); frf_noise_hf(i_hf)]; % Combine the FRF

% Compute FRF function from u to Vs (force sensor)
[frf_sweep, ~]    = tfestimate(leg_enc_sweep.u,    leg_enc_sweep.Vs,    win, Noverlap, Nfft, 1/Ts);
[frf_noise_hf, ~] = tfestimate(leg_enc_noise_hf.u, leg_enc_noise_hf.Vs, win, Noverlap, Nfft, 1/Ts);
iff_with_enc_frf = [frf_sweep(i_lf); frf_noise_hf(i_hf)]; % Combine the FRF

% Compute FRF function from u to de (encoder)
[frf_sweep, ~]    = tfestimate(leg_enc_sweep.u,    leg_enc_sweep.de,    win, Noverlap, Nfft, 1/Ts);
[frf_noise_hf, ~] = tfestimate(leg_enc_noise_hf.u, leg_enc_noise_hf.de, win, Noverlap, Nfft, 1/Ts);
enc_frf = [frf_sweep(i_lf); frf_noise_hf(i_hf)]; % Combine the FRF



% System identification is performed in two cases:
% - no encoder is fixed to the strut (Figure ref:fig:test_struts_bench_leg_front)
% - one encoder is fixed to the strut (Figure ref:fig:test_struts_bench_leg_coder)

% #+name: fig:test_struts_bench_leg_with_without_enc
% #+caption: Struts fixed to the test bench with clamped flexible joints. The coder can be fixed to the struts (\subref{fig:test_struts_bench_leg_coder}) or removed (\subref{fig:test_struts_bench_leg_front})
% #+attr_latex: :options [htbp]
% #+begin_figure
% #+attr_latex: :caption \subcaption{\label{fig:test_struts_bench_leg_coder}Strut with encoder}
% #+attr_latex: :options {0.5\textwidth}
% #+begin_subfigure
% #+attr_latex: :height 6cm
% [[file:figs/test_struts_bench_leg_coder.jpg]]
% #+end_subfigure
% #+attr_latex: :caption \subcaption{\label{fig:test_struts_bench_leg_front}Strut without encoder}
% #+attr_latex: :options {0.5\textwidth}
% #+begin_subfigure
% #+attr_latex: :height 6cm
% [[file:figs/test_struts_bench_leg_front.jpg]]
% #+end_subfigure
% #+end_figure

% The obtained frequency response functions are compared in Figure ref:fig:test_struts_effect_encoder.
% It is found that the encoder has very little effect on the transfer function from excitation voltage $u$ to the axial motion of the strut $d_a$ as measured by the interferometer (Figure ref:fig:test_struts_effect_encoder_int).
% This means that the axial motion of the strut is unaffected by the presence of the encoder.
% Similarly, it has very little effect on the transfer function from $u$ to the sensor stack voltage $V_s$ (Figure ref:fig:test_struts_effect_encoder_iff).
% This means that the integral force feedback control strategy should be as effective whether the encoders are fixed to the struts.


%% Plot the FRF from u to da with and without the encoder
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', '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/u$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-7, 1e-3]);
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);

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]);

%% Compare the IFF plant with and without the encoders
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', '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/u$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
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]);

% Comparison of the encoder and interferometer
% <<ssec:test_struts_comp_enc_int>>

% The dynamics as measured by the encoder and by the interferometers are compared in Figure ref:fig:test_struts_comp_enc_int.

% The dynamics from the excitation voltage $u$ to the measured displacement by the encoder $d_e$ presents a behavior that is much more complex than the dynamics to the displacement as measured by the interferometer (comparison made in Figure ref:fig:test_struts_comp_enc_int).
% Three additional resonance frequencies can be observed at 197Hz, 290Hz and 376Hz.
% These resonance frequencies correspond to flexible modes of the strut that were studied in Section ref:sec:test_struts_flexible_modes.

% The good news is that these resonances are not seen on the interferometer and are therefore not impacting the axial motion of the strut (which is what is important for the hexapod positioning).
% However, these resonances are making the use of encoder fixed to the strut difficult.


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

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(enc_frf),          'DisplayName', 'Encoder');
plot(f, abs(int_with_enc_frf), 'DisplayName', 'Interferometer');
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/u$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
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]);

% Comparison of all the Struts
% <<ssec:test_struts_comp_all_struts>>


%% Numbers of the measured legs
strut_nums = [1 2 3 4 5];

%% Load the measurement data
% First identification (low frequency noise)
leg_noise = {};
for i = 1:length(strut_nums)
    leg_noise(i) = {load(sprintf('frf_data_leg_coder_%i_noise.mat', strut_nums(i)), 'u', 'Vs', 'de', 'da')};
end

% Second identification (high frequency noise)
leg_noise_hf = {};
for i = 1:length(strut_nums)
    leg_noise_hf(i) = {load(sprintf('frf_data_leg_coder_%i_noise_hf.mat', strut_nums(i)), 'u', 'Vs', 'de', 'da')};
end

%% Compute FRF - From u to de (encoder)
enc_frf = zeros(length(f), length(strut_nums));

for i = 1:length(strut_nums)
    [frf_lf, ~] = tfestimate(leg_noise{i}.u,    detrend(leg_noise{i}.de, 0),    win, Noverlap, Nfft, 1/Ts);
    [frf_hf, ~] = tfestimate(leg_noise_hf{i}.u, detrend(leg_noise_hf{i}.de, 0), win, Noverlap, Nfft, 1/Ts);
    enc_frf(:, i) = [frf_lf(i_lf); frf_hf(i_hf)];
end

%% Compute FRF - From u to da (interferometer)
int_frf = zeros(length(f), length(strut_nums));
for i = 1:length(strut_nums)
    [frf_lf, ~] = tfestimate(leg_noise{i}.u,    leg_noise{i}.da,    win, Noverlap, Nfft, 1/Ts);
    [frf_hf, ~] = tfestimate(leg_noise_hf{i}.u, leg_noise_hf{i}.da, win, Noverlap, Nfft, 1/Ts);
    int_frf(:, i) = [frf_lf(i_lf); frf_hf(i_hf)];
end

%% Compute FRF - From u to Vs (force sensor)
iff_frf = zeros(length(f), length(strut_nums));
for i = 1:length(strut_nums)
    [frf_lf, ~] = tfestimate(leg_noise{i}.u,    leg_noise{i}.Vs,    win, Noverlap, Nfft, 1/Ts);
    [frf_hf, ~] = tfestimate(leg_noise_hf{i}.u, leg_noise_hf{i}.Vs, win, Noverlap, Nfft, 1/Ts);
    iff_frf(:, i) = [frf_lf(i_lf); frf_hf(i_hf)];
end



% Then, the dynamics of all the mounted struts (only 5 at the time of the experiment) are all measured using the same test bench.
% The obtained dynamics from $u$ to $d_a$ are compared in Figure ref:fig:test_struts_comp_interf_plants while is dynamics from $u$ to $V_s$ are compared in Figure ref:fig:test_struts_comp_iff_plants.
% Very good match can be observed between all the struts.


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

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

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

%% Plot the FRF from u to Vs
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(strut_nums)
    plot(f, abs(iff_frf(:, i)), ...
         'DisplayName', sprintf('Leg %i', strut_nums(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/u$ [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(strut_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]);



% #+name: fig:test_struts_comp_plants
% #+caption: Comparison of the measured plants
% #+attr_latex: :options [htbp]
% #+begin_figure
% #+attr_latex: :caption \subcaption{\label{fig:test_struts_comp_interf_plants}$u$ to $d_a$}
% #+attr_latex: :options {0.49\textwidth}
% #+begin_subfigure
% #+attr_latex: :width \linewidth
% [[file:figs/test_struts_comp_interf_plants.png]]
% #+end_subfigure
% #+attr_latex: :caption \subcaption{\label{fig:test_struts_comp_iff_plants}$u$ to $V_s$}
% #+attr_latex: :options {0.49\textwidth}
% #+begin_subfigure
% #+attr_latex: :width \linewidth
% [[file:figs/test_struts_comp_iff_plants.png]]
% #+end_subfigure
% #+end_figure

% The same comparison is made for the transfer function from $u$ to $d_e$ (encoder output) in Figure ref:fig:test_struts_comp_enc_plants.
% This time, large dynamics differences are observed between the 5 struts.
% Even-though the same peaks are seen for all of the struts (95Hz, 200Hz, 300Hz, 400Hz), the amplitude of the peaks are not the same.
% Moreover, the location or even the presence of complex conjugate zeros is changing from one strut to the other.

% It will be further investigated why such differences are observed (see Section ref:ssec:test_struts_effect_misalignment).


%% Bode plot of the FRF from u to de
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

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

ax2 = nexttile;
hold on;
for i = 1:length(strut_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([-180, 180]);

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

%% Save the estimated FRF for further analysis
save('./mat/meas_struts_frf.mat', 'f', 'enc_frf', 'int_frf', 'iff_frf', 'strut_nums');