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

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

addpath('APA300ML/');

open('APA300ML.slx');

% Import Mass Matrix, Stiffness Matrix, and Interface Nodes Coordinates
% We first extract the stiffness and mass matrices.

K = readmatrix('mat_K.CSV');
M = readmatrix('mat_M.CSV');



% #+caption: First 10x10 elements of the Mass matrix
% #+RESULTS:
% |    0.01 |  -2e-06 |  1e-06 |  6e-09 |  5e-05 | -5e-09 | -0.0005 |  -7e-07 |  6e-07 | -3e-09 |
% |  -2e-06 |    0.01 |  8e-07 | -2e-05 | -8e-09 |  2e-09 |  -9e-07 | -0.0002 |  1e-08 | -9e-07 |
% |   1e-06 |   8e-07 |  0.009 |  5e-10 |  1e-09 | -1e-09 |  -5e-07 |   3e-08 |  6e-05 |  1e-10 |
% |   6e-09 |  -2e-05 |  5e-10 |  3e-07 |  2e-11 | -3e-12 |   3e-09 |   9e-07 | -4e-10 |  3e-09 |
% |   5e-05 |  -8e-09 |  1e-09 |  2e-11 |  6e-07 | -4e-11 |  -1e-06 |  -2e-09 |  1e-09 | -8e-12 |
% |  -5e-09 |   2e-09 | -1e-09 | -3e-12 | -4e-11 |  1e-07 |  -2e-09 |  -1e-09 | -4e-10 | -5e-12 |
% | -0.0005 |  -9e-07 | -5e-07 |  3e-09 | -1e-06 | -2e-09 |    0.01 |   1e-07 | -3e-07 | -2e-08 |
% |  -7e-07 | -0.0002 |  3e-08 |  9e-07 | -2e-09 | -1e-09 |   1e-07 |    0.01 | -4e-07 |  2e-05 |
% |   6e-07 |   1e-08 |  6e-05 | -4e-10 |  1e-09 | -4e-10 |  -3e-07 |  -4e-07 |  0.009 | -2e-10 |
% |  -3e-09 |  -9e-07 |  1e-10 |  3e-09 | -8e-12 | -5e-12 |  -2e-08 |   2e-05 | -2e-10 |  3e-07 |


% Then, we extract the coordinates of the interface nodes.

