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

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

%% Path for functions, data and scripts
addpath('./mat/'); % Path for data
addpath('./src/'); % Path for functions

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

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

%% Open Simscape Model
mdl = 'test_struts_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
io(io_i) = linio([mdl, '/da'], 1, 'openoutput'); io_i = io_i + 1; % Interferometer

freqs = logspace(1, 3, 1000);

% 2Dof model
% The strut is initialized with default parameters (optimized parameters identified from previous experiments).

%% Initialize structure containing data for the Simscape model
n_hexapod = struct();
n_hexapod.flex_bot = initializeBotFlexibleJoint('type', '4dof');
n_hexapod.flex_top = initializeTopFlexibleJoint('type', '4dof');
n_hexapod.actuator = initializeAPA('type', '2dof');

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



% The dynamics is identified and shown in Figure ref:fig:strut_bench_model_bode.

%% Run the linearization
Gs = exp(-s*1e-4)*linearize(mdl, io, 0.0, opts);
Gs.InputName  = {'u'};
Gs.OutputName = {'Vs', 'de', 'da'};

%% Bode plot of the transfer functions
figure;
tiledlayout(3, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

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

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

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

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

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



% #+name: fig:strut_bench_model_bode
% #+caption: Identified transfer function from $u$ to $V_s$ and from $u$ to $d_e,d_a$ using the simple 2DoF model for the APA
% #+RESULTS:
% [[file:figs/strut_bench_model_bode.png]]

% The experimentally measured FRF are loaded.

%% Load measured FRF
load('meas_struts_frf.mat', 'f', 'enc_frf', 'int_frf', 'iff_frf', 'strut_nums', 'strut_align');



% The FRF from $u$ to $d_a$ as well as from $u$ to $V_s$ are shown in Figure ref:fig:comp_strut_plant_after_opt and compared with the model.
% They are both found to match quite well with the model.

%% Compare the FRF and identified dynamics from u to Vs and da
figure;
tiledlayout(3, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(int_frf(:, 1)), 'color', [0,0,0,0.2], ...
     'DisplayName', 'Meas. FRF');
for i = 2:length(strut_nums)
    plot(f, abs(int_frf(:, i)), 'color', [0,0,0,0.2], ...
         'HandleVisibility', 'off');
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gs('da', 'u'), freqs, 'Hz'))), '-', ...
     'DisplayName', 'Model')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_a/u$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);
legend('location', 'northeast');

ax1b = nexttile([2,1]);
hold on;
plot(f, abs(iff_frf(:, i)), 'color', [0,0,0,0.2], ...
     'DisplayName', 'Meas. FRF');
for i = 1:length(strut_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', 'u'), freqs, 'Hz'))), '-', ...
     'DisplayName', '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');

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



% #+name: fig:comp_strut_plant_after_opt
% #+caption: Comparison of the measured FRF and the optimized model
% #+RESULTS:
% [[file:figs/comp_strut_plant_after_opt.png]]

% The measured FRF from $u$ to $d_e$ (encoder) is compared with the model in Figure ref:fig:comp_strut_plant_iff_after_opt.

%% Compare the FRF and identified dynamics from u to de
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', 'Meas. FRF');
for i = 2:length(strut_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('de', 'u'), freqs, 'Hz'))), '-', ...
     'DisplayName', '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');

ax2 = nexttile;
hold on;
for i = 1:length(strut_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', 'u'), 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([20, 2e3]);

% Comparison with the Flexible Model
% The strut is initialized with default parameters (optimized parameters identified from previous experiments).


%% Initialize structure containing data for the Simscape model
n_hexapod = struct();
n_hexapod.flex_bot = initializeBotFlexibleJoint('type', '4dof');
n_hexapod.flex_top = initializeTopFlexibleJoint('type', '4dof');
n_hexapod.actuator = initializeAPA('type', 'flexible');

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



% The dynamics is identified and shown in Figure ref:fig:strut_bench_model_bode.

%% Run the linearization
Gs = exp(-s*1e-4)*linearize(mdl, io, 0.0, opts);
Gs.InputName  = {'u'};
Gs.OutputName = {'Vs', 'de', 'da'};



% - [ ] Add encoder plot

% The FRF from $u$ to $d_a$ as well as from $u$ to $V_s$ are shown in Figure ref:fig:comp_strut_plant_after_opt and compared with the model.
% They are both found to match quite well with the model.

%% Compare the FRF and identified dynamics from u to Vs and da
figure;
tiledlayout(3, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(enc_frf(:, 1)), 'color', [colors(2,:), 0.5], ...
     'DisplayName', 'FRF - Encoder');
plot(f, abs(int_frf(:, 1)), 'color', [0,0,0, 0.2], ...
     'DisplayName', 'FRF - Interferometer');
for i = 2:length(strut_nums)
    plot(f, abs(int_frf(:, i)), 'color', [0,0,0, 0.2], ...
         'HandleVisibility', 'off');
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gs('da', 'u'), freqs, 'Hz'))), '--', ...
     'DisplayName', 'Model - Interferometer')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_a/u$ [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', [colors(2,:), 0.2], ...
     'DisplayName', 'Meas. FRF');
