%% 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

%% Colors for the figures
colors = colororder;

%% Uniaxial Simscape model name
mdl = 'nass_uniaxial_model';

%% Load the micro-station parameters
load('uniaxial_micro_station_parameters.mat')

%% Load the PSD of disturbances
load('uniaxial_disturbance_psd.mat', 'f', 'psd_ft', 'psd_xf');

%% Load Active Damping Controller
load('uniaxial_active_damping_controllers.mat', 'K_iff_vc', 'K_iff_md', 'K_iff_pz', ...
                                                'K_rdc_vc', 'K_rdc_md', 'K_rdc_pz', ...
                                                'K_dvf_vc', 'K_dvf_md', 'K_dvf_pz');

%% Load High Authority Controllers
load('uniaxial_high_authority_controllers.mat', 'K_hac_vc', 'K_hac_md', 'K_hac_pz');

%% Frequency Vector [Hz]
freqs = logspace(0, 3, 1000);

% Impact on the plant dynamics
% <<ssec:uniaxial_payload_dynamics_effect_dynamics>>

% To study the impact of sample dynamics, the following sample configurations are considered:
% - rigid sample, corresponding to Figure ref:fig:uniaxial_paylaod_dynamics_rigid_schematic
% - two flexible samples with a resonance at $\omega_s = 200\,\text{Hz}$ and at $\omega_s = 20\,\text{Hz}$ respectively, corresponding to Figure ref:fig:uniaxial_paylaod_dynamics_schematic
% - for all cases, two sample masses are considered: $m_s = 1\,\text{kg}$ and $m_s = 50\,\text{kg}$

% The transfer functions from the nano-hexapod force to the motion of the nano-hexapod top platform are computed for all the above configurations and are compared for a soft Nano-Hexapod $k_n = 0.01\,N/\mu m$ in Figure ref:fig:uniaxial_payload_dynamics_soft_nano_hexapod.

% It can be seen that the mode of the sample adds an anti-resonance followed by a resonance (zero/pole pattern).
% The frequency of the anti-resonance corresponds to the "free" resonance of the sample $\omega_s = \sqrt{k_s/m_s}$.
% The flexibility of the sample also changes the high frequency gain (the mass line is shifted from $\frac{1}{(m_n + m_s)s^2}$ to $\frac{1}{m_ns^2}$).


%% Soft Nano-Hexapod
% Light payload mass
mn = 15; % Nano-Hexapod mass [kg]
ms = 1; % Sample Mass [kg]
kn = 1e4; % Nano-Hexapod (soft) Stiffness [N/m]
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]

% Rigid sample
G_vc_rigid_light = 1/((mn + ms)*s^2 + cn*s + kn);

% Soft Sample
ws = 2*pi*20;
ks = ms * ws^2;
cs = 2*0.01*sqrt(ms*ks);
G_vc_soft_light = (ms*s^2 + cs*s + ks)/((mn*s^2 + cn*s + kn)*(ms*s^2 + cs*s + ks) + ms*s^2*(cs*s + ks));

% Stiff Sample
ws = 2*pi*200;
ks = ms * ws^2;
cs = 2*0.01*sqrt(ms*ks);
G_vc_stiff_light = (ms*s^2 + cs*s + ks)/((mn*s^2 + cn*s + kn)*(ms*s^2 + cs*s + ks) + ms*s^2*(cs*s + ks));

% Heavy payload mass
mn = 15; % Nano-Hexapod mass [kg]
ms = 50; % Sample Mass [kg]
kn = 1e4; % Nano-Hexapod (soft) Stiffness [N/m]
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]

% Rigid sample
G_vc_rigid_heavy = 1/((mn + ms)*s^2 + cn*s + kn);

% Soft Sample
ws = 2*pi*20;
ks = ms * ws^2;
cs = 2*0.01*sqrt(ms*ks);
G_vc_soft_heavy = (ms*s^2 + cs*s + ks)/((mn*s^2 + cn*s + kn)*(ms*s^2 + cs*s + ks) + ms*s^2*(cs*s + ks));

% Stiff Sample
ws = 2*pi*200;
ks = ms * ws^2;
cs = 2*0.01*sqrt(ms*ks);
G_vc_stiff_heavy = (ms*s^2 + cs*s + ks)/((mn*s^2 + cn*s + kn)*(ms*s^2 + cs*s + ks) + ms*s^2*(cs*s + ks));

