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

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

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

% Neglected support compliance

% Let's first neglect the limited compliance of the micro-station and use the uniaxial model show in Figure ref:fig:uniaxial_support_compliance_nano_hexapod_only.
% Let's choose a nano-hexapod mass (including the payload) of $20\,\text{kg}$ and three hexapod stiffnesses such that their resonance frequencies are at $\omega_{\nu} = 10\,\text{Hz}$, $\omega_{\nu} = 70\,\text{Hz}$ and $\omega_{\nu} = 400\,\text{Hz}$.

% The obtained transfer functions from $F$ to $L^\prime$ are shown in Figure ref:fig:uniaxial_effect_support_compliance_neglected.
% When neglecting the support compliance, large feedback bandwidth can be achieve for all three Nano-Hexapod.


%% Nano-Hexapod Parameters
m = 20; % Mass [kg]

% "Soft" Nano-Hexapod
k_soft = m*(2*pi*10)^2; % Stiffness [N/m]
c_soft = 0.1*2*sqrt(m*k_soft); % Damping [N/(m/s)]

% "Mid" Nano-Hexapod
k_mid = m*(2*pi*70)^2; % Stiffness [N/m]
c_mid = 0.1*2*sqrt(m*k_mid); % Damping [N/(m/s)]

% "Stiff" Nano-Hexapod
k_stiff = m*(2*pi*400)^2; % Stiffness [N/m]
c_stiff = 0.1*2*sqrt(m*k_stiff); % Damping [N/(m/s)]

%% Compute the transfer functions for considered nano-hexapods - From F to L'
% "Soft" Nano-Hexapod
G_soft_a = 1/(m*s^2 + c_soft*s + k_soft); % Transfer function from F to L'

% "Mid" Nano-Hexapod
G_mid_a = 1/(m*s^2 + c_mid*s + k_mid); % Transfer function from F to L'

% "Stiff" Nano-Hexapod
G_stiff_a = 1/(m*s^2 + c_stiff*s + k_stiff); % Transfer function from F to L'

%% Obtained transfer functions from F to L when neglecing support compliance
freqs = logspace(0, 3, 1000);

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

ax1 = nexttile();
hold on;
plot(freqs, abs(squeeze(freqresp(G_soft_a, freqs, 'Hz'))), '-', 'color', colors(1,:));
text(50, 5e-5, '$\omega_\nu =$ 10Hz', 'horizontalalignment', 'center');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude [m/N]');
xticks([1e0, 1e1, 1e2]);
title('"Soft" $\nu$-hexapod');

ax2 = nexttile();
hold on;
plot(freqs, abs(squeeze(freqresp(G_mid_a, freqs, 'Hz'))), '-', 'color', colors(1,:));
text(70, 3e-6, '$\omega_\nu =$ 70Hz', 'horizontalalignment', 'center');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
set(gca, 'YTickLabel',[]);
xticks([1e0, 1e1, 1e2]);
title('"Mid" $\nu$-hexapod');

ax3 = nexttile();
hold on;
plot(freqs, abs(squeeze(freqresp(G_stiff_a, freqs, 'Hz'))), '-', 'color', colors(1,:), ...
     'DisplayName', '$L^\prime/F$');
text(200, 8e-8, '$\omega_\nu =$ 400Hz', 'horizontalalignment', 'center');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'YTickLabel',[]);
legend('location', 'northeast');
xticks([1e0, 1e1, 1e2]);
title('"Stiff" $\nu$-hexapod');

linkaxes([ax1,ax2,ax3],'xy');
xlim([freqs(1), freqs(end)]);
ylim([1e-9, 1e-4]);

% Effect of support compliance on $L/F$

% Let's now add some support compliance and use the model shown in Figure ref:fig:uniaxial_support_compliance_test_system.
% The parameters of the support (i.e. $m^{\prime}$, $c^{\prime}$ and $k^{\prime}$) are chosen in such a way that the main vertical mode at $\omega_\mu = 70\,\text{Hz}$ seen on the micro-station is matched (Figure ref:fig:uniaxial_comp_frf_meas_model, yellow curve).


