%% test_id31_5_experiments.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/';

%% 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 Tomography scans with robust controller
data_tomo_m0_Wz6 = load("2023-08-11_11-37_tomography_1rpm_m0.mat");
data_tomo_m0_Wz6.time = Ts*[0:length(data_tomo_m0_Wz6.Rz)-1];

data_tomo_m1_Wz6 = load("2023-08-11_11-15_tomography_1rpm_m1.mat");
data_tomo_m1_Wz6.time = Ts*[0:length(data_tomo_m1_Wz6.Rz)-1];

data_tomo_m2_Wz6 = load("2023-08-11_10-59_tomography_1rpm_m2.mat");
data_tomo_m2_Wz6.time = Ts*[0:length(data_tomo_m2_Wz6.Rz)-1];

data_tomo_m3_Wz6 = load("2023-08-11_10-24_tomography_1rpm_m3.mat");
data_tomo_m3_Wz6.time = Ts*[0:length(data_tomo_m3_Wz6.Rz)-1];

%% Find best circle fit for all experiments
[~, i_m0] = find(data_tomo_m0_Wz6.hac_status == 1);
[x_m0, y_m0, R_m0] = circlefit(data_tomo_m0_Wz6.Dx_int(1:i_m0), data_tomo_m0_Wz6.Dy_int(1:i_m0));
fun = @(theta)rms((data_tomo_m0_Wz6.Dx_int(1:i_m0) - (x_m0 + R_m0*cos(data_tomo_m0_Wz6.Rz(1:i_m0)+theta(1)))).^2 + ...
                  (data_tomo_m0_Wz6.Dy_int(1:i_m0) - (y_m0 + R_m0*sin(data_tomo_m0_Wz6.Rz(1:i_m0)+theta(1)))).^2);
delta_theta_m0 = fminsearch(fun, 0);

[~, i_m1] = find(data_tomo_m1_Wz6.hac_status == 1);
[x_m1, y_m1, R_m1] = circlefit(data_tomo_m1_Wz6.Dx_int(1:i_m1), data_tomo_m1_Wz6.Dy_int(1:i_m1));
fun = @(theta)rms((data_tomo_m1_Wz6.Dx_int(1:i_m1) - (x_m1 + R_m1*cos(data_tomo_m1_Wz6.Rz(1:i_m1)+theta(1)))).^2 + ...
                  (data_tomo_m1_Wz6.Dy_int(1:i_m1) - (y_m1 + R_m1*sin(data_tomo_m1_Wz6.Rz(1:i_m1)+theta(1)))).^2);
delta_theta_m1 = fminsearch(fun, 0);

[~, i_m2] = find(data_tomo_m2_Wz6.hac_status == 1);
[x_m2, y_m2, R_m2] = circlefit(data_tomo_m2_Wz6.Dx_int(1:i_m2), data_tomo_m2_Wz6.Dy_int(1:i_m2));
fun = @(theta)rms((data_tomo_m2_Wz6.Dx_int(1:i_m2) - (x_m2 + R_m2*cos(data_tomo_m2_Wz6.Rz(1:i_m2)+theta(1)))).^2 + ...
                  (data_tomo_m2_Wz6.Dy_int(1:i_m2) - (y_m2 + R_m2*sin(data_tomo_m2_Wz6.Rz(1:i_m2)+theta(1)))).^2);
delta_theta_m2 = fminsearch(fun, 0);

[~, i_m3] = find(data_tomo_m3_Wz6.hac_status == 1);
[x_m3, y_m3, R_m3] = circlefit(data_tomo_m3_Wz6.Dx_int(1:i_m3), data_tomo_m3_Wz6.Dy_int(1:i_m3));
fun = @(theta)rms((data_tomo_m3_Wz6.Dx_int(1:i_m3) - (x_m3 + R_m3*cos(data_tomo_m3_Wz6.Rz(1:i_m3)+theta(1)))).^2 + ...
                  (data_tomo_m3_Wz6.Dy_int(1:i_m3) - (y_m3 + R_m3*sin(data_tomo_m3_Wz6.Rz(1:i_m3)+theta(1)))).^2);
delta_theta_m3 = fminsearch(fun, 0);

