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

sample = 0.00075; % サンプリング周期
%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 = '[ u; G0(1); G0(2) + W ]'; % 一般化プラントの出力信号
outputvar = '[ W; G0(1); G0(2) + a ]'; % 一般化プラントの出力信号
input_to_G0 = '[ w; u ]'; % Gyu0 の入力信号は u
%input_to_W = '[ a ]'; % W の入力信号は a
input_to_W = '[ u ]'; % W の入力信号は u
cleanupsysic = 'yes';
ic_ms = sysic;

%%%%%%%%% ここまで，hinf.m と全く同じ %%%%%%%%

% 一般化プラントの離散化(step-invariant transformation)
tmp = c2d(ss(ic_ms.a, ic_ms.b, ic_ms.c, ic_ms.d), sample);
[Ag_d,Bg_d,Cg_d,Dg_d] = ssdata(tmp);
ic_ms_discrete = pck(Ag_d, Bg_d, Cg_d, Dg_d);


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

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

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

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

    [K, clp1, gopt] = dhfsyn(ic_ms_discrete_d, 1, 1, 1., 1., 1e-2, sample); %, 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_discrete_zoh.mat d k K;
        end
      end
      k_low = k;
    end

  end

  dk_hist = [dk_hist; d, k_opt_local];

end

save data/dk_hist_discrete_zoh.dat dk_hist -ascii;

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_discrete_zoh.mat;

[A, B, C, D] = unpck(K);
K = ss(A, B, C, D, sample);

if max(abs(eig(A))) >= 1.0
  fprintf(1, 'original controller is unstable\n');
else
  fprintf(1, 'original controller is stable\n');
end

% H∞補償器のボード線図
f = logspace(1, 2.82, 300);
w = 2*pi*f;
K_g = frd(K, w);
K0 = n4sid(K_g, 20, 'co', 'none', 'Focus', 'Stability'); % 安定な補償器に近似する
K0 = ss(K0.a, K0.b, K0.c, K0.d, sample);
K0_g = frd(K0, w);

figure(2);
bode(K_g, 'b-', K0_g, 'r-');

% 補償器のデータをファイルにセーブする
n = size(K0.a, 1);
A = K0.a;
B = K0.b;
C = K0.c;
D = K0.d;
save data/controller_discrete_zoh.dat sample A B C D -ascii -double;
% 補償器の次数だけ別ファイルにセーブ(指数表示にならないように)
fid = fopen('data/controller_order_discrete.dat', 'w');
fprintf(fid, '%d\n', n);
fclose(fid);
save data/controller_discrete_zoh.mat sample A B C D;