%% Parameters of the support compliance
w0h = 2*pi*70; % [rad/s]
xih = 0.1; % [-]

mh = 20; % [kg]
kh = mh*w0h^2;
ch = xih*2*sqrt(kh*mh);

%% Compute the transfer functions from F to L and from F to d for considered Nano-Hexapods
% "Soft" Nano-Hexapod
G_soft = (mh*s^2 + ch*s + kh)/(m*s^2*(c_soft*s + k_soft) + (m*s^2 + c_soft*s + k_soft)*(mh*s^2 + ch*s + kh)); % d/F
G_soft_r = (1 - m*s^2*G_soft)/(c_soft*s + k_soft); % L/F

% "Mid" Nano-Hexapod
G_mid = (mh*s^2 + ch*s + kh)/(m*s^2*(c_mid*s + k_mid) + (m*s^2 + c_mid*s + k_mid)*(mh*s^2 + ch*s + kh)); % d/F
G_mid_r = (1 - m*s^2*G_mid)/(c_mid*s + k_mid); % L/F

% "Stiff" Nano-Hexapod
G_stiff = (mh*s^2 + ch*s + kh)/(m*s^2*(c_stiff*s + k_stiff) + (m*s^2 + c_stiff*s + k_stiff)*(mh*s^2 + ch*s + kh)); % d/F
G_stiff_r = (1 - m*s^2*G_stiff)/(c_stiff*s + k_stiff); % L/F



% The transfer functions from $F$ to $L$ (i.e. control of the relative motion of the nano-hexapod) and from $L$ to $d$ (i.e. control of the position between the nano-hexapod and the fixed granite) can then be computed.

% When the relative displacement of the nano-hexapod $L$ is to be controlled (dynamics shown in Figure ref:fig:uniaxial_effect_support_compliance_dynamics), having a stiff nano-hexapod (i.e. with a suspension mode at higher frequency than the mode of the support) makes the dynamics less affected by the limited support compliance (Figure ref:fig:uniaxial_effect_support_compliance_dynamics, right).

% This is why it is very common to have stiff piezoelectric stages fixed at the very top of positioning stages.
% In such case, the control of the piezoelectric stage using its integrated metrology (typically capacitive sensors) is quite simple as the plant is not much affected by the dynamics of the support on which is it fixed.
% # Add references of such stations with piezo stages on top

% If a soft nano-hexapod is used, the support dynamics appears in the dynamics between $F$ and $L$ (see Figure ref:fig:uniaxial_effect_support_compliance_dynamics, left) which will impact the control robustness and performance.


%% Effect of the support compliance on the transfer functions from F to L and from F to d
figure;
freqs = logspace(0, 3, 1000);

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

ax1 = nexttile();
hold on;
plot(freqs, abs(squeeze(freqresp(G_soft_a, freqs, 'Hz'))), '-', 'color', colors(1,:));
plot(freqs, abs(squeeze(freqresp(G_soft_r, freqs, 'Hz'))), '-', 'color', colors(2,:));
loglog(10.^(0.3*cos(0:0.01:2*pi)+log10(60)), ...
       10.^(0.6*sin(0:0.01:2*pi)+log10(4e-7)), 'k--');
text(8, 3e-7, sprintf('Support\nDynamics'), 'horizontalalignment', 'center');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xticks([1e0, 1e1, 1e2]);
title('"Soft" $\nu$-hexapod');
ylabel('Magnitude [m/N]');


