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

%% Data directory
data_dir = './mat/';

% Simulink Model name
mdl = 'nass_model';

%% Colors for the figures
colors = colororder;

%% Frequency Vector [Hz]
freqs = logspace(0, 3, 1000);

%% Identify the IFF plant dynamics using the Simscape model

% Initialize each Simscape model elements
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeSimplifiedNanoHexapod();
initializeSample('type', 'cylindrical', 'm', 1);

% Initial Simscape Configuration
initializeSimscapeConfiguration('gravity', false);
initializeDisturbances('enable', false);
initializeLoggingConfiguration('log', 'none');
initializeController('type', 'open-loop');
initializeReferences();

% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'],     1, 'input');                  io_i = io_i + 1; % Actuator Inputs [N]
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL');  io_i = io_i + 1; % Strut errors [m]

% Identify HAC Plant without using IFF
initializeSample('type', 'cylindrical', 'm', 1);
G_m1 = linearize(mdl, io);
G_m1.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_m1.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

initializeSample('type', 'cylindrical', 'm', 25);
G_m25 = linearize(mdl, io);
G_m25.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_m25.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

initializeSample('type', 'cylindrical', 'm', 50);
G_m50 = linearize(mdl, io);
G_m50.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_m50.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

% Effect of Rotation
initializeSample('type', 'cylindrical', 'm', 1);
initializeReferences(...
    'Rz_type', 'rotating', ...
    'Rz_period', 1); % 360 deg/s

G_m1_Rz = linearize(mdl, io, 0.1);
G_m1_Rz.InputName  = {'f1',  'f2',  'f3',  'f4',  'f5',  'f6'};
G_m1_Rz.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

