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

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

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

%% Colors for the figures
colors = colororder;

% Hysteresis
% <<ssec:test_apa_hysteresis>>

% As the payload is vertically guided without friction, the hysteresis of the APA can be estimated from the motion of the payload.

% A quasi static sinusoidal excitation $V_a$ with an offset of $65\,V$ (halfway between $-20\,V$ and $150\,V$), and an amplitude varying from $4\,V$ up to $80\,V$.

% For each excitation amplitude, the vertical displacement $d_e$ of the mass is measured and displayed as a function of the applied voltage..


%% Load measured data - hysteresis
apa_hyst = load('frf_data_1_hysteresis.mat', 't', 'u', '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



% The measured displacements as a function of the output voltages are shown in Figure ref:fig:test_apa_meas_hysteresis.
% It is interesting to see that the hysteresis is increasing with the excitation amplitude.


%% Measured displacement as a function of the output voltage
figure;

hold on;
for i = [6,5,4,2]
    i_lim = apa_hyst.t > i*5-1 & apa_hyst.t < i*5;
    plot(20*apa_hyst.u(i_lim), 1e6*detrend(apa_hyst.de(i_lim), 0), ...
         'DisplayName', sprintf('$V_a = 65 + %.0f \\sin (\\omega t) \\ [V]$', 20*ampls(i)))
end
hold off;
xlabel('Stack Voltage $V_a$ [V]'); ylabel('Displacement $d_e$ [$\mu$m]');
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
xlim([-20, 150]);
ylim([-120, 120]);

% Axial stiffness
% <<ssec:test_apa_stiffness>>

% In order to estimate the stiffness of the APA, a weight with known mass $m_a = 6.4\,\text{kg}$ is added on top of the suspended granite and the deflection $d_e$ is measured using the encoder.

% The APA stiffness can then be estimated from equation eqref:eq:test_apa_stiffness.

% \begin{equation} \label{eq:test_apa_stiffness}
%   k_{\text{apa}} = \frac{m_a g}{\Delta d_e}
% \end{equation}


%% Load data for stiffness measurement
apa_nums = [1 2 4 5 6 8];
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

added_mass = 6.4; % Added mass [kg]



% The measured displacement $d_e$ as a function of time is shown in Figure ref:fig:test_apa_meas_stiffness_time.
% It can be seen that there are some drifts in the measured displacement (probably due to piezoelectric creep) and the that displacement does not come back to the initial position after the mass is removed (probably due to piezoelectric hysteresis).
% These two effects induce some uncertainties in the measured stiffness.


%% Plot the deflection at a function of time
figure;
hold on;
plot(apa_mass{2}.t(1:100:end)-apa_mass{2}.t(1), 1e6*apa_mass{2}.de(1:100:end), 'k-');
plot([0,20], [-0.4, -0.4], 'k--', 'LineWidth', 0.5)
plot([0,20], [-4.5, -4.5], 'k--', 'LineWidth', 0.5)
plot([0,20], [-37.4, -37.4], 'k--', 'LineWidth', 0.5)
% first stroke for stiffness measurements
anArrow = annotation('doublearrow', 'LineWidth', 0.5);
anArrow.Parent = gca;
anArrow.Position = [2, -0.4, 0, -37];
text(2.5, -20, sprintf('$d_1$'), 'horizontalalignment', 'left');

% second stroke for stiffness measurements
anArrow = annotation('doublearrow', 'LineWidth', 0.5);
anArrow.Parent = gca;
anArrow.Position = [18, -37.4, 0, 32.9];
text(18.5, -20, sprintf('$d_2$'), 'horizontalalignment', 'left');

% annotation('textarrow',[],y,'String',' Growth ','FontSize',13,'Linewidth',2)
hold off;
xlabel('Time [s]'); ylabel('Displacement $d_e$ [$\mu$m]');



% #+name: tab:test_apa_measured_stiffnesses
% #+caption: Measured stiffnesses (in $N/\mu m$)
% #+attr_latex: :environment tabularx :width 0.2\linewidth :align ccc
% #+attr_latex: :center t :booktabs t :float t
% #+RESULTS:
% | APA | $k_1$ | $k_2$ |
% |-----+-------+-------|
% |   1 |  1.68 |   1.9 |
% |   2 |  1.69 |   1.9 |
% |   4 |   1.7 |  1.91 |
% |   5 |   1.7 |  1.93 |
% |   6 |   1.7 |  1.92 |
% |   8 |  1.73 |  1.98 |

% The stiffness can also be computed using equation eqref:eq:test_apa_res_freq by knowing the main vertical resonance frequency $\omega_z \approx 95\,\text{Hz}$ (estimated by the dynamical measurements shown in section ref:ssec:test_apa_meas_dynamics) and the suspended mass $m_{\text{sus}} = 5.7\,\text{kg}$.

% \begin{equation} \label{eq:test_apa_res_freq}
% \omega_z = \sqrt{\frac{k}{m_{\text{sus}}}}
% \end{equation}

% The obtain stiffness is $k \approx 2\,N/\mu m$ which is close to the values found in the documentation and by the "static deflection" method.


% However, changes in the electrical impedance connected to the piezoelectric stacks impacts the mechanical compliance (or stiffness) of the piezoelectric stack [[cite:&reza06_piezoel_trans_vibrat_contr_dampin chap. 2]].

% To estimate this effect, the stiffness of the APA if measured using the "static deflection" method in two cases:
% - $k_{\text{os}}$: piezoelectric stacks left unconnected (or connect to the high impedance ADC)
% - $k_{\text{sc}}$: piezoelectric stacks short circuited (or connected to the voltage amplifier with small output impedance)

% The open-circuit stiffness is estimated at $k_{\text{oc}} \approx 2.3\,N/\mu m$ and the closed-circuit stiffness $k_{\text{sc}} \approx 1.7\,N/\mu m$.


%% Load Data
add_mass_oc = load('frf_data_1_add_mass_open_circuit.mat',   't', 'de');
add_mass_cc = load('frf_data_1_add_mass_closed_circuit.mat', '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));

%% Estimation of the stiffness in Open Circuit and Closed-Circuit
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_sc = 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)));

