function [F0,F1] = ptopdes2(A,B,C,D,S)
% PTOPDES2 - Robust stabilization by PI feedback of a polytope of
%           second-order systems
%
% Given a cell array A = {A1(s),A2(s)..} of quadratic polynomial
% matrices, a constant input matrix B, a constant output matrix C,
% and a quadratic polynomial matrix D(s), the instruction
%
%   [F0,F1] = PTOPDES2(A,B,C,D)
%
% attemps to find two constant feedback matrices F0 and F1 such
% that the second order system A(s)*x = B*u, y = C*x affected by
% polytopic uncertainty (system matrix A(s) belongs to the polytope
% generated by vertices Ai(s)) is robustly stabilized by the proportional
% and derivative feedback u = (F0+F1*s)*y
%
% Polynomial matrix D(s) corresponds to a central (or nominal) closed-loop
% polynomial matrix, for example D(s) = (Ai(s)+B*F0*C+B*F1*C*s) 
% obtained by stabilizing one vertex plant Ai(s)
%
% By default, B and C are identity
%
% With the optional input argument
%
%   [F0,F1] = PTOPDES2(A,B,C,D,S)
% 
% one can specify an Hermitian 2x2 stability region matrix S such that
% each closed-loop pole Z must satisfy the quadratic inequality
% S(1,1)+S(1,2)*Z+S(2,1)*Z'+S(2,2)*Z*Z' < 0. Standard choices are
% S=[0 1;1 0] (left half-plane) and S=[-1 0;0 1] (unit disk).
% The default value of S is determined by the variable symbol of input
% polynomials A and D.

% Written by Didier Henrion, May 15, 2002.
% Last modified by Didier Henrion, May 24, 2002.
% Copyright 2002 by PolyX, Ltd.  
  
global PGLOBAL;

eval('PGLOBAL.FORMAT;',...
     'error(''Use PINIT to initialize the Polynomial Toolbox.'');');

if nargin < 4,
  error('Invalid number of input arguments');
end;

verbose = strcmp(PGLOBAL.VERBOSE, 'yes');
tol = PGLOBAL.ZEROING;

% Check whether SeDuMi and its LMI interface are installed
if exist('sedumi') ~= 2,
  error('SeDuMi is not properly installed.');
end;
if exist('@sdmpb/sdmpb') ~= 2,
  error('LMI interface is not properly installed.');
end;

D = pol(D);
n = size(D,1);
if deg(D) > 2, error('Invalid degree of fourth input polynomial'); end;
if size(D,2) ~= n, error('Invalid size of fourth input polynomial'); end;

symb = symbol(D);
if isa(A,'double') | isa(A,'pol'), A = {A}; end;
N = length(A);
for i = 1:N,
 newsymb = symbol(A{i});
 if isempty(symb), newsymb = symb;
 elseif ~isempty(newsymb) & ~strcmp(symb,newsymb),
  error('Inconsistent variable symbols in input arguments');
 end;
 if deg(A{i}) > 2, error('Invalid degree of polynomial polytope'); end;
end;
if size(A{1}) ~= [n n], error('Invalid size of first input polynomial'); end;

if isempty(B), B = eye(n); end;
if size(B,1) ~= n, error('Invalid size of second input matrix'); end;
m = size(B,2);

if isempty(C), C = eye(n); end;
if size(C,2) ~= n, error('Invalid size of third input matrix'); end;
p = size(C,1);

if nargin < 5, S = []; end; 
if isempty(S),
 switch symb,
  case {'z^-1','d'}
   S = [1 0;0 -1];
  case {'z','q'}
   S = [-1 0;0 1];
  otherwise
   S = [0 1;1 0]; 
 end;
end;
if ~isa(S,'cell'),
 S = {S};
end;
nS = length(S);
for i = 1:nS,
 if ~isa(S{i},'double') | (size(S{i}) ~= [2 2]),
  error('Invalid stability region');
 end;
end;

lmi = sdmpb;
[lmi, varF0] = sdmvar(lmi, m, p, 'F0');
[lmi, varF1] = sdmvar(lmi, m, p, 'F1');
for j = 1:nS,
 for i = 1:N,
  [lmi, varP{i,j}] = sdmvar(lmi, 2*n, 's');
 end;
end;
[lmi, varnorm] = sdmvar(lmi, 1, 1);

coefD = zeros(n,3*n);
for d = 0:2,
 coefD(:,1+d*n:(d+1)*n) = D{d};
end;
for j = 1:nS,
 for i = 1:N,
  coefN = zeros(n,3*n);
  for d = 0:2,
   coefN(:,1+d*n:(d+1)*n) = A{i}{d};
  end;
  name = ['Sommet ' int2str(i) ' Region ' int2str(j)];
  [lmi, index] = sdmlmi(lmi, 3*n, name);
  lmi = sdmineq(lmi, -index, 0, coefD', coefN);
  lmi = sdmineq(lmi, -index, varF1, coefD'*B, [zeros(p,n) C zeros(p,n)]);
  lmi = sdmineq(lmi, -index, varF0, coefD'*B, [C zeros(p,2*n)]);
  Q = kron([1 0 0;0 1 0;0 1 0;0 0 1],eye(n));
  lmi = sdmineq(lmi, index, varP{i,j}, Q', [], S{j});
 end;
end;


% minimization of norm of feedback matrices
[lmi, index] = sdmlmi(lmi, p+2*m, 'Min norm');
left = [eye(p); zeros(2*m,p)];
lmi = sdmineq(lmi, -index, varnorm, left);
left = [zeros(p,m); eye(m); zeros(m)];
right = [eye(p) zeros(p,2*m)];
lmi = sdmineq(lmi, -index, varF0, left, right);
left = [zeros(p+m,m); eye(m)];
lmi = sdmineq(lmi, -index, varF1, left, right);
term = [zeros(p) zeros(p,2*m); zeros(2*m,p) eye(2*m)];
lmi = sdmineq(lmi, -index, 0, term, term/2);
if verbose,
  disp('Minimize norm of feedback matrices');
end;

lmi = sdmobj(lmi, varnorm, -1, 1);

%lmi = lmi - 1e-3;

pars.fid = verbose;
lmi = sdmsol(lmi,pars,1e8);

feas = get(lmi, 'feas');

% Retrieve controller

if (feas == 0) | (feas == 1),
  
% P = cell(N,nS);
% for i = 1:N,
%  for j = 1:nS,
%   P{i,j} = lmi(varP{i,j});
%  end;
% end;
 F0 = lmi(varF0);
 F1 = lmi(varF1);

 if verbose,
   disp('A robustly stabilizing controller was found');
 end;

else

 F0 = []; F1 = [];
 if verbose,
   disp('No controller was found');
 end;
 
end;

