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

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

addpath('./mat/');

% Cedrat Technology

hi = load('cedrat_la75b_high_1_stack.mat', 't', 'V_in', 'V_out');
me = load('cedrat_la75b_med_1_stack.mat', 't', 'V_in', 'V_out');
lo = load('cedrat_la75b_low_1_stack.mat', 't', 'V_in', 'V_out');

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

[tf_hi, f] = tfestimate(hi.V_in, hi.V_out, win, [], [], 1/Ts);
[co_hi, ~] = mscohere(hi.V_in, hi.V_out, win, [], [], 1/Ts);

[tf_me, ~] = tfestimate(me.V_in, me.V_out, win, [], [], 1/Ts);
[co_me, ~] = mscohere(me.V_in, me.V_out, win, [], [], 1/Ts);

[tf_lo, ~] = tfestimate(lo.V_in, lo.V_out, win, [], [], 1/Ts);
[co_lo, ~] = mscohere(lo.V_in, lo.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_lo), 'DisplayName', 'low')
plot(f, abs(tf_me), 'DisplayName', 'med')
plot(f, abs(tf_hi), 'DisplayName', 'high')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude'); xlabel('Frequency [Hz]');
hold off;
legend('location', 'southwest');
ylim([1, 50]);

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

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

% PI

hi = load('pi_505_high.mat', 't', 'V_in', 'V_out');
lo = load('pi_505_low.mat', 't', 'V_in', 'V_out');

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

[tf_hi, f] = tfestimate(hi.V_in, hi.V_out, win, [], [], 1/Ts);
[co_hi, ~] = mscohere(hi.V_in, hi.V_out, win, [], [], 1/Ts);

[tf_lo, ~] = tfestimate(lo.V_in, lo.V_out, win, [], [], 1/Ts);
[co_lo, ~] = mscohere(lo.V_in, lo.V_out, win, [], [], 1/Ts);

figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(f, abs(tf_hi), 'DisplayName', 'high')
plot(f, abs(tf_lo), 'DisplayName', 'low')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude'); xlabel('Frequency [Hz]');
hold off;
legend('location', 'southwest');
ylim([0.1, 20]);

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

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