%% 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    = 'on';

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

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

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

% Run the linearization
G_h_h = linearize(mdl,io, 0);
G_h_h = G_h_h(1:3, 1:3);
G_h_h = minreal(G_h_h);

% Input/Output names
G_h_h.InputName  = {'Fux', 'Fuy', 'Fuz'};
G_h_h.OutputName = {'Dux', 'Duy', 'Duz'};

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

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

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

% Input/Output names
G_g_g.InputName  = {'Fgx', 'Fgy', 'Fgz'};
G_g_g.OutputName = {'Dgx', 'Dgy', 'Dgz'};

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

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

% Run the linearization
G_h_g = linearize(mdl,io, 0);
G_h_g = G_h_g(1:3, 1:3);
G_h_g = minreal(G_h_g);

% Input/Output names
G_h_g.InputName  = {'Fhx', 'Fhy', 'Fhz'};
G_h_g.OutputName = {'Dgx', 'Dgy', 'Dgz'};

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

%%
save('../mat/id_G_h_h.mat', 'G_h_h');
save('../mat/id_G_g_g.mat', 'G_g_g');
save('../mat/id_G_h_g.mat', 'G_h_g');