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

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

load('mat/conf_simulink.mat');
open('nass_model.slx')

% Initialization
% We initialize all the stages with the default parameters.

initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();



% The worst case scenario is a rotation speed of 60rpm with a payload mass of 10Kg.

initializeSample('mass', 10);



% We don't include gravity nor disturbances in this model as it adds complexity to the simulations and does not alter the obtained dynamics.

initializeSimscapeConfiguration('gravity', true);
initializeDisturbances('enable', false);
initializeLoggingConfiguration('log', 'none');

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

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'],     1, 'openinput');              io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station'],  3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station'],  3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'En');   io_i = io_i + 1;

% Identification when not rotating
% We set the range of stiffness that we want to use.

Ks = logspace(3,9,7); % [N/m]



% We don't move any stage and no controller is used.

initializeReferences();
initializeController();

Gk_iff = {zeros(length(Ks))};
Gk_dvf = {zeros(length(Ks))};
Gk_err = {zeros(length(Ks))};

for i = 1:length(Ks)
  initializeNanoHexapod('k', Ks(i));

  %% Run the linearization
  G = linearize(mdl, io);
  G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
  G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
                  'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ...
                  'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};

  Gk_iff(i) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
  Gk_dvf(i) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};

  Jinvt = tf(inv(nano_hexapod.J)');
  Jinvt.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
  Jinvt.OutputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
  Gk_err(i) = {-minreal(G({'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'},                {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))*Jinvt};
end

% Identification when rotating at maximum speed
% We now set the reference path such that the Spindle is rotating at 60rpm and such that it is at the zero position at the time of the identification.

Rz_rpm = 60;

initializeReferences('Rz_type', 'rotating', ...
                     'Rz_period', 60/Rz_rpm, ... % Rotation period [s]
                     'Rz_amplitude', -0.2*(2*pi*Rz_rpm/60)); % Angle offset [rad]

load('mat/nass_references.mat', 'Rz'); % We load the reference for the Spindle
[~, i_end] = min(abs(Rz.signals.values)); % Obtain the indice where the spindle angle is zero
t_sim = Rz.time(i_end); % Simulation time before identification [s]



% We here use a decentralized controller that is used to stabilize the nano-hexapod until the identification is made.
% This controller virtually adds stiffness in each of the nano-hexapod leg.

k_sta = -1e8;
initializeController('type', 'stabilizing');

Gk_wz_iff = {zeros(length(Ks))};
Gk_wz_dvf = {zeros(length(Ks))};
Gk_wz_err = {zeros(length(Ks))};

for i = 1:length(Ks)
  initializeNanoHexapod('k', Ks(i));

  %% Run the linearization
  G = linearize(mdl, io, t_sim);
  G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
  G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
                  'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ...
                  'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};

  Gk_wz_iff(i) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
  Gk_wz_dvf(i) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};

  Jinvt = tf(inv(nano_hexapod.J)');
  Jinvt.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
  Jinvt.OutputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
  Gk_wz_err(i) = {-minreal(G({'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'},                {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))*Jinvt};
end

save('mat/optimal_stiffness_Gk_wz.mat', 'Ks', ...
     'Gk_iff',    'Gk_dvf',    'Gk_err', ...
     'Gk_wz_iff', 'Gk_wz_dvf', 'Gk_wz_err');

% Change of dynamics

load('mat/optimal_stiffness_Gk_wz.mat');



% Change of dynamics for decentralized IFF control.

freqs = logspace(-1, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Gk_iff)
  set(gca,'ColorOrderIndex',i);
  plot(freqs, abs(squeeze(freqresp(Gk_iff{i}(   'Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-');
  set(gca,'ColorOrderIndex',i);
  plot(freqs, abs(squeeze(freqresp(Gk_wz_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
title('Soft Nano-Hexapod');

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Gk_iff)
  set(gca,'ColorOrderIndex',i);
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gk_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-', ...
       'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
  set(gca,'ColorOrderIndex',i);
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gk_wz_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '--', ...
       'HandleVisibility', 'off');
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]);
legend('location', 'northeast');

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



% Change of dynamics from $F_x$ to $D_x$.

freqs = logspace(-1, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Gk_err)
  set(gca,'ColorOrderIndex',i);
  plot(freqs, abs(squeeze(freqresp(Gk_err{i}(   'Ex', 'Fx'), freqs, 'Hz'))), '-');
  set(gca,'ColorOrderIndex',i);
  plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Gk_err)
  set(gca,'ColorOrderIndex',i);
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gk_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
       'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
  set(gca,'ColorOrderIndex',i);
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gk_wz_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '--', ...
       'HandleVisibility', 'off');
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]);
legend('location', 'northeast');

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



% Change of dynamics from $F_z$ to $D_z$.

freqs = logspace(-1, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Gk_err)
  set(gca,'ColorOrderIndex',i);
  plot(freqs, abs(squeeze(freqresp(Gk_err{i}(   'Ez', 'Fz'), freqs, 'Hz'))), '-');
  set(gca,'ColorOrderIndex',i);
  plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i}('Ez', 'Fz'), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('Soft Nano-Hexapod');

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Gk_err)
  set(gca,'ColorOrderIndex',i);
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gk_err{i}('Ez', 'Fz'), freqs, 'Hz'))), '-', ...
       'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
  set(gca,'ColorOrderIndex',i);
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gk_wz_err{i}('Ez', 'Fz'), freqs, 'Hz'))), '--', ...
       'HandleVisibility', 'off');
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]);
legend('location', 'northeast');

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

