%% test_nhexa_3_model.m
% Compare the measured dynamics from u to de and to Vs with the Simscape model

%% 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 STEPS
addpath('./subsystems/'); % Path for Subsystems Simulink files

%% Initialize Parameters for Simscape model
table_type = 'Rigid'; % On top of vibration table
device_type = 'None'; % On top of vibration table
payload_num = 0; % No Payload

% Simulink Model name
mdl = 'test_nhexa_simscape';

%% Colors for the figures
colors = colororder;

%% Frequency Vector
freqs = logspace(log10(10), log10(2e3), 1000);

%% Set to true only if all the dynamics should again computed
% from the simscape model
extract_simscape_dynamics = false;
if extract_simscape_dynamics

    %% Extract the transfer function matrix from the Simscape model
    % Initialization of the Simscape model
    table_type = 'Suspended'; % On top of vibration table
    device_type = 'Hexapod'; % Nano-Hexapod
    payload_num = 0; % No Payload

    n_hexapod = initializeNanoHexapod('flex_bot_type', '4dof', ...
                                      'flex_top_type', '4dof', ...
                                      'motion_sensor_type', 'plates', ...
                                      'actuator_type', '2dof');

    % Identify the FRF matrix from u to [de,Vs]
    clear io; io_i = 1;
    io(io_i) = linio([mdl, '/u'],  1, 'openinput');  io_i = io_i + 1; % Actuator Inputs
    io(io_i) = linio([mdl, '/de'], 1, 'openoutput'); io_i = io_i + 1; % Encoders
    io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Encoders

    G_de = {};
    G_Vs = {};

    for i = [0:3]
        payload_num = i; % Change the payload on the nano-hexapod
        G = exp(-s*1e-4)*linearize(mdl, io, 0.0);
        G.InputName  = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
        G.OutputName = {'de1', 'de2', 'de3', 'de4', 'de5', 'de6', ...
                        'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'};
        G_de(i+1) = {G({'de1', 'de2', 'de3', 'de4', 'de5', 'de6'},:)};
        G_Vs(i+1) = {G({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'},:)};
    end

    % Save the identified plants
    save('./mat/test_nhexa_simscape_masses.mat', 'G_Vs', 'G_de')

    %% The same identification is performed, but this time with
    % "flexible" model of the APA
    table_type = 'Suspended'; % On top of vibration table
    device_type = 'Hexapod'; % Nano-Hexapod
    payload_num = 0; % No Payload

    n_hexapod = initializeNanoHexapod('flex_bot_type', '4dof', ...
                                      'flex_top_type', '4dof', ...
                                      'motion_sensor_type', 'plates', ...
                                      'actuator_type', 'flexible');

    G_de = {};
    G_Vs = {};

    for i = [0:3]
        payload_num = i; % Change the payload on the nano-hexapod
        G = exp(-s*1e-4)*linearize(mdl, io, 0.0);
        G.InputName  = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
        G.OutputName = {'de1', 'de2', 'de3', 'de4', 'de5', 'de6', ...
                        'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'};
        G_de(i+1) = {G({'de1', 'de2', 'de3', 'de4', 'de5', 'de6'},:)};
        G_Vs(i+1) = {G({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'},:)};
    end

    % Save the identified plants
    save('./mat/test_nhexa_simscape_flexible_masses.mat', 'G_Vs', 'G_de')

end

%% Load Simscape Model and measured FRF
sim_ol  = load('test_nhexa_simscape_masses.mat', 'G_Vs', 'G_de');
frf_ol  = load('test_nhexa_identified_frf_masses.mat', 'f', 'G_Vs', 'G_de');

%% Diagonal elements of the FRF matrix from u to de
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:,1, 1)), 'color', [colors(1,:),0.5], ...
     'DisplayName', '$d_{ei}/u_i$ - FRF')
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{1}(1,1), freqs, 'Hz'))), 'color', [colors(2,:),0.5], ...
     'DisplayName', '$d_{ei}/u_i$ - Model')
