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

addpath('./STEPS/'); % Path for Simscape Model

%% Linearization options
opts = linearizeOptions;
opts.SampleTime = 0;

%% Open Simscape Model
mdl = 'test_apa_simscape'; % Name of the Simulink File
open(mdl); % Open Simscape Model

%% Colors for the figures
colors = colororder;

%% Input/Output definition of the Model
clear io; io_i = 1;
io(io_i) = linio([mdl, '/u'],  1, 'openinput');  io_i = io_i + 1; % DAC Voltage
io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Sensor Voltage
io(io_i) = linio([mdl, '/de'], 1, 'openoutput'); io_i = io_i + 1; % Encoder

%% Frequency vector for analysis
freqs = 5*logspace(0, 3, 1000);

% Identification of the Actuator and Sensor constants

% Once the APA300ML /super element/ is included in the Simscape model, the transfer function from $F_a$ to $d_L$ and $d_e$ can be extracted.
% The gains $g_a$ and $g_s$ are then tuned such that the gains of the transfer functions match the identified ones.
% By doing so, $g_s = 4.9\,V/\mu m$ and $g_a = 23.2\,N/V$ are obtained.


%% Identification of the actuator and sensor "constants"
% Initialize the APA as a flexible body with unity "constants"
n_hexapod.actuator = initializeAPA(...
    'type', 'flexible', ...
    'ga', 1, ...
    'gs', 1);

c_granite = 50; % Rought estimation of the damping added by the air bearing

% Identify the dynamics
G_norm = linearize(mdl, io, 0.0, opts);
G_norm.InputName  = {'u'};
G_norm.OutputName = {'Vs', 'de'};

% Load Identification Data to estimate the two gains
load('meas_apa_frf.mat', 'f', 'Ts', 'enc_frf', 'iff_frf', 'apa_nums');

% Actuator Constant in [N/V]
ga = -mean(abs(enc_frf(f>10 & f<20)))./dcgain(G_norm('de', 'u'));

% Sensor Constant in [V/m]
gs = -mean(abs(iff_frf(f>400 & f<500)))./(ga*abs(squeeze(freqresp(G_norm('Vs', 'u'), 1e3, 'Hz'))));



% To ensure that the sensitivities $g_a$ and $g_s$ are physically valid, it is possible to estimate them from the physical properties of the piezoelectric stack material.

% From [[cite:&fleming14_desig_model_contr_nanop_system p. 123]], the relation between relative displacement $d_L$ of the sensor stack and generated voltage $V_s$ is given by eqref:eq:test_apa_piezo_strain_to_voltage and from [[cite:&fleming10_integ_strain_force_feedb_high]] the relation between the force $F_a$ and the applied voltage $V_a$ is given by eqref:eq:test_apa_piezo_voltage_to_force.

% \begin{subequations}
% \begin{align}
%   V_s &= \underbrace{\frac{d_{33}}{\epsilon^T s^D n}}_{g_s} d_L \label{eq:test_apa_piezo_strain_to_voltage} \\
%   F_a &= \underbrace{d_{33} n k_a}_{g_a} \cdot V_a, \quad k_a = \frac{c^{E} A}{L} \label{eq:test_apa_piezo_voltage_to_force}
% \end{align}
% \end{subequations}

% Unfortunately, the manufacturer of the stack was not willing to share the piezoelectric material properties of the stack used in the APA300ML.
% However, based on the available properties of the APA300ML stacks in the data-sheet, the soft Lead Zirconate Titanate "THP5H" from Thorlabs seemed to match quite well the observed properties.
% The properties of this "THP5H" material used to compute $g_a$ and $g_s$ are listed in Table ref:tab:test_apa_piezo_properties.

% From these parameters, $g_s = 5.1\,V/\mu m$ and $g_a = 26\,N/V$ were obtained, which are close to the constants identified using the experimentally identified transfer functions.

% #+name: tab:test_apa_piezo_properties
% #+caption: Piezoelectric properties used for the estimation of the sensor and actuators sensitivities
% #+attr_latex: :environment tabularx :width 1\linewidth :align ccX
% #+attr_latex: :center t :booktabs t
% | *Parameter*    | *Value*                    | *Description*                                                |
% |----------------+----------------------------+--------------------------------------------------------------|
% | $d_{33}$       | $680 \cdot 10^{-12}\,m/V$  | Piezoelectric constant                                       |
% | $\epsilon^{T}$ | $4.0 \cdot 10^{-8}\,F/m$   | Permittivity under constant stress                           |
% | $s^{D}$        | $21 \cdot 10^{-12}\,m^2/N$ | Elastic compliance understand constant electric displacement |
% | $c^{E}$        | $48 \cdot 10^{9}\,N/m^2$   | Young's modulus of elasticity                                |
% | $L$            | $20\,mm$ per stack         | Length of the stack                                          |
% | $A$            | $10^{-4}\,m^2$             | Area of the piezoelectric stack                              |
% | $n$            | $160$ per stack            | Number of layers in the piezoelectric stack                  |