% Change of coupling

load('mat/optimal_stiffness_Gk_wz.mat');



% Change of coupling from $F_x$ to $D_y$ when not rotating and when rotating at 60rpm.

freqs = logspace(-1, 3, 1000);

figure;

hold on;
for i = 1:length(Gk_err)
  set(gca,'ColorOrderIndex',i);
  plot(freqs, abs(squeeze(freqresp(Gk_err{i}(   'Ey', 'Fx'), freqs, 'Hz'))), '-', ...
       'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
  set(gca,'ColorOrderIndex',i);
  plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i}('Ey', 'Fx'), freqs, 'Hz'))), '--', ...
       'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
xlim([freqs(1), freqs(end)]);
legend('location', 'northeast');



% Comparison of the coupling from $F_x$ to $D_y$ when rotating at 60rpm to the direct term $F_x$ to $D_x$.

freqs = logspace(-1, 3, 1000);

figure;

hold on;
for i = 1:length(Gk_err)
  set(gca,'ColorOrderIndex',i);
  plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
       'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
  set(gca,'ColorOrderIndex',i);
  plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i}('Ey', 'Fx'), freqs, 'Hz'))), '--', ...
       'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
xlim([freqs(1), freqs(end)]);
legend('location', 'northeast');

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

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

load('mat/conf_simulink.mat');
open('nass_model.slx')

% Identification of the micro-station compliance
% We initialize all the stages with the default parameters.

initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod('type', 'compliance');



% We put nothing on top of the micro-hexapod.

initializeAxisc('type', 'none');
initializeMirror('type', 'none');
initializeNanoHexapod('type', 'none');
initializeSample('type', 'none');

initializeReferences();
initializeDisturbances();
initializeController();
initializeSimscapeConfiguration();
initializeLoggingConfiguration();



% And we identify the dynamics from forces/torques applied on the micro-hexapod top platform to the motion of the micro-hexapod top platform at the same point.

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

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Micro-Station/Micro Hexapod/Compliance/Fm'], 1, 'openinput');       io_i = io_i + 1; % Direct Forces/Torques applied on the micro-hexapod top platform
io(io_i) = linio([mdl, '/Micro-Station/Micro Hexapod/Compliance/Dm'], 1, 'output'); io_i = io_i + 1; % Absolute displacement of the top platform

