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

%% Colors for the figures
colors = colororder;

%% Frequency Vector
freqs = logspace(1, 3, 1000);

% Simulation
% In this section, we suppose that we are in the frame of one fast jack (all transformations are already done), and we wish to create a LUT for one fast jack.

% Let's say with make a Bragg angle scan between 10deg and 60deg during 100s.

Fs = 10e3; % Sample Frequency [Hz]
t = 0:1/Fs:10; % Time vector [s]
theta = linspace(10, 40, length(t)); % Bragg Angle [deg]



% The IcePAP steps are following the theoretical formula:
% \begin{equation}
% d_z = \frac{d_{\text{off}}}{2 \cos \theta}
% \end{equation}
% with $\theta$ the bragg angle and $d_{\text{off}} = 10\,mm$.

% The motion to follow is then:

perfect_motion = 10e-3./(2*cos(theta*pi/180)); % Perfect motion [m]



% And the IcePAP is generated those steps:

icepap_steps = perfect_motion; % IcePAP steps measured by Speedgoat [m]

%% Steps as a function of the bragg angle
figure;
plot(theta, icepap_steps);
xlabel('Bragg Angle [deg]'); ylabel('IcePAP Steps [m]');



% #+name: fig:bragg_angle_icepap_steps_idealized
% #+caption: IcePAP Steps as a function of the Bragg Angle
% #+RESULTS:
% [[file:figs/bragg_angle_icepap_steps_idealized.png]]

% Then, we are measuring the motion of the Fast Jack using the Interferometer.
% The motion error is larger than in reality to be angle to see it more easily.

motion_error = 100e-6*sin(2*pi*perfect_motion/1e-3); % Error motion [m]

measured_motion = perfect_motion + motion_error; % Measured motion of the Fast Jack [m]

%% Measured Motion and Idealized Motion
figure;
hold on;
plot(icepap_steps, measured_motion, ...
    'DisplayName', 'Measured Motion');
plot(icepap_steps, perfect_motion, 'k--', ...
    'DisplayName', 'Ideal Motion');
hold off;
xlabel('IcePAP Steps [m]'); ylabel('Measured Motion [m]');
legend('location', 'southeast');



% #+name: fig:measured_and_ideal_motion_fast_jacks
% #+caption: Measured motion as a function of the IcePAP Steps
% #+RESULTS:
% [[file:figs/measured_and_ideal_motion_fast_jacks.png]]

% Let's now compute the lookup table.
% For each micrometer of the IcePAP step, another step is associated that correspond to a position closer to the wanted position.


%% Get range for the LUT
% We correct only in the range of tested/measured motion
lut_range = round(1e6*min(icepap_steps)):round(1e6*max(icepap_steps)); % IcePAP steps [um]

%% Initialize the LUT
lut = zeros(size(lut_range));

%% For each um in this range
for i = 1:length(lut_range)
    % Get points indices where the measured motion is closed to the wanted one
    close_points = measured_motion > 1e-6*lut_range(i) - 500e-9 & measured_motion < 1e-6*lut_range(i) + 500e-9;
    % Get the corresponding closest IcePAP step
    lut(i) = round(1e6*mean(icepap_steps(close_points))); % [um]
end

%% Generated Lookup Table
figure;
plot(lut_range, lut);
xlabel('IcePAP input step [um]'); ylabel('Lookup Table output [um]');



% #+name: fig:generated_lut_icepap
% #+caption: Generated Lookup Table
% #+RESULTS:
% [[file:figs/generated_lut_icepap.png]]

% The current LUT implementation is the following:

motion_error_lut = zeros(size(lut_range));
for i = 1:length(lut_range)
    % Get points indices where the icepap step is close to the wanted one
    close_points = icepap_steps > 1e-6*lut_range(i) - 500e-9 & icepap_steps < 1e-6*lut_range(i) + 500e-9;
    % Get the corresponding motion error
    motion_error_lut(i) = lut_range(i) + (lut_range(i) - round(1e6*mean(measured_motion(close_points)))); % [um]
end



% Let's compare the two Lookup Table in Figure [[fig:lut_comparison_two_methods]].

%% Comparison of the two Generated Lookup Table
figure;
hold on;
plot(lut_range, lut, ...
    'DisplayName', 'New LUT');
plot(lut_range, motion_error_lut, ...
    'DisplayName', 'Old LUT');
hold off;
xlabel('IcePAP input step [um]'); ylabel('Lookup Table output [um]');
legend('location', 'southeast');



% #+name: fig:lut_comparison_two_methods
% #+caption: Comparison of the two lookup tables
% #+RESULTS:
% [[file:figs/lut_comparison_two_methods.png]]

% If we plot the "corrected steps" for all steps for both methods, we clearly see the difference (Figure [[fig:lut_correct_and_motion_error]]).


%% Corrected motion and motion error at each step position
figure;
hold on;
plot(lut_range, lut-lut_range, ...
    'DisplayName', 'New LUT');
plot(lut_range, motion_error_lut-lut_range, ...
    'DisplayName', 'Old LUT');
hold off;
xlabel('IcePAP Steps [um]'); ylabel('Corrected motion [um]');
ylim([-110, 110])
legend('location', 'southeast');



% #+name: fig:lut_correct_and_motion_error
% #+caption: LUT correction and motion error as a function of the IcePAP steps
% #+RESULTS:
% [[file:figs/lut_correct_and_motion_error.png]]

% Let's now implement both LUT to see which implementation is correct.

icepap_steps_output_new = lut(round(1e6*icepap_steps)-lut_range(1)+1);
i = round(1e6*icepap_steps)-motion_error_lut(1)+1;
i(i>length(motion_error_lut)) = length(motion_error_lut);
icepap_steps_output_old = motion_error_lut(i);

motion_new = zeros(size(icepap_steps_output_new));
motion_old = zeros(size(icepap_steps_output_old));

for i = 1:length(icepap_steps_output_new)
    [~, i_step] = min(abs(icepap_steps_output_new(i) - 1e6*icepap_steps));
    motion_new(i) = measured_motion(i_step);

    [~, i_step] = min(abs(icepap_steps_output_old(i) - 1e6*icepap_steps));
    motion_old(i) = measured_motion(i_step);
end



% The output motion with both LUT are shown in Figure [[fig:compare_old_new_lut_motion]].
% It is confirmed that the new LUT is the correct one.
% Also, it is interesting to note that the old LUT gives an output motion that is above the ideal one, as was seen during the experiments.

%% Measured Motion and Idealized Motion
% Use only middle motion where the LUT is working
i = round(0.1*length(icepap_steps)):round(0.9*length(icepap_steps));
figure;
hold on;
plot(icepap_steps(i), motion_new(i), ...
    'DisplayName', 'Motion (new LUT)');
plot(icepap_steps(i), motion_old(i), ...
    'DisplayName', 'Motion (old LUT)');
plot(icepap_steps(i), perfect_motion(i), 'k--', ...
    'DisplayName', 'Ideal Motion');
hold off;
xlabel('IcePAP Steps [m]'); ylabel('Measured Motion [m]');
legend('location', 'southeast');