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

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

addpath('./mat/');

% Cedrat Technology
% Load Data

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');



% Compute Coherence and Transfer functions

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);



% 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_1), 'DisplayName', '1 stack')
plot(f, abs(tf_2), 'DisplayName', '2 stacks')
plot(f, abs(tf_3), 'DisplayName', '3 stacks')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude'); xlabel('Frequency [Hz]');
hold off;
legend('location', 'southwest');
ylim([1, 40]);

ax2 = subplot(2, 1, 2);
hold on;
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)
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]);

% PI

piezo1 = load('pi_505_high.mat', 't', 'V_in', 'V_out');
piezo2 = load('pi_505_high_2_stacks.mat', 't', 'V_in', 'V_out');
piezo3 = load('pi_505_high_3_stacks.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);



% 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_1), 'DisplayName', '1 stack')
plot(f, abs(tf_2), 'DisplayName', '2 stacks')
plot(f, abs(tf_3), 'DisplayName', '3 stacks')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude'); xlabel('Frequency [Hz]');
hold off;
legend('location', 'southwest');
ylim([0.05, 11]);

ax2 = subplot(2, 1, 2);
hold on;
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)
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]);
