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

%% Colors for the figures
colors = colororder;

%% Uniaxial Simscape model name
mdl = 'nass_uniaxial_model';

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

%% Load the micro-station parameters
load('uniaxial_micro_station_parameters.mat')

% Nano-Hexapod Parameters
% The parameters for the nano-hexapod and sample are:
% - $m_s$ the sample mass that can vary from 1kg up to 50kg
% - $m_n$ the nano-hexapod mass which is set to 15kg
% - $k_n$ the nano-hexapod stiffness, which can vary depending on the chosen architecture/technology

% As a first example, let's choose a nano-hexapod stiffness of $10\,N/\mu m$ and a sample mass of 10kg.


%% Nano-Hexapod Parameters
mn = 15; % [kg]
kn = 1e7; % [N/m]
cn = 2*0.01*sqrt(mn*kn); % [N/(m/s)]

%% Sample Mass
ms = 10; % [kg]

% Obtained Dynamics
% The sensitivity to disturbances (i.e. $x_f$, $f_t$ and $f_s$) are shown in Figure ref:fig:uniaxial_sensitivity_dist_first_params.
% The /plant/ (i.e. the transfer function from actuator force $f$ to measured displacement $d$) is shown in Figure ref:fig:uniaxial_plant_first_params.

% For further analysis, 9 configurations are considered: three nano-hexapod stiffnesses ($k_n = 0.01\,N/\mu m$, $k_n = 1\,N/\mu m$ and $k_n = 100\,N/\mu m$) combined with three sample's masses ($m_s = 1\,kg$, $m_s = 25\,kg$ and $m_s = 50\,kg$).


%% Use 1DoF Nano-Hexpod model
model_config = struct();
model_config.nhexa = "1dof";
model_config.controller = "open_loop";

%% Identify the transfer function from disturbances and force actuator to d
clear io; io_i = 1;
io(io_i) = linio([mdl, '/controller'],       1, 'openinput');  io_i = io_i + 1; % Force Actuator
io(io_i) = linio([mdl, '/fs'],               1, 'openinput');  io_i = io_i + 1; % Force applied on the sample
io(io_i) = linio([mdl, '/micro_station/xf'], 1, 'openinput');  io_i = io_i + 1; % Floor Motion
io(io_i) = linio([mdl, '/micro_station/ft'], 1, 'openinput');  io_i = io_i + 1; % Stage disturbances
io(io_i) = linio([mdl, '/d'] ,               1, 'openoutput'); io_i = io_i + 1; % Metrology

%% Perform the model extraction
G_ol = linearize(mdl, io, 0.0);
G_ol.InputName = {'f', 'fs', 'xf', 'ft'};
G_ol.OutputName = {'d'};

