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

% Metrology Kinematics
% <<ssec:test_id31_metrology_kinematics>>

% The developed short-stroke metrology system is schematically shown in Figure ref:fig:test_id31_metrology_kinematics.
% The point of interest is indicated by the blue frame $\{B\}$, which is located $H = 150\,mm$ above the nano-hexapod's top platform.
% The spheres have a diameter $d = 25.4\,mm$, and indicated dimensions are $l_1 = 60\,mm$ and $l_2 = 16.2\,mm$.
% In order to compute the pose of the $\{B\}$ frame with respect to the granite (i.e. with respect to the fixed interferometer heads), the measured (small) displacements $[d_1,\ d_2,\ d_3,\ d_4,\ d_5]$ by the interferometers are first written as a function of the (small) linear and angular motion of the $\{B\}$ frame $[D_x,\ D_y,\ D_z,\ R_x,\ R_y]$ eqref:eq:test_id31_metrology_kinematics.

% \begin{equation}\label{eq:test_id31_metrology_kinematics}
% d_1 = D_y - l_2 R_x, \quad d_2 = D_y + l_1 R_x, \quad d_3 = -D_x - l_2 R_y, \quad d_4 = -D_x + l_1 R_y, \quad d_5 = -D_z
% \end{equation}

% #+attr_latex: :options [b]{0.48\linewidth}
% #+begin_minipage
% #+name: fig:test_id31_metrology_kinematics
% #+caption: Schematic of the measurement system. Measured distances are indicated by red arrows.
% #+attr_latex: :scale 1 :float nil
% [[file:figs/test_id31_metrology_kinematics.png]]
% #+end_minipage
% \hfill
% #+attr_latex: :options [b]{0.48\linewidth}
% #+begin_minipage
% #+name: fig:test_id31_align_top_sphere_comparators
% #+attr_latex: :width \linewidth :float nil
% #+caption: The top sphere is aligned with the rotation axis of the spindle using two probes.
% [[file:figs/test_id31_align_top_sphere_comparators.jpg]]
% #+end_minipage

% The five equations eqref:eq:test_id31_metrology_kinematics can be written in a matrix form, and then inverted to have the pose of the $\{B\}$ frame as a linear combination of the measured five distances by the interferometers eqref:eq:test_id31_metrology_kinematics_inverse.

% \begin{equation}\label{eq:test_id31_metrology_kinematics_inverse}
% \begin{bmatrix}
%    D_x \\ D_y \\ D_z \\ R_x \\ R_y
% \end{bmatrix} = {\underbrace{\begin{bmatrix}
%    0 &  1 &  0 & -l_2 &  0   \\
%    0 &  1 &  0 &  l_1 &  0   \\
%   -1 &  0 &  0 &  0   & -l_2 \\
%   -1 &  0 &  0 &  0   &  l_1 \\
%    0 &  0 & -1 &  0   &  0
% \end{bmatrix}}_{\bm{J_d}}}^{-1} \cdot \begin{bmatrix}
%   d_1 \\ d_2 \\ d_3 \\ d_4 \\ d_5
% \end{bmatrix}
% \end{equation}


%% Geometrical parameters of the metrology system
H = 150e-3;
l1 = (150-48-42)*1e-3;
l2 = (76.2+48+42-150)*1e-3;

% Computation of the Transformation matrix
Hm = [ 0  1  0 -l2  0;
       0  1  0  l1  0;
      -1  0  0  0  -l2;
      -1  0  0  0   l1;
       0  0 -1  0   0];

% Fine Alignment of reference spheres using interferometers
% <<ssec:test_id31_metrology_sphere_fine_alignment>>

% Thanks to the first alignment of the two reference spheres with the spindle axis (Section ref:ssec:test_id31_metrology_sphere_rought_alignment) and to the fine adjustment of the interferometers orientations (Section ref:ssec:test_id31_metrology_alignment), the spindle can perform complete rotations while still having interference for all five interferometers.
% This metrology can therefore be used to better align the axis defined by the two spheres' center with the spindle axis.

% The alignment process is made by few iterations.
% First, the spindle is scanned and the alignment errors are recorded.
% From the errors, the motion of the micro-hexapod to better align the spheres with the spindle axis is computed and the micro-hexapod is positioned accordingly.
% Then, the spindle is scanned again, and the new alignment errors are recorded.

