tg = slrt;

%%
f = SimulinkRealTime.openFTP(tg);
mget(f, 'int_enc.dat', 'data');
close(f);

%% Convert the Data
data = SimulinkRealTime.utils.getFileScopeData('data/int_enc.dat').data;

interferometer = data(:, 1);
encoder = data(:, 2);
u = data(:, 3);
t = data(:, 4);

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

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

%%
run setup;

win = hann(ceil(10/Ts));

[p_i, f] = pwelch(interferometer, win, [], [], 1/Ts);
[p_e, ~] = pwelch(encoder, win, [], [], 1/Ts);

%%
figure;
hold on;
plot(f, sqrt(p_i), 'DisplayName', 'Interferometer');
plot(f, sqrt(p_e), 'DisplayName', 'Encoder');
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('ASD [$m/\sqrt{Hz}$]'); xlabel('Frequency [Hz]');
grid;
legend('location', 'northeast');

%%
run setup;

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);

%%
figure;

hold on;
plot(f, co_i_est, '-')
plot(f, co_e_est, '-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Coherence'); xlabel('Frequency [Hz]');
hold off;

%%
figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(f, abs(tf_i_est), '-', 'DisplayName', 'Int')
plot(f, abs(tf_e_est), '-', 'DisplayName', 'Enc')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude'); xlabel('Frequency [Hz]');
hold off;

ax2 = subplot(2, 1, 2);
hold on;
plot(f, 180/pi*unwrap(angle(tf_i_est)), '-')
plot(f, 180/pi*unwrap(angle(tf_e_est)), '-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Phase'); xlabel('Frequency [Hz]');
hold off;

linkaxes([ax1,ax2], 'x');