%% test_id31_4_hac.m

%% 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
addpath('./STEPS/'); % Path for STEPS
addpath('./subsystems/'); % Path for Subsystems Simulink files

%% Data directory
data_dir = './mat/';

% Simulink Model name
mdl = 'nass_model_id31';

%% Colors for the figures
colors = colororder;

%% Frequency Vector
freqs = logspace(log10(1), log10(2e3), 1000);

%% Sampling Time
Ts = 1e-4;

%% Specifications for Experiments
specs_dz_peak = 50; % [nm]
specs_dy_peak = 100; % [nm]
specs_ry_peak = 0.85; % [urad]
specs_dz_rms = 15; % [nm RMS]
specs_dy_rms = 30; % [nm RMS]
specs_ry_rms = 0.25; % [urad RMS]

% Load the estimated damped plant from the multi-body model
load('test_id31_simscape_damped_plants.mat', 'Gm_hac_m0_Wz0', 'Gm_hac_m1_Wz0', 'Gm_hac_m2_Wz0', 'Gm_hac_m3_Wz0');
% Load the measured damped plants
load('test_id31_identified_damped_plants.mat', 'G_hac_m0_Wz0', 'G_hac_m1_Wz0', 'G_hac_m2_Wz0', 'G_hac_m3_Wz0', 'f');
% Load the undamped plant for comparison
load('test_id31_identified_open_loop_plants.mat', 'G_int_m0_Wz0', 'G_int_m1_Wz0', 'G_int_m2_Wz0', 'G_int_m3_Wz0', 'f');

figure;
tiledlayout(2, 3, 'TileSpacing', 'tight', 'Padding', 'tight');

ax1 = nexttile();
hold on;
plot(f, abs(G_hac_m0_Wz0(:, 1, 1)));
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0_Wz0('eL1', 'u1'), freqs, 'Hz'))));
text(12, 3e-5, '$\epsilon_{\mathcal{L}1}/u_1^\prime$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/V]');
yticks([1e-7, 1e-6, 1e-5]);

ax2 = nexttile();
hold on;
plot(f, abs(G_hac_m0_Wz0(:, 2, 1)));
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0_Wz0('eL2', 'u1'), freqs, 'Hz'))));
text(12, 3e-5, '$\epsilon_{\mathcal{L}2}/u_1^\prime$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);

ax3 = nexttile();
hold on;
plot(f, abs(G_hac_m0_Wz0(:, 3, 1)))
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0_Wz0('eL3', 'u1'), freqs, 'Hz'))))
text(12, 3e-5, '$\epsilon_{\mathcal{L}3}/u_1^\prime$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);

ax4 = nexttile();
hold on;
plot(f, abs(G_hac_m0_Wz0(:, 4, 1)));
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0_Wz0('eL4', 'u1'), freqs, 'Hz'))));
text(12, 3e-5, '$\epsilon_{\mathcal{L}4}/u_1^\prime$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
xticks([10, 20, 50, 100, 200])
yticks([1e-7, 1e-6, 1e-5]);

ax5 = nexttile();
hold on;
plot(f, abs(G_hac_m0_Wz0(:, 5, 1)));
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0_Wz0('eL5', 'u1'), freqs, 'Hz'))));
text(12, 3e-5, '$\epsilon_{\mathcal{L}5}/u_1^\prime$', 'Horiz','left', 'Vert','top')
hold off;
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xticks([10, 20, 50, 100, 200])

ax6 = nexttile();
hold on;
plot(f, abs(G_hac_m0_Wz0(:, 6, 1)), ...
     'DisplayName', 'Measurements');
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0_Wz0('eL6', 'u1'), freqs, 'Hz'))), ...
     'DisplayName', 'Model (2-DoF APA)');
text(12, 3e-5, '$\epsilon_{\mathcal{L}6}/u_1^\prime$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
xticks([10, 20, 50, 100, 200])
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'xy');
xlim([10, 5e2]); ylim([1e-7, 4e-5]);

