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

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

addpath('./mat/');

% Results

ce_results = load('cedrat_la75b_high_1_stack.mat', 't', 'V_in', 'V_out');
pi_results = load('pi_505_high.mat', 't', 'V_in', 'V_out');

Ts = 1e-4;
win = hann(ceil(0.1/Ts));

[tf_ce, f] = tfestimate(ce_results.V_in, ce_results.V_out, win, [], [], 1/Ts);
[tf_pi, ~] = tfestimate(pi_results.V_in, pi_results.V_out, win, [], [], 1/Ts);



% We remove the phase delay due to the time delay of the ADC/DAC:

angle_delay = 180/pi*angle(squeeze(freqresp(exp(-s*Ts), f, 'Hz')));

figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(f, abs(tf_pi), 'DisplayName', 'PI')
plot(f, abs(tf_ce), 'DisplayName', 'Cedrat')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude'); xlabel('Frequency [Hz]');
hold off;
legend('location', 'southwest');
ylim([0.1, 50]);

ax2 = subplot(2, 1, 2);
hold on;
plot(f, 180/pi*unwrap(angle(tf_pi))-angle_delay)
plot(f, 180/pi*unwrap(angle(tf_ce))-angle_delay)
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Phase'); xlabel('Frequency [Hz]');
hold off;
ylim([-270, 90]);
yticks(-360:90:90)

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