%% Effect of the payload dynamics on the soft Nano-Hexapod. Light sample on the right, and heavy sample on the left
figure;
tiledlayout(3, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_vc_rigid_light, freqs, 'Hz'))), '-',  'color', colors(1,:), 'DisplayName', 'Rigid sample');
plot(freqs, abs(squeeze(freqresp(G_vc_stiff_light, freqs, 'Hz'))), '-', 'color', colors(2,:), 'DisplayName', '$\omega_s = 200\,Hz$');
plot(freqs, abs(squeeze(freqresp(G_vc_soft_light, freqs, 'Hz'))), '-', 'color', colors(3,:), 'DisplayName', '$\omega_s = 20\,Hz$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-10, 1e-2])
title('$k_n = 0.01\,N/\mu m$, $m_s = 1\,kg$');

ax2 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_vc_rigid_heavy, freqs, 'Hz'))), '-',  'color', colors(1,:), 'DisplayName', 'Rigid sample');
plot(freqs, abs(squeeze(freqresp(G_vc_stiff_heavy, freqs, 'Hz'))), '-', 'color', colors(2,:), 'DisplayName', '$\omega_s = 200\,Hz$');
plot(freqs, abs(squeeze(freqresp(G_vc_soft_heavy, freqs, 'Hz'))), '-', 'color', colors(3,:), 'DisplayName', '$\omega_s = 20\,Hz$');
plot(freqs, abs(squeeze(freqresp(1/(mn*s^2), freqs, 'Hz'))), '-', 'color', [0,0,0,0.5], 'DisplayName', '$\frac{1}{m_n s^2}$');
plot(freqs, abs(squeeze(freqresp(1/((mn + ms)*s^2), freqs, 'Hz'))), '--', 'color', [0,0,0,0.5], 'DisplayName', '$\frac{1}{(m_n + m_s) s^2}$');
text(2.2, 3e-3, '$\omega_n = \sqrt{\frac{k_n}{m_n + m_s}}$', 'horizontalalignment', 'left');
text(20, 1e-8, '$\omega_s = \sqrt{\frac{k_s}{m_s}}$', 'horizontalalignment', 'center');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
title('$k_n = 0.01\,N/\mu m$, $m_s = 50\,kg$');
ldg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
ldg.ItemTokenSize = [20, 1];
ylim([1e-10, 1e-2])

ax1b = nexttile();
hold on;
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_rigid_light, freqs, 'Hz')))), '-',  'color', colors(1,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_stiff_light, freqs, 'Hz')))), '-', 'color', colors(2,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_soft_light, freqs, 'Hz')))), '-', 'color', colors(3,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
xticks([1e0, 1e1, 1e2]);
yticks(-360:90:360);
ylim([-200, 20]);

ax2b = nexttile();
hold on;
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_rigid_heavy, freqs, 'Hz')))), '-',  'color', colors(1,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_stiff_heavy, freqs, 'Hz')))), '-', 'color', colors(2,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_soft_heavy, freqs, 'Hz')))), '-', 'color', colors(3,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
xticks([1e0, 1e1, 1e2]);
yticks(-360:90:360);
ylim([-200, 20]);

linkaxes([ax1,ax1b],'x');
xlim([1, 1000]);



% #+name: fig:uniaxial_payload_dynamics_soft_nano_hexapod
% #+caption: Effect of the payload dynamics on the soft Nano-Hexapod. Light sample on the right, and heavy sample on the left
% #+RESULTS:
% [[file:figs/uniaxial_payload_dynamics_soft_nano_hexapod.png]]

% The same transfer functions are now compared when using a stiff nano-hexapod ($k_n = 100\,N/\mu m$) in Figure ref:fig:uniaxial_payload_dynamics_stiff_nano_hexapod.
% In that case, the sample's resonance $\omega_n$ is smaller than the nano-hexapod resonance $\omega_n$.
% This changes the zero/pole pattern to a pole/zero pattern (the frequency of the zero still being equal to $\omega_s$).
% Even tough the added sample's flexibility still changes the high frequency mass line from $\frac{1}{(m_n + m_s)s^2}$ to $\frac{1}{m_ns^2}$, the overall dynamics is much less impacted, even if the sample mass is high (see yellow curve in Figure ref:fig:uniaxial_payload_dynamics_stiff_nano_hexapod, right).


%% Stiff Nano-Hexapod
% Light payload mass
mn = 15; % Nano-Hexapod mass [kg]
ms = 1; % Sample Mass [kg]
kn = 1e8; % Nano-Hexapod (soft) Stiffness [N/m]
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]

