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

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

addpath('./mat/');

% Load Data

oc = load('identification_open_circuit.mat', 't', 'encoder', 'u');
sc = load('identification_short_circuit.mat', 't', 'encoder', 'u');

% Transfer Functions

Ts = 1e-4; % Sampling Time [s]
win = hann(ceil(10/Ts));

[tf_oc_est, f] = tfestimate(oc.u, oc.encoder, win, [], [], 1/Ts);
[co_oc_est, ~] = mscohere(  oc.u, oc.encoder, win, [], [], 1/Ts);

[tf_sc_est, ~] = tfestimate(sc.u, sc.encoder, win, [], [], 1/Ts);
[co_sc_est, ~] = mscohere(  sc.u, sc.encoder, win, [], [], 1/Ts);

figure;
hold on;
plot(f, co_oc_est, '-')
plot(f, co_sc_est, '-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Coherence'); xlabel('Frequency [Hz]');
hold off;
xlim([0.5, 5e3]);



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



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

ax1 = nexttile;
hold on;
plot(f, abs(tf_oc_est), '-', 'DisplayName', 'Open-Circuit')
plot(f, abs(tf_sc_est), '-', 'DisplayName', 'Short-Circuit')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-7, 3e-4]);
legend('location', 'southwest');

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(tf_oc_est), '-')
plot(f, 180/pi*angle(tf_sc_est), '-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Phase'); xlabel('Frequency [Hz]');
hold off;
yticks(-360:90:360);
axis padded 'auto x'

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