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

%% Define options for bode plots
bode_opts = bodeoptions;

bode_opts.Title.FontSize  = 12;
bode_opts.XLabel.FontSize = 12;
bode_opts.YLabel.FontSize = 12;
bode_opts.FreqUnits       = 'Hz';
bode_opts.MagUnits        = 'abs';
bode_opts.MagScale        = 'log';
bode_opts.PhaseWrapping   = 'on';
bode_opts.PhaseVisible    = 'off';

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

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

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

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

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

% Post-process the linearized function
G_ty = preprocessIdTf(G_ty_raw, f_low, f_high);

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

% Bode Plot of the linearized function
figure;
bode(G_ty, bode_opts)

%% Tilt Stage
% Input/Output definition
io(1) = linio([mdl, '/My'],1,'input');
io(2) = linio([mdl, '/Tilt'],1,'output');

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

% Post-process the linearized function
G_ry = preprocessIdTf(G_ry_raw, f_low, f_high);

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

% Bode Plot of the linearized function
figure;
bode(G_ry, bode_opts)


%% Spindle
% Input/Output definition
io(1) = linio([mdl, '/Mz'],1,'input');
io(2) = linio([mdl, '/Spindle'],1,'output');

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

% Post-process the linearized function
G_rz = preprocessIdTf(G_rz_raw, f_low, f_high);

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

% Bode Plot of the linearized function
figure;
bode(G_rz, bode_opts)


%% Hexapod Symetrie
% Input/Output definition
io(1) = linio([mdl, '/Fhexa_cart'],1,'input');
io(2) = linio([mdl, '/Hexapod Symetrie'],1,'output');

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

% Post-process the linearized function
G_hexa = preprocessIdTf(G_hexa_raw, f_low, f_high);

% 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', 'Dhexa_x', 'Dhexa_y', 'Dhexa_z'};

% Bode Plot of the linearized function
figure;
bode(G_hexa, bode_opts)

%% NASS
% Input/Output definition
io(1) = linio([mdl, '/Fnass_cart'],1,'input');
io(2) = linio([mdl, '/NASS'],1,'output');

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

% Post-process the linearized function
G_nass = preprocessIdTf(G_nass_raw, f_low, f_high);

% 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', 'Dnass_x', 'Dnass_y', 'Dnass_z'};

% Bode Plot of the linearized function
figure;
bode(G_nass, bode_opts)

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

%% Functions
function G = preprocessIdTf(G0, f_low, f_high)
    [~,G1] = freqsep(G0, 2*pi*f_low);
    [G2,~] = freqsep(G1, 2*pi*f_high);
    G = minreal(G2);
end