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

% Geometrical Measurements
% <<sec:test_apa_geometrical_measurements>>

% To measure the flatness of the two mechanical interfaces of the APA300ML, a small measurement bench is installed on top of a metrology granite with very good flatness.

% As shown in Figure ref:fig:test_apa_flatness_setup, the APA is fixed to a clamp while a measuring probe[fn:3] is used to measure the height of 4 points on each of the APA300ML interfaces.

% From the X-Y-Z coordinates of the measured 8 points, the flatness is estimated by best fitting[fn:4] a plane through all the points.

% #+name: fig:test_apa_flatness_setup
% #+attr_latex: :width 0.4\linewidth
% #+caption: Measurement setup for flatness estimation of the two mechanical interfaces
% [[file:figs/test_apa_flatness_setup.png]]


%% 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

% Stroke and Hysteresis Measurement
% <<sec:test_apa_stroke_measurements>>

% The goal is here to verify that the stroke of the APA300ML is as specified in the datasheet.
% To do so, one side of the APA is fixed to the granite, and a displacement probe[fn:2] is located on the other side as shown in Figure ref:fig:test_apa_stroke_bench.

% Then, the voltage across the two actuator stacks is varied from $-20\,V$ to $150\,V$ using a DAC and a voltage amplifier.
% Note that the voltage is here slowly varied as the displacement probe has a very low measurement bandwidth (see Figure ref:fig:test_apa_stroke_bench, left).

% #+name: fig:test_apa_stroke_bench
% #+caption: Bench to measured the APA stroke
% #+attr_latex: :width 0.9\linewidth
% [[file:figs/test_apa_stroke_bench.jpg]]

% The measured APA displacement is shown as a function of the applied voltage in Figure ref:fig:test_apa_stroke_result, right.

% Typical hysteresis curves for piezoelectric stack actuators can be observed.
% The measured stroke is approximately $250\,\mu m$ when using only two of the three stacks, which is enough for the current application.
% This is even above what is specified as the nominal stroke in the data-sheet ($304\,\mu m$, therefore $\approx 200\,\mu m$ if only two stacks are used).

% It is clear from Figure ref:fig:test_apa_stroke_result that "APA 3" has an issue compared to the other units.
% This confirms the abnormal electrical measurements made in Section ref:sec:test_apa_electrical_measurements.
% This unit was send sent back to Cedrat and a new one was shipped back.
% From now on, only the six APA that behave as expected will be used.


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

%% Results of the measured APA stroke
figure;
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

% Generated voltage across the two piezoelectric stack actuators to estimate the stroke of the APA300ML
ax1 = nexttile();
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
ax2 = nexttile();
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]);

% Flexible Mode Measurement
% SCHEDULED: <2024-03-27 Wed>
% <<sec:test_apa_spurious_resonances>>

% In this section, the flexible modes of the APA300ML are investigated both experimentally and using a Finite Element Model.

% To experimentally estimate these modes, the APA is fixed on one end (see Figure ref:fig:test_apa_meas_setup_torsion).
% A Laser Doppler Vibrometer[fn:6] is used to measure the difference of motion between two "red" points (i.e. the torsion of the APA along the vertical direction) and an instrumented hammer[fn:7] is used to excite the flexible modes.
% Using this setup, the transfer function from the injected force to the measured rotation can be computed in different conditions and the frequency and mode shapes of the flexible modes can be estimated.

% The flexible modes for the same condition (i.e. one mechanical interface of the APA300ML fixed) are estimated using a finite element software and the results are shown in Figure ref:fig:test_apa_mode_shapes.

% #+name: fig:test_apa_mode_shapes
% #+caption: Spurious resonances - Change this with the updated FEM analysis of the APA300ML
% #+attr_latex: :width 0.9\linewidth
% [[file:figs/test_apa_mode_shapes.png]]

% #+name: fig:test_apa_meas_setup_torsion
% #+caption: Measurement setup with a Laser Doppler Vibrometer and one instrumental hammer. Here the $Z$ torsion is measured.
% #+attr_latex: :width 0.6\linewidth
% [[file:figs/test_apa_meas_setup_torsion.jpg]]

% Two other similar measurements are performed to measured the bending of the APA around the $X$ direction and around the $Y$ direction (see Figure ref:fig:test_apa_meas_setup_modes).

% #+name: fig:test_apa_meas_setup_modes
% #+caption: Experimental setup to measured flexible modes of the APA300ML. For the bending in the $X$ direction, the impact point is located at the back of the top measurement point. For the bending in the $Y$ direction, the impact point is located on the back surface of the top interface (on the back of the 2 measurements points).
% #+begin_figure
% #+attr_latex: :caption \subcaption{\label{fig:test_apa_meas_setup_X_bending}$X$ bending}
% #+attr_latex: :options {0.49\textwidth}
% #+begin_subfigure
% #+attr_latex: :width 0.95\linewidth
% [[file:figs/test_apa_meas_setup_X_bending.jpg]]
% #+end_subfigure
% #+attr_latex: :caption \subcaption{\label{fig:test_apa_meas_setup_Y_bending}$Y$ Bending}
% #+attr_latex: :options {0.49\textwidth}
% #+begin_subfigure
% #+attr_latex: :width 0.95\linewidth
% [[file:figs/test_apa_meas_setup_Y_bending.jpg]]
% #+end_subfigure
% #+end_figure


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

%% Z-Torsion identification
% Load data
torsion = load('apa300ml_torsion_top.mat');

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

% Load Data
torsion = load('apa300ml_torsion_left.mat');

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



% The three measured frequency response functions are shown in Figure ref:fig:test_apa_meas_freq_compare.
% - a clear $x$ bending mode at $280\,\text{Hz}$
% - a clear $y$ bending mode at $412\,\text{Hz}$
% - for the $z$ torsion test, the $y$ bending mode is also excited and observed, and we may see a mode at $800\,\text{Hz}$


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