%% Clear Workspace and Close figures
clear; close all; clc;

%% Intialize Laplace variable
s = zpk('s');

addpath('./mat/');

% Load Data
% The measurement data are loaded and the offset are removed using the =detrend= command.


load('int_enc_comp.mat', 'interferometer', 'encoder', 'u', 't');
Ts = 1e-4; % Sampling Time [s]

interferometer = detrend(interferometer, 0);
encoder = detrend(encoder, 0);
u = detrend(u, 0);

% Time Domain Results
% One period of the displacement of the mass as measured by the encoder and interferometer are shown in Figure [[fig:int_enc_one_cycle]].
% It consist of the sinusoidal motion at 0.5Hz with an amplitude of approximately $70\mu m$.

% The frequency of the motion is chosen such that no resonance in the system is excited.
% This should improve the coherence between the measurements made by the encoder and interferometer.


figure;
hold on;
plot(t, encoder,        '-',  'DisplayName', 'Encoder')
plot(t, interferometer, '--', 'DisplayName', 'Interferometer')
hold off;
xlabel('Time [s]'); ylabel('Displacement [m]');
legend('location', 'southeast');
xlim([50, 52])



% #+name: fig:int_enc_one_cycle
% #+caption: One cycle measurement
% #+RESULTS:
% [[file:figs/int_enc_one_cycle.png]]

% The difference between the two measurements during the same period is shown in Figure [[fig:int_enc_one_cycle_error]].


figure;
hold on;
plot(t, encoder - interferometer, 'DisplayName', 'Difference')
hold off;
xlabel('Time [s]'); ylabel('Displacement [m]');
legend('location', 'northeast');
xlim([50, 52])

% Difference between Encoder and Interferometer as a function of time
% The data is filtered using a second order low pass filter with a cut-off frequency $\omega_0$ as defined below.


w0 = 2*pi*5; % [rad/s]
xi = 0.7;

G_lpf = 1/(1 + 2*xi/w0*s + s^2/w0^2);



% After filtering, the data is "re-shaped" such that we can superimpose all the measured periods as shown in Figure [[fig:int_enc_error_mean_time]].
% This gives an idea of the measurement error as given by the Attocube during a $70 \mu m$ motion.

d_err_mean = reshape(lsim(G_lpf, encoder - interferometer, t), [2/Ts floor(Ts/2*length(encoder))]);
d_err_mean = d_err_mean - mean(d_err_mean);

figure;
hold on;
for i_i = 1:size(d_err_mean, 2)
  plot(t(1:size(d_err_mean, 1)), d_err_mean(:, i_i), 'k-')
end
plot(t(1:size(d_err_mean, 1)), mean(d_err_mean, 2), 'r-')
hold off;
xlabel('Time [s]'); ylabel('Displacement [m]');

% Difference between Encoder and Interferometer as a function of position
% Figure [[fig:int_enc_error_mean_time]] gives the measurement error as a function of time.
% We here wish the compute this measurement error as a function of the position (as measured by the encoer).

% To do so, all the attocube measurements corresponding to each position measured by the Encoder (resolution of $1nm$) are averaged.
% Figure [[fig:int_enc_error_mean_position]] is obtained where we clearly see an error with a period comparable to the motion range and a much smaller period corresponding to the non-linear period errors that we wish the estimate.

[e_sorted, ~, e_ind] = unique(encoder);

i_mean = zeros(length(e_sorted), 1);
for i = 1:length(e_sorted)
  i_mean(i) = mean(interferometer(e_ind == i));
end

i_mean_error = (i_mean - e_sorted);

figure;
hold on;
% plot(encoder, interferometer - encoder, 'k.', 'DisplayName', 'Difference')
plot(1e6*(e_sorted), 1e9*(i_mean_error))
hold off;
xlabel('Encoder Measurement [$\mu m$]'); ylabel('Measrement Error [nm]');



% #+name: fig:int_enc_error_mean_position
% #+caption: Difference between the two measurement as a function of the measured position by the encoder, averaged for all the cycles
% #+RESULTS:
% [[file:figs/int_enc_error_mean_position.png]]

% The period of the non-linearity seems to be equal to $765 nm$ which corresponds to half the wavelength of the Laser ($1.53 \mu m$).
% For the motion range done here, the non-linearity is measured over ~18 periods which permits to do some averaging.


win_length = 1530/2; % length of the windows (corresponds to 765 nm)
num_avg = floor(length(e_sorted)/win_length); % number of averaging

i_init = ceil((length(e_sorted) - win_length*num_avg)/2); % does not start at the extremity

e_sorted_mean_over_period = mean(reshape(i_mean_error(i_init:i_init+win_length*num_avg-1), [win_length num_avg]), 2);



% The obtained periodic non-linearity is shown in Figure [[fig:int_non_linearity_period_wavelength]].


figure;
hold on;
plot(1e-3*(0:win_length-1), 1e9*(e_sorted_mean_over_period))
hold off;
xlabel('Displacement [$\mu m$]'); ylabel('Measurement Non-Linearity [nm]');