[int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes('out_nodes_3D.txt');

% Piezoelectric parameters
% In order to make the conversion from applied voltage to generated force or from the strain to the generated voltage, we need to defined some parameters corresponding to the piezoelectric material:

d33 = 300e-12; % Strain constant [m/V]
n   = 80;      % Number of layers per stack
eT  = 1.6e-8;  % Permittivity under constant stress [F/m]
sD  = 1e-11;   % Compliance under constant electric displacement [m2/N]
ka  = 235e6;   % Stack stiffness [N/m]
C   = 5e-6;    % Stack capactiance [F]



% The ratio of the developed force to applied voltage is:
% #+name: eq:piezo_voltage_to_force
% \begin{equation}
%   F_a = g_a V_a, \quad g_a = d_{33} n k_a
% \end{equation}
% where:
% - $F_a$: developed force in [N]
% - $n$: number of layers of the actuator stack
% - $d_{33}$: strain constant in [m/V]
% - $k_a$: actuator stack stiffness in [N/m]
% - $V_a$: applied voltage in [V]

% If we take the numerical values, we obtain:

d33*n*ka % [N/V]



% #+RESULTS:
% : 5.64

% From cite:fleming14_desig_model_contr_nanop_system (page 123), the relation between relative displacement of the sensor stack and generated voltage is:
% #+name: eq:piezo_strain_to_voltage
% \begin{equation}
%   V_s = \frac{d_{33}}{\epsilon^T s^D n} \Delta h
% \end{equation}
% where:
% - $V_s$: measured voltage in [V]
% - $d_{33}$: strain constant in [m/V]
% - $\epsilon^T$: permittivity under constant stress in [F/m]
% - $s^D$: elastic compliance under constant electric displacement in [m^2/N]
% - $n$: number of layers of the sensor stack
% - $\Delta h$: relative displacement in [m]

% If we take the numerical values, we obtain:

1e-6*d33/(eT*sD*n) % [V/um]

% Stiffness

m = 0.001;



% The transfer function from vertical external force to the relative vertical displacement is identified.


%% Name of the Simulink File
mdl = 'APA300ML';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Fd'], 1, 'openinput');  io_i = io_i + 1;
io(io_i) = linio([mdl, '/z'],  1, 'openoutput'); io_i = io_i + 1;

G = linearize(mdl, io);



% The inverse of its DC gain is the axial stiffness of the APA:

1e-6/dcgain(G) % [N/um]

% Resonance Frequency
% The resonance frequency is specified to be between 650Hz and 840Hz.
% This is also the case for the FEM model (Figure [[fig:apa300ml_resonance]]).


freqs = logspace(2, 4, 5000);

figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G, freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude');
hold off;

% Amplification factor
% The amplification factor is the ratio of the vertical displacement to the stack displacement.


%% Name of the Simulink File
mdl = 'APA300ML';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'], 1, 'openinput');  io_i = io_i + 1;
io(io_i) = linio([mdl, '/z'], 1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/d'], 1, 'openoutput'); io_i = io_i + 1;

G = linearize(mdl, io);



% The ratio of the two displacement is computed from the FEM model.

abs(dcgain(G(1,1))./dcgain(G(2,1)))



% #+RESULTS:
% : 5.0749

% This is actually correct and approximately corresponds to the ratio of the piezo height and length:

75/15

% Stroke

% Estimation of the actuator stroke:
% \[ \Delta H = A n \Delta L \]
% with:
% - $\Delta H$ Axial Stroke of the APA
% - $A$ Amplification factor (5 for the APA300ML)
% - $n$ Number of stack used
% - $\Delta L$ Stroke of the stack (0.1% of its length)


1e6 * 5 * 3 * 20e-3 * 0.1e-2

% Identification of the Dynamics from actuator to replace displacement
% We first set the mass to be approximately zero.

m = 0.01;



% The dynamics is identified from the applied force to the measured relative displacement.

%% Name of the Simulink File
mdl = 'APA300ML';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'], 1, 'openinput');  io_i = io_i + 1;
io(io_i) = linio([mdl, '/z'], 1, 'openoutput'); io_i = io_i + 1;

Gh = -linearize(mdl, io);



% The same dynamics is identified for a payload mass of 10Kg.

m = 10;

%% Name of the Simulink File
mdl = 'APA300ML';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'], 1, 'openinput');  io_i = io_i + 1;
io(io_i) = linio([mdl, '/z'], 1, 'openoutput'); io_i = io_i + 1;

Ghm = -linearize(mdl, io);

freqs = logspace(0, 4, 5000);

figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(Gh, freqs, 'Hz'))), '-');
plot(freqs, abs(squeeze(freqresp(Ghm, freqs, 'Hz'))), '-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
hold off;

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gh, freqs, 'Hz')))), '-', ...
     'DisplayName', '$m = 0kg$');
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Ghm, freqs, 'Hz')))), '-', ...
     'DisplayName', '$m = 10kg$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
yticks(-360:90:360);
ylim([-360 0]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
legend('location', 'southwest');



% #+name: fig:apa300ml_plant_dynamics
% #+caption: Transfer function from forces applied by the stack to the axial displacement of the APA
% #+RESULTS:
% [[file:figs/apa300ml_plant_dynamics.png]]

% The root locus corresponding to Direct Velocity Feedback with a mass of 10kg is shown in Figure [[fig:apa300ml_dvf_root_locus]].

figure;

gains = logspace(0, 5, 500);

hold on;
plot(real(pole(Ghm)),  imag(pole(G)), 'kx');
plot(real(tzero(Ghm)),  imag(tzero(G)), 'ko');
for k = 1:length(gains)
    cl_poles = pole(feedback(Ghm, gains(k)*s));
    plot(real(cl_poles), imag(cl_poles), 'k.');
end
hold off;
axis square;
xlim([-500, 10]); ylim([0, 510]);

xlabel('Real Part'); ylabel('Imaginary Part');

% Identification of the Dynamics from actuator to force sensor
% Let's use 2 stacks as a force sensor and 1 stack as force actuator.

% The transfer function from actuator voltage to sensor voltage is identified and shown in Figure [[fig:apa300ml_iff_plant]].

m = 10;

%% Name of the Simulink File
mdl = 'APA300ML';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Va'], 1, 'openinput');  io_i = io_i + 1;
io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1;

Giff = -linearize(mdl, io);

freqs = logspace(0, 4, 5000);

figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(Giff, freqs, 'Hz'))), '-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
hold off;

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Giff, freqs, 'Hz')))), '-');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
yticks(-360:90:360);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);



