%% Script Description
% Make the same identification as Marc did
% Should comment out the nano-hexapod and sample before
% runing this script.

%%
clear; close all; clc;

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

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

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

% Run the linearization
G_h_h_raw = linearize(mdl,io, 0);
G_h_h_raw = G_h_h_raw(1:3, 1:3);
G_h_h = preprocessIdTf(G_h_h_raw, 10, 10000);

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

% Bode Plot of the linearized function
bodeFig({G_h_h(1, 1), G_h_h(2, 2), G_h_h(3, 3)})
legend({'$F_{h_x} \rightarrow D_{h_x}$', '$F_{h_y} \rightarrow D_{h_y}$', '$F_{h_z} \rightarrow D_{h_z}$'})
legend('location', 'southwest')
exportFig('id_marc_h_to_h', 'normal-normal', struct('path', 'Identification'))

%% Granite
% Input/Output definition
io(1) = linio([mdl, '/Micro-Station/F_granite'],1,'input');
io(2) = linio([mdl, '/Micro-Station/Granite_Inertial_Sensor'],1,'output');

% Run the linearization
G_g_g_raw = linearize(mdl,io, 0);
G_g_g = preprocessIdTf(G_g_g_raw, 10, 10000);

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

% Bode Plot of the linearized function
bodeFig({G_g_g(1, 1), G_g_g(2, 2), G_g_g(3, 3)})
legend({'$F_{g_x} \rightarrow D_{g_x}$', '$F_{g_y} \rightarrow D_{g_y}$', '$F_{g_z} \rightarrow D_{g_z}$'})
legend('location', 'southwest')
exportFig('id_marc_g_to_g', 'normal-normal', struct('path', 'Identification'))

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

% Run the linearization
G_h_g_raw = linearize(mdl,io, 0);
G_h_g_raw = G_h_g_raw(1:3, 1:3);
G_h_g = preprocessIdTf(G_h_g_raw, 10, 10000);

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

% Bode Plot of the linearized function
bodeFig({G_h_g(1, 1), G_h_g(2, 2), G_h_g(3, 3)})
legend({'$F_{h_x} \rightarrow D_{g_x}$', '$F_{h_y} \rightarrow D_{g_y}$', '$F_{h_z} \rightarrow D_{g_z}$'})
legend('location', 'southwest')
exportFig('id_marc_h_to_g', 'normal-normal', struct('path', 'Identification'))

%%
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');