%% Script Description
% 
%%
clear; close all; clc;

%% Options for preprocessing the identified transfer functions
f_low = 10;
f_high = 10000;

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

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

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

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

G_f_to_d = preprocessIdTf(G_f_to_d, 10, 10000);

% Input/Output names
G_f_to_d.InputName  = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
G_f_to_d.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};

% Bode Plot of the linearized function
bodeFig({G_f_to_d(1, 1), G_f_to_d(2, 2), G_f_to_d(3, 3)}, struct('phase', true))
legend({'$F_{n_x} \rightarrow D_{x}$', '$F_{n_y} \rightarrow D_{y}$', '$F_{n_z} \rightarrow D_{z}$'})

%%
save('./mat/G_f_to_d.mat', 'G_f_to_d');