% #+name: fig:apa300ml_iff_plant
% #+caption: Transfer function from actuator to force sensor
% #+RESULTS:
% [[file:figs/apa300ml_iff_plant.png]]

% For root locus corresponding to IFF is shown in Figure [[fig:apa300ml_iff_root_locus]].

figure;

gains = logspace(0, 5, 500);

hold on;
plot(real(pole(Giff)),  imag(pole(Giff)), 'kx');
plot(real(tzero(Giff)),  imag(tzero(Giff)), 'ko');
for k = 1:length(gains)
    cl_poles = pole(feedback(Giff, gains(k)/s));
    plot(real(cl_poles), imag(cl_poles), 'k.');
end
hold off;
axis square;
xlim([-500, 10]); ylim([0, 510]);

xlabel('Real Part'); ylabel('Imaginary Part');

% Identification for a simpler model
% The goal in this section is to identify the parameters of a simple APA model from the FEM.
% This can be useful is a lower order model is to be used for simulations.

% The presented model is based on cite:souleille18_concep_activ_mount_space_applic.

% The model represents the Amplified Piezo Actuator (APA) from Cedrat-Technologies (Figure [[fig:souleille18_model_piezo]]).
% The parameters are shown in the table below.

% #+name: fig:souleille18_model_piezo
% #+caption: Picture of an APA100M from Cedrat Technologies. Simplified model of a one DoF payload mounted on such isolator
% [[file:./figs/souleille18_model_piezo.png]]

% #+caption:Parameters used for the model of the APA 100M
% |       | Meaning                                                        |
% |-------+----------------------------------------------------------------|
% | $k_e$ | Stiffness used to adjust the pole of the isolator              |
% | $k_1$ | Stiffness of the metallic suspension when the stack is removed |
% | $k_a$ | Stiffness of the actuator                                      |
% | $c_1$ | Added viscous damping                                          |

% The goal is to determine $k_e$, $k_a$ and $k_1$ so that the simplified model fits the FEM model.

% \[ \alpha = \frac{x_1}{f}(\omega=0) = \frac{\frac{k_e}{k_e + k_a}}{k_1 + \frac{k_e k_a}{k_e + k_a}} \]
% \[ \beta = \frac{x_1}{F}(\omega=0) = \frac{1}{k_1 + \frac{k_e k_a}{k_e + k_a}} \]

% If we can fix $k_a$, we can determine $k_e$ and $k_1$ with:
% \[ k_e = \frac{k_a}{\frac{\beta}{\alpha} - 1} \]
% \[ k_1 = \frac{1}{\beta} - \frac{k_e k_a}{k_e + k_a} \]


m = 10;

%% Name of the Simulink File
mdl = 'APA300ML';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Fd'], 1, 'openinput');  io_i = io_i + 1; % External Vertical Force [N]
io(io_i) = linio([mdl, '/w'],  1, 'openinput');  io_i = io_i + 1; % Base Motion [m]
io(io_i) = linio([mdl, '/Fa'], 1, 'openinput');  io_i = io_i + 1; % Actuator Force [N]
io(io_i) = linio([mdl, '/z'],  1, 'openoutput'); io_i = io_i + 1; % Vertical Displacement [m]
io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensor [V]
io(io_i) = linio([mdl, '/d'],  1, 'openoutput'); io_i = io_i + 1; % Stack Displacement [m]

G = linearize(mdl, io);

G.InputName = {'Fd', 'w', 'Fa'};
G.OutputName = {'y', 'Fs', 'd'};



% From the identified dynamics, compute $\alpha$ and $\beta$

alpha = abs(dcgain(G('y', 'Fa')));
beta  = abs(dcgain(G('y', 'Fd')));



% $k_a$ is estimated using the following formula:

ka = 0.8/abs(dcgain(G('y', 'Fa')));


% The factor can be adjusted to better match the curves.

% Then $k_e$ and $k_1$ are computed.

ke = ka/(beta/alpha - 1);
k1 = 1/beta - ke*ka/(ke + ka);



% #+RESULTS:
% |    | Value [N/um] |
% |----+--------------|
% | ka |         40.5 |
% | ke |          1.5 |
% | k1 |          0.4 |

