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

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

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

%% Load Plants Dynamics
load('uniaxial_plants.mat', 'G_vc_light', 'G_md_light', 'G_pz_light', ...
                            'G_vc_mid',   'G_md_mid',   'G_pz_mid', ...
                            'G_vc_heavy', 'G_md_heavy', 'G_pz_heavy');

%% Sensitivity to disturbances for three different nano-hexpod stiffnesses
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_vc_light('d', 'fs'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_md_light('d', 'fs'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_pz_light('d', 'fs'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d/f_{s}$ [m/N]'); xlabel('Frequency [Hz]');
xticks([1e0, 1e1, 1e2]);
xlim([1, 500]);

%% Sensitivity to disturbances for three different nano-hexpod stiffnesses
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_vc_light('d', 'ft'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_md_light('d', 'ft'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_pz_light('d', 'ft'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d/f_{t}$ [m/N]'); xlabel('Frequency [Hz]');
xticks([1e0, 1e1, 1e2]);
xlim([1, 500]);

%% Sensitivity to disturbances for three different nano-hexpod stiffnesses
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_vc_light('d', 'xf'), freqs, 'Hz'))), 'DisplayName', '$k_n = 0.01\,N/\mu m$');
plot(freqs, abs(squeeze(freqresp(G_md_light('d', 'xf'), freqs, 'Hz'))), 'DisplayName', '$k_n = 1   \,N/\mu m$');
plot(freqs, abs(squeeze(freqresp(G_pz_light('d', 'xf'), freqs, 'Hz'))), 'DisplayName', '$k_n = 100 \,N/\mu m$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d/x_{f}$ [m/m]'); xlabel('Frequency [Hz]');
xticks([1e0, 1e1, 1e2]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xlim([1, 500]);

%% Cumulative Amplitude Spectrum of the relative motion d, due to both the floor motion and the stage vibrations
figure;
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_vc_light('d', 'ft'), f, 'Hz'))).^2)))), '-', 'color', colors(1,:), 'DisplayName', '$f_t$');
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_md_light('d', 'ft'), f, 'Hz'))).^2)))), '-', 'color', colors(2,:), 'DisplayName', '$f_t$');
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_pz_light('d', 'ft'), f, 'Hz'))).^2)))), '-', 'color', colors(3,:), 'DisplayName', '$f_t$');
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_xf.*abs(squeeze(freqresp(G_vc_light('d', 'xf'), f, 'Hz'))).^2)))), '--', 'color', colors(1,:), 'DisplayName', '$x_f$ ($k_n = 0.01\,N/\mu m$)');
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_xf.*abs(squeeze(freqresp(G_md_light('d', 'xf'), f, 'Hz'))).^2)))), '--', 'color', colors(2,:), 'DisplayName', '$x_f$ ($k_n = 1   \,N/\mu m$)');
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_xf.*abs(squeeze(freqresp(G_pz_light('d', 'xf'), f, 'Hz'))).^2)))), '--', 'color', colors(3,:), 'DisplayName', '$x_f$ ($k_n = 100 \,N/\mu m$)');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('CAS [m]'); xlabel('Frequency [Hz]');
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;

xlim([1, 500]);
ylim([1e-12, 3e-6])

%% Cumulative Amplitude Spectrum of the relative motion d due to all disturbances, for two sample masses
figure;
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_vc_light('d', 'ft'), f, 'Hz'))).^2 + ...
                                     psd_xf.*abs(squeeze(freqresp(G_vc_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
     'color', colors(1,:), 'DisplayName', '$m_s = 1\,kg$, $k_n = 0.01\,N/\mu m$');
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_md_light('d', 'ft'), f, 'Hz'))).^2 + ...
                                     psd_xf.*abs(squeeze(freqresp(G_md_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
     'color', colors(2,:), 'DisplayName', '$m_s = 1\,kg$, $k_n = 1\,N/\mu m$');
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_pz_light('d', 'ft'), f, 'Hz'))).^2 + ...
                                     psd_xf.*abs(squeeze(freqresp(G_pz_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
     'color', colors(3,:), 'DisplayName', '$m_s = 1\,kg$, $k_n = 100\,N/\mu m$');
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_vc_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
                                     psd_xf.*abs(squeeze(freqresp(G_vc_heavy('d', 'xf'), f, 'Hz'))).^2)))), '--', ...
     'color', colors(1,:), 'DisplayName', '$m_s = 50\,kg$, $k_n = 0.01\,N/\mu m$');
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_md_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
                                     psd_xf.*abs(squeeze(freqresp(G_md_heavy('d', 'xf'), f, 'Hz'))).^2)))), '--', ...
     'color', colors(2,:), 'DisplayName', '$m_s = 50\,kg$, $k_n = 1\,N/\mu m$');
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_pz_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
                                     psd_xf.*abs(squeeze(freqresp(G_pz_heavy('d', 'xf'), f, 'Hz'))).^2)))), '--', ...
     'color', colors(3,:), 'DisplayName', '$m_s = 50\,kg$, $k_n = 100\,N/\mu m$');
plot([1, 1e3], [20e-9, 20e-9], 'k--', 'HandleVisibility', 'off');
text(4, 1e-8, '20 nm RMS', 'horizontalalignment', 'center');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('CAS [m]'); xlabel('Frequency [Hz]');
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

xlim([1, 500]);
ylim([1e-12, 3e-6])