gam_opt = 10;
dgam_hist = [];
for d = 0.5:0.1:1
  gam_low = 1e-5;
  gam_high = 10.0;
  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(6);
plot(dgam_hist(:,1), dgam_hist(:,2), 'o-');
xlabel('d');
ylabel('gam');

figure(7);
S = 1/(1 + P0 * K_opt);
bodemag(S, 'b', gam_opt/WS, 'g');
hold on;
for c=0.8:0.05:1.2
  P = 1/((s-1)*(c*s + 1));
  S_tilde = 1/(1 + P * K_opt);
  bodemag(S_tilde, 'r');
end
bodemag(S, 'b', gam_opt/WS, 'g');
legh = legend('$S$', '$\gamma/W_S$', '$\tilde S$', 'Location', 'NorthWest');
set(legh, 'interpreter', 'latex');
title('sensitivity gain for perturbed plant');

figure(8);
T = 1 - S; % gain plot of complementary sensitivity function
bodemag(T, 'b', 1/WT, 'g');
legend('T', '1/WT');
title('complementary sensitivity gain');
