echo off;
close all; % close all figures
clear;

solve = 1; % if 1 then hinfsyn is executed, otherwise the previous solution is utilized

sample = 0.0002; % sampling period for control experiment
format long e;

freq = logspace(1, 3.3, 300);
omega = 2*pi*freq;


% general plant settings
load data/nominal.mat; % load nominal plant data
load data/weight.mat; % load weighting function

systemnames = 'G0 W'; % elements of generalized plant
inputvar = '[ a; w; u ]'; % input to generalized plant
outputvar = '[ W; G0(1); G0(2) + a ]'; % output from generalized plant
input_to_G0 = '[ w; u ]'; % input to G0 is [w; u]
input_to_W = '[ u ]'; % input to W is u
cleanupsysic = 'yes';
sysic; % generalized plant is generated as ic_ms

if solve==1

% one dimensional search for constant scaling d
dk_hist = [];
k_opt_global = 0;
for d = 5:1:15 % <--_ edit here !!!

  % maximization of k by bisection method
  k_opt_local = 0;
  k_low = 0.1;
  k_high = 10.;
  while k_high - k_low > 0.01 % if k_high and k_low are closed then quit the loop

    k = (k_low + k_high) / 2.; % the middle point of k
    ic_ms_d = mmult(sdiag(d, k, 1), ic_ms, sdiag(1/d, 1, 1)); % generalized plant with constant scaling and performance scaling k

    fprintf('d = %f, k = %f ... ', d, k);

    [K, clp1, gopt] = hinfsyn(ic_ms_d, 1, 1, 1., 1., 1e-2, 2, 1e-10, 1e-6, 0); % calculation of H infinity controller

    if isempty(gopt)
      fprintf('infeasible\n');
      k_high = k;
    else
      fprintf('feasible\n');
      if k_opt_local < k
        k_opt_local = k;
        if k_opt_global < k_opt_local
          k_opt_global = k_opt_local;
          save data/optimal.mat d k K;
        end
      end
      k_low = k;
    end

  end

  dk_hist = [dk_hist; d, k_opt_local];

end

save data/dk_hist.dat dk_hist -ascii;

else

load data/dk_hist.dat

end

figure(1);
plot(dk_hist(:,1), dk_hist(:,2), 'o-');
xlabel('d', 'FontSize', 20);
ylabel('k', 'FontSize', 20);
print -djpeg90 -r70 data/dk_hist.jpg;

load data/optimal.mat;

% Bode plot of controller
K_g = frsp(K, omega);
K_Hz = scliv(K_g, 1/(2*pi));

% discretization of controller
[Ac,Bc,Cc,Dc] = unpck(K);
[Ad,Bd,Cd,Dd] = bilin(Ac,Bc,Cc,Dc,1,'Tustin',sample);
Kd = pck(Ad, Bd, Cd, Dd);
Kd_g = frsp(Kd, omega, sample);
Kd_Hz = scliv(Kd_g, 1/(2*pi));

figure(2);
subplot(2,1,1);
%vplot('liv,lm', K_Hz, 'b', Kd_Hz, 'r');
vplot('liv,lm', K_Hz, 'b');
%axis([10, 500, 1e-3, 1e3]);
ylabel('Gain', 'FontSize', 20);
title('Bode diagram of controller', 'FontSize', 20);

subplot(2,1,2);
%vplot('liv,p', K_Hz, 'b', Kd_Hz, 'r');
vplot('liv,p', K_Hz, 'b');
%axis([10, 500, -400, 200]);
xlabel('Frequency [Hz]', 'FontSize', 18);
ylabel('Phase [deg]', 'FontSize', 18);
set(findobj('type','text'),'fontsize',18);
set(findobj('type','axes'),'fontsize',18);
set(findobj('type','line'),'linewidth',3);
print -djpeg90 -r70 data/controller.jpg;

% store controller realization
n = size(Ad, 1);
save data/controller.dat sample Ad Bd Cd Dd -ascii -double;

% store controller order
fid = fopen('data/controller_order.dat', 'w');
fprintf(fid, '%d\n', n);
fclose(fid);
save data/controller.mat Ac Bc Cc Dc;

ic_ms_d = mmult(sdiag(1, k, 1), ic_ms, sdiag(1, 1, 1));
clp = slft(ic_ms_d, K);
clp_g = frsp(clp, omega);

% mu plot
clp_Hz = scliv(clp_g, 1/(2*pi));
mu_out = mu(clp_Hz);
figure(3);
vplot('liv,lm', mu_out);

xlabel('Frequency [Hz]', 'FontSize', 18);
ylabel('Maxmum singular value', 'FontSize', 18);
set(findobj('type','text'),'fontsize',18);
set(findobj('type','axes'),'fontsize',18);
set(findobj('type','line'),'linewidth',2);
title('Maximum singular value', 'FontSize', 18);
print -djpeg90 -r70 data/mu.jpg;
