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

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

%% Path for functions, data and scripts
addpath('./src/'); % Path for scripts
addpath('./mat/'); % Path for data

%% Colors for the figures
colors = colororder;

%% Measured height for all the APA at the 8 locations
apa1 = 1e-6*[0, -0.5 , 3.5  , 3.5  , 42   , 45.5, 52.5 , 46];
apa2 = 1e-6*[0, -2.5 , -3   , 0    , -1.5 , 1   , -2   , -4];
apa3 = 1e-6*[0, -1.5 , 15   , 17.5 , 6.5  , 6.5 , 21   , 23];
apa4 = 1e-6*[0, 6.5  , 14.5 , 9    , 16   , 22  , 29.5 , 21];
apa5 = 1e-6*[0, -12.5, 16.5 , 28.5 , -43  , -52 , -22.5, -13.5];
apa6 = 1e-6*[0, -8   , -2   , 5    , -57.5, -62 , -55.5, -52.5];
apa7 = 1e-6*[0, 9    , -18.5, -30  , 31   , 46.5, 16.5 , 7.5];

apa = {apa1, apa2, apa3, apa4, apa5, apa6, apa7};

%% X-Y positions of the measurements points
W = 20e-3;   % Width [m]
L = 61e-3;   % Length [m]
d = 1e-3;    % Distance from border [m]
l = 15.5e-3; % [m]

pos = [[-L/2 + d,      W/2 - d];
       [-L/2 + l - d,  W/2 - d];
       [-L/2 + l - d, -W/2 + d];
       [-L/2 + d,     -W/2 + d];
       [L/2 - l + d,   W/2 - d];
       [L/2 - d,       W/2 - d];
       [L/2 - d,      -W/2 + d];
       [L/2 - l + d,  -W/2 + d]]';

%% Using fminsearch to find the best fitting plane
apa_d = zeros(1, 7); % Measured flatness of the APA
for i = 1:7
    fun = @(x)max(abs(([pos; apa{i}]-[0;0;x(1)])'*([x(2:3);1]/norm([x(2:3);1]))));
    x0 = [0;0;0];
    [x, min_d] = fminsearch(fun,x0);
    apa_d(i) = min_d;
end

%% Load the measured strokes
load('meas_apa_stroke.mat', 'apa300ml_2s')

%% Generated voltage across the two piezoelectric stack actuators to estimate the stroke of the APA300ML
figure;
plot(apa300ml_2s{1}.t - apa300ml_2s{1}.t(1), 20*apa300ml_2s{1}.V, 'k-')
xlabel('Time [s]'); ylabel('Voltage [V]')
ylim([-20, 160])

%% Measured displacement as a function of the applied voltage
figure;
hold on;
for i = 1:7
    plot(20*apa300ml_2s{i}.V, 1e6*apa300ml_2s{i}.d, 'DisplayName', sprintf('APA %i', i))
end
hold off;
xlabel('Voltage [V]'); ylabel('Displacement [$\mu m$]')
legend('location', 'southwest', 'FontSize', 8)
xlim([-20, 150]); ylim([-250, 0]);

%% X-Bending Identification
% Load Data
bending_X = load('apa300ml_bending_X_top.mat');

% Spectral Analysis setup
Ts = bending_X.Track1_X_Resolution; % Sampling Time [s]
Nfft = floor(1/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

% Compute the transfer function from applied force to measured rotation
[G_bending_X, f]  = tfestimate(bending_X.Track1, bending_X.Track2, win, Noverlap, Nfft, 1/Ts);

%% Y-Bending identification
% Load Data
bending_Y = load('apa300ml_bending_Y_top.mat');

% Compute the transfer function
[G_bending_Y, ~]  = tfestimate(bending_Y.Track1, bending_Y.Track2, win, Noverlap, Nfft, 1/Ts);

figure;
hold on;
plot(f, abs(G_bending_X), 'DisplayName', '$X$ bending');
plot(f, abs(G_bending_Y), 'DisplayName', '$Y$ bending');
text(280, 5.5e-2,{'280Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
text(412, 1.5e-2,{'412Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude');
xlim([100, 1e3]); ylim([5e-5, 2e-1]);
legend('location', 'northeast', 'FontSize', 8)