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

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

addpath('./mat/');

% Long and Slow measurement
% The first measurement was made during ~17 hours with a sampling time of $T_s = 0.1\,s$.


load('long_test_plastic.mat', 'x', 't')
Ts = 0.1; % [s]

figure;
plot(t/60/60, 1e9*x)
xlim([0, 17.5]);
xlabel('Time [h]'); ylabel('Displacement [nm]');



% #+name: fig:long_meas_time_domain_full
% #+caption: Long measurement time domain data
% #+RESULTS:
% [[file:figs/long_meas_time_domain_full.png]]

% Let's fit the data with a step response to a first order low pass filter (Figure [[fig:long_meas_time_domain_fit]]).


f = @(b,x) b(1)*(1 - exp(-x/b(2)));

y_cur = x(t < 17.5*60*60);
t_cur = t(t < 17.5*60*60);

nrmrsd = @(b) norm(y_cur - f(b,t_cur)); % Residual Norm Cost Function
B0 = [400e-9, 2*60*60];                        % Choose Appropriate Initial Estimates
[B,rnrm] = fminsearch(nrmrsd, B0);     % Estimate Parameters ‘B’



% The corresponding time constant is (in [h]):

B(2)/60/60



% #+RESULTS:
% : 2.0658


figure;
hold on;
plot(t_cur/60/60, 1e9*y_cur);
plot(t_cur/60/60, 1e9*f(B, t_cur));
hold off;
xlim([0, 17.5])
xlabel('Time [h]'); ylabel('Displacement [nm]');



% #+name: fig:long_meas_time_domain_fit
% #+caption: Fit of the measurement data with a step response of a first order low pass filter
% #+RESULTS:
% [[file:figs/long_meas_time_domain_fit.png]]

% We can see in Figure [[fig:long_meas_time_domain_full]] that there is a transient period where the measured displacement experiences some drifts.
% This is probably due to thermal effects.
% We only select the data between =t1= and =t2=.
% The obtained displacement is shown in Figure [[fig:long_meas_time_domain_zoom]].


t1 = 10.5; t2 = 17.5; % [h]

x = x(t > t1*60*60 & t < t2*60*60);
x = x - mean(x);
t = t(t > t1*60*60 & t < t2*60*60);
t = t - t(1);

figure;
plot(t/60/60, 1e9*x);
xlabel('Time [h]'); ylabel('Measured Displacement [nm]')



% #+name: fig:long_meas_time_domain_zoom
% #+caption: Kept data (removed slow drifts during the first hours)
% #+RESULTS:
% [[file:figs/long_meas_time_domain_zoom.png]]

% The Power Spectral Density of the measured displacement is computed

win = hann(ceil(length(x)/20));
[p_1, f_1] = pwelch(x, win, [], [], 1/Ts);



% As a low pass filter was used in the measurement process, we multiply the PSD by the square of the inverse of the filter's norm.

G_lpf = 1/(1 + s/2/pi);
p_1 = p_1./abs(squeeze(freqresp(G_lpf, f_1, 'Hz'))).^2;



% Only frequencies below 2Hz are taken into account (high frequency noise will be measured afterwards).

p_1 = p_1(f_1 < 2);
f_1 = f_1(f_1 < 2);

% Short and Fast measurement
% An second measurement is done in order to estimate the high frequency noise of the interferometer.
% The measurement is done with a sampling time of $T_s = 0.1\,ms$ and a duration of ~100s.


load('short_test_plastic.mat')
Ts = 1e-4; % [s]

x = detrend(x, 0);



% The time domain measurement is shown in Figure [[fig:short_meas_time_domain]].


figure;
plot(t, 1e9*x)
xlabel('Time [s]'); ylabel('Displacement [nm]');



% #+name: fig:short_meas_time_domain
% #+caption: Time domain measurement with the high sampling rate
% #+RESULTS:
% [[file:figs/short_meas_time_domain.png]]

% The Power Spectral Density of the measured displacement is computed

win = hann(ceil(length(x)/20));
[p_2, f_2] = pwelch(x, win, [], [], 1/Ts);

% Obtained Amplitude Spectral Density of the measured displacement

% The computed ASD of the two measurements are combined in Figure [[fig:psd_combined]].


figure;
hold on;
plot(f_1(8:end), sqrt(p_1(8:end)), 'k-');
plot(f_2(8:end), sqrt(p_2(8:end)), 'k-');
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('ASD [$m/\sqrt{Hz}$]'); xlabel('Frequency [Hz]');