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

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

addpath('./mat/');

% Compute Impedance

R = 10;    % Resistive Load used [Ohm]
V = 0.998; % Output Voltage without any load [V]
Vp = 0.912; % Output Voltage with resistice load [V]

R * (V - Vp)/Vp;



% #+RESULTS:
% : 0.94298


R = 47;    % Resistive Load used [Ohm]
V = 4.960; % Output Voltage without any load [V]
Vp = 4.874; % Output Voltage with resistice load [V]

R * (V - Vp)/Vp;

% Effect of Impedance on the phase drop

C_1 = 5e-6; % Capacitance in [F]
C_2 = 10e-6; % Capacitance in [F]
C_3 = 15e-6; % Capacitance in [F]

Ri = R * (V - Vp)/Vp; % Internal resistance [Ohm]
G0 = 20;

G_1 = G0/(1+Ri*C_1*s);
G_2 = G0/(1+Ri*C_2*s);
G_3 = G0/(1+Ri*C_3*s);

piezo1 = load('cedrat_la75b_med_1_stack.mat', 't', 'V_in', 'V_out');
piezo2 = load('cedrat_la75b_med_2_stack.mat', 't', 'V_in', 'V_out');
piezo3 = load('cedrat_la75b_med_3_stack.mat', 't', 'V_in', 'V_out');

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

[tf_1, f] = tfestimate(piezo1.V_in, piezo1.V_out, win, [], [], 1/Ts);
[co_1, ~] = mscohere(piezo1.V_in, piezo1.V_out, win, [], [], 1/Ts);

[tf_2, ~] = tfestimate(piezo2.V_in, piezo2.V_out, win, [], [], 1/Ts);
[co_2, ~] = mscohere(piezo2.V_in, piezo2.V_out, win, [], [], 1/Ts);

[tf_3, ~] = tfestimate(piezo3.V_in, piezo3.V_out, win, [], [], 1/Ts);
[co_3, ~] = mscohere(piezo3.V_in, piezo3.V_out, win, [], [], 1/Ts);

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

freqs = logspace(1, 4, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(G_1, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_3, freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(f, abs(tf_1), '--')
plot(f, abs(tf_2), '--')
plot(f, abs(tf_3), '--')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_1, freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(G_2, freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(G_3, freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(f, 180/pi*unwrap(angle(tf_1))-angle_delay, '--')
plot(f, 180/pi*unwrap(angle(tf_2))-angle_delay, '--')
plot(f, 180/pi*unwrap(angle(tf_3))-angle_delay, '--')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-90, 45]);
yticks([-90:15:45]);

linkaxes([ax1,ax2],'x');

% PI

R = 10;    % Resistive Load used [Ohm]
V = 1.059; % Output Voltage without any load [V]
Vp = 0.828; % Output Voltage with resistice load [V]

R * (V - Vp)/Vp



% #+RESULTS:
% : 2.7899


R = 10;    % Resistive Load used [Ohm]
V = 2.092; % Output Voltage without any load [V]
Vp = 1.637; % Output Voltage with resistice load [V]

R * (V - Vp)/Vp