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

%%
load('../mat/G_f_to_d.mat', 'G_f_to_d');

%%
G = G_f_to_d(2, 2);

%% Some post processing of the plant
[G, ~] = freqsep(G, 2*pi*1000);
[~, G] = freqsep(G, 2*pi*1);

%% Verify the post processing
figure;
bode(G, G_f_to_d(2, 2));

%% Try sisotool
sisotool(G)

%%
gain = 1e8;

%%
figure;
bode(gain*G, bode_opts)

%%
[~,~,~,Wpm] = margin(gain*G);

% Wpm = 180*2*pi;

%%
s = tf('s');
Ky = gain*(s/(0.2*Wpm)+1)/(s/(10*Wpm)+1)/(1+s/(2*pi*100));%*(s+2*pi*10)/(s+2*pi*0.0001);

%% Compute Closed loop transfer function
figure;
bode(Ky*G, bode_opts)

%%
K = tf(zeros(6));
K(2,2) = Ky;

%%
save('../mat/controller.mat', 'K')