%% Run the linearization
Gm = linearize(mdl, io, 0);
Gm.InputName  = {'Fmx', 'Fmy', 'Fmz', 'Mmx', 'Mmy', 'Mmz'};
Gm.OutputName = {'Dx', 'Dy', 'Dz', 'Drx', 'Dry', 'Drz'};

labels = {'$D_x/F_{x}$', '$D_y/F_{y}$', '$D_z/F_{z}$', '$R_{x}/M_{x}$', '$R_{y}/M_{y}$', '$R_{R}/M_{z}$'};

freqs = logspace(1, 3, 1000);

figure;

hold on;
plot(freqs, abs(squeeze(freqresp(Gm(1, 1), freqs, 'Hz'))), 'k-', 'DisplayName', labels{1});
for i = 2:6
  plot(freqs, abs(squeeze(freqresp(Gm(1, i), freqs, 'Hz'))), 'DisplayName', labels{i});
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
ylabel('Compliance');
legend('location', 'northwest');

% Initialization

initializeReferences();
initializeDisturbances();
initializeController();
initializeSimscapeConfiguration();
initializeLoggingConfiguration();
initializeSimscapeConfiguration('gravity', false);

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

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'],     1, 'openinput');              io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station'],  3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station'],  3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'En');   io_i = io_i + 1;

Ks = logspace(3,9,7); % [N/m]

initializeSample('type', 'rigid', 'mass', 20);

% Rigid micro-station

initializeGround('type', 'rigid');
initializeGranite('type', 'rigid');
initializeTy('type', 'rigid');
initializeRy('type', 'rigid');
initializeRz('type', 'rigid');
initializeMicroHexapod('type', 'rigid');

initializeAxisc('type', 'rigid');
initializeMirror('type', 'rigid');

Gmr_iff = {zeros(length(Ks))};
Gmr_dvf = {zeros(length(Ks))};
Gmr_err = {zeros(length(Ks))};

for i = 1:length(Ks)
  initializeNanoHexapod('k', Ks(i));

  G = linearize(mdl, io);
  G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
  G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
                  'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ...
                  'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};

  Gmr_iff(i) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
  Gmr_dvf(i) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};

  Jinvt = tf(inv(nano_hexapod.J)');
  Jinvt.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
  Jinvt.OutputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
  Gmr_err(i) = {-minreal(G({'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'},                {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))*Jinvt};
end

% Flexible micro-station

initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();

initializeAxisc();
initializeMirror();

Gmf_iff = {zeros(length(Ks))};
Gmf_dvf = {zeros(length(Ks))};
Gmf_err = {zeros(length(Ks))};

for i = 1:length(Ks)
  initializeNanoHexapod('k', Ks(i));

  G = linearize(mdl, io);
  G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
  G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
                  'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ...
                  'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};

  Gmf_iff(i) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
  Gmf_dvf(i) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};

  Jinvt = tf(inv(nano_hexapod.J)');
  Jinvt.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
  Jinvt.OutputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
  Gmf_err(i) = {-minreal(G({'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'},                {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))*Jinvt};
end

save('mat/optimal_stiffness_micro_station_compliance.mat', 'Ks', ...
     'Gmr_iff',    'Gmr_dvf',    'Gmr_err', ...
     'Gmf_iff',    'Gmf_dvf',    'Gmf_err');

% Obtained Dynamics

load('mat/optimal_stiffness_micro_station_compliance.mat');



% IFF plant

freqs = logspace(-1, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Ks)
  set(gca,'ColorOrderIndex',i);
  plot(freqs, abs(squeeze(freqresp(Gmr_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-');
  set(gca,'ColorOrderIndex',i);
  plot(freqs, abs(squeeze(freqresp(Gmf_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Ks)
  set(gca,'ColorOrderIndex',i);
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gmr_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-');
  set(gca,'ColorOrderIndex',i);
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gmf_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '--');
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)]);



% DVF plant

freqs = logspace(-1, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Ks)
  set(gca,'ColorOrderIndex',i);
  plot(freqs, abs(squeeze(freqresp(Gmr_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))), '-');
  set(gca,'ColorOrderIndex',i);
  plot(freqs, abs(squeeze(freqresp(Gmf_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Ks)
  set(gca,'ColorOrderIndex',i);
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gmr_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))), '-');
  set(gca,'ColorOrderIndex',i);
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gmf_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))), '--');
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)]);



