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

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

%% Colors for the figures
colors = colororder;

% First Identification
% <<sec:simscape_bench_apa_first_id>>

% The APA is first initialized with default parameters:

%% Initialize the structure with default values
n_hexapod = struct();
n_hexapod.actuator = initializeAPA(...
    'type', '2dof', ...
    'Ga', 1, ... % Actuator constant [N/V]
    'Gs', 1);    % Sensor constant [V/m]



% The transfer function from excitation voltage $V_a$ (before the amplification of $20$ due to the PD200 amplifier) to:
% 1. the sensor stack voltage $V_s$
% 2. the measured displacement by the encoder $d_e$


%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Va'],  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

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

%% Run the linearization
Ga = linearize(mdl, io, 0.0, opts);
Ga.InputName  = {'Va'};
Ga.OutputName = {'Vs', 'de', 'da'};



% The obtain dynamics are shown in Figure ref:fig:apa_model_bench_bode_vs and ref:fig:apa_model_bench_bode_dl_z.
% It can be seen that:
% - the shape of these bode plots are very similar to the one measured in Section ref:sec:dynamical_meas_apa expect from a change in gain and exact location of poles and zeros
% - there is a sign error for the transfer function from $V_a$ to $V_s$.
%   This will be corrected by taking a negative "sensor gain".
% - the low frequency zero of the transfer function from $V_a$ to $V_s$ is minimum phase as expected.
%   The measured FRF are showing non-minimum phase zero, but it is most likely due to measurements artifacts.


%% Bode plot of the transfer function from u to taum
freqs = logspace(1, 3, 1000);

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

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(Ga('Vs', 'Va'), freqs, 'Hz'))), 'k-')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Ga('Vs', 'Va'), freqs, 'Hz'))), 'k-')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:45:360);
ylim([-180, 0])

linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);



% #+name: fig:apa_model_bench_bode_vs
% #+caption: Bode plot of the transfer function from $V_a$ to $V_s$
% #+RESULTS:
% [[file:figs/apa_model_bench_bode_vs.png]]


%% Bode plot of the transfer function from Va to de and da
freqs = logspace(1, 3, 1000);

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

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(Ga('de', 'Va'), freqs, 'Hz'))), 'DisplayName', 'Encoder')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'southwest');

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Ga('de', 'Va'), freqs, 'Hz'))))
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:45:360);
ylim([-180, 0])

linkaxes([ax1,ax2],'x');

% Identification Data
% Let's load the measured FRF from the DAC voltage to the measured encoder and to the sensor stack voltage.

%% Load Data
load('meas_apa_frf.mat', 'f', 'Ts', 'enc_frf', 'iff_frf', 'apa_nums');

% 2DoF APA
% Let's initialize the APA as a 2DoF model with unity sensor and actuator gains.

%% Initialize a 2DoF APA with Ga=Gs=1
n_hexapod.actuator = initializeAPA(...
    'type', '2dof', ...
    'ga', 1, ...
    'gs', 1);

% Identification without actuator or sensor constants
% The transfer function from $V_a$ to $V_s$, $d_e$ and $d_a$ is identified.

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Va'], 1, 'openinput');  io_i = io_i + 1; % Actuator 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
io(io_i) = linio([mdl, '/da'], 1, 'openoutput'); io_i = io_i + 1; % Attocube

%% Identification
Gs = linearize(mdl, io, 0.0, options);
Gs.InputName  = {'Va'};
Gs.OutputName = {'Vs', 'de', 'da'};

% Actuator Constant
% Then, the actuator constant can be computed as shown in Eq. eqref:eq:actuator_constant_formula by dividing the measured DC gain of the transfer function from $V_a$ to $d_e$ by the estimated DC gain of the transfer function from $V_a$ (in truth the actuator force called $F_a$) to $d_e$ using the Simscape model.

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

% Sensor Constant
% Similarly, the sensor constant can be estimated using Eq. eqref:eq:sensor_constant_formula.


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

% Comparison
% Let's now initialize the APA with identified sensor and actuator constant:

%% Set the identified constants
n_hexapod.actuator = initializeAPA(...
    'type', '2dof', ...
    'ga', ga, ... % Actuator gain [N/V]
    'gs', gs);    % Sensor gain [V/m]



% And identify the dynamics with included constants.

%% Identify again the dynamics with correct Ga,Gs
Gs = linearize(mdl, io, 0.0, options);
Gs = Gs*exp(-Ts*s);
Gs.InputName  = {'Va'};
Gs.OutputName = {'Vs', 'de', 'da'};



% The transfer functions from $V_a$ to $d_e$ are compared in Figure ref:fig:apa_act_constant_comp and the one from $V_a$ to $V_s$ are compared in Figure ref:fig:apa_sens_constant_comp.


%% Bode plot of the transfer function from u to de
freqs = logspace(1,4,1000);

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

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(apa_nums)
    plot(f, abs(enc_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d\mathcal{L}_m/u$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);

ax2 = nexttile;
hold on;
for i = 1:length(apa_nums)
    plot(f, 180/pi*angle(enc_frf(:,1)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))))
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]);



% #+name: fig:apa_act_constant_comp
% #+caption: Comparison of the experimental data and Simscape model ($V_a$ to $d_e$)
% #+RESULTS:
% [[file:figs/apa_act_constant_comp.png]]


%% Bode plot of the transfer function from Va to Vs (both Simscape and measured FRF)
freqs = logspace(1,4,1000);

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

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(apa_nums)
    plot(f, abs(iff_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $\tau_m/u$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-2, 1e2]);