% The damping in the system is adjusted to match the FEM model if necessary.

c1 = 1e2;



% The analytical model of the simpler system is defined below:

Ga = 1/(m*s^2 + k1 + c1*s + ke*ka/(ke + ka)) * ...
     [ 1 ,              k1 + c1*s + ke*ka/(ke + ka)  , ke/(ke + ka) ;
      -ke*ka/(ke + ka), ke*ka/(ke + ka)*m*s^2 ,       -ke/(ke + ka)*(m*s^2 + c1*s + k1)];

Ga.InputName = {'Fd', 'w', 'Fa'};
Ga.OutputName = {'y', 'Fs'};



% And the DC gain is adjusted for the force sensor:

F_gain = dcgain(G('Fs', 'Fd'))/dcgain(Ga('Fs', 'Fd'));



% The dynamics of the FEM model and the simpler model are compared in Figure [[fig:apa300ml_comp_simpler_model]].


freqs = logspace(0, 5, 1000);

figure;
tiledlayout(2, 3, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'y', 'w'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Ga('y', 'w'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('$x_1/w$ [m/m]');
ylim([1e-6, 1e2]);

ax2 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'y', 'Fa'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Ga('y', 'Fa'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('$x_1/f$ [m/N]');
ylim([1e-14, 1e-6]);

ax3 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'y', 'Fd'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Ga('y', 'Fd'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('$x_1/F$ [m/N]');
ylim([1e-14, 1e-4]);

ax4 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Fs', 'w'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(F_gain*Ga('Fs', 'w'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
ylabel('$F_s/w$ [m/m]');
ylim([1e2, 1e8]);

ax5 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Fs', 'Fa'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(F_gain*Ga('Fs', 'Fa'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
ylabel('$F_s/f$ [m/N]');
ylim([1e-4, 1e1]);

ax6 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Fs', 'Fd'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(F_gain*Ga('Fs', 'Fd'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
ylabel('$F_s/F$ [m/N]');
ylim([1e-7, 1e2]);

linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');



% #+name: fig:apa300ml_comp_simpler_model
% #+caption: Comparison of the Dynamics between the FEM model and the simplified one
% #+RESULTS:
% [[file:figs/apa300ml_comp_simpler_model.png]]

% The simplified model has also been implemented in Simscape.

% The dynamics of the Simscape simplified model is identified and compared with the FEM one in Figure [[fig:apa300ml_comp_simpler_simscape]].

%% Name of the Simulink File
mdl = 'APA300ML_simplified';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Fd'], 1, 'openinput');  io_i = io_i + 1; % External Vertical Force [N]
io(io_i) = linio([mdl, '/w'],  1, 'openinput');  io_i = io_i + 1; % Base Motion [m]
io(io_i) = linio([mdl, '/Fa'], 1, 'openinput');  io_i = io_i + 1; % Actuator Force [N]
io(io_i) = linio([mdl, '/y'],  1, 'openoutput'); io_i = io_i + 1; % Vertical Displacement [m]
io(io_i) = linio([mdl, '/Fs'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensor [V]

Gs = linearize(mdl, io);

Gs.InputName = {'Fd', 'w', 'Fa'};
Gs.OutputName = {'y', 'Fs'};

freqs = logspace(0, 5, 1000);

figure;
tiledlayout(2, 3, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'y', 'w'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gs('y', 'w'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('$x_1/w$ [m/m]');
ylim([1e-6, 1e2]);

ax2 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'y', 'Fa'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gs('y', 'Fa'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('$x_1/f$ [m/N]');
ylim([1e-14, 1e-6]);

ax3 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'y', 'Fd'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gs('y', 'Fd'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('$x_1/F$ [m/N]');
ylim([1e-14, 1e-4]);

ax4 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Fs', 'w'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(F_gain*Gs('Fs', 'w'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
ylabel('$F_s/w$ [m/m]');
ylim([1e2, 1e8]);

ax5 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Fs', 'Fa'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(F_gain*Gs('Fs', 'Fa'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
ylabel('$F_s/f$ [m/N]');
ylim([1e-4, 1e1]);

ax6 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Fs', 'Fd'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(F_gain*Gs('Fs', 'Fd'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
ylabel('$F_s/F$ [m/N]');
ylim([1e-7, 1e2]);

linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');