% Dynamics
% <<ssec:test_apa_meas_dynamics>>

% In this section, the dynamics of the system from the excitation voltage $u$ to encoder measured displacement $d_e$  and to the force sensor voltage $V_s$ is identified.


%% Identification using sweep sine (low frequency)
load('frf_data_sweep.mat');
load('frf_data_noise_hf.mat');

%% Sampling Frequency
Ts = 1e-4; % Sampling Time [s]
Fs = 1/Ts; % Sampling Frequency [Hz]

%% "Hanning" windows used for the spectral analysis:
Nfft = floor(2/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

%% Separation of frequencies: low freqs using sweep sine, and high freq using noise
% Only used to have the frequency vector "f"
[~, f] = tfestimate(apa_sweep{1}.u, apa_sweep{1}.de, win, Noverlap, Nfft, 1/Ts);
i_lf = f <= 350;
i_hf = f >  350;

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

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

%% Save the identified dynamics for further analysis
save('mat/meas_apa_frf.mat', 'f', 'Ts', 'enc_frf', 'iff_frf', 'apa_nums');



% The obtained transfer functions for the 6 APA between the excitation voltage $u$ and the encoder displacement $d_e$ are shown in Figure ref:fig:test_apa_frf_encoder.
% The obtained transfer functions are close to a mass-spring-damper system.
% The following can be observed:
% - A "stiffness line" indicating a static gain equal to $\approx -17\,\mu m/V$.
%   The minus sign comes from the fact that an increase in voltage stretches the piezoelectric stack that then reduces the height of the APA
% - A lightly damped resonance at $95\,\text{Hz}$
% - A "mass line" up to $\approx 800\,\text{Hz}$, above which some resonances appear. These additional resonances might be coming from the limited stiffness of the encoder support or from the limited compliance of the APA support.


%% Plot the FRF from u to de
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', '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/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(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]);



% #+name: fig:test_apa_frf_encoder
% #+caption: Estimated Frequency Response Function from generated voltage $u$ to the encoder displacement $d_e$ for the 6 APA300ML
% #+RESULTS:
% [[file:figs/test_apa_frf_encoder.png]]

