%% 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
addpath('./src/'); % Path for Functions

%% Colors for the figures
colors = colororder;

%% Nano-Hexapod on top of Micro-Station model
mdl = 'nass_rotating_model';

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

%% Load controllers
load('nass_controllers.mat');

%% System parameters
mn = 15; % Nano-Hexapod mass [kg]
ms = 1; % Sample Mass [kg]

% General Configuration
model_config = struct();
model_config.controller = "open_loop"; % Default: Open-Loop
model_config.Tuv_type   = "normal";    % Default: 2DoF stage

% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/controller'],   1, 'openinput');  io_i = io_i + 1; % Actuator Forces [Fu, Fv]
io(io_i) = linio([mdl, '/fd'],           1, 'openinput');  io_i = io_i + 1; % Direct Forces on Sample [Fdx, Fdy]
io(io_i) = linio([mdl, '/xf'],           1, 'openinput');  io_i = io_i + 1; % Floor Motion [Dfx, Dfy]
io(io_i) = linio([mdl, '/ft'],           1, 'openinput');  io_i = io_i + 1; % Micro-Station Disturbances [Ftx, Fty]
io(io_i) = linio([mdl, '/nano_hexapod'], 1, 'openoutput'); io_i = io_i + 1; % [Fmu, Fmv]
io(io_i) = linio([mdl, '/nano_hexapod'], 2, 'openoutput'); io_i = io_i + 1; % [Du, Dv]
io(io_i) = linio([mdl, '/ext_metrology'],1, 'openoutput'); io_i = io_i + 1; % [Dx, Dy]

%% Identify plant without parallel stiffness
% Voice Coil (i.e. soft) Nano-Hexapod
kn = 1e4; % Nano-Hexapod Stiffness [N/m]
cn = 2*0.005*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]

Wz = 0; % Rotating Velocity [rad/s]
G_vc_norot = linearize(mdl, io, 0.0);
G_vc_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
G_vc_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

Wz = 2*pi; % Rotating Velocity [rad/s]
G_vc_fast = linearize(mdl, io, 0.0);
G_vc_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
G_vc_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

% APA (i.e. relatively stiff) Nano-Hexapod
kn = 1e6; % Nano-Hexapod Stiffness [N/m]
cn = 2*0.005*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]

Wz = 0; % Rotating Velocity [rad/s]
G_md_norot = linearize(mdl, io, 0.0);
G_md_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
G_md_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

Wz = 2*pi; % Rotating Velocity [rad/s]
G_md_fast = linearize(mdl, io, 0.0);
G_md_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
G_md_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

% Piezoelectric (i.e. stiff) Nano-Hexapod
kn = 1e8; % Nano-Hexapod Stiffness [N/m]
cn = 2*0.005*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]

Wz = 0; % Rotating Velocity [rad/s]
G_pz_norot = linearize(mdl, io, 0.0);
G_pz_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
G_pz_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

Wz = 2*pi; % Rotating Velocity [rad/s]
G_pz_fast = linearize(mdl, io, 0.0);
G_pz_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
G_pz_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

%% Identify plants with Parallel stiffnesses
model_config.Tuv_type   = "parallel_k";    % Default: 2DoF stage

% Voice Coil
kp = 1e3;
cp = 2*0.001*sqrt((ms + mn)*kp);
kn = 1e4-kp; % Nano-Hexapod Stiffness [N/m]
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]

% Identify dynamics
Wz = 2*pi; % [rad/s]
G_vc_kp_fast = linearize(mdl, io, 0);
G_vc_kp_fast.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
G_vc_kp_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

Wz = 0; % [rad/s]
G_vc_kp_norot = linearize(mdl, io, 0);
G_vc_kp_norot.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
G_vc_kp_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

% APA
kp = 1e4;
cp = 2*0.001*sqrt((ms + mn)*kp);
kn = 1e6 - kp; % Nano-Hexapod Stiffness [N/m]
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]

% Identify dynamics
Wz = 2*pi; % [rad/s]
G_md_kp_fast = linearize(mdl, io, 0);
G_md_kp_fast.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
G_md_kp_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

