function [ K, gopt, clsys, scaling_output, scalng_input ] = ...
  csfdlqhinflmi(Gh, W, m2, deltaports, isRepeatedScalarScaling)

%
% csfdlqhinflmi:
%
%  Solve the robust stabilization and the performance problem via
%  the constantly scaled H-infinity control problem for the
%  generalized plant with the following special structure:
%
%                   +----------+
%           +---+   |          |   +---+
%   zeta <--+Pp +<--+          +<--+Pm +<-- nu
%           +---+   |          |   +---+
%                   |          |
%           +---+   |          |
%    z   <--+ W +<--+    Gh    +<---------- w
%           +---+   |          |
%                   |          |
%          y  +-----+          +<----+ u
%             |     |          |     |
%             |     +----------+     |
%             |                      |
%             |         +---+        |
%             +-------->+ K +--------+
%                       +---+
%
%  where zeta and nu are the output and the input connected to
%  uncertainty block. z is the control output, while w is the
%  exogeneous input. y is the measurement output and u is the control
%  output. Pp and Pm are the scaling matrix for output and input,
%  respectively. W is the frequency weighting function and K is the
%  controller to be designed.
%
%  INPUT ARGUMENTS:
%
%  Gh: generalized plant, lti object or filename of simulink model
%
%    The realization of Gh is assumed as
%
%      Gh = | Ah  | Bh0  Bh1  Bh2  |
%           +-----+----------------+
%           | Ch0 | Dh00 Dh01 Dh02 |
%           | Ch1 | Dh10 Dh11 Dh12 |
%
%    The matrices correspond to y MUST NOT be given. In solving the
%    problem, C2 = I and Dh2* = 0 are assumed. Dh10 and Dh11 are
%    assumed to be zero, no matter what values are given in Gh.
%
%  W: frequency dependent weighting function , lti object or
%     filename of simulink model.
%
%    if W is empty, the robust stabilization problem is
%    solved. Otherwise, the robust performance problem is solved.
%
%  m2: size of u
%
%  delta_ports: matrix specifying the block structure of the
%               uncertainty block
%
%    delta_ports = | p01  m01 |
%                  | p02  m02 |
%                  |  :    :  |
%                  | p0N  m0N |
%
%    where p0k and m0k are the input and the output size of k-th
%    block. N is the number of the blocks.
%
%  isRepeatedScalarScaling: vector specifying where the scaling matrix
%  is repeated scalar.
%
%    If the k-th element is nonzero, the k-th scaling matrix is
%    assumed the repeated scalar block. Otherwise, the k-th element is
%    assumed the full block. If this argument is ommited, all the
%    elements are assumed to be zero.
%
%  OUTPUT ARGUMENTS:
%
%  K: controller, lti object
%
%  gopt: optimal robust performance or robust stability index
%
%    If the robust performance problem is solved, the optimal
%    robust performance index is given by gopt. If the robust
%    stabilization problem is solved, gopt < 0 implies that the
%    problem is feasible. Otherwise, the problem is infeasible.
%
% clsys: closed-loop system, lti object
%
%   closed-loop system composed by Gh and K 
%
% scaling_output: matrix corresponding to Pp
%
% scaling_input: matrix corresponding to Pm
%
 
  %% 不確かさの情報

  pm0 = sum(deltaports,1);
  p0 = pm0(1);
  m0 = pm0(2);
  Npm0 = size(deltaports,1);

  if nargin < 5
    isRepeatedScalarScaling = zeros(Npm0,1);
  end

  %% 一般化プラント

  if isstr(Gh)
    [Ah,Bh,Ch,Dh] = linmod(Gh);
  elseif isa(Gh,'lti')
    [Ah,Bh,Ch,Dh] = ssdata(Gh);
  else
    error('csfdlqhinflmi', 'Gh must be either filename or lti object');
  end

  nh = size(Ah,1);
  mh = size(Bh,2);
  ph = size(Ch,1);

  m1 = mh - m0 - m2;
  ph1 = ph - p0;

  %% 重み関数

  if isempty(W)
      robust_stabilization = 1;
      W = ss(zeros(ph1,ph1));
  else
      robust_stabilization = 0;
  end

  if isstr(W)
    [Aw,Bw,Cw,Dw] = linmod(W);
  elseif isa(W,'lti')
    [Aw,Bw,Cw,Dw] = ssdata(W);
  else
    error('csfdlqhinflmi', ['W must be either filename, lti object ' ...
                        'or empty']);
  end

  nw = size(Aw,1);
  mw = size(Bw,2);
  p1 = size(Cw,1);

  if (ph1 ~= mw) 
    error('csfdlqhinflmi', 'size mimatch between Gh and W');
  end

  %% Gh の係数の分割

  Bh0 = Bh(:,1:m0);
  Bh1 = Bh(:,m0+1:m0+m1);
  Bh2 = Bh(:,m0+m1+1:end);

  Ch0 = Ch(1:p0,:);
  Ch1 = Ch(p0+1:end,:);

  Dh00 = Dh(1:p0,1:m0);
  Dh01 = Dh(1:p0,m0+1:m0+m1);
  Dh02 = Dh(1:p0,m0+m1+1:end);

  Dh12 = Dh(p0+1:end,m0+m1+1:end);

  if Dh(p0+1:end,1:m0+m1) ~= zeros(ph1,m0+m1)
    error('csfdlqhinflmi', 'Dh00 and Dh01 must be zero');
  end

  %% Gh, W の併合系の計算

  A = [ Aw, Bw*Ch1; zeros(nh,nw), Ah ];

  B0 = [ zeros(nw,m0); Bh0 ];
  B1 = [ zeros(nw,m1); Bh1 ];
  B2 = [ Bw*Dh12; Bh2 ];

  C0 = [ zeros(p0,nw), Ch0 ];
  C1 = [ Cw, Dw*Ch1 ];

  D00 = Dh00;           D01 = Dh01;           D02 = Dh02;
  D10 = zeros(p1,m0);   D11 = zeros(p1,m1);   D12 = Dw*Dh12;

  bound = max(max(abs([A, B0, B1, B2; C0, D00, D01, D02; C1, D10, D11, D12])));

  %% コントローラー計算

  % LMI の変数の設定

  Pp = {};
  Pm = {};
  Ak = {};
  Bk = {};
  Ck = {};
  Dk = {};
  
  X = sdpvar(nh+nw,nh+nw,'symmetric');
  CC = sdpvar(m2,nh+nw,'full');
  gamma = sdpvar(1,1);

  for k = 1:Npm0;

    p0i{k} = deltaports(k,1);
    m0i{k} = deltaports(k,2);

    if isRepeatedScalarScaling(k)
      p_var = sdpvar(1,1,'symmetric');
      pp{k} = p_var*eye(p0i{k});
      pm{k} = p_var*eye(m0i{k});
    else
      if (p0i{k} ~= m0i{k}) 
	error('csfdlqhinflmi', 'delta port number configurations are not consistent');
      end
      p_var = sdpvar(p0i{k},p0i{k},'symmetric'); 
      pp{k} = p_var;
      pm{k} = p_var;
    end

    Pp = blkdiag(Pp, pp{k});
    Pm = blkdiag(Pm, pm{k});

  end

  % この位置で定義するとマトモな解が得られない
  % YALMIP のバグ？
  % X = sdpvar(nh+nw,nh+nw,'symmetric');
  % CC = sdpvar(m2,nh+nw,'full');
  % gamma = sdpvar(1,1);

  % 制約条件の設定

  delta = bound*1e-7;

  if robust_stabilization

    N1 = nh+nw+p0;

    HB =  [
      B0;
      D00;
    ];

    H1 = [
      (A*X+B2*CC)+(A*X+B2*CC)', (C0*X+D02*CC)';
      C0*X+D02*CC, -Pp;
    ] + HB * Pm * HB' - gamma*eye(N1,N1);

    H2 = X - delta*eye(nh+nw);

    H3 = gamma + bound;

    C = [ H1 < 0, H2 > 0, H3 > 0 ];

  else

    N1 = nh+nw+p0+p1+m1;

    HB =  [
      B0;
      D00;
      D10;
      zeros(m1,m0)
    ];

    H1 = [
      (A*X+B2*CC)+(A*X+B2*CC)', (C0*X+D02*CC)', (C1*X+D12*CC)', B1;
      C0*X+D02*CC, -Pp, zeros(p0,p1), D01;
      C1*X+D12*CC, zeros(p1,p0), -gamma*eye(p1), D11;
      B1', D01', D11', -gamma*eye(m1)
    ] + HB * Pm * HB' + delta*eye(N1,N1);

    H2 = X - delta*eye(nh+nw);

    C = [ H1 < 0, H2 > 0 ];

  end

  % 最適化

  solvesdp(C,gamma);

  gopt = double(gamma);
  Fh = double(CC)/double(X);

  Fh1 = Fh(:,1:nw);
  Fh2 = Fh(:,nw+1:end);

  K = ss(Aw+Bw*Dh12*Fh1,Bw*Ch1+Bw*Dh12*Fh2,Fh1,Fh2);

  scaling_output = double(Pp);
  scaling_input = double(Pm);

  Gh_full = ss(Ah, Bh, [Ch; eye(nh)], [Dh; zeros(nh,mh)]);

  clsys = lft(Gh_full,K);
