%%
clear; close all; clc;

%% Load IFF Controllers
load('./mat/K_iff_crit.mat', 'K_iff_light_vc', 'K_iff_light_pz', 'K_iff_heavy_vc', 'K_iff_heavy_pz');

%% Light Sample
initializeSample(struct('mass', 1));

initializeNanoHexapod(struct('actuator', 'lorentz'));
K_iff = K_iff_light_vc; %#ok
save('./mat/controllers.mat', 'K_iff');
G_iff_light_vc = identifyPlant();

initializeNanoHexapod(struct('actuator', 'piezo'));
K_iff = K_iff_light_pz; %#ok
save('./mat/controllers.mat', 'K_iff');
G_iff_light_pz = identifyPlant();

%% Heavy Sample
initializeSample(struct('mass', 50));

initializeNanoHexapod(struct('actuator', 'lorentz'));
K_iff = K_iff_heavy_vc; %#ok
save('./mat/controllers.mat', 'K_iff');
G_iff_heavy_vc = identifyPlant();

initializeNanoHexapod(struct('actuator', 'piezo'));
K_iff = K_iff_heavy_pz;
save('./mat/controllers.mat', 'K_iff', '-append');
G_iff_heavy_pz = identifyPlant();

%% Save the obtained transfer functions
save('./mat/G_iff.mat', 'G_iff_light_vc', 'G_iff_light_pz', 'G_iff_heavy_vc', 'G_iff_heavy_pz');