%% Script Description
% Run an identification of each stage from input to output
% Save all computed transfer functions into one .mat file

%%
clear; close all; clc;

%%
initializeSample(struct('mass', 20));

%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;

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

%% Y-Translation Stage
% Input/Output definition
io(1) = linio([mdl, '/Micro-Station/Fy'],           1,'openinput');
io(2) = linio([mdl, '/Micro-Station/Translation y'],1,'output');

% Run the linearization
G_ty = linearize(mdl,io, 0);

% Input/Output names
G_ty.InputName  = {'Fy'};
G_ty.OutputName = {'Dy'};

%% Tilt Stage
% Input/Output definition
io(1) = linio([mdl, '/Micro-Station/Ry'],  1,'openinput');
io(2) = linio([mdl, '/Micro-Station/Tilt'],1,'output');

% Run the linearization
G_ry = linearize(mdl,io, 0);

% Input/Output names
G_ry.InputName  = {'My'};
G_ry.OutputName = {'Ry'};

%% Spindle
% Input/Output definition
io(1) = linio([mdl, '/Micro-Station/Rz'],     1,'openinput');
io(2) = linio([mdl, '/Micro-Station/Spindle'],1,'output');

% Run the linearization
G_rz = linearize(mdl,io, 0);

% Input/Output names
G_rz.InputName  = {'Mz'};
G_rz.OutputName = {'Rz'};

%% Hexapod Symetrie
% Input/Output definition
io(1) = linio([mdl, '/Micro-Station/Fm'],           1,'openinput');
io(2) = linio([mdl, '/Micro-Station/Micro_Hexapod'],1,'output');

% Run the linearization
G_hexa = linearize(mdl,io, 0);

% Input/Output names
G_hexa.InputName  = {'Fhexa_x', 'Fhexa_y', 'Fhexa_z', 'Mhexa_x', 'Mhexa_y', 'Mhexa_z'};
G_hexa.OutputName = {'Dhexa_x', 'Dhexa_y', 'Dhexa_z', 'Rhexa_x', 'Rhexa_y', 'Rhexa_z'};

%% NASS
% Input/Output definition
io(1) = linio([mdl, '/Micro-Station/Fn'],          1,'openinput');
io(2) = linio([mdl, '/Micro-Station/Nano_Hexapod'],1,'output');

% Run the linearization
G_nass = linearize(mdl,io, 0);

% Input/Output names
G_nass.InputName  = {'Fnass_x', 'Fnass_y', 'Fnass_z', 'Mnass_x', 'Mnass_y', 'Mnass_z'};
G_nass.OutputName = {'Dnass_x', 'Dnass_y', 'Dnass_z', 'Rnass_x', 'Rnass_y', 'Rnass_z'};

%% Save all transfer function
save('./mat/identified_tf.mat', 'G_ty', 'G_ry', 'G_rz', 'G_hexa', 'G_nass')