%% Effect of rotation on the HAC plant
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_m1(1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, $\Omega = 0$')
plot(freqs, abs(squeeze(freqresp(G_m1_Rz(1,1), freqs, 'Hz'))), 'color', colors(2,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, $\Omega = 360$ deg/s')
plot(freqs, abs(squeeze(freqresp(G_m1(1,2), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_j$')
plot(freqs, abs(squeeze(freqresp(G_m1_Rz(1,2), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_j$')
for i = 1:5
    for j = i+1:6
        plot(freqs, abs(squeeze(freqresp(G_m1(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(freqs, abs(squeeze(freqresp(G_m1_Rz(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i = 2:6
    plot(freqs, abs(squeeze(freqresp(G_m1(i,i), freqs, 'Hz'))), 'color', colors(1,:), ...
         'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G_m1_Rz(i,i), freqs, 'Hz'))), 'color', colors(2,:), ...
         'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-11, 2e-5]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m1(i,i), freqs, 'Hz')))), 'color', colors(1,:));
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m1_Rz(i,i), freqs, 'Hz')))), 'color', colors(2,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-200, 20]);
yticks([-180:45:180]);

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

%% Effect of payload's mass on the HAC plant
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_m1( 1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, 1 kg')
plot(freqs, abs(squeeze(freqresp(G_m25(1,1), freqs, 'Hz'))), 'color', colors(2,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, 25 kg')
plot(freqs, abs(squeeze(freqresp(G_m50(1,1), freqs, 'Hz'))), 'color', colors(3,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, 50 kg')
for i = 1:5
    for j = i+1:6
        plot(freqs, abs(squeeze(freqresp(G_m1(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(freqs, abs(squeeze(freqresp(G_m25(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(freqs, abs(squeeze(freqresp(G_m50(i,j), freqs, 'Hz'))), 'color', [colors(3,:), 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i = 2:6
    plot(freqs, abs(squeeze(freqresp(G_m1( i,i), freqs, 'Hz'))), 'color', colors(1,:), ...
         'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G_m25(i,i), freqs, 'Hz'))), 'color', colors(2,:), ...
         'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G_m50(i,i), freqs, 'Hz'))), 'color', colors(3,:), ...
         'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-11, 2e-5]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m1(i,i), freqs, 'Hz')))), 'color', colors(1,:));
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m25(i,i), freqs, 'Hz')))), 'color', colors(2,:));
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m50(i,i), freqs, 'Hz')))), 'color', colors(3,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-200, 20]);
yticks([-180:45:180]);

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

%% Identify HAC Plant with IFF
initializeReferences(); % No Spindle Rotation
initializeController('type', 'iff'); % Implemented IFF controller
load('nass_K_iff.mat', 'Kiff'); % Load designed IFF controller

% 1kg payload
initializeSample('type', 'cylindrical', 'm', 1);
G_hac_m1 = linearize(mdl, io);
G_hac_m1.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_hac_m1.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

% 25kg payload
initializeSample('type', 'cylindrical', 'm', 25);
G_hac_m25 = linearize(mdl, io);
G_hac_m25.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_hac_m25.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

% 50kg payload
initializeSample('type', 'cylindrical', 'm', 50);
G_hac_m50 = linearize(mdl, io);
G_hac_m50.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_hac_m50.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

% Check stability
if not(isstable(G_hac_m1) && isstable(G_hac_m25) && isstable(G_hac_m50))
    warning('One of HAC plant is not stable')
end

%% Comparison of the OL plant and the plant with IFF - 1kg payload
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_m1( 1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$ - OL')
plot(freqs, abs(squeeze(freqresp(G_hac_m1(1,1), freqs, 'Hz'))), 'color', colors(2,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$ - IFF')
for i = 1:5
    for j = i+1:6
        plot(freqs, abs(squeeze(freqresp(G_m1(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(freqs, abs(squeeze(freqresp(G_hac_m1(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i = 2:6
    plot(freqs, abs(squeeze(freqresp(G_m1( i,i), freqs, 'Hz'))), 'color', colors(1,:), ...
         'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G_hac_m1(i,i), freqs, 'Hz'))), 'color', colors(2,:), ...
         'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-10, 5e-5]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m1(i,i), freqs, 'Hz')))), 'color', colors(1,:));
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_hac_m1(i,i), freqs, 'Hz')))), 'color', colors(2,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-200, 20]);
yticks([-180:45:180]);

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

%% Comparison of all the undamped FRF and all the damped FRF
figure;
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_m1(    1,1), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], 'DisplayName', '$\epsilon\mathcal{L}_i/f_i$ - OL');
plot(freqs, abs(squeeze(freqresp(G_hac_m1(1,1), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], 'DisplayName', '$\epsilon\mathcal{L}_i/f_i^\prime$ - IFF');
for i = 1:6
    plot(freqs, abs(squeeze(freqresp(G_m1( i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G_m25(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G_m50(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
end
for i = 1:6
    plot(freqs, abs(squeeze(freqresp(G_hac_m1( i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G_hac_m25(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G_hac_m50(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/N]'); set(gca, 'XTickLabel',[]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ylim([1e-10, 5e-5]);

ax2 = nexttile;
hold on;
for i =1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_m1( i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_m25(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_m50(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
end
for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_hac_m1( i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_hac_m25(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_hac_m50(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
ylim([-200, 20]);
yticks([-180:45:180]);

linkaxes([ax1,ax2],'x');
% xlim([1, 5e2]);

%% Identify plant with "rigid" micro-station
initializeGround('type', 'rigid');
initializeGranite('type', 'rigid');
initializeTy('type', 'rigid');
initializeRy('type', 'rigid');
initializeRz('type', 'rigid');
initializeMicroHexapod('type', 'rigid');
initializeSimplifiedNanoHexapod();
initializeSample('type', 'cylindrical', 'm', 25);

initializeReferences();
initializeController('type', 'open-loop'); % Implemented IFF controller
load('nass_K_iff.mat', 'Kiff'); % Load designed IFF controller

% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'],     1, 'input');                  io_i = io_i + 1; % Actuator Inputs [N]
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL');  io_i = io_i + 1; % Strut errors [m]

G_m25_rigid = linearize(mdl, io);
G_m25_rigid.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_m25_rigid.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

%% Effect of the micro-station limited compliance on the plant dynamics
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_m25_rigid( 1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$ - Rigid support')
plot(freqs, abs(squeeze(freqresp(G_m25(1,1), freqs, 'Hz'))), 'color', colors(2,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$ - $\mu$-station support')
for i = 1:5
    for j = i+1:6
        plot(freqs, abs(squeeze(freqresp(G_m25_rigid(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(freqs, abs(squeeze(freqresp(G_m25(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i = 2:6
    plot(freqs, abs(squeeze(freqresp(G_m25_rigid( i,i), freqs, 'Hz'))), 'color', colors(1,:), ...
         'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G_m25(i,i), freqs, 'Hz'))), 'color', colors(2,:), ...
         'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-10, 5e-5]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m25_rigid(i,i), freqs, 'Hz')))), 'color', colors(1,:));
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m25(i,i), freqs, 'Hz')))), 'color', colors(2,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-200, 20]);
yticks([-180:45:180]);

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

%% Identify Dynamics with a Stiff nano-hexapod (100N/um)

% Initialize each Simscape model elements
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeSimplifiedNanoHexapod('actuator_k', 1e8, 'actuator_kp', 0, 'actuator_c', 1e3);
initializeSample('type', 'cylindrical', 'm', 25);

% Initial Simscape Configuration
initializeSimscapeConfiguration('gravity', false);
initializeDisturbances('enable', false);
initializeLoggingConfiguration('log', 'none');
initializeController('type', 'open-loop');
initializeReferences();

% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'],     1, 'input');                  io_i = io_i + 1; % Actuator Inputs [N]
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL');  io_i = io_i + 1; % Strut errors [m]

% Identify Plant
G_m25_pz = linearize(mdl, io);
G_m25_pz.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_m25_pz.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

% Compare with Nano-Hexapod alone (rigid micro-station)
initializeGround('type', 'rigid');
initializeGranite('type', 'rigid');
initializeTy('type', 'rigid');
initializeRy('type', 'rigid');
initializeRz('type', 'rigid');
initializeMicroHexapod('type', 'rigid');

% Identify Plant
G_m25_pz_rigid = linearize(mdl, io);
G_m25_pz_rigid.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_m25_pz_rigid.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

%% Stiff nano-hexapod - Coupling with the micro-station
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_m25_pz_rigid(1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$ - Rigid')
plot(freqs, abs(squeeze(freqresp(G_m25_pz(1,1), freqs, 'Hz'))), 'color', colors(2,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$ - $\mu$-station')
plot(freqs, abs(squeeze(freqresp(G_m25_pz_rigid(1,2), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_j$ - Rigid')
plot(freqs, abs(squeeze(freqresp(G_m25_pz(1,2), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_j$ - $\mu$-station')
for i = 1:5
    for j = i+1:6
        plot(freqs, abs(squeeze(freqresp(G_m25_pz_rigid(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
             'HandleVisibility', 'off');
        plot(freqs, abs(squeeze(freqresp(G_m25_pz(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
             'HandleVisibility', 'off');
    end
end
for i = 2:6
    plot(freqs, abs(squeeze(freqresp(G_m25_pz_rigid(i,i), freqs, 'Hz'))), 'color', colors(1,:), ...
         'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G_m25_pz(i,i), freqs, 'Hz'))), 'color', colors(2,:), ...
         'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-12, 3e-7]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m25_pz_rigid(i,i), freqs, 'Hz')))), 'color', colors(1,:));
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m25_pz(i,i), freqs, 'Hz')))), 'color', colors(2,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-200, 20]);
yticks([-180:45:180]);

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

%% Identify Dynamics with a Soft nano-hexapod (0.01N/um)
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeSimplifiedNanoHexapod('actuator_k', 1e4, 'actuator_kp', 0, 'actuator_c', 1);

% Initialize each Simscape model elements
initializeSample('type', 'cylindrical', 'm', 25); % 25kg payload
initializeController('type', 'open-loop');

% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'],     1, 'input');                  io_i = io_i + 1; % Actuator Inputs [N]
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL');  io_i = io_i + 1; % Strut errors [m]

% Identify the dynamics without rotation
initializeReferences();
G_m1_vc = linearize(mdl, io);
G_m1_vc.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_m1_vc.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

% Identify the dynamics with 36 deg/s rotation
initializeReferences(...
    'Rz_type', 'rotating', ...
    'Rz_period', 10); % 36 deg/s

G_m1_vc_Rz_slow = linearize(mdl, io, 0.1);
G_m1_vc_Rz_slow.InputName  = {'f1',  'f2',  'f3',  'f4',  'f5',  'f6'};
G_m1_vc_Rz_slow.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

% Identify the dynamics with 360 deg/s rotation
initializeReferences(...
    'Rz_type', 'rotating', ...
    'Rz_period', 1); % 360 deg/s

G_m1_vc_Rz_fast = linearize(mdl, io, 0.1);
G_m1_vc_Rz_fast.InputName  = {'f1',  'f2',  'f3',  'f4',  'f5',  'f6'};
G_m1_vc_Rz_fast.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

%% Soft Nano-Hexapod - effect of rotational velocity on the dynamics
f = logspace(-1,2,200);
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(squeeze(freqresp(G_m1_vc(1,1), f, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$f_{ni}/f_i$ - $\Omega_z = 0$')
plot(f, abs(squeeze(freqresp(G_m1_vc_Rz_slow(1,1), f, 'Hz'))), 'color', colors(2,:), ...
     'DisplayName', '$f_{ni}/f_i$ - $\Omega_z = 36$ deg/s')
plot(f, abs(squeeze(freqresp(G_m1_vc_Rz_fast(1,1), f, 'Hz'))), 'color', colors(3,:), ...
     'DisplayName', '$f_{ni}/f_i$ - $\Omega_z = 360$ deg/s')
for i = 1:5
    for j = i+1:6
        plot(f, abs(squeeze(freqresp(G_m1_vc(i,j), f, 'Hz'))), 'color', [colors(1,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(f, abs(squeeze(freqresp(G_m1_vc_Rz_slow(i,j), f, 'Hz'))), 'color', [colors(2,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(f, abs(squeeze(freqresp(G_m1_vc_Rz_fast(i,j), f, 'Hz'))), 'color', [colors(3,:), 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i = 2:6
    plot(f, abs(squeeze(freqresp(G_m1_vc(i,i), f, 'Hz'))), 'color', colors(1,:), ...
         'HandleVisibility', 'off');
end
for i = 2:6
    plot(f, abs(squeeze(freqresp(G_m1_vc_Rz_slow(i,i), f, 'Hz'))), 'color', colors(2,:), ...
         'HandleVisibility', 'off');
end
for i = 2:6
    plot(f, abs(squeeze(freqresp(G_m1_vc_Rz_fast(i,i), f, 'Hz'))), 'color', colors(3,:), ...
         'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-9, 1e-2]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(f, 180/pi*angle(squeeze(freqresp(G_m1_vc(i,i), f, 'Hz'))), 'color', colors(1,:));
    plot(f, 180/pi*angle(squeeze(freqresp(G_m1_vc_Rz_slow(i,i), f, 'Hz'))), 'color', colors(2,:));
    plot(f, 180/pi*angle(squeeze(freqresp(G_m1_vc_Rz_fast(i,i), f, 'Hz'))), 'color', colors(3,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180:90:180]);

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

%% HAC Design
% Wanted crossover
wc = 2*pi*10; % [rad/s]

% Integrator
H_int = wc/s;

% Lead to increase phase margin
a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));

% Low Pass filter to increase robustness
H_lpf = 1/(1 + s/2/pi/80);

% Gain to have unitary crossover at wc
H_gain = 1./abs(evalfr(G_hac_m50(1,1), 1j*wc));

% Decentralized HAC
Khac = -H_gain  * ... % Gain
        H_int   * ... % Integrator
        H_lead   * ... % Low Pass filter
        H_lpf   * ... % Low Pass filter
        eye(6);       % 6x6 Diagonal

% The designed HAC controller is saved
save('./mat/nass_K_hac.mat', 'Khac');

%% "Diagonal" loop gain for the High Authority Controller
f = logspace(-1, 2, 1000);
figure;
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(squeeze(freqresp(Khac(i,i)*G_hac_m1( i,i), f, 'Hz'))), ...
     'color', [colors(1,:), 0.5], 'DisplayName', '1kg');
plot(f, abs(squeeze(freqresp(Khac(i,i)*G_hac_m25(i,i), f, 'Hz'))), ...
     'color', [colors(2,:), 0.5], 'DisplayName', '25kg');
plot(f, abs(squeeze(freqresp(Khac(i,i)*G_hac_m50(i,i), f, 'Hz'))), ...
     'color', [colors(3,:), 0.5], 'DisplayName', '50kg');
for i = 2:6
    plot(f, abs(squeeze(freqresp(Khac(i,i)*G_hac_m1( i,i), f, 'Hz'))), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
    plot(f, abs(squeeze(freqresp(Khac(i,i)*G_hac_m25(i,i), f, 'Hz'))), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
    plot(f, abs(squeeze(freqresp(Khac(i,i)*G_hac_m50(i,i), f, 'Hz'))), 'color', [colors(3,:), 0.5], 'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
ylim([1e-2, 1e2]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(f, 180/pi*angle(squeeze(freqresp(-Khac(i,i)*G_hac_m1( i,i), f, 'Hz'))), 'color', [colors(1,:), 0.5]);
    plot(f, 180/pi*angle(squeeze(freqresp(-Khac(i,i)*G_hac_m25(i,i), f, 'Hz'))), 'color', [colors(2,:), 0.5]);
    plot(f, 180/pi*angle(squeeze(freqresp(-Khac(i,i)*G_hac_m50(i,i), f, 'Hz'))), 'color', [colors(3,:), 0.5]);
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([0.1, 100]);

%% Characteristic Loci for the High Authority Controller
Ldet_m1 = zeros(6, length(freqs));
Lmimo_m1 = squeeze(freqresp(-G_hac_m1*Khac, freqs, 'Hz'));
for i_f = 2:length(freqs)
    Ldet_m1(:, i_f) = eig(squeeze(Lmimo_m1(:,:,i_f)));
end
Ldet_m25 = zeros(6, length(freqs));
Lmimo_m25 = squeeze(freqresp(-G_hac_m25*Khac, freqs, 'Hz'));
for i_f = 2:length(freqs)
    Ldet_m25(:, i_f) = eig(squeeze(Lmimo_m25(:,:,i_f)));
end
Ldet_m50 = zeros(6, length(freqs));
Lmimo_m50 = squeeze(freqresp(-G_hac_m50*Khac, freqs, 'Hz'));
for i_f = 2:length(freqs)
    Ldet_m50(:, i_f) = eig(squeeze(Lmimo_m50(:,:,i_f)));
end

figure;
hold on;
plot(real(squeeze(Ldet_m1(1,:))), imag(squeeze(Ldet_m1(1,:))), ...
        '.', 'color', colors(1, :), ...
        'DisplayName', '1kg');
plot(real(squeeze(Ldet_m1(1,:))),-imag(squeeze(Ldet_m1(1,:))), ...
        '.', 'color', colors(1, :), ...
        'HandleVisibility', 'off');
plot(real(squeeze(Ldet_m25(1,:))), imag(squeeze(Ldet_m25(1,:))), ...
        '.', 'color', colors(2, :), ...
        'DisplayName', '25kg');
plot(real(squeeze(Ldet_m25(1,:))),-imag(squeeze(Ldet_m25(1,:))), ...
        '.', 'color', colors(2, :), ...
        'HandleVisibility', 'off');
plot(real(squeeze(Ldet_m50(1,:))), imag(squeeze(Ldet_m50(1,:))), ...
        '.', 'color', colors(3, :), ...
        'DisplayName', '50kg');
plot(real(squeeze(Ldet_m50(1,:))),-imag(squeeze(Ldet_m50(1,:))), ...
        '.', 'color', colors(3, :), ...
        'HandleVisibility', 'off');
for i = 2:6
    plot(real(squeeze(Ldet_m1(i,:))), imag(squeeze(Ldet_m1(i,:))), ...
            '.', 'color', colors(1, :), ...
            'HandleVisibility', 'off');
    plot(real(squeeze(Ldet_m1(i,:))), -imag(squeeze(Ldet_m1(i,:))), ...
            '.', 'color', colors(1, :), ...
            'HandleVisibility', 'off');

    plot(real(squeeze(Ldet_m25(i,:))), imag(squeeze(Ldet_m25(i,:))), ...
            '.', 'color', colors(2, :), ...
            'HandleVisibility', 'off');
    plot(real(squeeze(Ldet_m25(i,:))), -imag(squeeze(Ldet_m25(i,:))), ...
            '.', 'color', colors(2, :), ...
            'HandleVisibility', 'off');

    plot(real(squeeze(Ldet_m50(i,:))), imag(squeeze(Ldet_m50(i,:))), ...
            '.', 'color', colors(3, :), ...
            'HandleVisibility', 'off');
    plot(real(squeeze(Ldet_m50(i,:))), -imag(squeeze(Ldet_m50(i,:))), ...
            '.', 'color', colors(3, :), ...
            'HandleVisibility', 'off');
end
plot(-1, 0, 'kx', 'HandleVisibility', 'off');
hold off;
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
xlabel('Real Part'); ylabel('Imaginary Part');
axis square
xlim([-1.8, 0.2]); ylim([-1, 1]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

%% Simulation of tomography experiments
% Sample is not centered with the rotation axis
% This is done by offsetfing the micro-hexapod by 0.9um
P_micro_hexapod = [0.9e-6; 0; 0]; % [m]

open(mdl);
set_param(mdl, 'StopTime', '2');

initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod('AP', P_micro_hexapod);
initializeSample('type', 'cylindrical', 'm', 1);

initializeSimscapeConfiguration('gravity', false);
initializeLoggingConfiguration('log', 'all', 'Ts', 1e-3);

initializeDisturbances(...
    'Dw_x',  true, ... % Ground Motion - X direction
    'Dw_y',  true, ... % Ground Motion - Y direction
    'Dw_z',  true, ... % Ground Motion - Z direction
    'Fdy_x', false, ... % Translation Stage - X direction
    'Fdy_z', false, ... % Translation Stage - Z direction
    'Frz_x', true, ... % Spindle - X direction
    'Frz_y', true, ... % Spindle - Y direction
    'Frz_z', true);    % Spindle - Z direction

initializeReferences(...
    'Rz_type', 'rotating', ...
    'Rz_period', 1, ...
    'Dh_pos', [P_micro_hexapod; 0; 0; 0]);

% Open-Loop Simulation without Nano-Hexapod - 1kg payload
initializeSimplifiedNanoHexapod('type', 'none');
initializeController('type', 'open-loop');
sim(mdl);
exp_tomo_ol_m1 = simout;

% Closed-Loop Simulation with NASS
initializeSimplifiedNanoHexapod();
initializeController('type', 'hac-iff');
load('nass_K_iff.mat', 'Kiff');
load('nass_K_hac.mat', 'Khac');

% 1kg payload
initializeSample('type', 'cylindrical', 'm', 1);
sim(mdl);
exp_tomo_cl_m1 = simout;

% 25kg payload
initializeSample('type', 'cylindrical', 'm', 25);
sim(mdl);
exp_tomo_cl_m25 = simout;

% 50kg payload
initializeSample('type', 'cylindrical', 'm', 50);
sim(mdl);
exp_tomo_cl_m50 = simout;

%% Simulation of tomography experiment - 1kg payload - 360deg/s - XY errors
figure;
hold on;
plot(1e6*exp_tomo_ol_m1.y.x.Data, 1e6*exp_tomo_ol_m1.y.y.Data, 'DisplayName', 'OL')
plot(1e6*exp_tomo_cl_m1.y.x.Data(1e3:end), 1e6*exp_tomo_cl_m1.y.y.Data(1e3:end), 'color', colors(2,:), 'DisplayName', 'CL')
hold off;
xlabel('$D_x$ [$\mu$m]'); ylabel('$D_y$ [$\mu$m]');
axis equal
xlim([-2, 2]); ylim([-2, 2]);
xticks([-2:1:2]);
yticks([-2:1:2]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

%% Simulation of tomography experiment - 1kg payload - 360deg/s - YZ errors
figure;
tiledlayout(2, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(1e6*exp_tomo_ol_m1.y.y.Data,          1e6*exp_tomo_ol_m1.y.z.Data, 'DisplayName', 'OL')
plot(1e6*exp_tomo_cl_m1.y.y.Data(1e3:end), 1e6*exp_tomo_cl_m1.y.z.Data(1e3:end), 'color', colors(2,:), 'DisplayName', 'CL')
hold off;
xlabel('$D_y$ [$\mu$m]'); ylabel('$D_z$ [$\mu$m]');
axis equal
xlim([-2, 2]); ylim([-0.4, 0.4]);
xticks([-2:1:2]);
yticks([-2:0.2:2]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile();
hold on;
plot(1e9*exp_tomo_cl_m1.y.y.Data(1e3:end), 1e9*exp_tomo_cl_m1.y.z.Data(1e3:end), 'color', colors(2,:), 'DisplayName', 'CL')
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
plot(100*cos(theta), 50*sin(theta), 'k--', 'DisplayName', 'Beam size')
hold off;
xlabel('$D_y$ [nm]'); ylabel('$D_z$ [nm]');
axis equal
xlim([-500, 500]); ylim([-100, 100]);
xticks([-500:100:500]);
yticks([-100:50:100]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

%% Simulation of tomography experiment - 1kg payload - 360deg/s - YZ errors
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(1e9*exp_tomo_cl_m1.y.y.Data(1e3:end), 1e9*exp_tomo_cl_m1.y.z.Data(1e3:end), 'color', colors(1,:), 'DisplayName', '$m = 1$ kg')
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
plot(100*cos(theta), 50*sin(theta), 'k--', 'DisplayName', 'Beam size')
hold off;
xlabel('$D_y$ [$\mu$m]'); ylabel('$D_z$ [$\mu$m]');
axis equal
xlim([-200, 200]); ylim([-100, 100]);
xticks([-200:50:200]); yticks([-100:50:100]);

%% Simulation of tomography experiment - 25kg payload - 360deg/s - YZ errors
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(1e9*exp_tomo_cl_m25.y.y.Data(1e3:end), 1e9*exp_tomo_cl_m25.y.z.Data(1e3:end), 'color', colors(2,:), 'DisplayName', '$m = 25$ kg')
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
plot(100*cos(theta), 50*sin(theta), 'k--', 'DisplayName', 'Beam size')
hold off;
xlabel('$D_y$ [$\mu$m]'); ylabel('$D_z$ [$\mu$m]');
axis equal
xlim([-200, 200]); ylim([-100, 100]);
xticks([-200:50:200]); yticks([-100:50:100]);

%% Simulation of tomography experiment - 50kg payload - 360deg/s - YZ errors
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(1e9*exp_tomo_cl_m50.y.y.Data(1e3:end), 1e9*exp_tomo_cl_m50.y.z.Data(1e3:end), 'color', colors(3,:), 'DisplayName', '$m = 50$ kg')
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
plot(100*cos(theta), 50*sin(theta), 'k--', 'DisplayName', 'Beam size')
hold off;
xlabel('$D_y$ [$\mu$m]'); ylabel('$D_z$ [$\mu$m]');
axis equal
xlim([-200, 200]); ylim([-100, 100]);
xticks([-200:50:200]); yticks([-100:50:100]);