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

% Tuning of the APA model
% <<ssec:test_apa_2dof_model_tuning>>

% 9 parameters ($m$, $k_1$, $c_1$, $k_e$, $c_e$, $k_a$, $c_a$, $g_s$ and $g_a$) have to be tuned such that the dynamics of the model (Figure ref:fig:test_apa_2dof_model_simscape) well represents the identified dynamics in Section ref:sec:test_apa_dynamics.

% #+name: fig:test_apa_2dof_model_simscape
% #+caption: Schematic of the two degrees of freedom model of the APA300ML with input $V_a$ and outputs $d_e$ and $V_s$
% [[file:figs/test_apa_2dof_model_simscape.png]]


%% Stiffness values for the 2DoF APA model
k1 = 0.38e6; % Estimated Shell Stiffness [N/m]

w0 = 2*pi*95; % Resonance frequency [rad/s]
m = 5.7; % Suspended mass [kg]
ktot = m*(w0)^2; % Total Axial Stiffness to have to wanted resonance frequency [N/m]

ka = 1.5*(ktot-k1); % Stiffness of the (two) actuator stacks [N/m]
ke = 2*ka; % Stiffness of the Sensor stack [N/m]

%% Damping values for the 2DoF APA model
c1 = 20;   % Damping for the Shell [N/(m/s)]
ca = 100;  % Damping of the actuators stacks [N/(m/s)]
ce = 2*ca; % Damping of the sensor stack [N/(m/s)]

%% Estimation ot the sensor and actuator gains
% Initialize the structure with unitary sensor and actuator "gains"
n_hexapod = struct();
n_hexapod.actuator = initializeAPA(...
    'type', '2dof', ...
    'k',  k1,  ...
    'ka', ka,  ...
    'ke', ke,  ...
    'c',  c1, ...
    'ca', ca, ...
    'ce', ce, ...
    'Ga', 1,   ... % Actuator constant [N/V]
    'Gs', 1    ... % Sensor constant [V/m]
);

c_granite = 0; % Do not take into account damping added by the air bearing

% Run the linearization
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');

% Estimation ot the Actuator Gain
fa = 10; % Frequency where the two FRF should match [Hz]
[~, i_f] = min(abs(f - fa));
ga = -abs(enc_frf(i_f,1))./abs(evalfr(G_norm('de', 'u'), 1i*2*pi*fa));

% Estimation ot the Sensor Gain
fs = 600; % Frequency where the two FRF should match [Hz]
[~, i_f] = min(abs(f - fs))
gs = -abs(iff_frf(i_f,1))./abs(evalfr(G_norm('Vs', 'u'), 1i*2*pi*fs))/ga;

% Obtained Dynamics
% <<ssec:test_apa_2dof_model_result>>

% The dynamics of the 2DoF APA300ML model is now extracted using optimized parameters (listed in Table ref:tab:test_apa_2dof_parameters) from the Simscape model.
% It is compared with the experimental data in Figure ref:fig:test_apa_2dof_comp_frf.

% A good match can be observed between the model and the experimental data, both for the encoder and for the force sensor.
% This indicates that this model represents well the axial dynamics of the APA300ML.


%% 2DoF APA300ML with optimized parameters
n_hexapod = struct();
n_hexapod.actuator = initializeAPA(...
    'type', '2dof', ...
    'k',  k1,  ...
    'ka', ka,  ...
    'ke', ke,  ...
    'c',  c1, ...
    'ca', ca, ...
    'ce', ce, ...
    'Ga', ga, ...
    'Gs', gs  ...
);

%% Identification of the APA300ML with optimized parameters
G_2dof = exp(-s*Ts)*linearize(mdl, io, 0.0, opts);
G_2dof.InputName  = {'u'};
G_2dof.OutputName = {'Vs', 'de'};

%% Comparison of the measured FRF and the optimized 2DoF model of the APA300ML
freqs = 5*logspace(0, 3, 1000);
figure;
tiledlayout(3, 2, '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_2dof('de', 'u'), freqs, 'Hz'))), '--', 'color', colors(2,:), 'DisplayName', '2DoF 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);

ax1b = 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_2dof('Vs', 'u'), freqs, 'Hz'))), '--', 'color', colors(2,:), 'DisplayName', '2DoF 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(enc_frf(:, i)), 'color', [0,0,0,0.2]);
end
plot(freqs, 180/pi*angle(squeeze(freqresp(G_2dof('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]);

ax2b = 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_2dof('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,ax1b,ax2b],'x');
xlim([10, 2e3]);