% The dynamics from $u$ to the measured voltage across the sensor stack $V_s$ is also identified and shown in Figure ref:fig:test_apa_frf_force.

% A lightly damped resonance is observed at $95\,\text{Hz}$ and a lightly damped anti-resonance at $41\,\text{Hz}$.
% No additional resonances is present up to at least $2\,\text{kHz}$ indicating at Integral Force Feedback can be applied without stability issues from high frequency flexible modes.

% As illustrated by the Root Locus, the poles of the closed-loop system converges to the zeros of the open-loop plant.
% Suppose that a controller with a very high gain is implemented such that the voltage $V_s$ across the sensor stack is zero.
% In that case, because of the very high controller gain, no stress and strain is present on the sensor stack (and on the actuator stacks are well, as they are both in series).
% Such closed-loop system would therefore virtually corresponds to a system for which the piezoelectric stacks have been removed and just the mechanical shell is kept.
% From this analysis, the axial stiffness of the shell can be estimated to be $k_{\text{shell}} = 5.7 \cdot (2\pi \cdot 41)^2 = 0.38\,N/\mu m$.
% # TODO - Compare with FEM result

% Such reasoning can lead to very interesting insight into the system just from an open-loop identification.


%% Plot the FRF from u to Vs
figure;
tiledlayout(2, 1, 'TileSpacing', 'Compact', '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/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(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]);

% Effect of the resistor on the IFF Plant
% <<ssec:test_apa_resistance_sensor_stack>>

% A resistor $R \approx 80.6\,k\Omega$ is added in parallel with the sensor stack which has the effect to form a high pass filter with the capacitance of the stack.

% As explain before, this is done for two reasons:
% 1. Limit the voltage offset due to the input bias current of the ADC
% 2. Limit the low frequency gain

% The (low frequency) transfer function from $u$ to $V_s$ with and without this resistor have been measured and are compared in Figure ref:fig:test_apa_effect_resistance.
% It is confirmed that the added resistor as the effect of adding an high pass filter with a cut-off frequency of $\approx 0.35\,\text{Hz}$.


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

%% Large Hanning window for good low frequency estimate
Nfft = floor(50/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

%% Compute the transfer functions from u to Vs
[frf_wo_k, f] = tfestimate(wo_k.u, wo_k.Vs, win, Noverlap, Nfft, 1/Ts);
[frf_wi_k, ~] = tfestimate(wi_k.u, wi_k.Vs, win, Noverlap, Nfft, 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', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(frf_wo_k), 'DisplayName', 'Without $R$');
plot(f, abs(frf_wi_k), 'DisplayName', 'With $R$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/u$ [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));
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]);

% Integral Force Feedback
% <<ssec:test_apa_iff_locus>>

% This test bench can also be used to estimate the damping added by the implementation of an Integral Force Feedback strategy.


%% Load identification Data
data = load("2023-03-17_11-28_iff_plant.mat");

%% Spectral Analysis setup
Ts = 1e-4; % Sampling Time [s]
Nfft = floor(5/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

%% Compute the transfer function from applied force to measured rotation
[G_iff, f]  = tfestimate(data.id_plant, data.Vs, win, Noverlap, Nfft, 1/Ts);



% First, the transfer function eqref:eq:test_apa_iff_manual_fit is manually tuned to match the identified dynamics from generated voltage $u$ to the measured sensor stack voltage $V_s$ in Section ref:ssec:test_apa_meas_dynamics.

% The obtained parameter values are $\omega_{\textsc{hpf}} = 0.4\, \text{Hz}$, $\omega_{z} = 42.7\, \text{Hz}$, $\xi_{z} = 0.4\,\%$, $\omega_{p} = 95.2\, \text{Hz}$, $\xi_{p} = 2\,\%$ and $g_0 = 0.64$.

% \begin{equation} \label{eq:test_apa_iff_manual_fit}
% G_{\textsc{iff},m}(s) = g_0 \cdot \frac{1 + 2 \xi_z \frac{s}{\omega_z} + \frac{s^2}{\omega_z^2}}{1 + 2 \xi_p \frac{s}{\omega_p} + \frac{s^2}{\omega_p^2}} \cdot \frac{s}{\omega_{\textsc{hpf}} + s}
% \end{equation}

% The comparison between the identified plant and the manually tuned transfer function is done in Figure ref:fig:test_apa_iff_plant_comp_manual_fit.


%% Basic manually tuned model
w0z = 2*pi*42.7; % Zero frequency
xiz = 0.004; % Zero damping

w0p = 2*pi*95.2; % Pole frequency
xip = 0.02; % Pole damping

G_iff_model = exp(-2*s*Ts)*0.64*(1 + 2*xiz/w0z*s + s^2/w0z^2)/(1 + 2*xip/w0p*s + s^2/w0p^2)*(s/(s+2*pi*0.4));

%% Identified IFF plant and manually tuned model of the plant
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(G_iff), 'color', colors(2,:), 'DisplayName', 'Identified plant')
plot(f, abs(squeeze(freqresp(G_iff_model, f, 'Hz'))), 'k--', 'DisplayName', 'Manual fit')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/u$ [V/V]'); set(gca, 'XTickLabel',[]);
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(G_iff), 'color', colors(2,:));
plot(f, 180/pi*angle(squeeze(freqresp(G_iff_model, 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([-90, 180])

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



% #+name: fig:test_apa_iff_plant_comp_manual_fit
% #+caption: Identified IFF plant and manually tuned model of the plant (a time delay of $200\,\mu s$ is added to the model of the plant to better match the identified phase)
% #+RESULTS:
% [[file:figs/test_apa_iff_plant_comp_manual_fit.png]]

% The implemented Integral Force Feedback Controller transfer function is shown in equation eqref:eq:test_apa_Kiff_formula.
% It contains an high pass filter (cut-off frequency of $2\,\text{Hz}$) to limit the low frequency gain, a low pass filter to add integral action above $20\,\text{Hz}$, a second low pass filter to add robustness to high frequency resonances and a tunable gain $g$.

% \begin{equation} \label{eq:test_apa_Kiff_formula}
% K_{\textsc{iff}}(s) = -10 \cdot g \cdot \frac{s}{s + 2\pi \cdot 2} \cdot \frac{1}{1 + 2\pi \cdot 20} \cdot \frac{1}{s + 2\pi\cdot 2000}
% \end{equation}


%% Integral Force Feedback Controller
K_iff = -10*(1/(s + 2*pi*20)) * ... % LPF: provides integral action above 20Hz
            (s/(s + 2*pi*2))  * ... % HPF: limit low frequency gain
            (1/(1 + s/2/pi/2e3));   % LPF: more robust to high frequency resonances



% To estimate how the dynamics of the APA changes when the Integral Force Feedback controller is implemented, the test bench shown in Figure ref:fig:test_apa_iff_schematic is used.
% The transfer function from the "damped" plant input $u\prime$ to the encoder displacement $d_e$ is identified for several IFF controller gains $g$.

% #+name: fig:test_apa_iff_schematic
% #+caption: Figure caption
% [[file:figs/test_apa_iff_schematic.png]]


%% Load Data
data = load("2023-03-17_14-10_damped_plants_new.mat");

%% Spectral Analysis setup
Ts = 1e-4; % Sampling Time [s]
Nfft = floor(1/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

%% Get the frequency vector
[~, f]  = tfestimate(data.data(1).id_plant(1:end), data.data(1).dL(1:end), win, Noverlap, Nfft, 1/Ts);

%% Gains used for analysis are between 1 and 50
i_kept = [5:10]

%% Identify the damped plant from u' to de for different IFF gains
G_dL_frf = {zeros(1,length(i_kept))};

for i = 1:length(i_kept)
    [G_dL, ~]  = tfestimate(data.data(i_kept(i)).id_plant(1:end), data.data(i_kept(i)).dL(1:end), win, Noverlap, Nfft, 1/Ts);
    G_dL_frf(i) = {G_dL};
end



% The identified dynamics are then fitted by second order transfer functions.
% The comparison between the identified damped dynamics and the fitted second order transfer functions is done in Figure ref:fig:test_apa_identified_damped_plants for different gains $g$.
% It is clear that large amount of damping is added when the gain is increased and that the frequency of the pole is shifted to lower frequencies.


%% Fit the data with 2nd order transfer function using vectfit3
opts = struct();

opts.stable = 1;    % Enforce stable poles
opts.asymp = 1;     % Force D matrix to be null
opts.relax = 1;     % Use vector fitting with relaxed non-triviality constraint
opts.skip_pole = 0; % Do NOT skip pole identification
opts.skip_res = 0;  % Do NOT skip identification of residues (C,D,E)
opts.cmplx_ss = 0;  % Create real state space model with block diagonal A

opts.spy1 = 0;      % No plotting for first stage of vector fitting
opts.spy2 = 0;      % Create magnitude plot for fitting of f(s)

Niter = 100; % Number of iteration.
N = 2; % Order of approximation
poles = [-25 - 1i*60, -25 + 1i*60]; % First get for the pole location

G_dL_id  = {zeros(1,length(i_kept))};

% Identification just between two frequencies
f_keep = (f>5 & f<200);

for i = 1:length(i_kept)
    %% Estimate resonance frequency and damping
    for iter = 1:Niter
        [G_est, poles, ~, frf_est] = vectfit3(G_dL_frf{i}(f_keep).', 1i*2*pi*f(f_keep)', poles, ones(size(f(f_keep)))', opts);
    end
    G_dL_id(i) = {ss(G_est.A, G_est.B, G_est.C, G_est.D)};
end

%% Identified dynamics from u' to de for different IFF gains
figure;
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
for i = 1:length(i_kept)
    plot(f, abs(G_dL_frf{i}), 'color', [colors(i,:), 1], 'DisplayName', sprintf('g = %.0f', data.gains(i_kept(i))))
    plot(f, abs(squeeze(freqresp(G_dL_id{i}, f, 'Hz'))), '--', 'color', [colors(i,:), 1], 'HandleVisibility', 'off')
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude $d_L/V_a$ [m/V]');
xlim([10, 1e3]);
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);



% #+name: fig:test_apa_identified_damped_plants
% #+caption: Identified dynamics (solid lines) and fitted transfer functions (dashed lines) from $u\prime$ to $d_e$ for different IFF gains
% #+RESULTS:
% [[file:figs/test_apa_identified_damped_plants.png]]

% The evolution of the pole in the complex plane as a function of the controller gain $g$ (i.e. the "root locus") is computed:
% - using the IFF plant model eqref:eq:test_apa_iff_manual_fit and the implemented controller eqref:eq:test_apa_Kiff_formula
% - from the fitted transfer functions of the damped plants experimentally identified for several controller gains

% The two obtained root loci are compared in Figure ref:fig:test_apa_iff_root_locus and are in good agreement considering that the damped plants were only fitted using a second order transfer function.


%% Root Locus of the APA300ML with Integral Force Feedback
% Comparison between the computed root locus from the plant model and the root locus estimated from the damped plant pole identification
gains = logspace(-1, 3, 1000);

figure;
hold on;
G_iff_poles = pole(pade(G_iff_model));
i = imag(G_iff_poles) > 100; % Only keep relevant poles
plot(real(G_iff_poles(i)),  imag(G_iff_poles(i)),  'kx', ...
    'DisplayName', '$g = 0$');
G_iff_zeros = tzero(G_iff_model);
i = imag(G_iff_zeros) > 100; % Only keep relevant zeros
plot(real(G_iff_zeros(i)), imag(G_iff_zeros(i)), 'ko', ...
    'HandleVisibility', 'off');

for g = gains
    clpoles = pole(feedback(pade(G_iff_model), g*K_iff, 1));
    i = imag(clpoles) > 100; % Only keep relevant poles
    plot(real(clpoles(i)), imag(clpoles(i)), 'k.', ...
        'HandleVisibility', 'off');
end

for i = 1:length(i_kept)
    plot(real(pole(G_dL_id{i})), imag(pole(G_dL_id{i})), 'x', 'color', [colors(i,:), 1], 'DisplayName', sprintf('g = %1.f', data.gains(i_kept(i))));
end
ylim([0, 700]);
xlim([-600,100]);
xlabel('Real Part')
ylabel('Imaginary Part')
axis square
legend('location', 'northwest');