% X direction

freqs = logspace(-1, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Ks)
  set(gca,'ColorOrderIndex',i);
  plot(freqs, abs(squeeze(freqresp(Gmr_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '-');
  set(gca,'ColorOrderIndex',i);
  plot(freqs, abs(squeeze(freqresp(Gmf_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Ks)
  set(gca,'ColorOrderIndex',i);
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gmr_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '-');
  set(gca,'ColorOrderIndex',i);
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gmf_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '--');
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)]);



% Z direction

freqs = logspace(-1, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Ks)
  set(gca,'ColorOrderIndex',i);
  plot(freqs, abs(squeeze(freqresp(Gmr_err{i}('Ez', 'Fz'), freqs, 'Hz'))), '-');
  set(gca,'ColorOrderIndex',i);
  plot(freqs, abs(squeeze(freqresp(Gmf_err{i}('Ez', 'Fz'), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Ks)
  set(gca,'ColorOrderIndex',i);
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gmr_err{i}('Ez', 'Fz'), freqs, 'Hz'))), '-');
  set(gca,'ColorOrderIndex',i);
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gmf_err{i}('Ez', 'Fz'), freqs, 'Hz'))), '--');
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)]);

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

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

load('mat/conf_simulink.mat');
open('nass_model.slx')

% Initialization
% We initialize all the stages with the default parameters.

initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();



% We don't include disturbances in this model as it adds complexity to the simulations and does not alter the obtained dynamics.

initializeSimscapeConfiguration('gravity', true);
initializeDisturbances('enable', false);



% We set the controller type to Open-Loop, and we do not need to log any signal.

initializeController();
initializeLoggingConfiguration('log', 'none');
initializeReferences();

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

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'],     1, 'openinput');              io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station'],  3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station'],  3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'En');   io_i = io_i + 1;

% Identification of the dynamics while change the payload dynamics
% - Change of mass: from 1kg to 50kg
% - Change of resonance frequency: from 50Hz to 500Hz
% - The damping ratio of the payload is fixed to $\xi = 0.2$

% We identify the dynamics for the following payload masses =Ms= and nano-hexapod leg's stiffnesses =Ks=:

Ms = [1, 20, 50]; % [Kg]
Ks = logspace(3,9,7); % [N/m]

Gm_iff = {zeros(length(Ks), length(Ms))};
Gm_dvf = {zeros(length(Ks), length(Ms))};
Gm_err = {zeros(length(Ks), length(Ms))};