% Rigid sample
G_pz_rigid_light = 1/((mn + ms)*s^2 + cn*s + kn);

% Soft Sample
ws = 2*pi*20;
ks = ms * ws^2;
cs = 2*0.01*sqrt(ms*ks);
G_pz_soft_light = (ms*s^2 + cs*s + ks)/((mn*s^2 + cn*s + kn)*(ms*s^2 + cs*s + ks) + ms*s^2*(cs*s + ks));

% Stiff Sample
ws = 2*pi*200;
ks = ms * ws^2;
cs = 2*0.01*sqrt(ms*ks);
G_pz_stiff_light = (ms*s^2 + cs*s + ks)/((mn*s^2 + cn*s + kn)*(ms*s^2 + cs*s + ks) + ms*s^2*(cs*s + ks));

% Heavy payload mass
mn = 15; % Nano-Hexapod mass [kg]
ms = 50; % Sample Mass [kg]
kn = 1e8; % Nano-Hexapod (soft) Stiffness [N/m]
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]

% Rigid sample
G_pz_rigid_heavy = 1/((mn + ms)*s^2 + cn*s + kn);

% Soft Sample
ws = 2*pi*20;
ks = ms * ws^2;
cs = 2*0.01*sqrt(ms*ks);
G_pz_soft_heavy = (ms*s^2 + cs*s + ks)/((mn*s^2 + cn*s + kn)*(ms*s^2 + cs*s + ks) + ms*s^2*(cs*s + ks));

% Stiff Sample
ws = 2*pi*200;
ks = ms * ws^2;
cs = 2*0.01*sqrt(ms*ks);
G_pz_stiff_heavy = (ms*s^2 + cs*s + ks)/((mn*s^2 + cn*s + kn)*(ms*s^2 + cs*s + ks) + ms*s^2*(cs*s + ks));

%% Effect of the payload dynamics on the stiff Nano-Hexapod. Light sample on the right, and heavy sample on the left
figure;
tiledlayout(3, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_pz_rigid_light, freqs, 'Hz'))), '-',  'color', colors(1,:));
plot(freqs, abs(squeeze(freqresp(G_pz_stiff_light, freqs, 'Hz'))), '-', 'color', colors(2,:));
plot(freqs, abs(squeeze(freqresp(G_pz_soft_light, freqs, 'Hz'))), '-', 'color', colors(3,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-10, 1e-2])
title('$k_n = 100\,N/\mu m$, $m_s = 1\,kg$');

