clear all;

s = tf('s');
sample = 0.00025; % sampling time for discretized controller

load data/nominal.mat;
load data/weight.mat;

WS = 0.1/(s+0.1); % weighting for sensitivity function
WS = ss(WS); % at least one sub-system is given in SSR to get G in SSR

systemnames = 'P0 WT WS';
inputvar = '[ w1; w2; u ]';
outputvar = '[ WT; WS; w2-w1-P0 ]';
input_to_P0 = '[ u ]';
input_to_WT = '[ P0 ]';
input_to_WS = '[ w2-w1-P0 ]';
cleanupsysic = 'yes';
Ghat = sysic;

gam_opt = 1000;
gam_opt_local = gam_opt;
dgam_hist = [];
for d = 1:1:10
  gam_low = 1e-5;
  gam_high = 1000;
  while gam_high - gam_low > 1e-4
    gam = (gam_low + gam_high) / 2.;
    [K, clp, gopt] = hinfsyn(mdiag(1/d,1/gam,1)*Ghat*mdiag(d,1,1), 1, 1, 'GMIN', 1., 'GMAX', 1, 'DISPLAY', 'off');
    fprintf('d = %f, gam = %f ... ', d, gam);
    if isempty(K)
      fprintf('infeasible\n');
      gam_low = gam;
    else
      fprintf('feasible\n');
      gam_high = gam;
      K_opt_local = K;
      gam_opt_local = gam;
    end
  end
  dgam_hist = [dgam_hist; d, gam_opt_local];
  if gam_opt > gam_opt_local
    gam_opt = gam_opt_local;
    K_opt = K_opt_local;
    d_opt = d;
  end
end

figure(3);
plot(dgam_hist(:,1), dgam_hist(:,2), 'o-');
xlabel('d');
ylabel('gam');

figure(4);
bode(K_opt);
title('controller');

Kd = c2d(K_opt, sample);
tmp = [Kd.a, Kd.b; Kd.c, Kd.d];
save data/controller.dat sample tmp -ascii -double;

n = size(K_opt.a, 1);
fid = fopen('data/controller_order.dat', 'w');
fprintf(fid, '%d\n', n);
fclose(fid);

save data/controller.mat K_opt Kd Ghat gam_opt d_opt dgam_hist;

figure(5);
K_g = frd(K_opt, w);
WS_g = frd(WS, w);
S_g = 1/(1 + P0_g * K_g);
S_tilde_g = 1/(1 + P_g * K_g);
bodemag(S_g, 'b', gam_opt/WS_g, 'g', S_tilde_g, 'r');
legh = legend('$S_0$', '$\gamma/W_S$', '$S$', 'Location', 'NorthWest');
set(legh, 'interpreter', 'latex');
title('sensitivity gain for actual plant');

figure(6);
T_g = 1 - S_g; % gain plot of complementary sensitivity function
WT_g = frd(WT, w);
bodemag(T_g, 'b', 1/WT_g, 'g');
legend('T', '1/WT');
title('complementary sensitivity gain');
