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

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

addpath('./mat/');

% PI
% Three measurements are done:
% - Slew Rate limitation at maximum
% - Slew Rate limitation at minimum
% - Notch Filter at maximum frequency


pi_sr_min       = load('pi_slew_rate_min.mat');
pi_sr_max       = load('pi_slew_rate_max.mat');
pi_sr_max_notch = load('pi_slew_rate_max_notch_high.mat');
pi_sr_load      = load('pi_slew_rate_max_notch_high_2stacks.mat');

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

[tf_sr_min, f] = tfestimate(pi_sr_min.V_in, pi_sr_min.V_out, win, [], [], 1/Ts);
[tf_sr_max, ~] = tfestimate(pi_sr_max.V_in, pi_sr_max.V_out, win, [], [], 1/Ts);
[tf_sr_max_notch, ~] = tfestimate(pi_sr_max_notch.V_in, pi_sr_max_notch.V_out, win, [], [], 1/Ts);
[tf_sr_load, ~] = tfestimate(pi_sr_load.V_in, pi_sr_load.V_out, win, [], [], 1/Ts);

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

figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(f, abs(tf_sr_min), 'DisplayName', 'Slew Rate - Min')
plot(f, abs(tf_sr_max), 'DisplayName', 'Slew Rate - Max')
plot(f, abs(tf_sr_max_notch), 'DisplayName', 'Remove Notch')
plot(f, abs(tf_sr_load), 'DisplayName', 'With Load')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude'); xlabel('Frequency [Hz]');
hold off;
legend('location', 'southwest');

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

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

% Transfer function of the Voltage Amplifier
% The identified transfer function still seems to match the one of a notch filter at 5kHz.


w_nf = 2*pi*5e3; % Notch Filter Frequency [rad/s]
G = 10.5*(s^2 + 2*w_nf*0.12*s + w_nf^2)/(s^2 + 2*w_nf*s + w_nf^2);

figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(f, abs(tf_sr_max_notch), 'DisplayName', 'Remove Notch')
plot(f, abs(squeeze(freqresp(G, f, 'Hz'))), 'DisplayName', 'Remove Notch')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude'); xlabel('Frequency [Hz]');
hold off;
legend('location', 'southwest');

ax2 = subplot(2, 1, 2);
hold on;
plot(f, 180/pi*unwrap(angle(tf_sr_max_notch))-angle_delay)
plot(f, 180/pi*angle(squeeze(freqresp(G, f, 'Hz'))), 'DisplayName', 'Remove Notch')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Phase'); xlabel('Frequency [Hz]');
hold off;
ylim([-180, 45]);
yticks(-360:45:90)

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

% With Load

R = 2.78; % Output Impedance [Ohm]
C = 9e-6; % Load capacitance [F]

G_amp = 10/(1 + s*R*C);

figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(f, abs(tf_sr_max_notch), 'DisplayName', 'No load')
plot(f, abs(tf_sr_load), 'DisplayName', '$10\mu F$ load')
plot(f, abs(squeeze(freqresp(G_amp, f, 'Hz'))), 'k--', 'DisplayName', 'Model')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude'); xlabel('Frequency [Hz]');
hold off;
legend('location', 'southwest');

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

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