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

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

colors = colororder;

addpath('mat/');

%% Load Data
bending_X = load('apa300ml_bending_X_top.mat');

%% Spectral Analysis setup
Ts = bending_X.Track1_X_Resolution; % Sampling Time [s]
win = hann(ceil(1/Ts));

%% Compute the transfer function from applied force to measured rotation
[G_bending_X, f]  = tfestimate(bending_X.Track1, bending_X.Track2, win, [], [], 1/Ts);

%% Plot the transfer function
figure;
hold on;
plot(f, abs(G_bending_X), 'k-');
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude');
xlim([50, 2e3]); ylim([1e-5, 2e-1]);
text(280, 5.5e-2,{'280Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
text(840, 2.0e-3,{'840Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
text(1400, 7.0e-3,{'1400Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')

%% Load Data
bending_Y = load('apa300ml_bending_Y_top.mat');

%% Compute the transfer function
[G_bending_Y, ~]  = tfestimate(bending_Y.Track1, bending_Y.Track2, win, [], [], 1/Ts);

%% Plot the transfer function
figure;
hold on;
plot(f, abs(G_bending_Y), 'k-');
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude');
xlim([50, 2e3]); ylim([1e-5, 3e-2])
text(412, 1.5e-2,{'412Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
text(1218, 1.5e-2,{'1220Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')

%% Load Data
torsion = load('apa300ml_torsion_left.mat');

%% Compute transfer function
[G_torsion, ~]  = tfestimate(torsion.Track1, torsion.Track2, win, [], [], 1/Ts);

%% Plot the transfer function
figure;
hold on;
plot(f, abs(G_torsion), 'k-');
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude');
xlim([50, 2e3]); ylim([1e-5, 2e-2])
text(415, 4.3e-3,{'415Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
text(267, 8e-4,{'267Hz'}, 'VerticalAlignment', 'bottom','HorizontalAlignment','center')
text(800, 6e-4,{'800Hz'}, 'VerticalAlignment', 'bottom','HorizontalAlignment','center')

%% Load data
torsion = load('apa300ml_torsion_top.mat');

%% Compute transfer function
[G_torsion_top, ~]  = tfestimate(torsion.Track1, torsion.Track2, win, [], [], 1/Ts);

%% Plot the two transfer functions
figure;
hold on;
plot(f, abs(G_torsion), 'k-', 'DisplayName', 'Left excitation');
plot(f, abs(G_torsion_top), '-', 'DisplayName', 'Top excitation');
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude');
xlim([50, 2e3]); ylim([1e-5, 2e-2])
text(415, 4.3e-3,{'415Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
text(267, 8e-4,{'267Hz'}, 'VerticalAlignment', 'bottom','HorizontalAlignment','center')
text(800, 2e-3,{'800Hz'}, 'VerticalAlignment', 'bottom','HorizontalAlignment','center')
legend('location', 'northwest');

figure;
hold on;
plot(f, abs(G_torsion),    'DisplayName', 'Torsion');
plot(f, abs(G_bending_X),  'DisplayName', 'Bending - X');
plot(f, abs(G_bending_Y),  'DisplayName', 'Bending - Y');
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude');
xlim([50, 2e3]); ylim([1e-5, 1e-1]);
legend('location', 'southeast');