ax2 = nexttile;
hold on;
for i = 1:length(apa_nums)
    plot(f, 180/pi*angle(iff_frf(:,1)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))))
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]);



% #+name: fig:apa_sens_constant_comp
% #+caption: Comparison of the experimental data and Simscape model ($V_a$ to $V_s$)
% #+RESULTS:
% [[file:figs/apa_sens_constant_comp.png]]


%% Compare the FRF and identified dynamics from Va to Vs and da
colors = colororder;

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', 'FRF');
for i = 2:length(apa_nums)
    plot(f, abs(enc_frf(:, i)), 'color', [0,0,0, 0.2], ...
         'HandleVisibility', 'off');
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gs('da', 'Va'), freqs, 'Hz'))), '--', ...
     'DisplayName', 'Model')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_a/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);
legend('location', 'southwest');

ax1b = nexttile([2,1]);
hold on;
plot(f, abs(iff_frf(:, i)), 'color', [0,0,0,0.2], ...
     'DisplayName', 'FRF');
for i = 1:length(apa_nums)
    plot(f, abs(iff_frf(:, i)), 'color', [0,0,0,0.2], ...
         'HandleVisibility', 'off');
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))), '--', ...
     'DisplayName', 'Model')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-2, 1e2]);
legend('location', 'southeast');

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
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('da', 'Va'), freqs, 'Hz'))), '--')
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
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))), '--')
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]);

% Flexible APA
% The Simscape APA model is initialized as a flexible one with unity "constants".

%% Initialize the APA as a flexible body
n_hexapod.actuator = initializeAPA(...
    'type', 'flexible', ...
    'ga', 1, ...
    'gs', 1);

% Identification without actuator or sensor constants
% The dynamics from $V_a$ to $V_s$, $d_e$ and $d_a$ is identified.

%% Identify the dynamics
Gs = linearize(mdl, io, 0.0, options);
Gs.InputName  = {'Va'};
Gs.OutputName = {'Vs', 'de', 'da'};

% Actuator Constant
% Then, the actuator constant can be computed as shown in Eq. eqref:eq:actuator_constant_formula:

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

% Sensor Constant

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

% Comparison
% Let's now initialize the flexible APA with identified sensor and actuator constant:

%% Set the identified constants
n_hexapod.actuator = initializeAPA(...
    'type', 'flexible', ...
    'ga', ga, ... % Actuator gain [N/V]
    'gs', gs);    % Sensor gain [V/m]



% And identify the dynamics with included constants.

%% Identify with updated constants
Gs = linearize(mdl, io, 0.0, options);
Gs = Gs*exp(-Ts*s);
Gs.InputName  = {'Va'};
Gs.OutputName = {'Vs', 'de', 'da'};



% The obtained dynamics is compared with the measured one in Figures ref:fig:apa_act_constant_comp_flex and ref:fig:apa_sens_constant_comp_flex.


%% Bode plot of the transfer function from V_a to d_e (both Simscape and measured FRF)
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(apa_nums)
    plot(f, abs(enc_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d\mathcal{L}_m/u$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-9, 1e-3]);

ax2 = nexttile;
hold on;
for i = 1:length(apa_nums)
    plot(f, 180/pi*angle(enc_frf(:,1)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))))
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]);



% #+name: fig:apa_act_constant_comp_flex
% #+caption: Comparison of the experimental data and Simscape model ($u$ to $d\mathcal{L}_m$)
% #+RESULTS:
% [[file:figs/apa_act_constant_comp_flex.png]]


%% Bode plot of the transfer function from Va to Vs (both Simscape and measured FRF)
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(apa_nums)
    plot(f, abs(iff_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $\tau_m/u$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-2, 1e2]);

ax2 = nexttile;
hold on;
for i = 1:length(apa_nums)
    plot(f, 180/pi*angle(iff_frf(:,1)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))))
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]);

% Optimize 2-DoF model to fit the experimental Data
% <<sec:simscape_bench_apa_tune_2dof_model>>
% The parameters of the 2DoF model presented in Section ref:sec:apa_2dof_model are now optimize such that the model best matches the measured FRF.

% After optimization, the following parameters are used:

%% Optimized parameters
n_hexapod.actuator = initializeAPA('type', '2dof', ...
                                   'Ga', -32.2, ...
                                   'Gs', 0.088, ...
                                   'k',  ones(6,1)*0.38e6, ...
                                   'ke', ones(6,1)*1.75e6, ...
                                   'ka', ones(6,1)*3e7, ...
                                   'c',  ones(6,1)*1.3e2, ...
                                   'ce', ones(6,1)*1e1, ...
                                   'ca', ones(6,1)*1e1 ...
                                   );

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Va'],  1, 'openinput');  io_i = io_i + 1; % Actuator 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

%% Identification with optimized parameters
Gs = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
Gs.InputName  = {'Va'};
Gs.OutputName = {'Vs', 'de'};



% The dynamics is identified using the Simscape model and compared with the measured FRF in Figure ref:fig:comp_apa_plant_after_opt.

%% Comparison of the experimental data and Simscape Model
freqs = 5*logspace(0, 3, 1000);
figure;
tiledlayout(3, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(apa_nums)
    plot(f, abs(enc_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);

ax1b = nexttile([2,1]);
hold on;
for i = 1:length(apa_nums)
    plot(f, abs(iff_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-2, 1e2]);

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
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))))
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
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))))
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]);