for i = 1:length(Ks)
  for j = 1:length(Ms)
    initializeNanoHexapod('k', Ks(i));
    initializeSample('mass', Ms(j), 'freq', 100*ones(6,1));

    G = linearize(mdl, io);
    G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
    G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
                    'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ...
                    'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};

    Gm_iff(i,j) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
    Gm_dvf(i,j) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};

    Jinvt = tf(inv(nano_hexapod.J)');
    Jinvt.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
    Jinvt.OutputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
    Gm_err(i,j) = {-minreal(G({'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'},                {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))*Jinvt};
  end
end



% We then identify the dynamics for the following payload resonance frequencies =Fs=:

Fs = [50, 200, 500]; % [Hz]

Gf_iff = {zeros(length(Ks), length(Fs))};
Gf_dvf = {zeros(length(Ks), length(Fs))};
Gf_err = {zeros(length(Ks), length(Fs))};

for i = 1:length(Ks)
  for j = 1:length(Fs)
    initializeNanoHexapod('k', Ks(i));
    initializeSample('mass', 20, 'freq', Fs(j)*ones(6,1));

    G = linearize(mdl, io);
    G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
    G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
                    'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ...
                    'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};

    Gf_iff(i,j) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
    Gf_dvf(i,j) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};

    Jinvt = tf(inv(nano_hexapod.J)');
    Jinvt.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
    Jinvt.OutputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
    Gf_err(i,j) = {-minreal(G({'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'},                {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))*Jinvt};
  end
end

save('mat/optimal_stiffness_Gm_Gf.mat', 'Ks', 'Ms', 'Fs', ...
     'Gm_iff',    'Gm_dvf',    'Gm_err', ...
     'Gf_iff',    'Gf_dvf',    'Gf_err');

% Change of optimal gain for decentralized control
% For each payload, compute the optimal gain for the IFF control.
% The optimal value corresponds to critical damping to *all* the 6 modes of the nano-hexapod.


load('mat/optimal_stiffness_Gm_Gf.mat');



% Change of Mass

freqs = logspace(-1, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Ks)
  for j = 1:length(Ms)
    set(gca,'ColorOrderIndex',i);
    plot(freqs, abs(squeeze(freqresp(Gm_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-');
  end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Ks)
  for j = 1:length(Ms)
    set(gca,'ColorOrderIndex',i);
    if j == 1
      plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-', ...
          'DisplayName', sprintf('$K = %.0e$ [N/m]', Ks(i)));
    else
      plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-', ...
           'HandleVisibility', 'off');
    end
  end
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]);
legend('location', 'northeast');

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



% Change of payload resonance frequency

freqs = logspace(-1, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Ks)
  for j = 1:length(Fs)
    set(gca,'ColorOrderIndex',i);
    plot(freqs, abs(squeeze(freqresp(Gf_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-');
  end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Ks)
  for j = 1:length(Fs)
    set(gca,'ColorOrderIndex',i);
    if j == 1
      plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-', ...
          'DisplayName', sprintf('$K = %.0e$ [N/m]', Ks(i)));
    else
      plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-', ...
           'HandleVisibility', 'off');
    end
  end
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]);
legend('location', 'northeast');

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

% Change of dynamics for the primary controller
% For each stiffness, plot the total spread of dynamics.


load('mat/optimal_stiffness_Gm_Gf.mat');

% Frequency variation
% Same payload mass, but different stiffness resulting in different resonance frequency.

% All curves

freqs = logspace(-1, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Ks)
  for j = 1:length(Fs)
    set(gca,'ColorOrderIndex',i);
    plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-');
  end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Ks)
  for j = 1:length(Fs)
    set(gca,'ColorOrderIndex',i);
    if j == 1
      plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
          'DisplayName', sprintf('$K = %.0e$ [N/m]', Ks(i)));
    else
      plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
           'HandleVisibility', 'off');
    end
  end
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]);
legend('location', 'northeast');

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



% X direction

i = 1;

freqs = logspace(-1, 3, 1000);

figure;

ax1 = subplot(2, 2, 1);
hold on;
for j = 1:length(Fs)
  plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title(sprintf('$k = %.0e$ [N/m]', Ks(i)))

ax2 = subplot(2, 2, 3);
hold on;
for j = 1:length(Fs)
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
       'DisplayName', sprintf('$\\omega_0 = %.0f$ [Hz]', Fs(j)));
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]);
legend('location', 'northeast');

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

i = 7;

ax1 = subplot(2, 2, 2);
hold on;
for j = 1:length(Fs)
  plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title(sprintf('$k = %.0e$ [N/m]', Ks(i)))

ax2 = subplot(2, 2, 4);
hold on;
for j = 1:length(Fs)
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
       'DisplayName', sprintf('$\\omega_0 = %.0f$ [Hz]', Fs(j)));
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]);
legend('location', 'northeast');

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



