% 相似変換
% 入力: controller_r.mat
% 出力:
%   IIR フィルタ係数: controller_IIR_r.dat
%   可制御正準形: controller0_r.dat
%   strans: controller_strans_r.dat
%   strans を部分毎に可制御正準形に変形したもの: controller_stans0_r.dat

%load data/controller_r.mat;
load data/controller_discrete_zoh.mat;
Ad = A;
Bd = B;
Cd = C;
Dd = D;

% 可制御正準形
[num, den] = ss2tf(Ad, Bd, Cd, Dd);
[Ad0, Bd0, Cd0, Dd0] = tf2ss(num, den);
save data/controller0_r.dat sample Ad0 Bd0 Cd0 Dd0 -ascii;
save data/controller0_r_d.dat sample Ad0 Bd0 Cd0 Dd0 -ascii -double;

% strans
Gd = pck(Ad, Bd, Cd, Dd);
[Gs, V] = strans(Gd);
[As, Bs, Cs, Ds] = unpck(Gs);
save data/controller_strans_r.dat sample As Bs Cs Ds -ascii;
save data/controller_strans_r_d.dat sample As Bs Cs Ds -ascii -double;

% IIR フィルタ係数
save data/controller_IIR_r.dat sample den num -ascii;
save data/controller_IIR_r_d.dat sample den num -ascii -double;

% 各モード毎に可制御正準形に変換する
n = size(As, 1);
As0 = [];
Bs0 = [];
Cs0 = [];
Ds0 = Ds;

epsilon = 1e-10; % 0 とみなす閾値

k = 1;
while k < n
  k
  if abs(As(k+1,k)) < epsilon
    n_tmp = 0;
  else
    n_tmp = 1;
  end

  [num_tmp, den_tmp] = ss2tf(As(k:k+n_tmp,k:k+n_tmp), Bs(k:k+n_tmp,:), Cs(:,k:k+n_tmp), Ds);
  [A_tmp, B_tmp, C_tmp, D_tmp] = tf2ss(num_tmp, den_tmp);
  As0 = mdiag(As0, A_tmp);
  Bs0 = [Bs0; B_tmp];
  Cs0 = [Cs0, C_tmp];
  if abs(eig(A_tmp)) > 1
    eig(A_tmp)
  end
%  else
%    [num_tmp, den_tmp] = ss2tf(As(k:k+1,k:k+1), Bs(k:k+1,:), Cs(:,k:k+1), Ds);
%    [Atmp, Btmp, Ctmp, Dtmp] = ts2ss(num_tmp, den_tmp);
%  end

  k = k + 1 + n_tmp;

end

%最後に1×1のブロックが残った場合
if k == n
  As0 = mdiag(As0, As(n,n));
  Bs0 = [Bs0; Bs(n,1)];
  Cs0 = [Cs0, Cs(1,n)];
end

save data/controller0_mode_r.dat sample As0 Bs0 Cs0 Ds0 -ascii;
save data/controller0_mode_r_d.dat sample As0 Bs0 Cs0 Ds0 -ascii -double;