for i = 1:length(strut_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', 'u'), freqs, 'Hz'))), '--', ...
     'DisplayName', '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');

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(enc_frf(:, 1)), 'color', [colors(2,:), 0.5], ...
     'HandleVisibility', 'off');
for i = 1:length(strut_nums)
    plot(f, 180/pi*(angle(int_frf(:, i))), 'color', [0,0,0, 0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('da', 'u'), 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(strut_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', 'u'), 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]);

% Perfectly aligned APA
% Let's first consider that the strut is perfectly mounted such that the two flexible joints and the APA are aligned.

%% Initialize Simscape data
n_hexapod.flex_bot = initializeBotFlexibleJoint('type', '4dof');
n_hexapod.flex_top = initializeTopFlexibleJoint('type', '4dof');
n_hexapod.actuator = initializeAPA('type', 'flexible');



% And define the inputs and outputs of the models:
% - Input: voltage generated by the DAC
% - Output: measured displacement by the encoder

% The transfer function is identified and shown in Figure ref:fig:comp_enc_frf_align_perfect.

%% Identification
Gs = exp(-s*1e-4)*linearize(mdl, io, 0.0, opts);
Gs.InputName  = {'u'};
Gs.OutputName = {'Vs', 'de', 'da'};



% From Figure ref:fig:comp_enc_frf_align_perfect, it is clear that:
% 1. The model with perfect alignment is not matching the measured FRF
% 2. The mode at 200Hz is not present in the identified dynamics of the Simscape model
% 3. The measured FRF have different shapes


%% Measured FRF from Vs to de and identified dynamics using the flexible APA
freqs = 2*logspace(0, 3, 1000);

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

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(enc_frf(:, i)), 'color', [0,0,0,0.2], ...
     'DisplayName', 'Meas. FRF');
for i = 2:length(strut_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('de', 'u'), freqs, 'Hz'))), '-', ...
     'DisplayName', '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');

ax2 = nexttile;
hold on;
for i = 1:length(strut_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', 'u'), 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]);

% Effect of a misalignment in y
% Let's compute the transfer function from output DAC voltage $V_s$ to the measured displacement by the encoder $d_e$ for several misalignment in the $y$ direction:

%% Considered misalignments
dy_aligns = [-0.5, -0.1, 0, 0.1, 0.5]*1e-3; % [m]

%% Transfer functions from u to de for all the misalignment in y direction
Gs_align = {zeros(length(dy_aligns), 1)};

for i = 1:length(dy_aligns)
    n_hexapod.actuator = initializeAPA('type', 'flexible', 'd_align_bot', [0; dy_aligns(i); 0], 'd_align_top', [0; dy_aligns(i); 0]);

    G = exp(-s*1e-4)*linearize(mdl, io, 0.0, opts);
    G.InputName  = {'u'};
    G.OutputName = {'Vs', 'de', 'da'};

    Gs_align(i) = {G};
end



% The obtained dynamics are shown in Figure ref:fig:effect_misalignment_y.


%% Transfer function from Vs to de - effect of x-misalignment
freqs = 2*logspace(0, 3, 1000);

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

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(dy_aligns)
    plot(freqs, abs(squeeze(freqresp(Gs_align{i}('de', 'u'), freqs, 'Hz'))), ...
         'DisplayName', sprintf('$d_y = %.1f$ [mm]', 1e3*dy_aligns(i)));
end
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');

ax2 = nexttile;
hold on;
for i = 1:length(dy_aligns)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gs_align{i}('de', 'u'), freqs, 'Hz'))));
end
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]);

% Effect of a misalignment in x
% Let's compute the transfer function from output DAC voltage to the measured displacement by the encoder for several misalignment in the $x$ direction:

%% Considered misalignments
dx_aligns = [-0.1, -0.05, 0, 0.05, 0.1]*1e-3; % [m]

%% Transfer functions from u to de for all the misalignment in x direction
Gs_align = {zeros(length(dx_aligns), 1)};