%% Estimate "Sensor Constant" - (THP5H)
d33 = 680e-12;        % Strain constant [m/V]
n   = 160;            % Number of layers per stack
eT  = 4500*8.854e-12; % Permittivity under constant stress [F/m]
sD  = 21e-12;         % Compliance under constant electric displacement [m2/N]

gs_th = d33/(eT*sD*n);   % Sensor Constant [V/m]

%% Estimate "Actuator Constant" - (THP5H)
d33 = 680e-12;   % Strain constant [m/V]
n   = 320;       % Number of layers

cE  = 1/sD;      % Youngs modulus [N/m^2]
A   = (10e-3)^2; % Area of the stacks [m^2]
L   = 40e-3;     % Length of the two stacks [m]
ka  = cE*A/L;    % Stiffness of the two stacks [N/m]

ga_th = d33*n*ka;   % Actuator Constant [N/V]

% Comparison of the obtained dynamics

% The obtained dynamics using the /super element/ with the tuned "sensor sensitivity" and "actuator sensitivity" are compared with the experimentally identified frequency response functions in Figure ref:fig:test_apa_super_element_comp_frf.
% A good match between the model and the experimental results was observed.
% It is however surprising that the model is "softer" than the measured system, as finite element models usually overestimate the stiffness (see Section ref:ssec:test_apa_spurious_resonances for possible explanations).

% Using this simple test bench, it can be concluded that the /super element/ model of the APA300ML captures the axial dynamics of the actuator (the actuator stacks, the force sensor stack as well as the shell used as a mechanical lever).


%% Idenfify the dynamics of the Simscape model with correct actuator and sensor "constants"
% Initialize the APA
n_hexapod.actuator = initializeAPA(...
    'type', 'flexible', ...
    'ga',   23.2, ... % Actuator sensitivity [N/V]
    'gs',   -4.9e6);    % Sensor sensitivity [V/m]

% Identify with updated constants
G_flex = exp(-Ts*s)*linearize(mdl, io, 0.0, opts);
G_flex.InputName  = {'u'};
G_flex.OutputName = {'Vs', 'de'};

%% Comparison of the measured FRF and the "Flexible" model of the APA300ML
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(enc_frf(:, 1)), 'color', [0,0,0,0.2], 'DisplayName', 'Identified');
for i = 1:length(apa_nums)
    plot(f, abs(enc_frf(:, i)), 'color', [0,0,0,0.2], 'HandleVisibility', 'off');
end
plot(freqs, abs(squeeze(freqresp(G_flex('de', 'u'), freqs, 'Hz'))), '--', 'color', colors(2,:), 'DisplayName', '"Flexible" Model')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/u$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);

ax2 = nexttile;
hold on;
for i = 1:length(apa_nums)
    plot(f, 180/pi*angle(enc_frf(:, i)), 'color', [0,0,0,0.2]);
end
plot(freqs, 180/pi*angle(squeeze(freqresp(G_flex('de', 'u'), freqs, 'Hz'))), '--', 'color', colors(2,:))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);

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

%% Comparison of the measured FRF and the "Flexible" model of the APA300ML
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(iff_frf(:, 1)), 'color', [0,0,0,0.2], 'DisplayName', 'Identified');
for i = 2:length(apa_nums)
    plot(f, abs(iff_frf(:, i)), 'color', [0,0,0,0.2], 'HandleVisibility', 'off');
end
plot(freqs, abs(squeeze(freqresp(G_flex('Vs', 'u'), freqs, 'Hz'))), '--', 'color', colors(2,:), 'DisplayName', '"Flexible" Model')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/u$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-2, 1e2]);
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);

ax2 = nexttile;
hold on;
for i = 1:length(apa_nums)
    plot(f, 180/pi*angle(iff_frf(:, i)), 'color', [0,0,0,0.2]);
end
plot(freqs, 180/pi*angle(squeeze(freqresp(G_flex('Vs', 'u'), freqs, 'Hz'))), '--', 'color', colors(2,:))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);

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