%% Tomography experiment at 1rpm with 26kg payload
figure;
hold on;
plot(1e6*data_tomo_m2_Wz6.Dx_int(1:10:i_m2), 1e6*data_tomo_m2_Wz6.Dy_int(1:10:i_m2), 'DisplayName', '$m = 26$ kg (OL)')
plot(1e6*data_tomo_m2_Wz6.Dx_int(i_m2:10:i_m2+1e4), 1e6*data_tomo_m2_Wz6.Dy_int(i_m2:10:i_m2+1e4), 'color', colors(3,:), 'HandleVisibility', 'off')
plot(1e6*data_tomo_m2_Wz6.Dx_int(i_m2+1e4:10:end), 1e6*data_tomo_m2_Wz6.Dy_int(i_m2+1e4:10:end), 'color', colors(2,:), 'DisplayName', '$m = 26$ kg (CL)')
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
plot(1e6*(x_m2 + R_m2*cos(theta)), 1e6*(y_m2 + R_m2*sin(theta)), 'k--', 'DisplayName', 'Best Circular Fit')
hold off;
xlabel('$D_x$ [$\mu$m]'); ylabel('$D_y$ [$\mu$m]');
axis equal
xlim([-20, 100]); ylim([-20, 100]);
xticks([-20:20:100]);
yticks([-20:20:100]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

%% Measured radial errors of the Spindle
figure;
hold on;
plot(1e6*(data_tomo_m2_Wz6.Dx_int(1:10:i_m2) - (x_m2 + R_m2*cos(data_tomo_m2_Wz6.Rz(1:10:i_m2)+delta_theta_m2))), ...
     1e6*(data_tomo_m2_Wz6.Dy_int(1:10:i_m2) - (y_m2 + R_m2*sin(data_tomo_m2_Wz6.Rz(1:10:i_m2)+delta_theta_m2))), 'color', colors(1,:), 'DisplayName', '$m = 26$ kg (OL)')
plot(1e6*detrend(data_tomo_m2_Wz6.Dx_int(i_m2+1e4:10:end), 0), 1e6*detrend(data_tomo_m2_Wz6.Dy_int(i_m2+1e4:10:end), 0), 'color', colors(2,:), 'DisplayName', '$m = 26$ kg (CL)')
hold off;
xlabel('$D_x$ [$\mu$m]'); ylabel('$D_y$ [$\mu$m]');
axis equal
xlim([-0.6, 0.4]); ylim([-0.4, 0.6]);
xticks([-0.6:0.2:0.4]);
yticks([-0.4:0.2:0.6]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

%% Tomography experiment at 1rpm - Results in the YZ - All masses tested
figure;
tiledlayout(2, 2, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile;
hold on;
plot(1e9*detrend(data_tomo_m0_Wz6.Dy_int(1:10:i_m0) - y_m0 - R_m0*sin(data_tomo_m0_Wz6.Rz(1:10:i_m0)+delta_theta_m0), 0), 1e9*detrend(data_tomo_m0_Wz6.Dz_int(1:10:i_m0), 0), 'DisplayName', 'OL')
plot(1e9*detrend(data_tomo_m0_Wz6.Dy_int(i_m0+1e4:10:end), 0), 1e9*detrend(data_tomo_m0_Wz6.Dz_int(i_m0+1e4:10:end), 0), 'DisplayName', 'CL')
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
plot(100*cos(theta), 50*sin(theta), 'k--', 'DisplayName', 'Beam')
text(-430, 90, '$m = 0$ kg', 'Horiz','left', 'Vert','top', 'FontWeight', 'bold')
hold off;
xlabel('$D_y$ [nm]'); ylabel('$D_z$ [nm]');
axis equal
xlim([-450, 450]); ylim([-100, 100]);
xticks([-400:100:400]); yticks([-50:50:50]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
plot(1e9*detrend(data_tomo_m1_Wz6.Dy_int(1:10:i_m1) - y_m1 - R_m1*sin(data_tomo_m1_Wz6.Rz(1:10:i_m1)+delta_theta_m1), 0), 1e9*detrend(data_tomo_m1_Wz6.Dz_int(1:10:i_m1), 0), 'DisplayName', 'OL')
plot(1e9*detrend(data_tomo_m1_Wz6.Dy_int(i_m1+1e4:10:end), 0), 1e9*detrend(data_tomo_m1_Wz6.Dz_int(i_m1+1e4:10:end), 0), 'DisplayName', 'CL')
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
plot(100*cos(theta), 50*sin(theta), 'k--', 'HandleVisibility', 'off')
text(-430, 90, '$m = 13$ kg', 'Horiz','left', 'Vert','top', 'FontWeight', 'bold')
hold off;
xlabel('$D_y$ [nm]'); ylabel('$D_z$ [nm]');
axis equal
xlim([-450, 450]); ylim([-100, 100]);
xticks([-400:100:400]); yticks([-50:50:50]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax3 = nexttile;
hold on;
plot(1e9*detrend(data_tomo_m2_Wz6.Dy_int(1:10:i_m2) - y_m2 - R_m2*sin(data_tomo_m2_Wz6.Rz(1:10:i_m2)+delta_theta_m2), 0), 1e9*detrend(data_tomo_m2_Wz6.Dz_int(1:10:i_m2), 0), 'DisplayName', 'OL')
plot(1e9*detrend(data_tomo_m2_Wz6.Dy_int(i_m2+1e4:10:end), 0), 1e9*detrend(data_tomo_m2_Wz6.Dz_int(i_m2+1e4:10:end), 0), 'DisplayName', 'CL')
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
plot(100*cos(theta), 50*sin(theta), 'k--', 'HandleVisibility', 'off')
text(-430, 90, '$m = 26$ kg', 'Horiz','left', 'Vert','top', 'FontWeight', 'bold')
hold off;
xlabel('$D_y$ [nm]'); ylabel('$D_z$ [nm]');
axis equal
xlim([-450, 450]); ylim([-100, 100]);
xticks([-400:100:400]); yticks([-50:50:50]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax4 = nexttile;
hold on;
plot(1e9*detrend(data_tomo_m3_Wz6.Dy_int(1:10:i_m3) - y_m3 - R_m3*sin(data_tomo_m3_Wz6.Rz(1:10:i_m3)+delta_theta_m3), 0), 1e9*detrend(data_tomo_m3_Wz6.Dz_int(1:10:i_m3), 0), 'DisplayName', 'OL')
plot(1e9*detrend(data_tomo_m3_Wz6.Dy_int(i_m3+1e4:10:end), 0), 1e9*detrend(data_tomo_m3_Wz6.Dz_int(i_m3+1e4:10:end), 0), 'DisplayName', 'CL')
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
plot(100*cos(theta), 50*sin(theta), 'k--', 'HandleVisibility', 'off')
text(-860, 180, '$m = 39$ kg', 'Horiz','left', 'Vert','top', 'FontWeight', 'bold')
hold off;
xlabel('$D_y$ [nm]'); ylabel('$D_z$ [nm]');
axis equal
xlim([-900, 900]); ylim([-200, 200]);
xticks([-800:200:800]); yticks([-100:100:100]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

%% Estimate RMS of the errors while in closed-loop and open-loop - Tomography at 6deg/s
% No mass
data_tomo_m0_Wz6.Dy_rms_cl = rms(detrend(data_tomo_m0_Wz6.Dy_int(i_m0+1e4:end), 0));
data_tomo_m0_Wz6.Dz_rms_cl = rms(detrend(data_tomo_m0_Wz6.Dz_int(i_m0+1e4:end), 0));
data_tomo_m0_Wz6.Ry_rms_cl = rms(detrend(data_tomo_m0_Wz6.Ry_int(i_m0+1e4:end), 0));

% Remove eccentricity for OL errors
data_tomo_m0_Wz6.Dy_rms_ol = rms(data_tomo_m0_Wz6.Dy_int(1:i_m0) - (y_m0 + R_m0*sin(data_tomo_m0_Wz6.Rz(1:i_m0)+delta_theta_m0)));
data_tomo_m0_Wz6.Dz_rms_ol = rms(detrend(data_tomo_m0_Wz6.Dz_int(1:i_m0), 0));
[x0, y0, R] = circlefit(data_tomo_m0_Wz6.Rx_int(1:i_m0), data_tomo_m0_Wz6.Ry_int(1:i_m0));
fun = @(theta)rms((data_tomo_m0_Wz6.Rx_int(1:i_m0) - (x0 + R*cos(data_tomo_m0_Wz6.Rz(1:i_m0)+theta(1)))).^2 + ...
                  (data_tomo_m0_Wz6.Ry_int(1:i_m0) - (y0 + R*sin(data_tomo_m0_Wz6.Rz(1:i_m0)+theta(1)))).^2);
delta_theta = fminsearch(fun, 0);
data_tomo_m0_Wz6.Ry_rms_ol = rms(data_tomo_m0_Wz6.Ry_int(1:i_m0) - (y0 + R*sin(data_tomo_m0_Wz6.Rz(1:i_m0)+delta_theta)));

% 1 "layer mass"
data_tomo_m1_Wz6.Dy_rms_cl = rms(detrend(data_tomo_m1_Wz6.Dy_int(i_m1+1e4:end), 0));
data_tomo_m1_Wz6.Dz_rms_cl = rms(detrend(data_tomo_m1_Wz6.Dz_int(i_m1+1e4:end), 0));
data_tomo_m1_Wz6.Ry_rms_cl = rms(detrend(data_tomo_m1_Wz6.Ry_int(i_m1+1e4:end), 0));

% Remove eccentricity for OL errors
data_tomo_m1_Wz6.Dy_rms_ol = rms(data_tomo_m1_Wz6.Dy_int(1:i_m1) - (y_m1 + R_m1*sin(data_tomo_m1_Wz6.Rz(1:i_m1)+delta_theta_m1)));
data_tomo_m1_Wz6.Dz_rms_ol = rms(detrend(data_tomo_m1_Wz6.Dz_int(1:i_m1), 0));
[x0, y0, R] = circlefit(data_tomo_m1_Wz6.Rx_int(1:i_m1), data_tomo_m1_Wz6.Ry_int(1:i_m1));
fun = @(theta)rms((data_tomo_m1_Wz6.Rx_int(1:i_m1) - (x0 + R*cos(data_tomo_m1_Wz6.Rz(1:i_m1)+theta(1)))).^2 + ...
                  (data_tomo_m1_Wz6.Ry_int(1:i_m1) - (y0 + R*sin(data_tomo_m1_Wz6.Rz(1:i_m1)+theta(1)))).^2);
delta_theta = fminsearch(fun, 0);
data_tomo_m1_Wz6.Ry_rms_ol = rms(data_tomo_m1_Wz6.Ry_int(1:i_m1) - (y0 + R*sin(data_tomo_m1_Wz6.Rz(1:i_m1)+delta_theta)));

% 2 "layer masses"
data_tomo_m2_Wz6.Dy_rms_cl = rms(detrend(data_tomo_m2_Wz6.Dy_int(i_m2+1e4:end), 0));
data_tomo_m2_Wz6.Dz_rms_cl = rms(detrend(data_tomo_m2_Wz6.Dz_int(i_m2+1e4:end), 0));
data_tomo_m2_Wz6.Ry_rms_cl = rms(detrend(data_tomo_m2_Wz6.Ry_int(i_m2+1e4:end), 0));

% Remove eccentricity for OL errors
data_tomo_m2_Wz6.Dy_rms_ol = rms(data_tomo_m2_Wz6.Dy_int(1:i_m2) - (y_m2 + R_m2*sin(data_tomo_m2_Wz6.Rz(1:i_m2)+delta_theta_m2)));
data_tomo_m2_Wz6.Dz_rms_ol = rms(detrend(data_tomo_m2_Wz6.Dz_int(1:i_m2), 0));
[x0, y0, R] = circlefit(data_tomo_m2_Wz6.Rx_int(1:i_m2), data_tomo_m2_Wz6.Ry_int(1:i_m2));
fun = @(theta)rms((data_tomo_m2_Wz6.Rx_int(1:i_m2) - (x0 + R*cos(data_tomo_m2_Wz6.Rz(1:i_m2)+theta(1)))).^2 + ...
                  (data_tomo_m2_Wz6.Ry_int(1:i_m2) - (y0 + R*sin(data_tomo_m2_Wz6.Rz(1:i_m2)+theta(1)))).^2);
delta_theta = fminsearch(fun, 0);
data_tomo_m2_Wz6.Ry_rms_ol = rms(data_tomo_m2_Wz6.Ry_int(1:i_m2) - (y0 + R*sin(data_tomo_m2_Wz6.Rz(1:i_m2)+delta_theta)));

% 3 "layer masses"
data_tomo_m3_Wz6.Dy_rms_cl = rms(detrend(data_tomo_m3_Wz6.Dy_int(i_m3+1e4:end), 0));
data_tomo_m3_Wz6.Dz_rms_cl = rms(detrend(data_tomo_m3_Wz6.Dz_int(i_m3+1e4:end), 0));
data_tomo_m3_Wz6.Ry_rms_cl = rms(detrend(data_tomo_m3_Wz6.Ry_int(i_m3+1e4:end), 0));

% Remove eccentricity for OL errors
data_tomo_m3_Wz6.Dy_rms_ol = rms(data_tomo_m3_Wz6.Dy_int(1:i_m3) - (y_m3 + R_m3*sin(data_tomo_m3_Wz6.Rz(1:i_m3)+delta_theta_m3)));
data_tomo_m3_Wz6.Dz_rms_ol = rms(detrend(data_tomo_m3_Wz6.Dz_int(1:i_m3), 0));
[x0, y0, R] = circlefit(data_tomo_m3_Wz6.Rx_int(1:i_m3), data_tomo_m3_Wz6.Ry_int(1:i_m3));
fun = @(theta)rms((data_tomo_m3_Wz6.Rx_int(1:i_m3) - (x0 + R*cos(data_tomo_m3_Wz6.Rz(1:i_m3)+theta(1)))).^2 + ...
                  (data_tomo_m3_Wz6.Ry_int(1:i_m3) - (y0 + R*sin(data_tomo_m3_Wz6.Rz(1:i_m3)+theta(1)))).^2);
delta_theta = fminsearch(fun, 0);
data_tomo_m3_Wz6.Ry_rms_ol = rms(data_tomo_m3_Wz6.Ry_int(1:i_m3) - (y0 + R*sin(data_tomo_m3_Wz6.Rz(1:i_m3)+delta_theta)));

%% Experimental Results for Tomography at 180deg/s, no payload
data_tomo_m0_Wz180 = load('2023-08-17_15-26_tomography_30rpm_m0_robust.mat');

[~, i_m0] = find(data_tomo_m0_Wz180.hac_status == 1);
[x_m0, y_m0, R_m0] = circlefit(data_tomo_m0_Wz180.Dx_int(1:i_m0), data_tomo_m0_Wz180.Dy_int(1:i_m0));
fun = @(theta)rms((data_tomo_m0_Wz180.Dx_int(1:i_m0) - (x_m0 + R_m0*cos(data_tomo_m0_Wz180.Rz(1:i_m0)+theta(1)))).^2 + ...
                  (data_tomo_m0_Wz180.Dy_int(1:i_m0) - (y_m0 + R_m0*sin(data_tomo_m0_Wz180.Rz(1:i_m0)+theta(1)))).^2);
delta_theta_m0 = fminsearch(fun, 0);

%% Tomography at 180deg/s - Errors in the X/Y plane
figure;
hold on;
plot(1e6*data_tomo_m0_Wz180.Dx_int(1:i_m0), 1e6*data_tomo_m0_Wz180.Dy_int(1:i_m0), 'DisplayName', 'OL')
plot(1e6*data_tomo_m0_Wz180.Dx_int(i_m0:i_m0+1e4), 1e6*data_tomo_m0_Wz180.Dy_int(i_m0:i_m0+1e4), 'color', colors(3,:), 'HandleVisibility', 'off')
plot(1e6*data_tomo_m0_Wz180.Dx_int(i_m0+1e4:end), 1e6*data_tomo_m0_Wz180.Dy_int(i_m0+1e4:end), 'color', colors(2,:), 'DisplayName', 'CL')
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
plot(1e6*(x_m0 + R_m0*cos(theta)), 1e6*(y_m0 + R_m0*sin(theta)), 'k--', 'DisplayName', 'Circ. Fit')
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;

%% Tomography at 180deg/s - Errors in the Y/Z plane
figure;
tiledlayout(2, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(1e6*data_tomo_m0_Wz180.Dy_int(1:i_m0), 1e6*data_tomo_m0_Wz180.Dz_int(1:i_m0), 'DisplayName', 'OL')
plot(1e6*data_tomo_m0_Wz180.Dy_int(i_m0:i_m0+1e4), 1e6*data_tomo_m0_Wz180.Dz_int(i_m0:i_m0+1e4), 'color', colors(3,:), 'HandleVisibility', 'off')
plot(1e6*data_tomo_m0_Wz180.Dy_int(i_m0+1e4:end), 1e6*data_tomo_m0_Wz180.Dz_int(i_m0+1e4: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*data_tomo_m0_Wz180.Dy_int(i_m0+1e4:end), 1e9*data_tomo_m0_Wz180.Dz_int(i_m0+1e4: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]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

%% Estimate RMS of the errors while in closed-loop and open-loop - Tomography at 180deg/s
% No mass
data_tomo_m0_Wz180.Dy_rms_cl = rms(detrend(data_tomo_m0_Wz180.Dy_int(i_m0+1e4:end), 0));
data_tomo_m0_Wz180.Dz_rms_cl = rms(detrend(data_tomo_m0_Wz180.Dz_int(i_m0+1e4:end), 0));
data_tomo_m0_Wz180.Ry_rms_cl = rms(detrend(data_tomo_m0_Wz180.Ry_int(i_m0+1e4:end), 0));

% Remove eccentricity for OL errors
data_tomo_m0_Wz180.Dy_rms_ol = rms(data_tomo_m0_Wz180.Dy_int(1:i_m0) - (y_m0 + R_m0*sin(data_tomo_m0_Wz180.Rz(1:i_m0)+delta_theta_m0)));
data_tomo_m0_Wz180.Dz_rms_ol = rms(detrend(data_tomo_m0_Wz180.Dz_int(1:i_m0), 0));
[x0, y0, R] = circlefit(data_tomo_m0_Wz180.Rx_int(1:i_m0), data_tomo_m0_Wz180.Ry_int(1:i_m0));
fun = @(theta)rms((data_tomo_m0_Wz180.Rx_int(1:i_m0) - (x0 + R*cos(data_tomo_m0_Wz180.Rz(1:i_m0)+theta(1)))).^2 + ...
                  (data_tomo_m0_Wz180.Ry_int(1:i_m0) - (y0 + R*sin(data_tomo_m0_Wz180.Rz(1:i_m0)+theta(1)))).^2);
delta_theta = fminsearch(fun, 0);
data_tomo_m0_Wz180.Ry_rms_ol = rms(data_tomo_m0_Wz180.Ry_int(1:i_m0) - (y0 + R*sin(data_tomo_m0_Wz180.Rz(1:i_m0)+delta_theta)));

%% Jacobian to compute the motion in the X-Y-Z-Rx-Ry directions
J_int_to_X = [ 0                 0                -0.787401574803149 -0.212598425196851  0;
               0.78740157480315  0.21259842519685  0                  0                  0;
               0                 0                 0                  0                 -1;
             -13.1233595800525  13.1233595800525   0                  0                  0;
               0                 0               -13.1233595800525   13.1233595800525    0];

%% Parameters for frequency analysis computation
Nfft = floor(20.0/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

%% Open-Loop measurement
data_ol_Wz180 = load('2023-08-11_16-51_m0_lac_off.mat'); % no rotation

a = J_int_to_X*[data_ol_Wz180.d1; data_ol_Wz180.d2; data_ol_Wz180.d3; data_ol_Wz180.d4; data_ol_Wz180.d5];
data_ol_Wz180.Dx_int = a(1,:);
data_ol_Wz180.Dy_int = a(2,:);
data_ol_Wz180.Dz_int = a(3,:);
data_ol_Wz180.Rx_int = a(4,:);
data_ol_Wz180.Ry_int = a(5,:);

[data_ol_Wz180.pxx_Dx, data_ol_Wz180.f] = pwelch(detrend(data_ol_Wz180.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
[data_ol_Wz180.pxx_Dy, ~              ] = pwelch(detrend(data_ol_Wz180.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
[data_ol_Wz180.pxx_Dz, ~              ] = pwelch(detrend(data_ol_Wz180.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
[data_ol_Wz180.pxx_Rx, ~              ] = pwelch(detrend(data_ol_Wz180.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
[data_ol_Wz180.pxx_Ry, ~              ] = pwelch(detrend(data_ol_Wz180.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);

%% Effect of LAC - 180 deg/s
data_lac_Wz180 = load('2023-08-11_17-36_m0_lac_on_30rpm.mat');

a = J_int_to_X*[data_lac_Wz180.d1; data_lac_Wz180.d2; data_lac_Wz180.d3; data_lac_Wz180.d4; data_lac_Wz180.d5];
data_lac_Wz180.Dx_int = a(1,:);
data_lac_Wz180.Dy_int = a(2,:);
data_lac_Wz180.Dz_int = a(3,:);
data_lac_Wz180.Rx_int = a(4,:);
data_lac_Wz180.Ry_int = a(5,:);

[data_lac_Wz180.pxx_Dx, data_lac_Wz180.f] = pwelch(detrend(data_lac_Wz180.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
[data_lac_Wz180.pxx_Dy, ~               ] = pwelch(detrend(data_lac_Wz180.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
[data_lac_Wz180.pxx_Dz, ~               ] = pwelch(detrend(data_lac_Wz180.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
[data_lac_Wz180.pxx_Rx, ~               ] = pwelch(detrend(data_lac_Wz180.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
[data_lac_Wz180.pxx_Ry, ~               ] = pwelch(detrend(data_lac_Wz180.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);

%% Effect of HAC - 180 deg/s
data_hac_Wz180 = load('2023-08-11_16-49_m0_hac_on.mat');

a = J_int_to_X*[data_hac_Wz180.d1; data_hac_Wz180.d2; data_hac_Wz180.d3; data_hac_Wz180.d4; data_hac_Wz180.d5];
data_hac_Wz180.Dx_int = a(1,:);
data_hac_Wz180.Dy_int = a(2,:);
data_hac_Wz180.Dz_int = a(3,:);
data_hac_Wz180.Rx_int = a(4,:);
data_hac_Wz180.Ry_int = a(5,:);

[data_hac_Wz180.pxx_Dx, data_hac_Wz180.f] = pwelch(detrend(data_hac_Wz180.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
[data_hac_Wz180.pxx_Dy, ~               ] = pwelch(detrend(data_hac_Wz180.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
[data_hac_Wz180.pxx_Dz, ~               ] = pwelch(detrend(data_hac_Wz180.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
[data_hac_Wz180.pxx_Rx, ~               ] = pwelch(detrend(data_hac_Wz180.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
[data_hac_Wz180.pxx_Ry, ~               ] = pwelch(detrend(data_hac_Wz180.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);

% Compute closed-loop RMS errors
data_hac_Wz180.Dy_rms_cl = rms(detrend(data_hac_Wz180.Dy_int(1e4:end), 0));
data_hac_Wz180.Dz_rms_cl = rms(detrend(data_hac_Wz180.Dz_int(1e4:end), 0));
data_hac_Wz180.Ry_rms_cl = rms(detrend(data_hac_Wz180.Ry_int(1e4:end), 0));

%% Cumulative Amplitude Spectrum - Closed-Loop - Dy
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
hold on;
plot(data_ol_Wz180.f,  sqrt(flip(-cumtrapz(flip(data_ol_Wz180.f),  flip(data_ol_Wz180.pxx_Dy)))),  'DisplayName', sprintf('OL $%.1f \\mu m$',  1e6*rms(detrend(data_ol_Wz180.Dy_int,  0))));
plot(data_lac_Wz180.f, sqrt(flip(-cumtrapz(flip(data_lac_Wz180.f), flip(data_lac_Wz180.pxx_Dy)))), 'DisplayName', sprintf('LAC $%.1f \\mu m$', 1e6*rms(detrend(data_lac_Wz180.Dy_int, 0))));
plot(data_hac_Wz180.f, sqrt(flip(-cumtrapz(flip(data_hac_Wz180.f), flip(data_hac_Wz180.pxx_Dy)))), 'DisplayName', sprintf('HAC $%.0f nm$',     1e9*rms(detrend(data_hac_Wz180.Dy_int, 0))));
plot([1e-2, 1e4], 1e-9*[specs_dy_rms, specs_dy_rms], 'k--', 'DisplayName', sprintf('Spec: $%.0f$nm', specs_dy_rms))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('CAS [m]');
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xticks([1e0, 1e1, 1e2]); yticks([1e-9, 1e-8, 1e-7, 1e-6, 1e-5]);
xlim([0.1, 5e2]); ylim([1e-10, 2e-5]);

%% Cumulative Amplitude Spectrum - Closed-Loop - Dz
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
hold on;
plot(data_ol_Wz180.f,  sqrt(flip(-cumtrapz(flip(data_ol_Wz180.f),  flip(data_ol_Wz180.pxx_Dz)))),  'DisplayName', sprintf('OL $%.0f nm$',  1e9*rms(detrend(data_ol_Wz180.Dz_int,  0))));
plot(data_lac_Wz180.f, sqrt(flip(-cumtrapz(flip(data_lac_Wz180.f), flip(data_lac_Wz180.pxx_Dz)))), 'DisplayName', sprintf('LAC $%.0f nm$', 1e9*rms(detrend(data_lac_Wz180.Dz_int, 0))));
plot(data_hac_Wz180.f, sqrt(flip(-cumtrapz(flip(data_hac_Wz180.f), flip(data_hac_Wz180.pxx_Dz)))), 'DisplayName', sprintf('HAC $%.0f nm$', 1e9*rms(detrend(data_hac_Wz180.Dz_int, 0))));
plot([1e-2, 1e4], 1e-9*[specs_dz_rms, specs_dz_rms], 'k--', 'DisplayName', sprintf('Spec: $%.0f$nm', specs_dz_rms))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('CAS [m]');
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xticks([1e0, 1e1, 1e2]); yticks([1e-9, 1e-8, 1e-7, 1e-6, 1e-5]);
xlim([0.1, 5e2]); ylim([1e-10, 2e-5]);

%% Cumulative Amplitude Spectrum - Closed-Loop - Ry
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
hold on;
plot(data_ol_Wz180.f,  sqrt(flip(-cumtrapz(flip(data_ol_Wz180.f),  flip(data_ol_Wz180.pxx_Ry)))),  'DisplayName', sprintf('OL $%.0f \\mu$rad',  1e6*rms(detrend(data_ol_Wz180.Ry_int,  0))));
plot(data_lac_Wz180.f, sqrt(flip(-cumtrapz(flip(data_lac_Wz180.f), flip(data_lac_Wz180.pxx_Ry)))), 'DisplayName', sprintf('LAC $%.0f \\mu$rad', 1e6*rms(detrend(data_lac_Wz180.Ry_int, 0))));
plot(data_hac_Wz180.f, sqrt(flip(-cumtrapz(flip(data_hac_Wz180.f), flip(data_hac_Wz180.pxx_Ry)))), 'DisplayName', sprintf('HAC $%.2f \\mu$rad', 1e6*rms(detrend(data_hac_Wz180.Ry_int, 0))));
plot([1e-2, 1e4], 1e-6*[specs_ry_rms, specs_ry_rms], 'k--', 'DisplayName', sprintf('Spec: $%.2f \\mu$rad', specs_ry_rms))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('CAS [rad]');
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xticks([1e0, 1e1, 1e2]); yticks([1e-9, 1e-8, 1e-7, 1e-6, 1e-5]);
xlim([0.1, 5e2]); ylim([1e-10, 2e-5]);

%% Load data for the reflectivity scan
data_ry = load("2023-08-18_15-24_first_reflectivity_m0.mat");
data_ry.time = Ts*[0:length(data_ry.Ry_int)-1];

% Compute closed-loop errors
data_ry.Dy_rms_cl = rms(detrend(data_ry.e_dy,0)); % [m RMS]
data_ry.Dz_rms_cl = rms(detrend(data_ry.e_dz,0)); % [m RMS]
data_ry.Ry_rms_cl = rms(detrend(data_ry.e_ry,0)); % [rad RMS]

%% Ry reflectivity scan - Lateral error
figure;
hold on;
plot(data_ry.time, 1e9*data_ry.e_dy, 'DisplayName', sprintf('$\\epsilon D_y = %.0f$ nm RMS', 1e9*rms(data_ry.e_dy)))
plot([0, 6.2], [specs_dy_peak, specs_dy_peak], '--', 'color', colors(1,:), 'HandleVisibility', 'off');
plot([0, 6.2], [-specs_dy_peak, -specs_dy_peak], '--', 'color', colors(1,:), 'HandleVisibility', 'off');
hold off;
xlabel('Time [s]');
ylabel('$D_y$ error [nm]')
% legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
xlim([0, 6.2]);
ylim([-150, 150]);
xticks([0:2:6]);

%% Ry reflectivity scan - Vertical error
figure;
hold on;
plot(data_ry.time, 1e9*data_ry.e_dz, 'DisplayName', sprintf('$\\epsilon D_z = %.0f$ nm RMS', 1e9*rms(data_ry.e_dz)))
plot([0, 6.2], [specs_dz_peak, specs_dz_peak], '--', 'color', colors(1,:), 'HandleVisibility', 'off');
plot([0, 6.2], [-specs_dz_peak, -specs_dz_peak], '--', 'color', colors(1,:), 'HandleVisibility', 'off');
hold off;
xlabel('Time [s]');
ylabel('$D_z$ error [nm]')
% legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
xlim([0, 6.2]);
ylim([-100, 100]);
xticks([0:2:6]);

%% Ry reflectivity scan - Setpoint and Error
figure;
yyaxis left
hold on;
plot(data_ry.time, 1e6*data_ry.e_ry, 'DisplayName', '$\epsilon_{R_y}$')
plot([0, 6.2], [specs_ry_peak, specs_ry_peak], '--', 'HandleVisibility', 'off');
plot([0, 6.2], [-specs_ry_peak, -specs_ry_peak], '--', 'HandleVisibility', 'off');
hold off;
ylim([-2, 2])
ylabel('$R_y$ error [$\mu$rad]')
yyaxis right
hold on;
plot(data_ry.time, 1e6*data_ry.Ry_int, 'DisplayName', '$R_y$')
plot(data_ry.time, 1e6*data_ry.m_hexa_ry, 'k--', 'DisplayName', 'Setpoint')
hold off;
xlabel('Time [s]');
ylabel('$R_y$ motion [$\mu$rad]')
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xlim([0, 6.2]);
ylim([-310, 310]);
xticks([0:2:6]);

%% Load Dz steps data
data_dz_steps_10nm = load("2023-08-18_14-57_dz_mim_10_nm.mat");
data_dz_steps_10nm.time = Ts*[0:length(data_dz_steps_10nm.Dz_int)-1];

data_dz_steps_100nm = load("2023-08-18_14-57_dz_mim_100_nm.mat");
data_dz_steps_100nm.time = Ts*[0:length(data_dz_steps_100nm.Dz_int)-1];

data_dz_steps_1000nm = load("2023-08-18_14-57_dz_mim_1000_nm.mat");
data_dz_steps_1000nm.time = Ts*[0:length(data_dz_steps_1000nm.Dz_int)-1];

%% Dz MIM test with 10nm steps
figure;
hold on;
plot(data_dz_steps_10nm.time, 1e9*(data_dz_steps_10nm.Dz_int - mean(data_dz_steps_10nm.Dz_int(1:1000))), 'DisplayName', '$D_z$')
plot(data_dz_steps_10nm.time, 1e9*lsim(1/(1 + s/2/pi/20), data_dz_steps_10nm.Dz_int - mean(data_dz_steps_10nm.Dz_int(1:1000)), data_dz_steps_10nm.time), 'DisplayName', '$D_z$ (LPF)')
plot(data_dz_steps_10nm.time, 1e9*(data_dz_steps_10nm.m_hexa_dz-data_dz_steps_10nm.m_hexa_dz(1)), 'k--', 'DisplayName', 'Setpoint')
hold off;
xlabel('Time [s]');
ylabel('$D_z$ Motion [nm]');
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
xlim([0, 0.6]);
ylim([-10, 40]);
xticks([0:0.2:0.6]);
yticks([-10:10:50]);

%% Dz MIM test with 100nm steps
figure;
hold on;
plot(data_dz_steps_100nm.time, 1e9*(data_dz_steps_100nm.Dz_int - mean(data_dz_steps_100nm.Dz_int(1:1000))), 'DisplayName', '$D_z$')
plot(data_dz_steps_100nm.time, 1e9*(data_dz_steps_100nm.m_hexa_dz-data_dz_steps_100nm.m_hexa_dz(1)), 'k--', 'DisplayName', 'Setpoint')
hold off;
xlabel('Time [s]');
ylabel('$D_z$ Motion [nm]');
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
xlim([0, 0.6]);
% ylim([-10, 40]);
xticks([0:0.2:0.6]);
yticks([-0:100:300]);

%% Dz step response - Stabilization time is around 70ms
figure;
[~, i] = find(data_dz_steps_1000nm.m_hexa_dz>data_dz_steps_1000nm.m_hexa_dz(1));
i0 = i(1);

figure;
hold on;
plot(1e3*(data_dz_steps_1000nm.time-data_dz_steps_1000nm.time(i0)), 1e6*(data_dz_steps_1000nm.Dz_int - mean(data_dz_steps_1000nm.Dz_int(1:1000))))
plot(1e3*[-1, 1], 1e-3*[1000-20, 1000-20], 'k--')
plot(1e3*[-1, 1], 1e-3*[1000+20, 1000+20], 'k--')
xline(0, 'k--', 'LineWidth', 1.5)
xline(70, 'k--', 'LineWidth', 1.5)
hold off;
xlabel('Time [ms]');
ylabel('$D_z$ Motion [$\mu$m]');
xlim([-10, 140]);
ylim([-0.1, 1.6])
xticks([0, 70])
yticks([0, 1])

%% Dirty layer scans - 10um/s
data_dz_10ums = load("2023-08-18_15-33_dirty_layer_m0_small.mat");
data_dz_10ums.time = Ts*[0:length(data_dz_10ums.Dz_int)-1];

%% Dirty layer scans - 100um/s
data_dz_100ums = load("2023-08-18_15-32_dirty_layer_m0.mat");
data_dz_100ums.time = Ts*[0:length(data_dz_100ums.Dz_int)-1];

%% Performances for Dz scans - 10 um/s
% Determine when the motion starts and stops
i_dz_10ums  = abs(diff(data_dz_10ums.m_hexa_dz)/Ts-10e-6) < 10*eps;

% RMS error
data_dz_10ums.Dy_rms_cl = rms(detrend(data_dz_10ums.e_dy(i_dz_10ums), 0));
data_dz_10ums.Dz_rms_cl = rms(detrend(data_dz_10ums.e_dz(i_dz_10ums), 0));
data_dz_10ums.Ry_rms_cl = rms(detrend(data_dz_10ums.e_ry(i_dz_10ums), 0));

%% Performances for Dz scans - 100 um/s
i_dz_100ums = abs(diff(data_dz_100ums.m_hexa_dz)/Ts-100e-6) < 10*eps;

% RMS error
data_dz_100ums.Dy_rms_cl = rms(detrend(data_dz_100ums.e_dy(i_dz_100ums), 0));
data_dz_100ums.Dz_rms_cl = rms(detrend(data_dz_100ums.e_dz(i_dz_100ums), 0));
data_dz_100ums.Ry_rms_cl = rms(detrend(data_dz_100ums.e_ry(i_dz_100ums), 0));

%% Dz scan at 10um/s - Lateral error
figure;
hold on;
plot(data_dz_10ums.time, 1e9*data_dz_10ums.e_dy, 'DisplayName', sprintf('$\\epsilon D_y: %.0f$ nm RMS', 1e9*rms(data_dz_10ums.e_dy)))
plot([0, 2.2], [specs_dy_peak, specs_dy_peak], '--', 'color', colors(1,:), 'HandleVisibility', 'off');
plot([0, 2.2], [-specs_dy_peak, -specs_dy_peak], '--', 'color', colors(1,:), 'HandleVisibility', 'off');
hold off;
xlabel('Time [s]');
ylabel('$D_y$ error [nm]')
% leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
% leg.ItemTokenSize(1) = 15;
xlim([0, 2.2]);
ylim([-150, 150])

%% Dz scan at 10um/s - Vertical error
figure;
yyaxis left
hold on;
plot(data_dz_10ums.time, 1e9*data_dz_10ums.e_dz, 'DisplayName', '$\epsilon_{D_z}$')
plot([0, 2.2], [specs_dz_peak, specs_dz_peak], '--', 'HandleVisibility', 'off');
plot([0, 2.2], [-specs_dz_peak, -specs_dz_peak], '--', 'HandleVisibility', 'off');
hold off;
ylabel('$D_z$ error [nm]');
ylim([-100, 100]);
yticks([-50:50:50]);
yyaxis right
hold on;
plot(data_dz_10ums.time, 1e6*(data_dz_10ums.Dz_int), 'DisplayName', '$D_z$')
plot(data_dz_10ums.time, 1e6*(data_dz_10ums.m_hexa_dz), 'k--', 'DisplayName', 'Setpoint')
hold off;
xlabel('Time [s]');
ylabel('$D_z$ Motion [$\mu$m]');
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xlim([0, 2.2]);
ylim([-10, 10]);

%% Dz scan at 10um/s - Ry error
figure;
hold on;
plot(data_dz_10ums.time, 1e6*data_dz_10ums.e_ry, 'DisplayName', sprintf('$\\epsilon R_y: %.2f \\mu$rad RMS', 1e6*rms(data_dz_10ums.e_ry)))
plot([0, 2.2], [specs_ry_peak, specs_ry_peak], '--', 'color', colors(1,:), 'HandleVisibility', 'off');
plot([0, 2.2], [-specs_ry_peak, -specs_ry_peak], '--', 'color', colors(1,:), 'HandleVisibility', 'off');
hold off;
xlabel('Time [s]');
ylabel('$R_y$ error [$\mu$rad]')
% leg = legend('location', 'north', 'FontSize', 8, 'NumColumns', 1);
% leg.ItemTokenSize(1) = 15;
xlim([0, 2.2]);
ylim([-2, 2]);

%% Dz scan at 100um/s - Lateral error
figure;
hold on;
plot(data_dz_100ums.time, 1e9*data_dz_100ums.e_dy, 'DisplayName', sprintf('$\\epsilon D_y = %.0f$ nm RMS', 1e9*rms(data_dz_100ums.e_dy)))
plot([0, 2.2], [specs_dy_peak, specs_dy_peak], '--', 'color', colors(1,:), 'HandleVisibility', 'off');
plot([0, 2.2], [-specs_dy_peak, -specs_dy_peak], '--', 'color', colors(1,:), 'HandleVisibility', 'off');
hold off;
xlabel('Time [s]');
ylabel('$D_y$ error [nm]')
% legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
xlim([0, 2.2]);
ylim([-150, 150])

%% Dz scan at 100um/s - Vertical error
figure;
yyaxis left
hold on;
plot(data_dz_100ums.time, 1e9*data_dz_100ums.e_dz, 'DisplayName', '$\epsilon_{d_z}$')
plot([0, 2.2], [specs_dz_peak, specs_dz_peak], '--', 'HandleVisibility', 'off');
plot([0, 2.2], [-specs_dz_peak, -specs_dz_peak], '--', 'HandleVisibility', 'off');
hold off;
ylabel('$D_z$ error [nm]');
ylim([-100, 100]);
yticks([-50:50:50]);
yyaxis right
hold on;
plot(data_dz_100ums.time, 1e6*(data_dz_100ums.Dz_int), 'DisplayName', '$D_z$')
plot(data_dz_100ums.time, 1e6*(data_dz_100ums.m_hexa_dz), 'k--', 'DisplayName', 'Setpoint')
hold off;
xlabel('Time [s]');
ylabel('$D_z$ Motion [$\mu$m]');
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xlim([0, 2.2]);
ylim([-100, 100]);

%% Dz scan at 100um/s - Tilt error
figure;
hold on;
plot(data_dz_100ums.time, 1e6*data_dz_100ums.e_ry, 'DisplayName', sprintf('$\\epsilon R_y = %.2f \\mu$rad RMS', 1e6*rms(data_dz_100ums.e_ry)))
plot([0, 2.2], [specs_ry_peak, specs_ry_peak], '--', 'color', colors(1,:), 'HandleVisibility', 'off');
plot([0, 2.2], [-specs_ry_peak, -specs_ry_peak], '--', 'color', colors(1,:), 'HandleVisibility', 'off');
hold off;
xlabel('Time [s]');
ylabel('$R_y$ error [$\mu$rad]')
% legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
xlim([0, 2.2]);
ylim([-2, 2])

%% Slow Ty scan (10um/s) - OL
data_ty_ol_10ums = load("2023-08-21_20-05_ty_scan_m1_open_loop_slow.mat");
data_ty_ol_10ums.time = Ts*[0:length(data_ty_ol_10ums.Dy_int)-1];

%% Slow Ty scan (10um/s) - CL
data_ty_cl_10ums = load("2023-08-21_20-07_ty_scan_m1_cf_closed_loop_slow.mat");
data_ty_cl_10ums.time = Ts*[0:length(data_ty_cl_10ums.Dy_int)-1];

%% Ty scan (at 10um/s) - Dy errors
figure;
hold on;
plot(1e6*data_ty_ol_10ums.Ty, 1e6*detrend(data_ty_ol_10ums.e_dy, 0), ...
     'DisplayName', 'Open-loop')
plot(1e6*data_ty_cl_10ums.Ty, 1e6*detrend(data_ty_cl_10ums.e_dy, 0), ...
     'DisplayName', 'Closed-loop')
plot([-100, 100], 1e-3*[specs_dy_peak, specs_dy_peak], 'k--', 'DisplayName', 'Specifications');
plot([-100, 100], 1e-3*[-specs_dy_peak, -specs_dy_peak], 'k--', 'HandleVisibility', 'off');
hold off;
xlabel('Ty position [$\mu$m]');
ylabel('$D_y$ error [$\mu$m]');
xlim([-100, 100])
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

%% Ty scan (at 10um/s) - Dz and Ry errors
figure;
hold on;
plot(1e6*data_ty_ol_10ums.Ty, 1e6*detrend(data_ty_ol_10ums.e_dz, 0), ...
     'DisplayName', 'Open-loop')
plot(1e6*data_ty_cl_10ums.Ty, 1e6*detrend(data_ty_cl_10ums.e_dz, 0), ...
     'DisplayName', 'Closed-loop')
plot([-100, 100], 1e-3*[specs_dz_peak, specs_dz_peak], 'k--', 'DisplayName', 'Specifications');
plot([-100, 100], 1e-3*[-specs_dz_peak, -specs_dz_peak], 'k--', 'HandleVisibility', 'off');
hold off;
xlabel('Ty position [$\mu$m]');
ylabel('$D_z$ error [$\mu$m]');
xlim([-100, 100])
ylim([-0.4, 0.4])
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

figure;
hold on;
plot(1e6*data_ty_ol_10ums.Ty, 1e6*data_ty_ol_10ums.e_ry, ...
     'DisplayName', 'Open-loop')
plot(1e6*data_ty_cl_10ums.Ty, 1e6*data_ty_cl_10ums.e_ry, ...
     'DisplayName', 'Closed-loop')
plot([-100, 100], [specs_ry_peak, specs_ry_peak], 'k--', 'DisplayName', 'Specifications');
plot([-100, 100], [-specs_ry_peak, -specs_ry_peak], 'k--', 'HandleVisibility', 'off');
hold off;
xlabel('Ty position [$\mu$m]');
ylabel('$R_y$ error [$\mu$rad]');
xlim([-100, 100]);
ylim([-10, 10])
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

%% Fast Ty scan (100um/s) - OL
data_ty_ol_100ums = load("2023-08-21_20-05_ty_scan_m1_open_loop.mat");
data_ty_ol_100ums.time = Ts*[0:length(data_ty_ol_100ums.Dy_int)-1];

%% Fast Ty scan (100um/s) - CL
data_ty_cl_100ums = load("2023-08-21_20-07_ty_scan_m1_cf_closed_loop.mat");
data_ty_cl_100ums.time = Ts*[0:length(data_ty_cl_100ums.Dy_int)-1];

%% Ty scan (at 100um/s) - Dy errors
figure;
hold on;
plot(1e6*data_ty_ol_100ums.Ty, 1e6*detrend(data_ty_ol_100ums.e_dy, 0), ...
     'DisplayName', 'Open-loop')
plot(1e6*data_ty_cl_100ums.Ty, 1e6*detrend(data_ty_cl_100ums.e_dy, 0), ...
     'DisplayName', 'Closed-loop')
plot([-100, 100], 1e-3*[specs_dy_peak, specs_dy_peak], 'k--', 'DisplayName', 'Specifications');
plot([-100, 100], 1e-3*[-specs_dy_peak, -specs_dy_peak], 'k--', 'HandleVisibility', 'off');
hold off;
xlabel('Ty position [$\mu$m]');
ylabel('$D_y$ error [$\mu$m]');
xlim([-100, 100]);
ylim([-3, 3]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

%% Ty scan (at 100um/s) - Dz and Ry errors
figure;
hold on;
plot(1e6*data_ty_ol_100ums.Ty, 1e6*detrend(data_ty_ol_100ums.e_dz, 0), ...
     'DisplayName', 'Open-loop')
plot(1e6*data_ty_cl_100ums.Ty, 1e6*detrend(data_ty_cl_100ums.e_dz, 0), ...
     'DisplayName', 'Closed-loop')
plot([-100, 100], 1e-3*[specs_dz_peak, specs_dz_peak], 'k--', 'DisplayName', 'Specifications');
plot([-100, 100], 1e-3*[-specs_dz_peak, -specs_dz_peak], 'k--', 'HandleVisibility', 'off');
hold off;
xlabel('Ty position [$\mu$m]');
ylabel('$D_z$ error [$\mu$m]');
xlim([-100, 100]);
ylim([-0.4, 0.4]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

figure;
hold on;
plot(1e6*data_ty_ol_100ums.Ty, 1e6*data_ty_ol_100ums.e_ry, ...
     'DisplayName', 'Open-loop')
plot(1e6*data_ty_cl_100ums.Ty, 1e6*data_ty_cl_100ums.e_ry, ...
     'DisplayName', 'Closed-loop')
plot([-100, 100], [specs_ry_peak, specs_ry_peak], 'k--', 'DisplayName', 'Specifications');
plot([-100, 100], [-specs_ry_peak, -specs_ry_peak], 'k--', 'HandleVisibility', 'off');
hold off;
xlabel('Ty position [$\mu$m]');
ylabel('$R_y$ error [$\mu$rad]');
xlim([-100, 100]);
ylim([-10, 10])
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

%% Compute errors for Dy scans
i_ty_ol_10ums = data_ty_ol_10ums.Ty > data_ty_ol_10ums.Ty(1) & data_ty_ol_10ums.Ty < data_ty_ol_10ums.Ty(end);
i_ty_cl_10ums = data_ty_cl_10ums.Ty > data_ty_cl_10ums.Ty(1) & data_ty_cl_10ums.Ty < data_ty_cl_10ums.Ty(end);
i_ty_ol_100ums = data_ty_ol_100ums.Ty > data_ty_ol_100ums.Ty(1) & data_ty_ol_100ums.Ty < data_ty_ol_100ums.Ty(end);
i_ty_cl_100ums = data_ty_cl_100ums.Ty > data_ty_cl_100ums.Ty(1) & data_ty_cl_100ums.Ty < data_ty_cl_100ums.Ty(end);

% RMS error
data_ty_ol_10ums.Dy_rms = rms(detrend(data_ty_ol_10ums.e_dy(i_ty_ol_10ums), 0));
data_ty_ol_10ums.Dz_rms = rms(detrend(data_ty_ol_10ums.e_dz(i_ty_ol_10ums), 0));
data_ty_ol_10ums.Ry_rms = rms(detrend(data_ty_ol_10ums.e_ry(i_ty_ol_10ums), 0));

data_ty_cl_10ums.Dy_rms = rms(detrend(data_ty_cl_10ums.e_dy(i_ty_cl_10ums), 0));
data_ty_cl_10ums.Dz_rms = rms(detrend(data_ty_cl_10ums.e_dz(i_ty_cl_10ums), 0));
data_ty_cl_10ums.Ry_rms = rms(detrend(data_ty_cl_10ums.e_ry(i_ty_cl_10ums), 0));

data_ty_ol_100ums.Dy_rms = rms(detrend(data_ty_ol_100ums.e_dy(i_ty_ol_100ums), 0));
data_ty_ol_100ums.Dz_rms = rms(detrend(data_ty_ol_100ums.e_dz(i_ty_ol_100ums), 0));
data_ty_ol_100ums.Ry_rms = rms(detrend(data_ty_ol_100ums.e_ry(i_ty_ol_100ums), 0));

data_ty_cl_100ums.Dy_rms = rms(detrend(data_ty_cl_100ums.e_dy(i_ty_cl_100ums), 0));
data_ty_cl_100ums.Dz_rms = rms(detrend(data_ty_cl_100ums.e_dz(i_ty_cl_100ums), 0));
data_ty_cl_100ums.Ry_rms = rms(detrend(data_ty_cl_100ums.e_ry(i_ty_cl_100ums), 0));

%% 100um/s - Robust controller
data_dt_100ums = load("2023-08-18_17-12_diffraction_tomo_m0.mat");
t = Ts*[0:length(data_dt_100ums.Dy_int)-1];
data_dt_100ums = structfun(@(field) field(t>1.0861),data_dt_100ums, 'UniformOutput', false);
data_dt_100ums.time = Ts*[0:length(data_dt_100ums.Dy_int)-1];

%% 500um/s - Complementary filters
data_dt_500ums = load("2023-08-21_15-15_diffraction_tomo_m0_fast_cf.mat");
t = Ts*[0:length(data_dt_500ums.Dy_int)-1];
data_dt_500ums = structfun(@(field) field(t>0.275),data_dt_500ums, 'UniformOutput', false);
data_dt_500ums.time = Ts*[0:length(data_dt_500ums.Dy_int)-1];

%% 1mm/s - Complementary filters
data_dt_1000ums = load("2023-08-21_15-16_diffraction_tomo_m0_fast_cf.mat");
t = Ts*[0:length(data_dt_1000ums.Dy_int)-1];
data_dt_1000ums = structfun(@(field) field(t>0.19),data_dt_1000ums, 'UniformOutput', false);
data_dt_1000ums.time = Ts*[0:length(data_dt_1000ums.Dy_int)-1];

%% Dy motion for several configured velocities
figure;
hold on;
plot(data_dt_1000ums.time, 1e6*data_dt_1000ums.Dy_int, 'color', colors(1,:), ...
     'DisplayName', '$1 mm/s$')
plot(data_dt_1000ums.time, 1e6*data_dt_1000ums.m_hexa_dy, 'k--', ...
     'HandleVisibility', 'off')
plot(data_dt_500ums.time, 1e6*data_dt_500ums.Dy_int, 'color', colors(2,:), ...
     'DisplayName', '$0.5 mm/s$')
plot(data_dt_500ums.time, 1e6*data_dt_500ums.m_hexa_dy, 'k--', ...
     'HandleVisibility', 'off')
plot(data_dt_100ums.time, 1e6*data_dt_100ums.Dy_int, 'color', colors(3,:), ...
     'DisplayName', '$0.1 mm/s$')
plot(data_dt_100ums.time, 1e6*data_dt_100ums.m_hexa_dy, 'k--', ...
     'DisplayName', 'Setpoint')
hold off;
xlim([0, 4]);
ylim([-110, 110]);
xlabel('Time [s]');
ylabel('$D_y$ position [$\mu$m]')
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);

%% Diffraction Tomography - Dy errors for several configured velocities
figure;
hold on;
plot(data_dt_1000ums.time, 1e9*(data_dt_1000ums.Dy_int - data_dt_1000ums.m_hexa_dy), ...
     'DisplayName', '$1 mm/s$')
plot(data_dt_500ums.time, 1e9*(data_dt_500ums.Dy_int - data_dt_500ums.m_hexa_dy), ...
     'DisplayName', '$0.5 mm/s$')
plot(data_dt_100ums.time, 1e9*(data_dt_100ums.Dy_int - data_dt_100ums.m_hexa_dy), ...
     'DisplayName', '$0.1 mm/s$')
plot([0, 6.2], [specs_dy_peak, specs_dy_peak], 'k--', 'DisplayName', 'Specs');
plot([0, 6.2], [-specs_dy_peak, -specs_dy_peak], 'k--', 'HandleVisibility', 'off');
hold off;
xlim([0, 3]);
xlabel('Time [s]');
ylabel('$D_y$ error [nm]')
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

%% Diffraction Tomography - Dz errors for several configured velocities
figure;
hold on;
plot(data_dt_1000ums.time, 1e9*data_dt_1000ums.Dz_int, ...
     'DisplayName', '$1 mm/s$')
plot(data_dt_500ums.time, 1e9*data_dt_500ums.Dz_int, ...
     'DisplayName', '$0.5 mm/s$')
plot(data_dt_100ums.time, 1e9*data_dt_100ums.Dz_int, ...
     'DisplayName', '$0.1 mm/s$')
plot([0, 6.2], [specs_dz_peak, specs_dz_peak], 'k--', 'DisplayName', 'Specs');
plot([0, 6.2], [-specs_dz_peak, -specs_dz_peak], 'k--', 'HandleVisibility', 'off');
hold off;
xlim([0, 4]);
ylim([-100, 100])
xlabel('Time [s]');
ylabel('$D_z$ position [nm]')
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

%% Diffraction Tomography - Ry errors for several configured velocities
figure;
hold on;
plot(data_dt_1000ums.time, 1e6*data_dt_1000ums.Ry_int, ...
     'DisplayName', '$1 mm/s$')
plot(data_dt_500ums.time, 1e6*data_dt_500ums.Ry_int, ...
     'DisplayName', '$0.5 mm/s$')
plot(data_dt_100ums.time, 1e6*data_dt_100ums.Ry_int, ...
     'DisplayName', '$0.1 mm/s$')
plot([0, 6.2], [specs_ry_peak, specs_ry_peak], 'k--', 'DisplayName', 'Specs');
plot([0, 6.2], [-specs_ry_peak, -specs_ry_peak], 'k--', 'HandleVisibility', 'off');
hold off;
xlim([0, 4]);
ylim([-1.5, 1.5])
xlabel('Time [s]');
ylabel('$R_y$ position [$\mu$rad]')
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

%% Computation of errors during diffraction tomography experiments
% Ignore acceleration and deceleration phases
acc_dt = 20e-3; % Acceleration phase to remove [s]
acc_n = acc_dt/Ts; % Number of points to delete

% Determine when the motion starts and stops
i_dt_100ums  = data_dt_100ums.m_hexa_dy>data_dt_100ums.m_hexa_dy(1) & data_dt_100ums.m_hexa_dy<data_dt_100ums.m_hexa_dy(end);
[~, i_acc] = find(diff(i_dt_100ums)>0); % Acceleration phases
for i = i_acc
    i_dt_100ums(i:i+acc_n) = 0;
end
[~, i_dec] = find(diff(i_dt_100ums)<0); % Deceleration phases
for i = i_dec(2:2:end)
    i_dt_100ums(i-acc_n:i) = 0;
end

i_dt_500ums  = data_dt_500ums.m_hexa_dy>data_dt_500ums.m_hexa_dy(1) & data_dt_500ums.m_hexa_dy<data_dt_500ums.m_hexa_dy(end);
[~, i_acc] = find(diff(i_dt_500ums)>0); % Acceleration phases
for i = i_acc
    i_dt_500ums(i:i+acc_n) = 0;
end
[~, i_dec] = find(diff(i_dt_500ums)<0); % Deceleration phases
for i = i_dec(2:2:end)
    i_dt_500ums(i-acc_n:i) = 0;
end

i_dt_1000ums  = data_dt_1000ums.m_hexa_dy>data_dt_1000ums.m_hexa_dy(1) & data_dt_1000ums.m_hexa_dy<data_dt_1000ums.m_hexa_dy(end);
[~, i_acc] = find(diff(i_dt_1000ums)>0); % Acceleration phases
for i = i_acc
    i_dt_1000ums(i:i+acc_n) = 0;
end
[~, i_dec] = find(diff(i_dt_1000ums)<0); % Deceleration phases
for i = i_dec(2:2:end)
    i_dt_1000ums(i-acc_n:i) = 0;
end

% RMS error
data_dt_100ums.Dy_rms_cl = rms(detrend(data_dt_100ums.Dy_int(i_dt_100ums)-data_dt_100ums.m_hexa_dy(i_dt_100ums), 0));
data_dt_100ums.Dz_rms_cl = rms(detrend(data_dt_100ums.Dz_int(i_dt_100ums), 0));
data_dt_100ums.Ry_rms_cl = rms(detrend(data_dt_100ums.Ry_int(i_dt_100ums), 0));

data_dt_500ums.Dy_rms_cl = rms(detrend(data_dt_500ums.Dy_int(i_dt_500ums)-data_dt_500ums.m_hexa_dy(i_dt_500ums), 0));
data_dt_500ums.Dz_rms_cl = rms(detrend(data_dt_500ums.Dz_int(i_dt_500ums), 0));
data_dt_500ums.Ry_rms_cl = rms(detrend(data_dt_500ums.Ry_int(i_dt_500ums), 0));

data_dt_1000ums.Dy_rms_cl = rms(detrend(data_dt_1000ums.Dy_int(i_dt_1000ums)-data_dt_1000ums.m_hexa_dy(i_dt_1000ums), 0));
data_dt_1000ums.Dz_rms_cl = rms(detrend(data_dt_1000ums.Dz_int(i_dt_1000ums), 0));
data_dt_1000ums.Ry_rms_cl = rms(detrend(data_dt_1000ums.Ry_int(i_dt_1000ums), 0));

%% Summary of results
% 1e9*data_tomo_m0_Wz6.Dy_rms_ol,   1e9*data_tomo_m0_Wz6.Dz_rms_ol,   1e6*data_tomo_m0_Wz6.Ry_rms_ol; %   Tomo - OL - 6deg/s - 0kg
% 1e9*data_tomo_m0_Wz6.Dy_rms_cl,   1e9*data_tomo_m0_Wz6.Dz_rms_cl,   1e6*data_tomo_m0_Wz6.Ry_rms_cl; %   Tomo - CL - 6deg/s - 0kg
% 1e9*data_tomo_m1_Wz6.Dy_rms_ol,   1e9*data_tomo_m1_Wz6.Dz_rms_ol,   1e6*data_tomo_m1_Wz6.Ry_rms_ol; %   Tomo - OL - 6deg/s - 13kg
% 1e9*data_tomo_m1_Wz6.Dy_rms_cl,   1e9*data_tomo_m1_Wz6.Dz_rms_cl,   1e6*data_tomo_m1_Wz6.Ry_rms_cl; %   Tomo - CL - 6deg/s - 13kg
% 1e9*data_tomo_m2_Wz6.Dy_rms_ol,   1e9*data_tomo_m2_Wz6.Dz_rms_ol,   1e6*data_tomo_m2_Wz6.Ry_rms_ol; %   Tomo - OL - 6deg/s - 26kg
% 1e9*data_tomo_m2_Wz6.Dy_rms_cl,   1e9*data_tomo_m2_Wz6.Dz_rms_cl,   1e6*data_tomo_m2_Wz6.Ry_rms_cl; %   Tomo - CL - 6deg/s - 26kg
% 1e9*data_tomo_m3_Wz6.Dy_rms_ol,   1e9*data_tomo_m3_Wz6.Dz_rms_ol,   1e6*data_tomo_m3_Wz6.Ry_rms_ol; %   Tomo - OL - 6deg/s - 39kg
% 1e9*data_tomo_m3_Wz6.Dy_rms_cl,   1e9*data_tomo_m3_Wz6.Dz_rms_cl,   1e6*data_tomo_m3_Wz6.Ry_rms_cl; %   Tomo - CL - 6deg/s - 39kg
% 1e9*data_tomo_m0_Wz180.Dy_rms_ol, 1e9*data_tomo_m0_Wz180.Dz_rms_ol, 1e6*data_tomo_m0_Wz180.Ry_rms_ol; % Tomo - OL - 180deg/s - 0kg
% 1e9*data_tomo_m0_Wz180.Dy_rms_cl, 1e9*data_tomo_m0_Wz180.Dz_rms_cl, 1e6*data_tomo_m0_Wz180.Ry_rms_cl; % Tomo - CL - 180deg/s - 0kg
% 1e9*data_hac_Wz180.Dy_rms_cl,     1e9*data_hac_Wz180.Dz_rms_cl,     1e6*data_hac_Wz180.Ry_rms_cl; %     Tomo - CL (high performance HAC) - 180deg/s - 0kg
% 1e9*data_ry.Dy_rms_cl,            1e9*data_ry.Dz_rms_cl,            1e6*data_ry.Ry_rms_cl; %            Ry 100urad/s
% 1e9*data_dz_10ums.Dy_rms_cl,      1e9*data_dz_10ums.Dz_rms_cl,      1e6*data_dz_10ums.Ry_rms_cl; %      Dz 10um/s
% 1e9*data_dz_100ums.Dy_rms_cl,     1e9*data_dz_100ums.Dz_rms_cl,     1e6*data_dz_100ums.Ry_rms_cl; %     Dz 100um/s
% 1e9*data_ty_ol_10ums.Dy_rms,      1e9*data_ty_ol_10ums.Dz_rms,      1e6*data_ty_ol_10ums.Ry_rms; %      Ty - OL - 10um/s
% 1e9*data_ty_cl_10ums.Dy_rms,      1e9*data_ty_cl_10ums.Dz_rms,      1e6*data_ty_cl_10ums.Ry_rms; %      Ty - CL - 10um/s
% 1e9*data_ty_ol_100ums.Dy_rms,     1e9*data_ty_ol_100ums.Dz_rms,     1e6*data_ty_ol_100ums.Ry_rms; %     Ty - OL - 100um/s
% 1e9*data_ty_cl_100ums.Dy_rms,     1e9*data_ty_cl_100ums.Dz_rms,     1e6*data_ty_cl_100ums.Ry_rms; %     Ty - CL - 100um/s
% 1e9*data_dt_100ums.Dy_rms_cl,     1e9*data_dt_100ums.Dz_rms_cl,     1e6*data_dt_100ums.Ry_rms_cl; %     Diffraction Tomo - CL - 6deg/s,  100um/s
% 1e9*data_dt_500ums.Dy_rms_cl,     1e9*data_dt_500ums.Dz_rms_cl,     1e6*data_dt_500ums.Ry_rms_cl; %     Diffraction Tomo - CL - 6deg/s,  500um/s
% 1e9*data_dt_1000ums.Dy_rms_cl,    1e9*data_dt_1000ums.Dz_rms_cl,    1e6*data_dt_1000ums.Ry_rms_cl; %    Diffraction Tomo - CL - 6deg/s, 1000um/s