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

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

% Load Data
% As usual, the measurement data are loaded.

load('int_enc_id_noise_bis.mat', 'interferometer', 'encoder', 'u', 't');



% The first 0.1 seconds are removed as it corresponds to transient behavior.

interferometer = interferometer(t>0.1);
encoder = encoder(t>0.1);
u = u(t>0.1);
t = t(t>0.1);



% Finally the offset are removed using the =detrend= command.

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

% Excitation and Measured Signals
% The excitation signal is a white noise filtered by a low pass filter to not excite too much the high frequency modes.

% The excitation signal is shown in Figure [[fig:encoder_identification_excitation_time]].

figure;
plot(t, u);
xlabel('Time [s]'); ylabel('Voltage [V]');



% #+name: fig:encoder_identification_excitation_time
% #+caption:
% #+RESULTS:
% [[file:figs/encoder_identification_excitation_time.png]]

% The measured motion by the interferometer and encoder is shown in Figure

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

% Identification
% Now the dynamics from the voltage sent to the voltage amplitude driving the APA95ML to the measured displacement by both the encoder and interferometer are computed.


Ts = 1e-4; % Sampling Time [s]
win = hann(ceil(10/Ts));

[tf_i_est, f] = tfestimate(u, interferometer, win, [], [], 1/Ts);
[co_i_est, ~] = mscohere(u, interferometer, win, [], [], 1/Ts);

[tf_e_est, ~] = tfestimate(u, encoder, win, [], [], 1/Ts);
[co_e_est, ~] = mscohere(u, encoder, win, [], [], 1/Ts);



% The obtained coherence is shown in Figure [[fig:identification_dynamics_coherence]].
% It is shown that the identification is good until 500Hz for the interferometer and until 1kHz for the encoder.


figure;
hold on;
plot(f, co_i_est, '-', 'DisplayName', 'Interferometer')
plot(f, co_e_est, '-', 'DisplayName', 'Encoder')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Coherence'); xlabel('Frequency [Hz]');
hold off;
xlim([0.5, 5e3]);
legend('location', 'southwest');



% #+name: fig:identification_dynamics_coherence
% #+caption:
% #+RESULTS:
% [[file:figs/identification_dynamics_coherence.png]]

% The compared dynamics as measured by the intereferometer and encoder are shown in Figure [[fig:identification_dynamics_bode]].


figure;
 tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

 ax1 = nexttile([2, 1]);
 hold on;
 plot(f, abs(tf_i_est), '-', 'DisplayName', 'Interferometer')
 plot(f, abs(tf_e_est), '-', 'DisplayName', 'Encoder')
 set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
 ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
 hold off;
 ylim([1e-7, 3e-4]);
 legend('location', 'southwest');

 ax2 = nexttile;
 hold on;
 plot(f, 180/pi*angle(tf_i_est), '-')
 plot(f, 180/pi*angle(tf_e_est), '-')
 set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
 ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
hold off;
 yticks(-360:90:360);
 axis padded 'auto x'

 linkaxes([ax1,ax2], 'x');
 xlim([0.5, 5e3]);