ax2 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_pz_rigid_heavy, freqs, 'Hz'))), '-',  'color', colors(1,:), 'DisplayName', 'Rigid sample');
plot(freqs, abs(squeeze(freqresp(G_pz_stiff_heavy, freqs, 'Hz'))), '-', 'color', colors(2,:), 'DisplayName', 'Stiff sample: $\omega_s = 200\,Hz$');
plot(freqs, abs(squeeze(freqresp(G_pz_soft_heavy, freqs, 'Hz'))), '-', 'color', colors(3,:), 'DisplayName', 'Soft sample: $\omega_s = 20\,Hz$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
title('$k_n = 100\,N/\mu m$, $m_s = 50\,kg$');
ylim([1e-10, 1e-2])
ldg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
ldg.ItemTokenSize = [20, 1];

ax1b = nexttile();
hold on;
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_rigid_light, freqs, 'Hz')))), '-',  'color', colors(1,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_stiff_light, freqs, 'Hz')))), '-', 'color', colors(2,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_soft_light, freqs, 'Hz')))), '-', 'color', colors(3,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
xticks([1e0, 1e1, 1e2]);
yticks(-360:90:360);
ylim([-200, 20]);

ax2b = nexttile();
hold on;
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_rigid_heavy, freqs, 'Hz')))), '-',  'color', colors(1,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_stiff_heavy, freqs, 'Hz')))), '-', 'color', colors(2,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_soft_heavy, freqs, 'Hz')))), '-', 'color', colors(3,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
xticks([1e0, 1e1, 1e2]);
yticks(-360:90:360);
ylim([-200, 20]);

linkaxes([ax1,ax1b],'x');
xlim([1, 1000]);



% #+name: fig:uniaxial_sample_flexibility_control
% #+caption: Uniaxial model considering a flexibility between the nano-hexapod top platform and the sample. In that case the measured and controlled distance $d$ is different from the distance $y$ that is wish to be controlled
% #+RESULTS:
% [[file:figs/uniaxial_sample_flexibility_control.png]]

% After the system dynamics extracted from the model, IFF is applied using the same gains as the ones used in Section ref:sec:uniaxial_active_damping.
% Thanks to the collocation between the nano-hexapod and the force sensor used for IFF, the damped plants are still stable and similar damping values are obtained than when considering a rigid sample.

% The High Authority Controllers used in Section ref:sec:uniaxial_position_control are then implemented on the damped plants.
% The obtained closed-loop systems are stable, indicating good robustness.

% Finally, closed-loop noise budgeting is computed for the obtained the closed-loop system and the cumulative amplitude spectrum of $d$ and $y$ are shown in Figure ref:fig:uniaxial_sample_flexibility_noise_budget_y.
% The cumulative amplitude spectrum of the measured distance $d$ (Figure ref:fig:uniaxial_sample_flexibility_noise_budget_d) shows that the added flexibility at the sample location have very little effect on the control performance.
% However, the cumulative amplitude spectrum of the distance $y$ (Figure ref:fig:uniaxial_sample_flexibility_noise_budget_y) shows that the stability of $y$ is degraded when the sample flexibility is considered and is degraded as $\omega_s$ is lowered.

% What happens is that above $\omega_s$, even though the motion $d$ can be controlled perfectly, the sample's mass is "isolated" from the motion of the nano-hexapod and the control on $y$ is not effective.


%% Nano-Hexpod model
model_config = struct();
model_config.controller = "open_loop";
mn = 15; % Nano-Hexapod mass [kg]
ms = 1; % Sample Mass [kg]

%% Identification
clear io; io_i = 1;
io(io_i) = linio([mdl, '/controller'],       1, 'openinput');  io_i = io_i + 1; % Actuator Force
io(io_i) = linio([mdl, '/micro_station/xf'], 1, 'openinput');  io_i = io_i + 1; % Floor Motion
io(io_i) = linio([mdl, '/micro_station/ft'], 1, 'openinput');  io_i = io_i + 1; % Stage vibrations
io(io_i) = linio([mdl, '/fs'],               1, 'openinput');  io_i = io_i + 1; % Direct sample forces
io(io_i) = linio([mdl, '/dL'],               1, 'openoutput'); io_i = io_i + 1; % Relative Motion Sensor
io(io_i) = linio([mdl, '/fm'],               1, 'openoutput'); io_i = io_i + 1; % Force Sensor
io(io_i) = linio([mdl, '/vn'] ,              1, 'openoutput'); io_i = io_i + 1; % Geophone
io(io_i) = linio([mdl, '/d'] ,               1, 'openoutput'); io_i = io_i + 1; % Metrology Output
io(io_i) = linio([mdl, '/y'] ,               1, 'openoutput'); io_i = io_i + 1; % Sample's position

%% Soft Nano-Hexapod
% Light payload mass
kn = 1e4; % Nano-Hexapod (soft) Stiffness [N/m]
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]

% Rigid Sample
model_config.nhexa = "1dof";
G_vc_light_rigid = linearize(mdl, io, 0.0);
G_vc_light_rigid.InputName = {'f', 'xf', 'ft', 'fs'};
G_vc_light_rigid.OutputName = {'dL', 'fm', 'vn', 'd', 'y'};

% Soft Sample
model_config.nhexa = "2dof";
ws = 2*pi*20;
ks = ms * ws^2;
cs = 2*0.01*sqrt(ms*ks);

G_vc_light_soft = linearize(mdl, io, 0.0);
G_vc_light_soft.InputName = {'f', 'xf', 'ft', 'fs'};
G_vc_light_soft.OutputName = {'dL', 'fm', 'vn', 'd', 'y'};