Wz = 0; % [rad/s]
G_md_kp_norot = linearize(mdl, io, 0);
G_md_kp_norot.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
G_md_kp_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

% Piezo
kp = 1e5;
cp = 2*0.001*sqrt((ms + mn)*kp);
kn = 1e8 - kp; % Nano-Hexapod Stiffness [N/m]
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]

% Identify dynamics
Wz = 2*pi; % [rad/s]
G_pz_kp_fast = linearize(mdl, io, 0);
G_pz_kp_fast.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
G_pz_kp_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

Wz = 0; % [rad/s]
G_pz_kp_norot = linearize(mdl, io, 0);
G_pz_kp_norot.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
G_pz_kp_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

%% Compute dampepd plants
% Closed-Loop Plants - IFF with HPF
G_vc_norot_iff_hpf = feedback(G_vc_norot, Kiff_hpf_vc, 'name');
G_vc_fast_iff_hpf  = feedback(G_vc_fast,  Kiff_hpf_vc, 'name');

G_md_norot_iff_hpf = feedback(G_md_norot, Kiff_hpf_md, 'name');
G_md_fast_iff_hpf  = feedback(G_md_fast,  Kiff_hpf_md, 'name');

G_pz_norot_iff_hpf = feedback(G_pz_norot, Kiff_hpf_pz, 'name');
G_pz_fast_iff_hpf  = feedback(G_pz_fast,  Kiff_hpf_pz, 'name');

% Closed-Loop Plants - IFF with Parallel Stiffness
G_vc_norot_iff_kp = feedback(G_vc_kp_norot, Kiff_kp_vc, 'name');
G_vc_fast_iff_kp  = feedback(G_vc_kp_fast,  Kiff_kp_vc, 'name');

G_md_norot_iff_kp = feedback(G_md_kp_norot, Kiff_kp_md, 'name');
G_md_fast_iff_kp  = feedback(G_md_kp_fast,  Kiff_kp_md, 'name');

G_pz_norot_iff_kp = feedback(G_pz_kp_norot, Kiff_kp_pz, 'name');
G_pz_fast_iff_kp  = feedback(G_pz_kp_fast,  Kiff_kp_pz, 'name');

% Closed-Loop Plants - RDC
G_vc_norot_rdc = feedback(G_vc_norot, Krdc_vc, 'name');
G_vc_fast_rdc  = feedback(G_vc_fast,  Krdc_vc, 'name');

G_md_norot_rdc = feedback(G_md_norot, Krdc_md, 'name');
G_md_fast_rdc  = feedback(G_md_fast,  Krdc_md, 'name');

G_pz_norot_rdc = feedback(G_pz_norot, Krdc_pz, 'name');
G_pz_fast_rdc  = feedback(G_pz_fast,  Krdc_pz, 'name');

%% Bode plot of the transfer function from nano-hexapod actuator to measured motion by the external metrology
freqs_vc = logspace(-1, 2, 1000);

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

ax1 = nexttile([2,1]);
hold on;
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast('Dx', 'Fu'), freqs_vc, 'Hz'))), 'color', zeros(1,3), ...
    'DisplayName', 'OL');
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast('Dy', 'Fu'), freqs_vc, 'Hz'))), 'color', [zeros(1,3), 0.5], ...
    'DisplayName', 'Coupling');
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_hpf('Dx', 'Fu'), freqs_vc, 'Hz'))), 'color', colors(1,:), ...
    'DisplayName', 'IFF + $k_p$');
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_hpf('Dy', 'Fu'), freqs_vc, 'Hz'))), 'color', [colors(1,:), 0.5], ...
    'HandleVisibility', 'off');
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_kp('Dx', 'Fu'), freqs_vc, 'Hz'))), 'color', colors(2,:), ...
    'DisplayName', 'IFF + HPF');
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_kp('Dy', 'Fu'), freqs_vc, 'Hz'))), 'color', [colors(2,:), 0.5], ...
    'HandleVisibility', 'off');
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_rdc('Dx', 'Fu'), freqs_vc, 'Hz'))), 'color', colors(3,:), ...
    'DisplayName', 'RDC');
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_rdc('Dy', 'Fu'), freqs_vc, 'Hz'))), 'color', [colors(3,:), 0.5], ...
    'HandleVisibility', 'off');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-8, 1e-2])