for i = 2:6
    plot(frf_ol.f, abs(frf_ol.G_de{1}(:,i, i)), 'color', [colors(1,:),0.5], ...
         'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{1}(i,i), freqs, 'Hz'))), 'color', [colors(2,:),0.5], ...
         'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-8, 5e-4]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(frf_ol.f, 180/pi*angle(frf_ol.G_de{1}(:,i, i)), 'color', [colors(1,:),0.5]);
    plot(freqs, 180/pi*angle(squeeze(freqresp(sim_ol.G_de{1}(i,i), freqs, 'Hz'))), 'color', [colors(2,:),0.5]);
end
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([freqs(1), freqs(end)]);

%% Diagonal elements of the FRF matrix from u to Vs
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(frf_ol.f, abs(frf_ol.G_Vs{1}(:,1, 1)), 'color', [colors(1,:),0.5], ...
     'DisplayName', '$V_{si}/u_i$ - FRF')
plot(freqs, abs(squeeze(freqresp(sim_ol.G_Vs{1}(1,1), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
     'DisplayName', '$V_{si}/u_i$ - Model')
for i = 2:6
    plot(frf_ol.f, abs(frf_ol.G_Vs{1}(:,i, i)), 'color', [colors(1,:),0.5], ...
         'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(sim_ol.G_Vs{1}(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
         'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
legend('location', 'southeast');

ax2 = nexttile;
hold on;
for i = 1:6
    plot(frf_ol.f, 180/pi*angle(frf_ol.G_Vs{1}(:,i, i)), 'color', [colors(1,:),0.5]);
    plot(freqs, 180/pi*angle(squeeze(freqresp(sim_ol.G_Vs{1}(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
end
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([freqs(1), freqs(end)]);

%% Comparison of the plants (encoder output) when tuning the misalignment
i_input = 1;

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

ax1 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:, 1, i_input)));
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{1}(1, i_input), freqs, 'Hz'))));
text(54, 4e-4, '$d_{e1}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/V]');

ax2 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:, 2, i_input)));
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{1}(2, i_input), freqs, 'Hz'))));
text(54, 4e-4, '$d_{e2}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);

ax3 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:, 3, i_input)), ...
     'DisplayName', 'Measurements');
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{1}(3, i_input), freqs, 'Hz'))), ...
     'DisplayName', 'Model (2-DoF APA)');
text(54, 4e-4, '$d_{e3}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax4 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:, 4, i_input)));
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{1}(4, i_input), freqs, 'Hz'))));
text(54, 4e-4, '$d_{e4}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
xticks([50, 100, 200, 400])

ax5 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:, 5, i_input)));
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{1}(5, i_input), freqs, 'Hz'))));
text(54, 4e-4, '$d_{e5}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xticks([50, 100, 200, 400])

ax6 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:, 6, i_input)));
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{1}(6, i_input), freqs, 'Hz'))));
text(54, 4e-4, '$d_{e6}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
xticks([50, 100, 200, 400])

linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'xy');
xlim([50, 5e2]); ylim([1e-8, 5e-4]);

%% Load the plant model with Flexible APA
flex_ol = load('test_nhexa_simscape_flexible_masses.mat', 'G_Vs', 'G_de');

%% Comparison of the plants (encoder output) when tuning the misalignment
i_input = 1;

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

ax1 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:, 1, i_input)));
plot(freqs, abs(squeeze(freqresp(flex_ol.G_de{1}(1, i_input), freqs, 'Hz'))));
text(54, 4e-4, '$d_{e1}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/V]');

ax2 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:, 2, i_input)));
plot(freqs, abs(squeeze(freqresp(flex_ol.G_de{1}(2, i_input), freqs, 'Hz'))));
text(54, 4e-4, '$d_{e2}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);

ax3 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:, 3, i_input)), ...
     'DisplayName', 'Measurements');
plot(freqs, abs(squeeze(freqresp(flex_ol.G_de{1}(3, i_input), freqs, 'Hz'))), ...
     'DisplayName', 'Model (Flexible APA)');
text(54, 4e-4, '$d_{e3}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax4 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:, 4, i_input)));
plot(freqs, abs(squeeze(freqresp(flex_ol.G_de{1}(4, i_input), freqs, 'Hz'))));
text(54, 4e-4, '$d_{e4}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
xticks([50, 100, 200, 400])

ax5 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:, 5, i_input)));
plot(freqs, abs(squeeze(freqresp(flex_ol.G_de{1}(5, i_input), freqs, 'Hz'))));
text(54, 4e-4, '$d_{e5}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xticks([50, 100, 200, 400])

ax6 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:, 6, i_input)));
plot(freqs, abs(squeeze(freqresp(flex_ol.G_de{1}(6, i_input), freqs, 'Hz'))));
text(54, 4e-4, '$d_{e6}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
xticks([50, 100, 200, 400])

linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'xy');
xlim([50, 5e2]); ylim([1e-8, 5e-4]);