for i = 1:length(dx_aligns)
    n_hexapod.actuator = initializeAPA('type', 'flexible', 'd_align_bot', [dx_aligns(i); 0; 0], 'd_align_top', [dx_aligns(i); 0; 0]);

    G = exp(-s*1e-4)*linearize(mdl, io, 0.0, opts);
    G.InputName  = {'u'};
    G.OutputName = {'Vs', 'de', 'da'};

    Gs_align(i) = {G};
end



% The obtained dynamics are shown in Figure ref:fig:effect_misalignment_x.

%% Transfer function from Vs to de - effect of x-misalignment
freqs = 2*logspace(0, 3, 1000);

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

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(dx_aligns)
    plot(freqs, abs(squeeze(freqresp(Gs_align{i}('de', 'u'), freqs, 'Hz'))), ...
         'DisplayName', sprintf('$d_x = %.2f$ [mm]', 1e3*dx_aligns(i)));
end
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');

ax2 = nexttile;
hold on;
for i = 1:length(dx_aligns)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gs_align{i}('de', 'u'), freqs, 'Hz'))));
end
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 with identified misalignment

strut_align = 1e-3*[[-0.60, -0.82, -0.40, -0.16]
                    [-0.30, -0.63, -0.67, -0.34]
                    [-0.88, -0.79, -0.07, -0.16]
                    [-0.48,  0.07, -0.46, -1.00]
                    [-0.33, -0.48, -0.64, -0.52]
                    [-0.34, -0.42, -0.63, -0.57]];

%% Idenfity the transfer function from actuator to encoder for all cases
Gs_align = {zeros(size(strut_align,1), 1)};

for i = 1:size(strut_align,1)
    n_hexapod.actuator = initializeAPA('type', 'flexible', ...
                                       'd_align_bot', [0; strut_align(i, 2) - strut_align(i, 4); 0], ...
                                       'd_align_top', [0; strut_align(i, 1) - strut_align(i, 3); 0]);

    G = exp(-s*1e-4)*linearize(mdl, io, 0.0, opts);
    G.InputName  = {'u'};
    G.OutputName = {'Vs', 'de', 'da'};

    Gs_align(i) = {G};
end

%% Comparison of the plants (encoder output) when tuning the misalignment
freqs = 2*logspace(0, 3, 1000);