ax2 = nexttile();
hold on;
plot(freqs, abs(squeeze(freqresp(G_mid_a, freqs, 'Hz'))), '-', 'color', colors(1,:));
plot(freqs, abs(squeeze(freqresp(G_mid_r, freqs, 'Hz'))), '-', 'color', colors(2,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
xticks([1e0, 1e1, 1e2]);
title('"Mid" $\nu$-hexapod');
set(gca, 'YTickLabel',[]);

ax3 = nexttile();
hold on;
plot(freqs, abs(squeeze(freqresp(G_stiff_a, freqs, 'Hz'))), '-', 'color', colors(1,:), ...
     'DisplayName', '$L^\prime/F$');
plot(freqs, abs(squeeze(freqresp(G_stiff_r, freqs, 'Hz'))), '-', 'color', colors(2,:), ...
     'DisplayName', '$L/F$');
loglog(10.^(0.3*cos(0:0.01:2*pi)+log10(50)), ...
       10.^(0.3*sin(0:0.01:2*pi)+log10(8e-9)), 'k--', 'HandleVisibility', 'off');
text(50, 3e-8, 'No effect', 'horizontalalignment', 'center');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'YTickLabel',[]);
xticks([1e0, 1e1, 1e2]);
title('"Stiff" $\nu$-hexapod');
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);

linkaxes([ax1,ax2,ax3],'xy');
xlim([freqs(1), freqs(end)]);
ylim([1e-9, 1e-4]);

% Effect of support compliance on $d/F$

% When the motion to be controlled is the relative displacement $d$ between the granite and the nano-hexapod's top platform, the effect of the support compliance on the plant dynamics is opposite than what was previously observed.
% Indeed, using a "soft" nano-hexapod (i.e. with a suspension mode at lower frequency than the mode of the support) makes the dynamics less affected by the support dynamics (Figure ref:fig:uniaxial_effect_support_compliance_dynamics_d, left).
% On the contrary, if a "stiff" nano-hexapod is used, the support dynamics appears in the plant dynamics (Figure ref:fig:uniaxial_effect_support_compliance_dynamics_d, right).


%% Effect of the support compliance on the transfer functions from F to L and from F to d
figure;
freqs = logspace(0, 3, 1000);

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

ax1 = nexttile();
hold on;
plot(freqs, abs(squeeze(freqresp(G_soft_a, freqs, 'Hz'))), '-', 'color', colors(1,:));
plot(freqs, abs(squeeze(freqresp(G_soft,   freqs, 'Hz'))), '-', 'color', colors(3,:));
loglog(10.^(0.3*cos(0:0.01:2*pi)+log10(60)), ...
       10.^(0.6*sin(0:0.01:2*pi)+log10(4e-7)), 'k--');
text(8, 3e-7, 'No effect', 'horizontalalignment', 'center');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xticks([1e0, 1e1, 1e2]);
title('"Soft" $\nu$-hexapod');
ylabel('Magnitude [m/N]');


ax2 = nexttile();
hold on;
plot(freqs, abs(squeeze(freqresp(G_mid_a, freqs, 'Hz'))), '-', 'color', colors(1,:));
plot(freqs, abs(squeeze(freqresp(G_mid,   freqs, 'Hz'))), '-', 'color', colors(3,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
xticks([1e0, 1e1, 1e2]);
title('"Mid" $\nu$-hexapod');
set(gca, 'YTickLabel',[]);

ax3 = nexttile();
hold on;
plot(freqs, abs(squeeze(freqresp(G_stiff_a, freqs, 'Hz'))), '-', 'color', colors(1,:), ...
     'DisplayName', '$L^\prime/F$');
plot(freqs, abs(squeeze(freqresp(G_stiff,   freqs, 'Hz'))), '-', 'color', colors(3,:), ...
     'DisplayName', '$d/F$');
loglog(10.^(0.4*cos(0:0.01:2*pi)+log10(50)), ...
       10.^(0.8*sin(0:0.01:2*pi)+log10(8e-9)), 'k--', 'HandleVisibility', 'off');
text(50, 2e-7, sprintf('Support\nDynamics'), 'horizontalalignment', 'center');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'YTickLabel',[]);
xticks([1e0, 1e1, 1e2]);
title('"Stiff" $\nu$-hexapod');
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);

linkaxes([ax1,ax2,ax3],'xy');
xlim([freqs(1), freqs(end)]);
ylim([1e-9, 1e-4]);
