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

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

addpath('./mat/');

% Results                                                         :ignore:
% Let's load the measurements.

load('meas_probe_against_encoder.mat', 't', 'd', 'dp', 'F')

%% Sampling time [s]
Ts = (t(end) - t(1))/(length(t)-1);

%% Remove first second
t = t(ceil(1/Ts):end);
d = -d(ceil(1/Ts):end);
dp = -dp(ceil(1/Ts):end);
F = F(ceil(1/Ts):end);



% The time domain measured displacement by the probe and by the encoder is shown in Figure [[fig:comp_encoder_probe_time]].


%% Time Domain plots
figure;
hold on;
plot(t, d, 'DisplayName', 'Encoder');
plot(t, dp, 'DisplayName', 'Probe');
hold off;
xlabel('Time [s]'); ylabel('Displacement [m]');



% #+name: fig:comp_encoder_probe_time
% #+caption: Time domain measurement
% #+RESULTS:
% [[file:figs/comp_encoder_probe_time.png]]

% If we zoom, we see that there is some delay between the encoder and the probe (Figure [[fig:comp_encoder_probe_time_zoom]]).


%% Zoom
figure;
hold on;
plot(t, d, 'DisplayName', 'Encoder');
plot(t, dp, 'DisplayName', 'Probe');
hold off;
xlabel('Time [s]'); ylabel('Displacement [m]');
xlim([7.7, 7.9])



% #+RESULTS:
% : The time delay is approximately 15.8 [ms]

% The measured mismatch between the encoder and the probe with and without compensating for the time delay are shown in Figure [[fig:comp_encoder_probe_mismatch]].


figure;
hold on;
plot(t, d-dp, 'DisplayName', 'Raw Mismatch');
plot(t(1:end-finddelay(d, dp)), d(1:end-finddelay(d, dp))-dp(finddelay(d, dp)+1:end), 'DisplayName', 'Removed Delay');
hold off;
xlabel('Time [s]'); ylabel('Measurement Missmatch [m]');



% #+name: fig:comp_encoder_probe_mismatch
% #+caption: Measurement mismatch, with and without delay compensation
% #+RESULTS:
% [[file:figs/comp_encoder_probe_mismatch.png]]

% Finally, the displacement of the probe is shown as a function of the displacement of the encoder and a linear fit is made (Figure [[fig:comp_encoder_probe_linear_fit]]).


figure;
hold on;
plot(1e3*d, 1e3*dp, 'DisplayName', 'Raw data');
plot(1e3*d, 1e3*d*(d\dp), 'DisplayName', sprintf('Linear fit: $\\alpha = %.5f$', (d\dp)));
hold on;
xlabel('Encoder [mm]'); ylabel('Probe [mm]');
legend('location', 'southeast')