% Rigid Sample
model_config.nhexa = "2dof";
ws = 2*pi*200;
ks = ms * ws^2;
cs = 2*0.01*sqrt(ms*ks);

G_vc_light_stiff = linearize(mdl, io, 0.0);
G_vc_light_stiff.InputName = {'f', 'xf', 'ft', 'fs'};
G_vc_light_stiff.OutputName = {'dL', 'fm', 'vn', 'd', 'y'};

%% Stiff Nano-Hexapod
% Light payload mass
kn = 1e8; % Nano-Hexapod (soft) Stiffness [N/m]
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]

% Rigid Sample
model_config.nhexa = "1dof";
G_pz_light_rigid = linearize(mdl, io, 0.0);
G_pz_light_rigid.InputName = {'f', 'xf', 'ft', 'fs'};
G_pz_light_rigid.OutputName = {'dL', 'fm', 'vn', 'd', 'y'};

% Soft Sample
model_config.nhexa = "2dof";
ws = 2*pi*20;
ks = ms * ws^2;
cs = 2*0.01*sqrt(ms*ks);

G_pz_light_soft = linearize(mdl, io, 0.0);
G_pz_light_soft.InputName = {'f', 'xf', 'ft', 'fs'};
G_pz_light_soft.OutputName = {'dL', 'fm', 'vn', 'd', 'y'};

% Rigid Sample
model_config.nhexa = "2dof";
ws = 2*pi*200;
ks = ms * ws^2;
cs = 2*0.01*sqrt(ms*ks);

G_pz_light_stiff = linearize(mdl, io, 0.0);
G_pz_light_stiff.InputName = {'f', 'xf', 'ft', 'fs'};
G_pz_light_stiff.OutputName = {'dL', 'fm', 'vn', 'd', 'y'};

%% Apply IFF and verify stability
% Soft Nano-Hexapod
G_iff_vc_light_rigid = feedback(G_vc_light_rigid, K_iff_vc, 'name', +1);
G_iff_vc_light_soft  = feedback(G_vc_light_soft , K_iff_vc, 'name', +1);
G_iff_vc_light_stiff = feedback(G_vc_light_stiff, K_iff_vc, 'name', +1);

isstable(G_iff_vc_light_rigid)
isstable(G_iff_vc_light_soft)
isstable(G_iff_vc_light_stiff)

% Stiff Nano-Hexapod
G_iff_pz_light_rigid = feedback(G_pz_light_rigid, K_iff_pz, 'name', +1);
G_iff_pz_light_soft  = feedback(G_pz_light_soft , K_iff_pz, 'name', +1);
G_iff_pz_light_stiff = feedback(G_pz_light_stiff, K_iff_pz, 'name', +1);

isstable(G_iff_pz_light_rigid)
isstable(G_iff_pz_light_soft)
isstable(G_iff_pz_light_stiff)

%% Compute closed-loop plants and verify stability
% Soft Nano-Hexapod
G_hac_iff_vc_light_rigid = feedback(G_iff_vc_light_rigid, K_hac_vc, 'name', -1);
G_hac_iff_vc_light_soft  = feedback(G_iff_vc_light_soft , K_hac_vc, 'name', -1);
G_hac_iff_vc_light_stiff = feedback(G_iff_vc_light_stiff, K_hac_vc, 'name', -1);

isstable(G_hac_iff_vc_light_rigid)
isstable(G_hac_iff_vc_light_soft)
isstable(G_hac_iff_vc_light_stiff)

% Stiff Nano-Hexapod
G_hac_iff_pz_light_rigid = feedback(G_iff_pz_light_rigid, K_hac_pz, 'name', -1);
G_hac_iff_pz_light_soft  = feedback(G_iff_pz_light_soft , K_hac_pz, 'name', -1);
G_hac_iff_pz_light_stiff = feedback(G_iff_pz_light_stiff, K_hac_pz, 'name', -1);

isstable(G_hac_iff_pz_light_rigid)
isstable(G_hac_iff_pz_light_soft)
isstable(G_hac_iff_pz_light_stiff)