% This iterative process is first performed for angular errors (Figure ref:fig:test_id31_metrology_align_rx_ry) and then for lateral errors (Figure ref:fig:test_id31_metrology_align_dx_dy).
% The remaining errors after alignment is in the order of $\pm5\,\mu\text{rad}$ in $R_x$ and $R_y$ orientations, $\pm 1\,\mu m$ in $D_x$ and $D_y$ directions and less than $0.1\,\mu m$ vertically.


%% Angular alignment
% Load Data
data_it0 = h5scan(data_dir, 'alignment', 'h1rx_h1ry', 1);
data_it1 = h5scan(data_dir, 'alignment', 'h1rx_h1ry_0002', 3);
data_it2 = h5scan(data_dir, 'alignment', 'h1rx_h1ry_0002', 5);

% Offset wrong points
i_it0 = find(abs(data_it0.Rx_int_filtered(2:end)-data_it0.Rx_int_filtered(1:end-1))>1e-5);
data_it0.Rx_int_filtered(i_it0+1:end) = data_it0.Rx_int_filtered(i_it0+1:end) + data_it0.Rx_int_filtered(i_it0) - data_it0.Rx_int_filtered(i_it0+1);
i_it1 = find(abs(data_it1.Rx_int_filtered(2:end)-data_it1.Rx_int_filtered(1:end-1))>1e-5);
data_it1.Rx_int_filtered(i_it1+1:end) = data_it1.Rx_int_filtered(i_it1+1:end) + data_it1.Rx_int_filtered(i_it1) - data_it1.Rx_int_filtered(i_it1+1);
i_it2 = find(abs(data_it2.Rx_int_filtered(2:end)-data_it2.Rx_int_filtered(1:end-1))>1e-5);
data_it2.Rx_int_filtered(i_it2+1:end) = data_it2.Rx_int_filtered(i_it2+1:end) + data_it2.Rx_int_filtered(i_it2) - data_it2.Rx_int_filtered(i_it2+1);

% Compute circle fit and get radius
[~, ~, R_it0, ~] = circlefit(1e6*data_it0.Rx_int_filtered, 1e6*data_it0.Ry_int_filtered);
[~, ~, R_it1, ~] = circlefit(1e6*data_it1.Rx_int_filtered, 1e6*data_it1.Ry_int_filtered);
[~, ~, R_it2, ~] = circlefit(1e6*data_it2.Rx_int_filtered, 1e6*data_it2.Ry_int_filtered);

%% Rx/Ry alignment of the spheres using the micro-station
figure;
hold on;
plot(1e6*data_it0.Rx_int_filtered, 1e6*data_it0.Ry_int_filtered, '-', ...
     'DisplayName', sprintf('$R_0 = %.0f \\mu$rad', R_it0))
plot(1e6*data_it1.Rx_int_filtered, 1e6*data_it1.Ry_int_filtered, '-', ...
     'DisplayName', sprintf('$R_1 = %.0f \\mu$rad', R_it1))
plot(1e6*data_it2.Rx_int_filtered, 1e6*data_it2.Ry_int_filtered, '-', 'color', colors(5,:), ...
     'DisplayName', sprintf('$R_2 = %.0f \\mu$rad', R_it2))
hold off;
xlabel('$R_x$ [$\mu$rad]'); ylabel('$R_y$ [$\mu$rad]');
axis equal
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
xlim([-600, 300]);
ylim([-100, 800]);

%% Eccentricity alignment
% Load Data
data_it0 = h5scan(data_dir, 'alignment', 'h1rx_h1ry_0002', 5);
data_it1 = h5scan(data_dir, 'alignment', 'h1dx_h1dy', 1);

% Offset wrong points
i_it0 = find(abs(data_it0.Dy_int_filtered(2:end)-data_it0.Dy_int_filtered(1:end-1))>1e-5);
data_it0.Dy_int_filtered(i_it0+1:end) = data_it0.Dy_int_filtered(i_it0+1:end) + data_it0.Dy_int_filtered(i_it0) - data_it0.Dy_int_filtered(i_it0+1);