% Z direction:
% We can see two mass lines for the soft nano-hexapod:
% - The first mass line corresponds to $\frac{1}{(m_n + m_p)s^2}$ where $m_p = 20\ [kg]$ is the mass of the payload and $m_n = 15\ [Kg]$ is the mass of the nano-hexapod top platform and attached mirror
% - The second mass line corresponds to $\frac{1}{m_n s^2}$


i = 1;

freqs = logspace(-1, 3, 1000);

figure;

ax1 = subplot(2, 2, 1);
hold on;
for j = 1:length(Fs)
  plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title(sprintf('$k = %.0e$ [N/m]', Ks(i)))

ax2 = subplot(2, 2, 3);
hold on;
for j = 1:length(Fs)
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-', ...
       'DisplayName', sprintf('$\\omega_0 = %.0f$ [Hz]', Fs(j)));
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]);
legend('location', 'northeast');

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

i = 7;

ax1 = subplot(2, 2, 2);
hold on;
for j = 1:length(Fs)
  plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title(sprintf('$k = %.0e$ [N/m]', Ks(i)))

ax2 = subplot(2, 2, 4);
hold on;
for j = 1:length(Fs)
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-', ...
       'DisplayName', sprintf('$\\omega_0 = %.0f$ [Hz]', Fs(j)));
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]);
legend('location', 'northeast');

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

% Mass variation
% All mixed, X direction

freqs = logspace(-1, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Ks)
  for j = 1:length(Ms)
    set(gca,'ColorOrderIndex',i);
    plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-');
  end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Ks)
  for j = 1:length(Ms)
    set(gca,'ColorOrderIndex',i);
    if j == 1
      plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
          'DisplayName', sprintf('$K = %.0e$ [N/m]', Ks(i)));
    else
      plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
           'HandleVisibility', 'off');
    end
  end
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]);
legend('location', 'northeast');

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



% All mixed, Z direction

freqs = logspace(-1, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Ks)
  for j = 1:length(Ms)
    set(gca,'ColorOrderIndex',i);
    plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-');
  end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Ks)
  for j = 1:length(Ms)
    set(gca,'ColorOrderIndex',i);
    if j == 1
      plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-', ...
          'DisplayName', sprintf('$K = %.0e$ [N/m]', Ks(i)));
    else
      plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-', ...
           'HandleVisibility', 'off');
    end
  end
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]);
legend('location', 'northeast');

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



% Z direction

freqs = logspace(-1, 3, 1000);

figure;

i = 1;

ax1 = subplot(2, 2, 1);
hold on;
for j = 1:length(Ms)
  plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title(sprintf('$k = %.0e$ [N/m]', Ks(i)))

ax2 = subplot(2, 2, 3);
hold on;
for j = 1:length(Ms)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-');
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)]);

i = 7;

ax1 = subplot(2, 2, 2);
hold on;
for j = 1:length(Ms)
  plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title(sprintf('$k = %.0e$ [N/m]', Ks(i)))

ax2 = subplot(2, 2, 4);
hold on;
for j = 1:length(Ms)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-', ...
        'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(j)));
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]);
legend('location', 'northeast');

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

% Total variation
% Total change of dynamics due to change of the payload:
% - mass from 1kg to 50kg
% - main resonance from 50Hz to 500Hz

% For a soft nano-hexapod

i = 1;

freqs = logspace(-1, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for j = 1:length(Fs)
  plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
end
for j = 1:length(Ms)
  plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
for j = 1:length(Fs)
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
for j = 1:length(Ms)
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
end
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)]);



% For a stiff nano-hexapod

i = 7;

freqs = logspace(-1, 3, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for j = 1:length(Fs)
  plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
end
for j = 1:length(Ms)
  plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
for j = 1:length(Fs)
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
for j = 1:length(Ms)
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
end
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)]);