tg = slrt;

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

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

d = data(:, 1);
acc_1 = data(:, 2);
acc_2 = data(:, 3);
geo_1 = data(:, 4);
geo_2 = data(:, 5);
u = data(:, 6);
f_meas = data(:, 7);
t = data(:, 8);

save('./mat/identification_noise_iff.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');

%%
d = detrend(d, 0);
acc_1 = detrend(acc_1, 0);
acc_2 = detrend(acc_2, 0);
geo_1 = detrend(geo_1, 0);
geo_2 = detrend(geo_2, 0);
u = detrend(u, 0);

%%
run setup;

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

[p_d, f] = pwelch(d, win, [], [], 1/Ts);
[p_acc1, ~] = pwelch(acc_1, win, [], [], 1/Ts);
[p_acc2, ~] = pwelch(acc_2, win, [], [], 1/Ts);
[p_geo1, ~] = pwelch(geo_1, win, [], [], 1/Ts);
[p_geo2, ~] = pwelch(geo_2, win, [], [], 1/Ts);

%%
figure;
hold on;
plot(f, p_acc1);
plot(f, p_acc2);
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('PSD [$(m/s^2)^2/Hz$]'); xlabel('Frequency [Hz]');

figure;
hold on;
plot(f, p_geo1);
plot(f, p_geo2);
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('PSD [$(m/s)^2/Hz$]'); xlabel('Frequency [Hz]');

figure;
hold on;
plot(f, p_d);
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('PSD [$m^2/Hz$]'); xlabel('Frequency [Hz]');

%%
run setup;

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

[tf_geo1_est, f] = tfestimate(d, geo_1, win, [], [], 1/Ts);
[co_geo1_est, ~] = mscohere(d, geo_1, win, [], [], 1/Ts);

[tf_geo2_est, ~] = tfestimate(d, geo_2, win, [], [], 1/Ts);
[co_geo2_est, ~] = mscohere(d, geo_2, win, [], [], 1/Ts);

[tf_acc1_est, ~] = tfestimate(d, acc_1, win, [], [], 1/Ts);
[co_acc1_est, ~] = mscohere(d, acc_1, win, [], [], 1/Ts);

[tf_acc2_est, ~] = tfestimate(d, acc_2, win, [], [], 1/Ts);
[co_acc2_est, ~] = mscohere(d, acc_2, win, [], [], 1/Ts);

%%
figure;

hold on;
plot(f, co_geo1_est, '-')
plot(f, co_geo2_est, '-')
plot(f, co_acc1_est, '-')
plot(f, co_acc2_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_geo1_est), '-', 'DisplayName', 'Geo1')
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_geo1_est)), '-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Phase'); xlabel('Frequency [Hz]');
hold off;

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

%%
figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(f, abs(tf_acc1_est), '-', 'DisplayName', 'Acc1')
plot(f, abs(tf_acc2_est), '-', 'DisplayName', 'Acc2')
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_acc1_est)), '-')
plot(f, 180/pi*unwrap(angle(tf_acc2_est)), '-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Phase'); xlabel('Frequency [Hz]');
hold off;

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

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

[p_acc_1, f] = pwelch(acc_1, win, [], [], 1/Ts);
[co_acc12, ~] = mscohere(acc_1, acc_2, win, [], [], 1/Ts);

[p_geo_1, ~] = pwelch(geo_1, win, [], [], 1/Ts);
[co_geo12, ~] = mscohere(geo_1, geo_2, win, [], [], 1/Ts);

p_acc_N = p_acc_1.*(1 - co_acc12);
p_geo_N = p_geo_1.*(1 - co_geo12);


figure;
hold on;
plot(f, sqrt(p_acc_N)./abs(tf_acc1_est));
plot(f, sqrt(p_geo_N)./abs(tf_geo1_est));
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('PSD'); xlabel('Frequency [Hz]');