% Compute circle fit and get radius
[~, ~, R_it0, ~] = circlefit(1e6*data_it0.Dx_int_filtered, 1e6*data_it0.Dy_int_filtered);
[~, ~, R_it1, ~] = circlefit(1e6*data_it1.Dx_int_filtered, 1e6*data_it1.Dy_int_filtered);

%% Dx/Dy alignment of the spheres using the micro-station
figure;
hold on;
plot(1e6*data_it0.Dx_int_filtered, 1e6*data_it0.Dy_int_filtered, '-', ...
     'DisplayName', sprintf('$R_0 = %.0f \\mu$m', R_it0))
plot(1e6*data_it1.Dx_int_filtered, 1e6*data_it1.Dy_int_filtered, '-', ...
     'DisplayName', sprintf('$R_1 = %.0f \\mu$m', R_it1))
hold off;
xlabel('$D_x$ [$\mu$m]'); ylabel('$D_y$ [$\mu$m]');
axis equal
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
xlim([-1, 21]);
ylim([-8, 14]);

% Estimated measurement volume
% <<ssec:test_id31_metrology_acceptance>>

% Because the interferometers are pointing to spheres and not flat surfaces, the lateral acceptance is limited.
% In order to estimate the metrology acceptance, the micro-hexapod is used to perform three accurate scans of $\pm 1\,mm$, respectively along the $x$, $y$ and $z$ axes.
% During these scans, the 5 interferometers are recorded individually, and the ranges in which each interferometer has enough coupling efficiency to be able to measure the displacement are estimated.
% Results are summarized in Table ref:tab:test_id31_metrology_acceptance.
% The obtained lateral acceptance for pure displacements in any direction is estimated to be around $+/-0.5\,mm$, which is enough for the current application as it is well above the micro-station errors to be actively corrected by the NASS.


%% Estimated acceptance of the metrology
% This is estimated by moving the spheres using the micro-hexapod

% Dx
data_dx = h5scan(data_dir, 'metrology_acceptance_new_align', 'dx', 1);

dx_acceptance = zeros(5,1);

for i = [1:size(dx_acceptance, 1)]
    % Find range in which the interferometers are measuring displacement
    dx_di = diff(data_dx.(sprintf('d%i', i))) == 0;
    if sum(dx_di) > 0
        dx_acceptance(i) = data_dx.h1tx(find(dx_di(501:end), 1) + 500) - ...
                           data_dx.h1tx(find(flip(dx_di(1:500)), 1));
    else
        dx_acceptance(i) = data_dx.h1tx(end) - data_dx.h1tx(1);
    end
end

% Dy
data_dy = h5scan(data_dir, 'metrology_acceptance_new_align', 'dy', 1);

dy_acceptance = zeros(5,1);

for i = [1:size(dy_acceptance, 1)]
    % Find range in which the interferometers are measuring displacement
    dy_di = diff(data_dy.(sprintf('d%i', i))) == 0;
    if sum(dy_di) > 0
        dy_acceptance(i) = data_dy.h1ty(find(dy_di(501:end), 1) + 500) - ...
                           data_dy.h1ty(find(flip(dy_di(1:500)), 1));
    else
        dy_acceptance(i) = data_dy.h1ty(end) - data_dy.h1ty(1);
    end
end

% Dz
data_dz = h5scan(data_dir, 'metrology_acceptance_new_align', 'dz', 1);

dz_acceptance = zeros(5,1);

for i = [1:size(dz_acceptance, 1)]
    % Find range in which the interferometers are measuring displacement
    dz_di = diff(data_dz.(sprintf('d%i', i))) == 0;
    if sum(dz_di) > 0
        dz_acceptance(i) = data_dz.h1tz(find(dz_di(501:end), 1) + 500) - ...
                           data_dz.h1tz(find(flip(dz_di(1:500)), 1));
    else
        dz_acceptance(i) = data_dz.h1tz(end) - data_dz.h1tz(1);
    end
end

% Estimated measurement errors
% <<ssec:test_id31_metrology_errors>>

