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

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

addpath('./mat/');

% Load Data                                                         :ignore:
% The data are loaded:

load('apa95ml_5kg_2a_1s.mat', 't', 'u', 'v');



% Any offset is removed.

u = detrend(u, 0); % Speedgoat DAC output Voltage [V]
v = detrend(v, 0); % Speedgoat ADC input Voltage (sensor stack) [V]



% Here, the amplifier gain is 20.

u = 20*u; % Actuator Stack Voltage [V]

% Compute TF estimate and Coherence                                 :ignore:
% The transfer function from the actuator voltage $V_a$ to the force sensor stack voltage $V_s$ is computed.


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

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

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



% The coherence is shown in Figure [[fig:apa95ml_5kg_cedrat_coh]].


figure;

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



% #+name: fig:apa95ml_5kg_cedrat_coh
% #+caption: Coherence
% #+RESULTS:
% [[file:figs/apa95ml_5kg_cedrat_coh.png]]

% The Simscape model is loaded and compared with the identified dynamics in Figure [[fig:bode_plot_force_sensor_voltage_comp_fem]].
% The non-minimum phase zero is just a side effect of the not so great identification.
% Taking longer measurements would results in a minimum phase zero.


load('mat/fem_simscape_models.mat', 'Gfm');

freqs = logspace(1, 4, 1000);

figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(tf_est))
plot(freqs, abs(squeeze(freqresp(Gfm, freqs, 'Hz'))))
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-3, 1e1]);

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(tf_est), ...
     'DisplayName', 'Identification')
plot(freqs, 180/pi*angle(squeeze(freqresp(Gfm, freqs, 'Hz'))), ...
     'DisplayName', 'FEM')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Phase'); xlabel('Frequency [Hz]');
hold off;
ylim([-180, 180]);
yticks(-180:90:180);
legend('location', 'northeast')

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