%% Cumulative Amplitude Spectrum of d - Effect of Sample's flexibility
figure;
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_vc_light_rigid('d', 'ft'), f, 'Hz'))).^2 + ...
                                     psd_xf.*abs(squeeze(freqresp(G_hac_iff_vc_light_rigid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
     'DisplayName', 'Rigid sample');
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_vc_light_stiff('d', 'ft'), f, 'Hz'))).^2 + ...
                                     psd_xf.*abs(squeeze(freqresp(G_hac_iff_vc_light_stiff('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
     'DisplayName', 'Stiff $\omega_s = 200\,$Hz');
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_vc_light_soft('d', 'ft'), f, 'Hz'))).^2 + ...
                                     psd_xf.*abs(squeeze(freqresp(G_hac_iff_vc_light_soft('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
     'DisplayName', 'Soft $\omega_s = 20\,$Hz');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xticks([1e0, 1e1, 1e2]);
ylabel('CAS of $d$ [m]'); xlabel('Frequency [Hz]');
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
title('$k_n = 0.01\,N/\mu m$');

ax2 = nexttile();
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_pz_light_rigid('d', 'ft'), f, 'Hz'))).^2 + ...
                                     psd_xf.*abs(squeeze(freqresp(G_hac_iff_pz_light_rigid('d', 'xf'), f, 'Hz'))).^2)))), '-');
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_pz_light_stiff('d', 'ft'), f, 'Hz'))).^2 + ...
                                     psd_xf.*abs(squeeze(freqresp(G_hac_iff_pz_light_stiff('d', 'xf'), f, 'Hz'))).^2)))), '-');
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_pz_light_soft('d', 'ft'), f, 'Hz'))).^2 + ...
                                     psd_xf.*abs(squeeze(freqresp(G_hac_iff_pz_light_soft('d', 'xf'), f, 'Hz'))).^2)))), '-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xticks([1e0, 1e1, 1e2]);
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
title('$k_n = 100\,N/\mu m$');
xlim([1, 500]);

linkaxes([ax1,ax2],'xy');
xlim([1, 500]);
ylim([2e-10, 2e-7])

%% Cumulative Amplitude Spectrum - Effect of Sample's flexibility
figure;
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_vc_light_rigid('y', 'ft'), f, 'Hz'))).^2 + ...
                                     psd_xf.*abs(squeeze(freqresp(G_hac_iff_vc_light_rigid('y', 'xf'), f, 'Hz'))).^2)))), '-', ...
     'DisplayName', 'Rigid sample');
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_vc_light_stiff('y', 'ft'), f, 'Hz'))).^2 + ...
                                     psd_xf.*abs(squeeze(freqresp(G_hac_iff_vc_light_stiff('y', 'xf'), f, 'Hz'))).^2)))), '-', ...
     'DisplayName', 'Stiff $\omega_s = 200\,$Hz');
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_vc_light_soft('y', 'ft'), f, 'Hz'))).^2 + ...
                                     psd_xf.*abs(squeeze(freqresp(G_hac_iff_vc_light_soft('y', 'xf'), f, 'Hz'))).^2)))), '-', ...
     'DisplayName', 'Soft $\omega_s = 20\,$Hz');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xticks([1e0, 1e1, 1e2]);
ylabel('CAS of $y$ [m]'); xlabel('Frequency [Hz]');
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
title('$k_n = 0.01\,N/\mu m$');

ax2 = nexttile();
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_pz_light_rigid('y', 'ft'), f, 'Hz'))).^2 + ...
                                     psd_xf.*abs(squeeze(freqresp(G_hac_iff_pz_light_rigid('y', 'xf'), f, 'Hz'))).^2)))), '-');
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_pz_light_stiff('y', 'ft'), f, 'Hz'))).^2 + ...
                                     psd_xf.*abs(squeeze(freqresp(G_hac_iff_pz_light_stiff('y', 'xf'), f, 'Hz'))).^2)))), '-');
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_pz_light_soft('y', 'ft'), f, 'Hz'))).^2 + ...
                                     psd_xf.*abs(squeeze(freqresp(G_hac_iff_pz_light_soft('y', 'xf'), f, 'Hz'))).^2)))), '-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xticks([1e0, 1e1, 1e2]);
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
title('$k_n = 100\,N/\mu m$');
xlim([1, 500]);

linkaxes([ax1,ax2],'xy');
xlim([1, 500]);
ylim([2e-10, 2e-7])