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

%% 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*350)^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 neglecting support compliance
freqs = logspace(0, 3, 1000);

figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_soft_a, freqs, 'Hz'))), '-', 'color', colors(1,:));
text(50, 5e-5, '$\omega_n =$ 10Hz', 'horizontalalignment', 'center');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude [m/N]');
xlim([freqs(1), freqs(end)]);
xticks([1e0, 1e1, 1e2]);
ylim([1e-9, 1e-4]);
yticks([1e-9, 1e-7, 1e-5]);

figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_mid_a, freqs, 'Hz'))), '-', 'color', colors(1,:));
text(70, 3e-6, '$\omega_n =$ 70Hz', 'horizontalalignment', 'center');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
xlim([freqs(1), freqs(end)]);
xticks([1e0, 1e1, 1e2]);
ylim([1e-9, 1e-4]);
yticks([1e-9, 1e-7, 1e-5]);

figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_stiff_a, freqs, 'Hz'))), '-', 'color', colors(1,:), ...
     'DisplayName', '$L^\prime/F$');
text(200, 8e-8, '$\omega_n =$ 400Hz', 'horizontalalignment', 'center');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
legend('location', 'northeast');
xlim([freqs(1), freqs(end)]);
xticks([1e0, 1e1, 1e2]);
ylim([1e-9, 1e-4]);
yticks([1e-9, 1e-7, 1e-5]);

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

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

figure;
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');
xlabel('Frequency [Hz]'); ylabel('Magnitude [m/N]');
xlim([freqs(1), freqs(end)]);
xticks([1e0, 1e1, 1e2]);
ylim([1e-9, 1e-4]);
yticks([1e-9, 1e-7, 1e-5]);

figure;
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]');
set(gca, 'YTickLabel',[]);
xlim([freqs(1), freqs(end)]);
xticks([1e0, 1e1, 1e2]);
ylim([1e-9, 1e-4]);
yticks([1e-9, 1e-7, 1e-5]);

figure;
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');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
xlim([freqs(1), freqs(end)]);
xticks([1e0, 1e1, 1e2]);
ylim([1e-9, 1e-4]);
yticks([1e-9, 1e-7, 1e-5]);

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

figure;
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');
xlabel('Frequency [Hz]'); ylabel('Magnitude [m/N]');
xlim([freqs(1), freqs(end)]);
xticks([1e0, 1e1, 1e2]);
ylim([1e-9, 1e-4]);
yticks([1e-9, 1e-7, 1e-5]);

figure;
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]');
set(gca, 'YTickLabel',[]);
xlim([freqs(1), freqs(end)]);
xticks([1e0, 1e1, 1e2]);
ylim([1e-9, 1e-4]);
yticks([1e-9, 1e-7, 1e-5]);

figure;
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');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
xlim([freqs(1), freqs(end)]);
xticks([1e0, 1e1, 1e2]);
ylim([1e-9, 1e-4]);
yticks([1e-9, 1e-7, 1e-5]);