%% Comparison of all the undamped FRF and all the damped FRF
figure;
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(G_int_m0_Wz0(:,1,1)), 'color', [colors(1,:), 0.5], 'DisplayName', 'Undamped - $\epsilon\mathcal{L}_i/u_i$');
plot(f, abs(G_hac_m0_Wz0(:,1,1)), 'color', [colors(2,:), 0.5], 'DisplayName', 'damped - $\epsilon\mathcal{L}_i/u_i^\prime$');
for i = 1:6
    plot(f, abs(G_int_m0_Wz0(:,i, i)), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
    plot(f, abs(G_int_m1_Wz0(:,i, i)), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
    plot(f, abs(G_int_m2_Wz0(:,i, i)), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
    plot(f, abs(G_int_m3_Wz0(:,i, i)), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
end
for i = 1:6
    plot(f, abs(G_hac_m0_Wz0(:,i, i)), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
    plot(f, abs(G_hac_m1_Wz0(:,i, i)), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
    plot(f, abs(G_hac_m2_Wz0(:,i, i)), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
    plot(f, abs(G_hac_m3_Wz0(:,i, i)), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ylim([2e-7, 4e-4]);

ax2 = nexttile;
hold on;
for i =1:6
    plot(f, 180/pi*unwrapphase(angle(-G_int_m0_Wz0(:,i, i)), f), 'color', [colors(1,:), 0.5]);
    plot(f, 180/pi*unwrapphase(angle(-G_int_m1_Wz0(:,i, i)), f), 'color', [colors(1,:), 0.5]);
    plot(f, 180/pi*unwrapphase(angle(-G_int_m2_Wz0(:,i, i)), f), 'color', [colors(1,:), 0.5]);
    plot(f, 180/pi*unwrapphase(angle(-G_int_m3_Wz0(:,i, i)), f), 'color', [colors(1,:), 0.5]);
end
for i = 1:6
    plot(f, 180/pi*unwrapphase(angle(G_hac_m0_Wz0(:,i, i)), f), 'color', [colors(2,:), 0.5]);
    plot(f, 180/pi*unwrapphase(angle(G_hac_m1_Wz0(:,i, i)), f), 'color', [colors(2,:), 0.5]);
    plot(f, 180/pi*unwrapphase(angle(G_hac_m2_Wz0(:,i, i)), f), 'color', [colors(2,:), 0.5]);
    plot(f, 180/pi*unwrapphase(angle(G_hac_m3_Wz0(:,i, i)), f), 'color', [colors(2,:), 0.5]);
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-270, 20])

linkaxes([ax1,ax2],'x');
xlim([1, 5e2]);

%% Interaction Analysis - RGA Number
rga_m0 = zeros(1,size(G_hac_m0_Wz0,1));
for i = 1:length(rga_m0)
    rga_m0(i) = sum(sum(abs(inv(squeeze(G_hac_m0_Wz0(i,:,:)).').*squeeze(G_hac_m0_Wz0(i,:,:)) - eye(6))));
end

rga_m1 = zeros(1,size(G_hac_m1_Wz0,1));
for i = 1:length(rga_m1)
    rga_m1(i) = sum(sum(abs(inv(squeeze(G_hac_m1_Wz0(i,:,:)).').*squeeze(G_hac_m1_Wz0(i,:,:)) - eye(6))));
end

rga_m2 = zeros(1,size(G_hac_m2_Wz0,1));
for i = 1:length(rga_m2)
    rga_m2(i) = sum(sum(abs(inv(squeeze(G_hac_m2_Wz0(i,:,:)).').*squeeze(G_hac_m2_Wz0(i,:,:)) - eye(6))));
end

rga_m3 = zeros(1,size(G_hac_m3_Wz0,1));
for i = 1:length(rga_m3)
    rga_m3(i) = sum(sum(abs(inv(squeeze(G_hac_m3_Wz0(i,:,:)).').*squeeze(G_hac_m3_Wz0(i,:,:)) - eye(6))));
end

%% RGA-number for the damped plants - Comparison of all the payload conditions
figure;
hold on;
plot(f, rga_m0, 'DisplayName', '$m = 0$ kg')
plot(f, rga_m1, 'DisplayName', '$m = 13$ kg')
plot(f, rga_m2, 'DisplayName', '$m = 26$ kg')
plot(f, rga_m3, 'DisplayName', '$m = 39$ kg')
hold off;
xlabel('Frequency [Hz]'); ylabel('RGA number');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlim([1, 1e2]); ylim([0, 10]);
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;

%% HAC Design
% Wanted crossover
wc = 2*pi*5; % [rad/s]

% Integrator
H_int = wc/s;

% Lead to increase phase margin
a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));

% Low Pass filter to increase robustness
H_lpf = 1/(1 + s/2/pi/30);

% Gain to have unitary crossover at 5Hz
[~, i_f] = min(abs(f - wc/2/pi));
H_gain = 1./abs(G_hac_m0_Wz0(i_f, 1, 1));

% Decentralized HAC
Khac = H_gain  * ... % Gain
       H_int   * ... % Integrator
       H_lpf   * ... % Low Pass filter
       eye(6);       % 6x6 Diagonal

% The designed HAC controller is saved
save('./mat/test_id31_K_hac_robust.mat', 'Khac');

%% Decentralized Loop gain for the High Authority Controller
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f(2:end), abs(G_hac_m0_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f(2:end), 'Hz'))), 'color', colors(1,:), 'DisplayName', '$0$ kg');
plot(f(2:end), abs(G_hac_m1_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f(2:end), 'Hz'))), 'color', colors(2,:), 'DisplayName', '$13$ kg');
plot(f(2:end), abs(G_hac_m2_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f(2:end), 'Hz'))), 'color', colors(3,:), 'DisplayName', '$26$ kg');
plot(f(2:end), abs(G_hac_m3_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f(2:end), 'Hz'))), 'color', colors(4,:), 'DisplayName', '$39$ kg');
xline(5, '--', 'linewidth', 1, 'color', [0,0,0,0.2], 'HandleVisibility', 'off')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
ylim([1e-5, 1e2]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
plot(f(2:end), 180/pi*angle(G_hac_m0_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f(2:end), 'Hz'))), 'color', colors(1,:));
plot(f(2:end), 180/pi*angle(G_hac_m1_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f(2:end), 'Hz'))), 'color', colors(2,:));
plot(f(2:end), 180/pi*angle(G_hac_m2_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f(2:end), 'Hz'))), 'color', colors(3,:));
plot(f(2:end), 180/pi*angle(G_hac_m3_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f(2:end), 'Hz'))), 'color', colors(4,:));
xline(5, '--', 'linewidth', 1, 'color', [0,0,0,0.2])
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-180, 180])

linkaxes([ax1,ax2],'x');
xlim([1, 1e3]);

%% Compute the Eigenvalues of the loop gain
Ldet = zeros(4, 6, length(f));

% Loop gain
Lmimo = pagemtimes(permute(G_hac_m0_Wz0, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
for i_f = 2:length(f)
    Ldet(1,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
end

Lmimo = pagemtimes(permute(G_hac_m1_Wz0, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
for i_f = 2:length(f)
    Ldet(2,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
end

Lmimo = pagemtimes(permute(G_hac_m2_Wz0, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
for i_f = 2:length(f)
    Ldet(3,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
end

Lmimo = pagemtimes(permute(G_hac_m3_Wz0, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
for i_f = 2:length(f)
    Ldet(4,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
end

%% Plot of the eigenvalues of L in the complex plane
figure;
hold on;
plot(real(squeeze(Ldet(1, 1,:))), imag(squeeze(Ldet(1, 1,:))), ...
     '.', 'color', colors(1, :), ...
     'DisplayName', '$m = 0$ kg');
plot(real(squeeze(Ldet(2, 1,:))), imag(squeeze(Ldet(2, 1,:))), ...
     '.', 'color', colors(2, :), ...
     'DisplayName', '$m = 13$ kg');
plot(real(squeeze(Ldet(3, 1,:))), imag(squeeze(Ldet(3, 1,:))), ...
     '.', 'color', colors(3, :), ...
     'DisplayName', '$m = 26$ kg');
plot(real(squeeze(Ldet(4, 1,:))), imag(squeeze(Ldet(4, 1,:))), ...
     '.', 'color', colors(4, :), ...
     'DisplayName', '$m = 39$ kg');
for i_mass = 1:4
    plot(real(squeeze(Ldet(i_mass, 1,:))), -imag(squeeze(Ldet(i_mass, 1,:))), ...
         '.', 'color', colors(i_mass, :), ...
         'HandleVisibility', 'off');
    for i = 1:6
        plot(real(squeeze(Ldet(i_mass, i,:))), imag(squeeze(Ldet(i_mass, i,:))), ...
             '.', 'color', colors(i_mass, :), ...
             'HandleVisibility', 'off');
        plot(real(squeeze(Ldet(i_mass, i,:))), -imag(squeeze(Ldet(i_mass, i,:))), ...
             '.', 'color', colors(i_mass, :), ...
             'HandleVisibility', 'off');
    end
end
plot(-1, 0, 'kx', 'HandleVisibility', 'off');
hold off;
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
xlabel('Real'); ylabel('Imag');
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;
axis square
xlim([-1.5, 0.5]); ylim([-1, 1]);

%% Tomography experiment
% Sample is not centered with the rotation axis
% This is done by offsetfing the micro-hexapod by 0.9um
P_micro_hexapod = [2.5e-6; 0; -0.3e-6]; % [m]

open(mdl);
set_param(mdl, 'StopTime', '3'); % 6 turns at 180deg/s (30rpm)

initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod('AP', P_micro_hexapod);
initializeNanoHexapod('flex_bot_type', '2dof', ...
                      'flex_top_type', '3dof', ...
                      'motion_sensor_type', 'plates', ...
                      'actuator_type', '2dof');
initializeSample('type', '0');

initializeSimscapeConfiguration('gravity', false);
initializeLoggingConfiguration('log', 'all', 'Ts', 1e-4);
initializeController('type', 'open-loop');

initializeDisturbances(...
    'Dw_x',  true, ... % Ground Motion - X direction
    'Dw_y',  true, ... % Ground Motion - Y direction
    'Dw_z',  true, ... % Ground Motion - Z direction
    'Fdy_x', false, ... % Translation Stage - X direction
    'Fdy_z', false, ... % Translation Stage - Z direction
    'Frz_x', true, ... % Spindle - X direction
    'Frz_y', true, ... % Spindle - Y direction
    'Frz_z', true);    % Spindle - Z direction

initializeReferences(...
    'Rz_type', 'rotating', ...
    'Rz_period', 360/180, ... % 180deg/s, 30rpm
    'Dh_pos', [P_micro_hexapod; 0; 0; 0]);

% Open-Loop Simulation
sim(mdl);
exp_tomo_ol_m0_Wz180 = simout;

% Closed-Loop Simulation
load('test_id31_K_iff.mat', 'Kiff');
load('test_id31_K_hac_robust.mat', 'Khac');
initializeController('type', 'hac-iff');
initializeSample('type', '0');
sim(mdl);
exp_tomo_cl_m0_Wz180 = simout;

% Save the simulation results
save('./mat/test_id31_exp_tomo_ol_cl_30rpm_sim.mat', 'exp_tomo_ol_m0_Wz180', 'exp_tomo_cl_m0_Wz180');

%% Simulation of tomography experiment - no payload, 30rpm - XY errors
figure;
hold on;
plot(1e6*exp_tomo_ol_m0_Wz180.y.x.Data, 1e6*exp_tomo_ol_m0_Wz180.y.y.Data, 'DisplayName', 'OL')
plot(1e6*exp_tomo_cl_m0_Wz180.y.x.Data(1:2e3), 1e6*exp_tomo_cl_m0_Wz180.y.y.Data(1:2e3), 'color', colors(3,:), 'HandleVisibility', 'off')
plot(1e6*exp_tomo_cl_m0_Wz180.y.x.Data(2e3:end), 1e6*exp_tomo_cl_m0_Wz180.y.y.Data(2e3:end), 'color', colors(2,:), 'DisplayName', 'CL')
hold off;
xlabel('$D_x$ [$\mu$m]'); ylabel('$D_y$ [$\mu$m]');
axis equal
xlim([-3, 3]); ylim([-3, 3]);
xticks([-3:1:3]);
yticks([-3:1:3]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

%% Simulation of tomography experiment - no payload, 30rpm - YZ errors
figure;
tiledlayout(2, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(1e6*exp_tomo_ol_m0_Wz180.y.y.Data, 1e6*exp_tomo_ol_m0_Wz180.y.z.Data, 'DisplayName', 'OL')
plot(1e6*exp_tomo_cl_m0_Wz180.y.y.Data(1:2e3), 1e6*exp_tomo_cl_m0_Wz180.y.z.Data(1:2e3), 'color', colors(3,:), 'HandleVisibility', 'off')
plot(1e6*exp_tomo_cl_m0_Wz180.y.y.Data(2e3:end), 1e6*exp_tomo_cl_m0_Wz180.y.z.Data(2e3:end), 'color', colors(2,:), 'DisplayName', 'CL')
hold off;
xlabel('$D_y$ [$\mu$m]'); ylabel('$D_z$ [$\mu$m]');
axis equal
xlim([-3, 3]); ylim([-0.6, 0.6]);
xticks([-3:1:3]);
yticks([-3:0.3:3]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile();
hold on;
plot(1e9*exp_tomo_cl_m0_Wz180.y.y.Data(2e3:end), 1e9*exp_tomo_cl_m0_Wz180.y.z.Data(2e3:end), 'color', colors(2,:), 'DisplayName', 'CL')
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
plot(100*cos(theta), 50*sin(theta), 'k--', 'DisplayName', 'Beam size')
hold off;
xlabel('$D_y$ [nm]'); ylabel('$D_z$ [nm]');
axis equal
xlim([-300, 300]); ylim([-100, 100]);
% xticks([-3:1:3]);
% yticks([-3:1:3]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

%% Simulation of tomography experiments at 1RPM with all payloads
% Configuration
open(mdl);
set_param(mdl, 'StopTime', '2'); % 30 degrees at 1rpm
initializeLoggingConfiguration('log', 'all', 'Ts', 1e-3);
initializeController('type', 'hac-iff');
initializeReferences(...
    'Rz_type', 'rotating', ...
    'Rz_period', 360/6, ... % 6deg/s, 1 rpm
    'Dh_pos', [P_micro_hexapod; 0; 0; 0]);

% Perform the simulations
initializeSample('type', '0');
sim(mdl);
exp_tomo_cl_m0_1rpm = simout;
initializeSample('type', '1');
sim(mdl);
exp_tomo_cl_m1_1rpm = simout;
initializeSample('type', '2');
sim(mdl);
exp_tomo_cl_m2_1rpm = simout;
initializeSample('type', '3');
sim(mdl);
exp_tomo_cl_m3_1rpm = simout;

% Save the simulation results
save('./mat/test_id31_exp_tomo_cl_1rpm_sim.mat', 'exp_tomo_cl_m0_1rpm', 'exp_tomo_cl_m1_1rpm', 'exp_tomo_cl_m2_1rpm', 'exp_tomo_cl_m3_1rpm');

%% Positioning errors in the Y-Z plane during tomography experiments simulated using the multi-body model
figure;
tiledlayout(2, 2, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile;
hold on;
plot(1e9*exp_tomo_cl_m0_1rpm.y.y.Data(1e3:end), 1e9*exp_tomo_cl_m0_1rpm.y.z.Data(1e3:end), 'color', colors(1,:), 'DisplayName', '$m = 0$ kg')
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
plot(100*cos(theta), 50*sin(theta), 'k--', 'DisplayName', 'Beam size')
axis equal
xticks([-400:100:400]); yticks([-100:100:100]);
xlabel('$D_y$ [nm]'); ylabel('$D_z$ [nm]');
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
plot(1e9*exp_tomo_cl_m1_1rpm.y.y.Data(1e3:end), 1e9*exp_tomo_cl_m1_1rpm.y.z.Data(1e3:end), 'color', colors(2,:), 'DisplayName', '$m = 13$ kg')
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
plot(100*cos(theta), 50*sin(theta), 'k--', 'HandleVisibility', 'off')
axis equal
xticks([-400:100:400]); yticks([-100:100:100]);
xlabel('$D_y$ [nm]'); ylabel('$D_z$ [nm]');
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax3 = nexttile;
hold on;
plot(1e9*exp_tomo_cl_m2_1rpm.y.y.Data(1e3:end), 1e9*exp_tomo_cl_m2_1rpm.y.z.Data(1e3:end), 'color', colors(3,:), 'DisplayName', '$m = 26$ kg')
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
plot(100*cos(theta), 50*sin(theta), 'k--', 'HandleVisibility', 'off')
axis equal
xticks([-400:100:400]); yticks([-100:100:100]);
xlabel('$D_y$ [nm]'); ylabel('$D_z$ [nm]');
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax4 = nexttile;
hold on;
plot(1e9*exp_tomo_cl_m3_1rpm.y.y.Data(1e3:end), 1e9*exp_tomo_cl_m3_1rpm.y.z.Data(1e3:end), 'color', colors(4,:), 'DisplayName', '$m = 39$ kg')
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
plot(100*cos(theta), 50*sin(theta), 'k--', 'HandleVisibility', 'off')
axis equal
xticks([-400:100:400]); yticks([-100:100:100]);
xlabel('$D_y$ [nm]'); ylabel('$D_z$ [nm]');
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

linkaxes([ax1,ax2,ax3, ax4],'xy');
xlim([-450, 450]); ylim([-100, 100]);