%% Sensitivity to disturbances
figure;
tiledlayout(1, 3, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(freqs, abs(squeeze(freqresp(G_ol('d', 'fs'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d/f_{s}$ [m/N]'); xlabel('Frequency [Hz]');
xticks([1e0, 1e1, 1e2]);

ax2 = nexttile();
hold on;
plot(freqs, abs(squeeze(freqresp(G_ol('d', 'ft'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d/f_{t}$ [m/N]'); xlabel('Frequency [Hz]');
xticks([1e0, 1e1, 1e2]);

ax3 = nexttile();
hold on;
plot(freqs, abs(squeeze(freqresp(G_ol('d', 'xf'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d/x_{f}$ [m/m]'); xlabel('Frequency [Hz]');
xticks([1e0, 1e1, 1e2]);

linkaxes([ax1,ax2,ax3],'x');
xlim([1, 500]);



% #+name: fig:uniaxial_sensitivity_dist_first_params
% #+caption: Sensitivity to disturbances
% #+RESULTS:
% [[file:figs/uniaxial_sensitivity_dist_first_params.png]]


%% Bode Plot of the transfer function from actuator forces to measured displacement by the metrology
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_ol('d', 'f'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d/f$ [m/N]'); set(gca, 'XTickLabel',[]);

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ol('d', 'f'), 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, 0]);

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

% Identification of all combination of stiffnesses / masses       :noexport:

%% Use 1DoF Nano-Hexpod model
model_config = struct();
model_config.nhexa = "1dof";
model_config.controller = "open_loop";

%% Nano-Hexapod Mass
mn = 15; % Nano-Hexapod mass [kg]

%% Identification of all combination of stiffnesses / masses
clear io; io_i = 1;
io(io_i) = linio([mdl, '/controller'],       1, 'openinput');  io_i = io_i + 1; % Actuator Force
io(io_i) = linio([mdl, '/micro_station/xf'], 1, 'openinput');  io_i = io_i + 1; % Floor Motion
io(io_i) = linio([mdl, '/micro_station/ft'], 1, 'openinput');  io_i = io_i + 1; % Stage vibrations
io(io_i) = linio([mdl, '/fs'],               1, 'openinput');  io_i = io_i + 1; % Direct sample forces
io(io_i) = linio([mdl, '/dL'],               1, 'openoutput'); io_i = io_i + 1; % Relative Motion Sensor
io(io_i) = linio([mdl, '/fm'],               1, 'openoutput'); io_i = io_i + 1; % Force Sensor
io(io_i) = linio([mdl, '/vn'] ,              1, 'openoutput'); io_i = io_i + 1; % Geophone
io(io_i) = linio([mdl, '/d'] ,               1, 'openoutput'); io_i = io_i + 1; % Metrology Output

%% Light Sample
ms = 1; % Sample Mass [kg]

% Voice Coil (i.e. soft) Nano-Hexapod
kn = 1e4; % Nano-Hexapod Stiffness [N/m]
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
G_vc_light = linearize(mdl, io, 0.0);
G_vc_light.InputName = {'f', 'xf', 'ft', 'fs'};
G_vc_light.OutputName = {'dL', 'fm', 'vn', 'd'};

% APA (i.e. relatively stiff) Nano-Hexapod
kn = 1e6; % Nano-Hexapod Stiffness [N/m]
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
G_md_light = linearize(mdl, io, 0.0);
G_md_light.InputName = {'f', 'xf', 'ft', 'fs'};
G_md_light.OutputName = {'dL', 'fm', 'vn', 'd'};

% Piezoelectric (i.e. stiff) Nano-Hexapod
kn = 1e8; % Nano-Hexapod Stiffness [N/m]
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
G_pz_light = linearize(mdl, io, 0.0);
G_pz_light.InputName = {'f', 'xf', 'ft', 'fs'};
G_pz_light.OutputName = {'dL', 'fm', 'vn', 'd'};

%% Mid Sample
ms = 25; % Sample Mass [kg]

% Voice Coil (i.e. soft) Nano-Hexapod
kn = 1e4; % Nano-Hexapod Stiffness [N/m]
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
G_vc_mid = linearize(mdl, io, 0.0);
G_vc_mid.InputName = {'f', 'xf', 'ft', 'fs'};
G_vc_mid.OutputName = {'dL', 'fm', 'vn', 'd'};

% APA (i.e. relatively stiff) Nano-Hexapod
kn = 1e6; % Nano-Hexapod Stiffness [N/m]
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
G_md_mid = linearize(mdl, io, 0.0);
G_md_mid.InputName = {'f', 'xf', 'ft', 'fs'};
G_md_mid.OutputName = {'dL', 'fm', 'vn', 'd'};

% Piezoelectric (i.e. stiff) Nano-Hexapod
kn = 1e8; % Nano-Hexapod Stiffness [N/m]
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
G_pz_mid = linearize(mdl, io, 0.0);
G_pz_mid.InputName = {'f', 'xf', 'ft', 'fs'};
G_pz_mid.OutputName = {'dL', 'fm', 'vn', 'd'};

%% Heavy Sample
ms = 50; % Sample Mass [kg]

% Voice Coil (i.e. soft) Nano-Hexapod
kn = 1e4; % Nano-Hexapod Stiffness [N/m]
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
G_vc_heavy = linearize(mdl, io, 0.0);
G_vc_heavy.InputName = {'f', 'xf', 'ft', 'fs'};
G_vc_heavy.OutputName = {'dL', 'fm', 'vn', 'd'};

% APA (i.e. relatively stiff) Nano-Hexapod
kn = 1e6; % Nano-Hexapod Stiffness [N/m]
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
G_md_heavy = linearize(mdl, io, 0.0);
G_md_heavy.InputName = {'f', 'xf', 'ft', 'fs'};
G_md_heavy.OutputName = {'dL', 'fm', 'vn', 'd'};

% Piezoelectric (i.e. stiff) Nano-Hexapod
kn = 1e8; % Nano-Hexapod Stiffness [N/m]
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
G_pz_heavy = linearize(mdl, io, 0.0);
G_pz_heavy.InputName = {'f', 'xf', 'ft', 'fs'};
G_pz_heavy.OutputName = {'dL', 'fm', 'vn', 'd'};

%% Save All Identified Plants
save('./mat/uniaxial_plants.mat', 'G_vc_light', 'G_md_light', 'G_pz_light', ...
                                  'G_vc_mid',   'G_md_mid',   'G_pz_mid', ...
                                  'G_vc_heavy', 'G_md_heavy', 'G_pz_heavy');