echo off;
close all; % figure を全部閉じる
clear;

solve = 1; % 1 なら hinfsyn を動かす，それ以外の場合は前の計算結果を使用

%sample = 0.0002; % サンプリング周期
sample = 0.0005; % サンプリング周期
format long e;

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


% 一般化プラントの構成
load data/nominal.mat; % ノミナルプラントのデータを読み込む
load data/weight.mat; % 重みのデータを読み込む

systemnames = 'G0 W'; % 一般化プラントを構成する要素
inputvar = '[ a; w; u ]'; % 一般化プラントの入力信号
outputvar = '[ W; G0(1); G0(2) + a ]'; % 一般化プラントの出力信号
input_to_G0 = '[ w; u ]'; % Gyu0 の入力信号は u
input_to_W = '[ u ]'; % W の入力信号は u
cleanupsysic = 'yes';
ic_ms = sysic;

if solve==1

% 定数スケーリングの探索
dk_hist = [];
k_opt_global = 0;
for d =0.1:0.4:5 % ここを編集

  % 二分法による k の最大化
  k_opt_local = 0;
  k_low = 0.1;
  k_high = 20.;
  while k_high - k_low > 0.01 % k_high と k_low が十分近付いたら終了

    k = (k_low + k_high) / 2.; % 中点
    ic_ms_d = mmult(sdiag(d, k, 1), ic_ms, sdiag(1/d, 1, 1)); % スケーリング後の一般化プラント

    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); % 補償器を求める

    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');
ylabel('k');
print -djpeg90 -r70 data/dk_hist.jpg;

load data/optimal.mat;

% H∞補償器のボード線図
K_g = frsp(K, omega);
K_Hz = scliv(K_g, 1/(2*pi));

% 離散化
[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, 2000, 1e-2, 10]);
ylabel('Gain');
title('Bode diagram of controller');

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

% 補償器のデータをファイルにセーブする
n = size(Ad, 1);
save data/controller.dat sample Ad Bd Cd Dd -ascii -double;

% 補償器の次数だけ別ファイルにセーブ(指数表示にならないように)
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);

% μプロット
clp_Hz = scliv(clp_g, 1/(2*pi));
mu_out = mu(clp_Hz);
figure(3);
vplot('liv,lm', mu_out);
axis([10, 2000, 1e-2, 1]);
xlabel('Frequency [Hz]');
ylabel('Maxmum singular value');
set(findobj('type','line'),'linewidth',2);
title('Maximum singular value');
print -djpeg90 -r70 data/mu.jpg;
