%%
clear; close all; clc;

%% Load Controllers
load('./mat/K_fb.mat', 'K_light_vc', 'K_light_pz', 'K_heavy_vc', 'K_heavy_pz');

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

initializeNanoHexapod(struct('actuator', 'lorentz'));
K = K_light_vc; %#ok
save('./mat/controllers.mat', 'K');
Gd_cl_light_vc = identifyPlant();

initializeNanoHexapod(struct('actuator', 'piezo'));
K = K_light_pz; %#ok
save('./mat/controllers.mat', 'K');
Gd_cl_light_pz = identifyPlant();

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

initializeNanoHexapod(struct('actuator', 'lorentz'));
K = K_heavy_vc; %#ok
save('./mat/controllers.mat', 'K');
G_cl_heavy_vc = identifyPlant();

initializeNanoHexapod(struct('actuator', 'piezo'));
K = K_heavy_pz;
save('./mat/controllers.mat', 'K');
G_cl_heavy_pz = identifyPlant();

%% Save the identified transfer functions
save('./mat/G_cl.mat', 'G_cl_light_vc', 'G_cl_light_pz', 'G_cl_heavy_vc', 'G_cl_heavy_pz');