figure;
tiledlayout(2, 3, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile();
hold on;
plot(f, abs(enc_frf(:, 1)));
plot(freqs, abs(squeeze(freqresp(Gs_align{1}('de', 'u'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/V]');

ax2 = nexttile();
hold on;
plot(f, abs(enc_frf(:, 2)));
plot(freqs, abs(squeeze(freqresp(Gs_align{2}('de', 'u'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);

ax3 = nexttile(4);
hold on;
plot(f, abs(enc_frf(:, 3)), 'DisplayName', 'Meas.');
plot(freqs, abs(squeeze(freqresp(Gs_align{3}('de', 'u'), freqs, 'Hz'))), ...
     'DisplayName', 'Model');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
legend('location', 'southwest', 'FontSize', 8);

ax4 = nexttile(5);
hold on;
plot(f, abs(enc_frf(:, 4)));
plot(freqs, abs(squeeze(freqresp(Gs_align{4}('de', 'u'), freqs, 'Hz'))));
hold off;
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');

ax5 = nexttile(6);
hold on;
plot(f, abs(enc_frf(:, 5)));
plot(freqs, abs(squeeze(freqresp(Gs_align{5}('de', 'u'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);

linkaxes([ax1,ax2,ax3,ax4,ax5],'xy');
xlim([20, 2e3]); ylim([1e-8, 1e-3]);

% Find the misalignment of each strut
% From the previous analysis on the effect of a $x$ and $y$ misalignment, it is possible to estimate the $x,y$ misalignment of the measured struts.

% The misalignment that gives the best match for the FRF are defined below.

%% Tuned misalignment [m]
d_aligns = [[-0.05,  -0.3,  0];
            [ 0,      0.5,  0];
            [-0.1,   -0.3,  0];
            [ 0,      0.3,  0];
            [-0.05,   0.05, 0]]'*1e-3;



% For each misalignment, the dynamics from the DAC voltage to the encoder measurement is identified.

%% Idenfity the transfer function from actuator to encoder for all cases
Gs_align = {zeros(size(d_aligns,2), 1)};

for i = 1:5
    n_hexapod.actuator = initializeAPA('type', 'flexible', 'd_align_top', d_aligns(:,i), 'd_align_bot', d_aligns(:,i));

    G = exp(-s*1e-4)*linearize(mdl, io, 0.0, opts);
    G.InputName  = {'u'};
    G.OutputName = {'Vs', 'de', 'da'};

    Gs_align(i) = {G};
end



% The results are shown in Figure ref:fig:comp_all_struts_corrected_misalign.

%% Comparison of the plants (encoder output) when tuning the misalignment
freqs = 2*logspace(0, 3, 1000);

figure;
tiledlayout(2, 3, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile();
hold on;
plot(f, abs(enc_frf(:, 1)));
plot(freqs, abs(squeeze(freqresp(Gs_align{1}('de', 'u'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/V]');

ax2 = nexttile();
hold on;
plot(f, abs(enc_frf(:, 2)));
plot(freqs, abs(squeeze(freqresp(Gs_align{2}('de', 'u'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);

ax3 = nexttile();
hold on;
plot(f, abs(enc_frf(:, 3)), 'DisplayName', 'Meas.');
plot(freqs, abs(squeeze(freqresp(Gs_align{3}('de', 'u'), freqs, 'Hz'))), ...
     'DisplayName', 'Model');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
legend('location', 'southwest', 'FontSize', 8);

ax4 = nexttile(5);
hold on;
plot(f, abs(enc_frf(:, 4)));
plot(freqs, abs(squeeze(freqresp(Gs_align{4}('de', 'u'), freqs, 'Hz'))));
hold off;
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');

ax5 = nexttile(6);
hold on;
plot(f, abs(enc_frf(:, 5)));
plot(freqs, abs(squeeze(freqresp(Gs_align{5}('de', 'u'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);

linkaxes([ax1,ax2,ax3,ax4,ax5],'xy');
xlim([20, 2e3]); ylim([1e-8, 1e-3]);

% Paper                                                         :noexport:

%% Comparison of the plants (encoder output) when tuning the misalignment
freqs = 2*logspace(0, 3, 1000);

colors = colororder;

figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(f, abs(enc_frf(:,1)), 'color', [colors(1,:),0.2], ...
     'DisplayName', 'FRF - $d_{e,i}/V_{a,i}$')
for i = 2:5
    plot(f, abs(enc_frf(:,i)), 'color', [colors(1,:),0.2], ...
         'HandleVisibility', 'off');
end

plot(f, abs(int_frf(:,1)), 'color', [colors(2,:),0.2], ...
     'DisplayName', 'FRF - $d_{a,i}/V_{a,i}$')
for i = 2:5
    plot(f, abs(int_frf(:,i)), 'color', [colors(2,:),0.2], ...
         'HandleVisibility', 'off');
end
plot(freqs, abs(squeeze(freqresp(Gs_align{1}('de', 'u'), freqs, 'Hz'))), '--', 'color', colors(1,:), ...
     'DisplayName', 'Model - $d_{e,i}/V_{a,i}$')
for i = 2:5
    plot(freqs, abs(squeeze(freqresp(Gs_align{i}('de', 'u'), freqs, 'Hz'))), '--', 'color', colors(1,:), ...
         'HandleVisibility', 'off');
end
plot(freqs, abs(squeeze(freqresp(Gs('da', 'u'), freqs, 'Hz'))), '--', 'color', colors(2,:), ...
     'DisplayName', 'Model - $d_{a,i}/V_{a,i}$')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/V]');
ylim([1e-8, 1e-3]);
legend('location', 'southwest')

ax2 = nexttile;
hold on;
for i = 1:5
    plot(f, 180/pi*angle(enc_frf(:,i)), 'color', [colors(1,:),0.2]);
    plot(f, 180/pi*(angle(int_frf(:, i)) - angle(squeeze(freqresp(exp(-s*2*1e-4), f, 'Hz')))), 'color', [colors(2,:),0.2]);
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gs_align{i}('de', 'u'), freqs, 'Hz'))), '--', 'color', colors(1,:));
end
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('da', 'u'), freqs, 'Hz'))), '--', 'color', colors(2,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);

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

% Effect of bending stiffness of the flexible joints
% <<sec:struts_effect_bending_stiff_joints>>

% Let's initialize an APA which is a little bit misaligned.

%% APA Initialization
n_hexapod.actuator = initializeAPA('type', 'flexible', 'd_align_bot', [0.1e-3; 0.5e-3; 0], 'd_align_top', [0.1e-3; 0.5e-3; 0]);



% The bending stiffnesses for which the dynamics is identified are defined below.

%% Tested bending stiffnesses [Nm/rad]
kRs = [3, 4, 5, 6, 7];



% Then the identification is performed for all the values of the bending stiffnesses.

%% Idenfity the transfer function from actuator to encoder for all bending stiffnesses
Gs = {zeros(length(kRs), 1)};

for i = 1:length(kRs)
    n_hexapod.flex_bot = initializeBotFlexibleJoint(...
        'type', '4dof', ...
        'kRx', kRs(i), ...
        'kRy', kRs(i));
    n_hexapod.flex_top = initializeTopFlexibleJoint(...
        'type', '4dof', ...
        'kRx', kRs(i), ...
        'kRy', kRs(i));

    G = exp(-s*1e-4)*linearize(mdl, io, 0.0, opts);
    G.InputName  = {'u'};
    G.OutputName = {'Vs', 'de', 'da'};

    Gs(i) = {G};
end



% The obtained dynamics from DAC voltage to encoder measurements are compared in Figure ref:fig:effect_enc_bending_stiff.

%% Plot the obtained transfer functions for all the bending stiffnesses
freqs = 2*logspace(1, 3, 1000);

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

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(kRs)
    plot(freqs, abs(squeeze(freqresp(Gs{i}('de', 'u'), freqs, 'Hz'))), ...
         'DisplayName', sprintf('$k_R = %.0f$ [Nm/rad]', kRs(i)));
end
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');

ax2 = nexttile;
hold on;
for i = 1:length(kRs)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('de', 'u'), freqs, 'Hz'))));
end
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([20, 2e3]);

% Effect of axial stiffness of the flexible joints
% <<sec:struts_effect_axial_stiff_joints>>

% The axial stiffnesses for which the dynamics is identified are defined below.

%% Tested axial stiffnesses [N/m]
kzs = [5e7 7.5e7 1e8 2.5e8];



% Then the identification is performed for all the values of the bending stiffnesses.

%% Idenfity the transfer function from actuator to encoder for all bending stiffnesses
Gs = {zeros(length(kzs), 1)};

for i = 1:length(kzs)
    n_hexapod.flex_bot = initializeBotFlexibleJoint(...
        'type', '4dof', ...
        'kz', kzs(i));
    n_hexapod.flex_top = initializeTopFlexibleJoint(...
        'type', '4dof', ...
        'kz', kzs(i));

    G = exp(-s*1e-4)*linearize(mdl, io, 0.0, opts);
    G.InputName  = {'u'};
    G.OutputName = {'Vs', 'de', 'da'};

    Gs(i) = {G};
end



% The obtained dynamics from DAC voltage to encoder measurements are compared in Figure ref:fig:effect_enc_axial_stiff.

%% Plot the obtained transfer functions for all the axial stiffnesses
freqs = 2*logspace(1, 3, 1000);

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

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(kzs)
    plot(freqs, abs(squeeze(freqresp(Gs{i}('de', 'u'), freqs, 'Hz'))), ...
         'DisplayName', sprintf('$k_z = %.1e$ [N/m]', kzs(i)));
end
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');

ax2 = nexttile;
hold on;
for i = 1:length(kzs)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('de', 'u'), freqs, 'Hz'))));
end
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([20, 2e3]);

% Effect of bending damping
% <<sec:struts_effect_bending_damping_joints>>
% Now let's study the effect of the bending damping of the flexible joints.

% The tested bending damping are defined below:

%% Tested bending dampings [Nm/(rad/s)]
cRs = [1e-3, 5e-3, 1e-2, 5e-2, 1e-1];



% Then the identification is performed for all the values of the bending damping.

%% Idenfity the transfer function from actuator to encoder for all bending dampins
Gs = {zeros(length(cRs), 1)};

for i = 1:length(cRs)
    n_hexapod.flex_bot = initializeBotFlexibleJoint(...
        'type', '4dof', ...
        'cRx', cRs(i), ...
        'cRy', cRs(i));
    n_hexapod.flex_top = initializeTopFlexibleJoint(...
        'type', '4dof', ...
        'cRx', cRs(i), ...
        'cRy', cRs(i));

    G = exp(-s*1e-4)*linearize(mdl, io, 0.0, opts);
    G.InputName  = {'u'};
    G.OutputName = {'Vs', 'de', 'da'};

    Gs(i) = {G};
end



% The results are shown in Figure ref:fig:effect_enc_bending_damp.

%% Plot the obtained transfer functions for all the bending stiffnesses
freqs = 2*logspace(1, 3, 1000);

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

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(cRs)
    plot(freqs, abs(squeeze(freqresp(Gs{i}('de', 'u'), freqs, 'Hz'))), ...
         'DisplayName', sprintf('$c_R = %.3f\\,[\\frac{Nm}{rad/s}]$', cRs(i)));
end
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', 'southwest');

ax2 = nexttile;
hold on;
for i = 1:length(cRs)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('de', 'u'), freqs, 'Hz'))));
end
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([20, 2e3]);