ax2 = nexttile;
hold on;
plot(freqs_vc, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_fast('Dx', 'Fu'), freqs_vc, 'Hz')))), 'color', zeros(1,3));
plot(freqs_vc, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_fast_iff_hpf('Dx', 'Fu'), freqs_vc, 'Hz')))), 'color', colors(1,:));
plot(freqs_vc, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_fast_iff_kp('Dx', 'Fu'), freqs_vc, 'Hz')))), 'color', colors(2,:));
plot(freqs_vc, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_fast_rdc('Dx', 'Fu'), freqs_vc, 'Hz')))), 'color', colors(3,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([ -200, 20]);

linkaxes([ax1,ax2],'x');
xlim([freqs_vc(1), freqs_vc(end)]);
xticks([1e-1, 1e0, 1e1]);

freqs_md = logspace(0, 3, 1000);
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs_md, abs(squeeze(freqresp(G_md_fast('Dx', 'Fu'), freqs_md, 'Hz'))), 'color', zeros(1,3), ...
    'DisplayName', 'OL');
plot(freqs_md, abs(squeeze(freqresp(G_md_fast('Dy', 'Fu'), freqs_md, 'Hz'))), 'color', [zeros(1,3), 0.5], ...
    'DisplayName', 'Coupling');
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_hpf('Dx', 'Fu'), freqs_md, 'Hz'))), 'color', colors(1,:), ...
    'DisplayName', 'IFF + $k_p$');
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_hpf('Dy', 'Fu'), freqs_md, 'Hz'))), 'color', [colors(1,:), 0.5], ...
    'HandleVisibility', 'off');
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_kp('Dx', 'Fu'), freqs_md, 'Hz'))), 'color', colors(2,:), ...
    'DisplayName', 'IFF + HPF');
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_kp('Dy', 'Fu'), freqs_md, 'Hz'))), 'color', [colors(2,:), 0.5], ...
    'HandleVisibility', 'off');
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_rdc('Dx', 'Fu'), freqs_md, 'Hz'))), 'color', colors(3,:), ...
    'DisplayName', 'RDC');
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_rdc('Dy', 'Fu'), freqs_md, 'Hz'))), 'color', [colors(3,:), 0.5], ...
    'HandleVisibility', 'off');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-10, 1e-4])

ax2 = nexttile;
hold on;
plot(freqs_md, 180/pi*unwrap(angle(squeeze(freqresp(G_md_fast('Dx', 'Fu'), freqs_md, 'Hz')))), 'color', zeros(1,3));
plot(freqs_md, 180/pi*unwrap(angle(squeeze(freqresp(G_md_fast_iff_hpf('Dx', 'Fu'), freqs_md, 'Hz')))), 'color', colors(1,:));
plot(freqs_md, 180/pi*unwrap(angle(squeeze(freqresp(G_md_fast_iff_kp('Dx', 'Fu'), freqs_md, 'Hz')))), 'color', colors(2,:));
plot(freqs_md, 180/pi*unwrap(angle(squeeze(freqresp(G_md_fast_rdc('Dx', 'Fu'), freqs_md, 'Hz')))), 'color', colors(3,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([ -200, 20]);

linkaxes([ax1,ax2],'x');
xlim([freqs_md(1), freqs_md(end)]);
xticks([1e0, 1e1, 1e2]);

freqs_pz = logspace(0, 3, 1000);
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast('Dx', 'Fu'), freqs_pz, 'Hz'))), 'color', zeros(1,3), ...
    'DisplayName', 'OL');
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast('Dy', 'Fu'), freqs_pz, 'Hz'))), 'color', [zeros(1,3), 0.5], ...
    'DisplayName', 'Coupling');
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_hpf('Dx', 'Fu'), freqs_pz, 'Hz'))), 'color', colors(1,:), ...
    'DisplayName', 'IFF + $k_p$');
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_hpf('Dy', 'Fu'), freqs_pz, 'Hz'))), 'color', [colors(1,:), 0.5], ...
    'HandleVisibility', 'off');
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_kp('Dx', 'Fu'), freqs_pz, 'Hz'))), 'color', colors(2,:), ...
    'DisplayName', 'IFF + HPF');
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_kp('Dy', 'Fu'), freqs_pz, 'Hz'))), 'color', [colors(2,:), 0.5], ...
    'HandleVisibility', 'off');
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_rdc('Dx', 'Fu'), freqs_pz, 'Hz'))), 'color', colors(3,:), ...
    'DisplayName', 'RDC');
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_rdc('Dy', 'Fu'), freqs_pz, 'Hz'))), 'color', [colors(3,:), 0.5], ...
    'HandleVisibility', 'off');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-12, 1e-6])