%% Bode plot for the transfer function from u to de
masses = [0, 13, 26, 39];
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i_mass = [0:3]
    plot(frf_ol.f, abs(frf_ol.G_de{i_mass+1}(:,1, 1)), 'color', [colors(i_mass+1,:), 0.2], ...
         'DisplayName', sprintf('Meas (%i kg)', masses(i_mass+1)));
    for i = 2:6
        plot(frf_ol.f, abs(frf_ol.G_de{i_mass+1}(:,i, i)), 'color', [colors(i_mass+1,:), 0.2], ...
             'HandleVisibility', 'off');
    end
    set(gca, 'ColorOrderIndex', i_mass+1)
    plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{i_mass+1}(1,1), freqs, 'Hz'))), '--', ...
         'DisplayName', 'Model');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/u$ [m/V]'); set(gca, 'XTickLabel',[]);
ylim([5e-8, 1e-3]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i_mass = [0:3]
    for i =1:6
        plot(frf_ol.f, 180/pi*angle(frf_ol.G_de{i_mass+1}(:,i, i)), 'color', [colors(i_mass+1,:), 0.2]);
    end
    set(gca, 'ColorOrderIndex', i_mass+1)
    plot(freqs, 180/pi*angle(squeeze(freqresp(sim_ol.G_de{i_mass+1}(1,1), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:45:360);
ylim([-45, 180]);

linkaxes([ax1,ax2],'x');
xlim([20, 2e2]);
xticks([20, 50, 100, 200])

%% Bode plot for the transfer function from u to Vs
masses = [0, 13, 26, 39];
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i_mass = 0:3
    plot(frf_ol.f, abs(frf_ol.G_Vs{i_mass+1}(:,1, 1)), 'color', [colors(i_mass+1,:), 0.2], ...
         'DisplayName', sprintf('Meas (%i kg)', masses(i_mass+1)));
    for i = 2:6
        plot(frf_ol.f, abs(frf_ol.G_Vs{i_mass+1}(:,i, i)), 'color', [colors(i_mass+1,:), 0.2], ...
             'HandleVisibility', 'off');
    end
    plot(freqs, abs(squeeze(freqresp(sim_ol.G_Vs{i_mass+1}(1,1), freqs, 'Hz'))), '--', 'color', colors(i_mass+1,:), ...
         'DisplayName', 'Model');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/u$ [V/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-3, 1e2]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i_mass = 0:3
    for i =1:6
        plot(frf_ol.f, 180/pi*angle(frf_ol.G_Vs{i_mass+1}(:,i, i)), 'color', [colors(i_mass+1,:), 0.2]);
    end
    plot(freqs, 180/pi*angle(squeeze(freqresp(sim_ol.G_Vs{i_mass+1}(i,i), freqs, 'Hz'))), '--', 'color', colors(i_mass+1,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);

linkaxes([ax1,ax2],'x');
xlim([20, 2e2]);
xticks([20, 50, 100, 200])

%% Comparison of the plants (encoder output) when tuning the misalignment
i_input = 1;

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

ax1 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{4}(:, 1, i_input)));
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{4}(1, i_input), freqs, 'Hz'))));
text(12, 4e-4, '$d_{e1}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/V]');

ax2 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{4}(:, 2, i_input)));
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{4}(2, i_input), freqs, 'Hz'))));
text(12, 4e-4, '$d_{e2}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);

ax3 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{4}(:, 3, i_input)), ...
     'DisplayName', 'Measurements');
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{4}(3, i_input), freqs, 'Hz'))), ...
     'DisplayName', 'Model (2-DoF APA)');
text(12, 4e-4, '$d_{e3}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax4 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{4}(:, 4, i_input)));
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{4}(4, i_input), freqs, 'Hz'))));
text(12, 4e-4, '$d_{e4}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
xticks([10, 50, 100, 200, 400])

ax5 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{4}(:, 5, i_input)));
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{4}(5, i_input), freqs, 'Hz'))));
text(12, 4e-4, '$d_{e5}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xticks([10, 50, 100, 200, 400])

ax6 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{4}(:, 6, i_input)));
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{4}(6, i_input), freqs, 'Hz'))));
text(12, 4e-4, '$d_{e6}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
xticks([10, 50, 100, 200, 400])

linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'xy');
xlim([10, 5e2]); ylim([1e-8, 5e-4]);