% When using the NASS, the accuracy of the sample's positioning is determined by the accuracy of the external metrology.
% However, the validation of the nano-hexapod, the associated instrumentation and the control architecture is independent of the accuracy of the metrology system.
% Only the bandwidth and noise characteristics of the external metrology are important.
% Yet, some elements effecting the accuracy of the metrology are discussed here.

% First, the "metrology kinematics" (discussed in Section ref:ssec:test_id31_metrology_kinematics) is only approximate (i.e. valid for very small displacements).
% This can be easily seen when performing lateral $[D_x,\,D_y]$ scans using the micro-hexapod while recording the vertical interferometer (Figure ref:fig:test_id31_xy_map_sphere).
% As the interferometer is pointing to a sphere and not to a plane, lateral motion of the sphere is seen as a vertical motion by the top interferometer.

% Then, the reference spheres have some deviations with respect to an ideal sphere [fn:6].
% They are initially meant to be used with capacitive sensors which are integrating the shape errors over large surfaces.
% When using interferometers, the size of the "light spot" on the sphere surface is a circle with a diameter approximately equal to $50\,\mu m$, and therefore the measurement is more sensitive to shape errors with small features.

% As the light from the interferometer is travelling through air (as opposed to being in vacuum), the measured distance is sensitive to any variation in the refractive index of the air.
% Therefore, any variation of air temperature, pressure or humidity will induce measurement errors.
% For instance, for a measurement length of $40\,mm$, a temperature variation of $0.1\,{}^oC$ (which is typical for the ID31 experimental hutch) induces an errors in the distance measurement of $\approx 4\,nm$.

% Interferometers are also affected by noise [[cite:&watchi18_review_compac_inter]].
% The effect of the noise on the translation and rotation measurements is estimated in Figure ref:fig:test_id31_interf_noise.


%% Interferometer noise estimation
data = load("test_id31_interf_noise.mat");

Ts = 1e-4;
Nfft = floor(5/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

[pxx_int, f] = pwelch(detrend(data.d, 0), win, Noverlap, Nfft, 1/Ts);

% Uncorrelated noise: square root of the sum of the squares
pxx_cart = pxx_int*sum(inv(Hm).^2, 2)';

rms_dxy = sqrt(trapz(f(f>1), pxx_cart((f>1),1))); % < 0.3 nm RMS
rms_dz  = sqrt(trapz(f(f>1), pxx_cart((f>1),3))); % < 0.3 nm RMS
rms_rxy = sqrt(trapz(f(f>1), pxx_cart((f>1),4))); % 5 nrad RMS

figure;
hold on;
plot(f, sqrt(pxx_cart(:,1)), 'DisplayName', sprintf('$D_{x,y}$, %.1f nmRMS', rms_dxy));
plot(f, sqrt(pxx_cart(:,3)), 'DisplayName', sprintf('$D_{z}$, %.1f nmRMS', rms_dz));
plot(f, sqrt(pxx_cart(:,4)), 'DisplayName', sprintf('$R_{x,y}$, %.1f nradRMS', rms_rxy));
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{nm,\ nrad}{\sqrt{Hz}}\right]$')
xlim([1, 1e3]); ylim([1e-3, 1]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

%% X-Y scan with the micro-hexapod, and record of the vertical interferometer
data = h5scan(data_dir, 'metrology_acceptance', 'after_int_align_meshXY', 1);

x = 1e3*detrend(data.h1tx, 0); % [um]
y = 1e3*detrend(data.h1ty, 0); % [um]
z = 1e6*data.Dz_int_filtered - max(data.Dz_int_filtered); % [um]

mdl = scatteredInterpolant(x, y, z);
[xg, yg] = meshgrid(unique(x), unique(y));
zg = mdl(xg, yg);

% Fit a sphere to the data
[sphere_center,sphere_radius] = sphereFit(1e-3*[x, y, z]);

%% XY mapping of the Z measurement by the interferometer
figure;
[~,c] = contour3(xg,yg,zg,30);
c.LineWidth = 3;
xlabel('$D_x$ [$\mu$m]');
ylabel('$D_y$ [$\mu$m]');
zlabel('$D_z$ [$\mu$m]');
zlim([-1, 0]);
xticks(-100:50:100); yticks(-100:50:100); zticks(-1:0.2:0);