% Load Data

ht = load('./mat/huddle_test.mat', 't', 'u', 'y');
load('./mat/apa95ml_5kg.mat', 't', 'u', 'y');

% Time Domain Data

figure;

subplot(1,2,1);
plot(t, u)
ylabel('Input Voltage [V]'); xlabel('Time [s]');


subplot(1,2,2);
plot(t, y)
ylabel('Output Displacement [m]'); xlabel('Time [s]');

% Comparison of the PSD with Huddle Test

Ts = t(end)/(length(t)-1);
Fs = 1/Ts;

win = hanning(ceil(1*Fs));

[pxx, f] = pwelch(y, win, [], [], Fs);
[pht, ~] = pwelch(ht.y, win, [], [], Fs);

figure;
hold on;
plot(f, sqrt(pxx), 'DisplayName', '5kg');
plot(f, sqrt(pht), 'DisplayName', 'Huddle Test');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD [$m/\sqrt{Hz}$]');
legend('locaation', 'norteast');

% Compute TF estimate and Coherence

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

[tf_est, f] = tfestimate(data(:, 1), -data(:, 2), win, [], [], 1/Ts);
[co_est, ~] = mscohere(  data(:, 1), -data(:, 2), win, [], [], 1/Ts);

% Coherence

figure;

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

% Transfer Function

figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(f, abs(tf_est), 'k-')
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*angle(tf_est), 'k-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Phase'); xlabel('Frequency [Hz]');
legend();
hold off;

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