%%
clear; close all; clc;

%% Load Plant
load('./mat/G_f_to_d.mat', 'G_20');

%% Load previously generated controllers
load('./mat/control_K_tx.mat', 'K_tx');
load('./mat/control_K_ty.mat', 'K_ty');
load('./mat/control_K_tz.mat', 'K_tz');
load('./mat/control_K_rx.mat', 'K_rx');
load('./mat/control_K_ry.mat', 'K_ry');
load('./mat/control_K_rz.mat', 'K_rz');

%%
sisotool('bode', G_20(1, 1), K_tx);
K_tx = C;
save('./mat/control_K_tx.mat', 'K_tx');

%%
sisotool('bode', G_20(2, 2), K_ty);
K_ty = C;
save('./mat/control_K_ty.mat', 'K_ty');

%%
sisotool('bode', G_20(3, 3), K_tz);
K_tz = C;
save('./mat/control_K_tz.mat', 'K_tz');

%%
sisotool('bode', G_20(4, 4), K_rx);
K_rx = C;
save('./mat/control_K_rx.mat', 'K_rx');

%%
sisotool('bode', G_20(5, 5), K_ry);
K_ry = C;
save('./mat/control_K_ry.mat', 'K_ry');

%%
sisotool('bode', G_20(6, 6), K_rz);
K_rz = C;
save('./mat/control_K_rz.mat', 'K_rz');

%%
K = tf(zeros(6));
K(1,1) = K_tx;
K(2,2) = K_ty;
K(3,3) = K_tz;
K(4,4) = K_rx;
K(5,5) = K_ry;
K(6,6) = K_rz;

%% Save the MIMO control
save('./mat/controller.mat', 'K');