tg = slrt;

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

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

acc_1 = data(:, 1);
acc_2 = data(:, 2);
geo_1 = data(:, 3);
geo_2 = data(:, 4);
t = data(:, 5);

save('./mat/huddle_test.mat', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 't');

%%
figure;
subplot(1,2,1);
plot(t, u)
subplot(1,2,2);
plot(t, y)

%%
load('../mat/apa95ml_5kg_10V.mat', 'u', 't', 'y');
ht = load('../mat/huddle_test.mat', 'u', 't', 'y');
u = u - mean(u);
y = y - mean(y);

%%
[pxx, f] = pwelch(y, win, [], [], 1/Ts);
[pht, ~] = pwelch(ht.y, win, [], [], 1/Ts);

figure;

hold on;
plot(f, pxx);
plot(f, pht);
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('PSD'); xlabel('Frequency [Hz]');

%%
run setup;

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

[tf_est, f] = tfestimate(u, y, win, [], [], 1/Ts);
[co_est, ~] = mscohere(u, y, win, [], [], 1/Ts);

%%
figure;

hold on;
plot(f, co_est, 'k-')
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_est), 'k-', 'DisplayName', 'Identified')
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_est)), 'k-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Phase'); xlabel('Frequency [Hz]');
hold off;

linkaxes([ax1,ax2], 'x');
xlim([10, 5000]);

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

[tf_est, f] = tfestimate(u, v, win, [], [], 1/Ts);
[co_est, ~] = mscohere(u, v, win, [], [], 1/Ts);

%%
figure;

hold on;
plot(f, co_est, 'k-')
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_est), 'k-', 'DisplayName', 'Identified')
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_est)), 'k-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Phase'); xlabel('Frequency [Hz]');
hold off;

linkaxes([ax1,ax2], 'x');
xlim([10, 5000]);