ldg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
ldg.ItemTokenSize = [20, 1];

ax2 = nexttile;
hold on;
plot(freqs_pz, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_fast('Dx', 'Fu'), freqs_pz, 'Hz')))), 'color', zeros(1,3));
plot(freqs_pz, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_fast_iff_hpf('Dx', 'Fu'), freqs_pz, 'Hz')))), 'color', colors(1,:));
plot(freqs_pz, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_fast_iff_kp('Dx', 'Fu'), freqs_pz, 'Hz')))), 'color', colors(2,:));
plot(freqs_pz, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_fast_rdc('Dx', 'Fu'), freqs_pz, 'Hz')))), 'color', colors(3,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([ -200, 20]);

linkaxes([ax1,ax2],'x');
xlim([freqs_pz(1), freqs_pz(end)]);
xticks([1e0, 1e1, 1e2]);

%% Effect of Floor motion on the position error - Comparison of active damping techniques for the three nano-hexapod stiffnesses
freqs = logspace(-1, 3, 1000);

figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_vc_fast('Dx', 'Dfx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
    'DisplayName', 'OL');
plot(freqs, abs(squeeze(freqresp(G_vc_fast_iff_hpf('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(1,:), ...
    'DisplayName', 'IFF + $k_p$');
plot(freqs, abs(squeeze(freqresp(G_vc_fast_iff_kp('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(2,:), ...
    'DisplayName', 'IFF + HPF');
plot(freqs, abs(squeeze(freqresp(G_vc_fast_rdc('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(3,:), ...
    'DisplayName', 'RDC');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/x_{f,x}$ [m/N]');
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
xtickangle(0)
ylim([1e-4, 1e2]);

figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_md_fast('Dx', 'Dfx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
    'DisplayName', 'OL');
plot(freqs, abs(squeeze(freqresp(G_md_fast_iff_hpf('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(1,:), ...
    'DisplayName', 'IFF + $k_p$');
plot(freqs, abs(squeeze(freqresp(G_md_fast_iff_kp('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(2,:), ...
    'DisplayName', 'IFF + HPF');
plot(freqs, abs(squeeze(freqresp(G_md_fast_rdc('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(3,:), ...
    'DisplayName', 'RDC');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/x_{f,x}$ [m/N]');
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
xtickangle(0)
ylim([1e-4, 1e2]);

figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_pz_fast('Dx', 'Dfx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
    'DisplayName', 'OL');
plot(freqs, abs(squeeze(freqresp(G_pz_fast_iff_hpf('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(1,:), ...
    'DisplayName', 'IFF + $k_p$');
plot(freqs, abs(squeeze(freqresp(G_pz_fast_iff_kp('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(2,:), ...
    'DisplayName', 'IFF + HPF');
plot(freqs, abs(squeeze(freqresp(G_pz_fast_rdc('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(3,:), ...
    'DisplayName', 'RDC');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/x_{f,x}$ [m/N]');
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
xtickangle(0)
ldg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
ldg.ItemTokenSize = [15, 1];
ylim([1e-4, 1e2]);

%% Effect of micro-station vibrations on the position error - Comparison of active damping techniques for the three nano-hexapod stiffnesses
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_vc_fast('Dx', 'Ftx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
    'DisplayName', 'OL');
plot(freqs, abs(squeeze(freqresp(G_vc_fast_iff_hpf('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(1,:), ...
    'DisplayName', 'IFF + $k_p$');
plot(freqs, abs(squeeze(freqresp(G_vc_fast_iff_kp('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(2,:), ...
    'DisplayName', 'IFF + HPF');
plot(freqs, abs(squeeze(freqresp(G_vc_fast_rdc('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(3,:), ...
    'DisplayName', 'RDC');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/f_{t,x}$ [m/N]');
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
xtickangle(0)
ylim([1e-12, 2e-7]);

figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_md_fast('Dx', 'Ftx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
    'DisplayName', 'OL');
plot(freqs, abs(squeeze(freqresp(G_md_fast_iff_hpf('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(1,:), ...
    'DisplayName', 'IFF + $k_p$');
plot(freqs, abs(squeeze(freqresp(G_md_fast_iff_kp('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(2,:), ...
    'DisplayName', 'IFF + HPF');
plot(freqs, abs(squeeze(freqresp(G_md_fast_rdc('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(3,:), ...
    'DisplayName', 'RDC');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/f_{t,x}$ [m/N]');
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
xtickangle(0)
ylim([1e-12, 2e-7]);

figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_pz_fast('Dx', 'Ftx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
    'DisplayName', 'OL');
plot(freqs, abs(squeeze(freqresp(G_pz_fast_iff_hpf('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(1,:), ...
    'DisplayName', 'IFF + $k_p$');
plot(freqs, abs(squeeze(freqresp(G_pz_fast_iff_kp('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(2,:), ...
    'DisplayName', 'IFF + HPF');
plot(freqs, abs(squeeze(freqresp(G_pz_fast_rdc('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(3,:), ...
    'DisplayName', 'RDC');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/f_{t,x}$ [m/N]');
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
xtickangle(0)
ldg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
ldg.ItemTokenSize = [20, 1];
ylim([1e-12, 2e-7]);

%% Effect of sample forces on the position error - Comparison of active damping techniques for the three nano-hexapod stiffnesses
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_vc_fast('Dx', 'Fdx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
    'DisplayName', 'OL');
plot(freqs, abs(squeeze(freqresp(G_vc_fast_iff_hpf('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(1,:), ...
    'DisplayName', 'IFF + $k_p$');
plot(freqs, abs(squeeze(freqresp(G_vc_fast_iff_kp('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(2,:), ...
    'DisplayName', 'IFF + HPF');
plot(freqs, abs(squeeze(freqresp(G_vc_fast_rdc('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(3,:), ...
    'DisplayName', 'RDC');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/f_{s,x}$ [m/N]');
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
xtickangle(0)
ylim([1e-8, 1e-2])

figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_md_fast('Dx', 'Fdx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
    'DisplayName', 'OL');
plot(freqs, abs(squeeze(freqresp(G_md_fast_iff_hpf('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(1,:), ...
    'DisplayName', 'IFF + $k_p$');
plot(freqs, abs(squeeze(freqresp(G_md_fast_iff_kp('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(2,:), ...
    'DisplayName', 'IFF + HPF');
plot(freqs, abs(squeeze(freqresp(G_md_fast_rdc('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(3,:), ...
    'DisplayName', 'RDC');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/f_{s,x}$ [m/N]');
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
xtickangle(0)
ylim([1e-8, 1e-2])

figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_pz_fast('Dx', 'Fdx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
    'DisplayName', 'OL');
plot(freqs, abs(squeeze(freqresp(G_pz_fast_iff_hpf('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(1,:), ...
    'DisplayName', 'IFF + $k_p$');
plot(freqs, abs(squeeze(freqresp(G_pz_fast_iff_kp('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(2,:), ...
    'DisplayName', 'IFF + HPF');
plot(freqs, abs(squeeze(freqresp(G_pz_fast_rdc('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(3,:), ...
    'DisplayName', 'RDC');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/f_{s,x}$ [m/N]');
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
xtickangle(0)
ldg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
ldg.ItemTokenSize = [20, 1];

linkaxes([ax1,ax2,ax3], 'y')
ylim([1e-8, 1e-2])
