%GPC2SS Builds state-space model equivalent to GPC model.
%
% [A,Bu,Bv,C,L,evalues] = gpc2ss(Apoly,Bpoly,Cpoly,verbose)
%
% Input arguments:
%  Apoly, Bpoly, Cpoly: A,B,C polynomials, or polynomial matrices,
%      of GPC model. If 3-D arrays, these are interpreted as polynomial
%      matrices: A(1/z)=I-A1*z^{-1}-A2*z^{-2}-...-Anz^{-n} *Note signs*
%                B(1/z)=B1*z^{-1}+B2*z^{-2}+...+Bp*z^{-p}
%                C(1/z)=C1*z^{-1}+C2*z^{-2}+...+Cq*z^{-q}
%      as follows: Apoly(i,j,k) = Ak(i,j), etc.
%      If Apoly,Bpoly,Cpoly are vectors, they are interpreted as
%      scalar polynomials as follows: Apoly(k) = Ak, etc.
%      NB: SIMO and MISO models must be entered as 3-D arrays.
%
%  verbose: 1 if output to terminal wanted, 0 otherwise. (Default: 1)
%
% Output arguments:
%  A,Bu,Bv,C: Matrices of equivalent state-space model,
%             x(k+1)=A*x(k)+Bu*u(k)+Bv*v(k),  y(k)=C*x(k).
%          L: Observer gain matrix of equivalent model.
%    evalues: Eigenvalues of observer: eig(A(eye-L*C)).
%
% Refs: "Predictive Control with Constraints", J.M.Maciejowski,
%       Prentice-Hall, 2001, ISBN: 0-201-39823-0, section 4.2.5.
%       "Exact state-space correspondence to GPC: observer
%       eigenvalue locations", J.M.Maciejowski and S.N.Redhead,
%       Proc. Asian Control Conf, Singapore, September 2002.
%
% See also: GPC2MOD

% J.M.Maciejowski, 16 September 2002.
% Copyright (C) 2002, All Rights reserved.

function [A,Bu,Bv,C,L,evalues] = gpc2ss(Apoly,Bpoly,Cpoly,verbose)

if nargin==0,
    disp('Usage:  [A,Bu,Bv,C,L,evalues] = gpc2ss(Apoly,Bpoly,Cpoly,verbose)')
    return
end

if nargin<4,
    verbose = 1;
end

% Extract dimensions and degrees from input data:
if ndims(Apoly)==3, % MIMO model, or possibly MISO or SIMO,
    if ndims(Bpoly) ~= 3 | ndims(Cpoly) ~= 3,
        error('Apoly,Bpoly,Cpoly must all be vectors or 3-D arrays')
    end
    [Arows,Acols,n] = size(Apoly);  % n is degree of A polynomial
    if Arows ~= Acols,
        error('The matrices in the A polynomial must be square.')
    end
    m = Arows; % Number of outputs.
    [Brows,ell,p] = size(Bpoly); % ell inputs, deg(Boly) = p
    if Brows ~= m,
        disp('Matrices in B polynomial must have same number of rows')
        error('as those in the A polynomial.')
    end
    [Crows,Ccols,q] = size(Cpoly); % deg(Cpoly) = q
    if Crows ~= m | Ccols ~= m,
        disp('Matrices in the C polynomial must be square and')
        error('of the same dimensions as those in the A polynomial')
    end
elseif length(find(size(Apoly)>1))==1,  % SISO model, % Is there a better way?
    if length(find(size(Bpoly)>1))~=1 | length(find(size(Cpoly)>1))~=1,
        error('Apoly,Bpoly,Cpoly must all be vectors or 3-D arrays')
    end
    m = 1; ell = 1; % Numbers of outputs and inputs
    n = length(Apoly); % Degree of Apoly
    p = length(Bpoly); % Degree of Bpoly
    q = length(Cpoly); % Degree of Cpoly
else
    error('Apoly,Bpoly,Cpoly must all be vectors or 3-D arrays.')
end

% Feedback to user:
if verbose,
    disp('GPC2SS has found:')
    disp(['Number of inputs:  ',int2str(ell)])
    disp(['Number of outputs: ',int2str(m)])
    disp(['Degree of A polynomial: ',int2str(n)])
    disp(['Degree of B polynomial: ',int2str(p)])
    disp(['Degree of C polynomial: ',int2str(q)])
end

ns = (n+q)*m+p*ell;  % Number of states
if verbose,
    disp(' ')
    disp(['Length of state vector: ',int2str(ns)])
end

% Build A matrix of state-space model:
A = zeros(ns,ns);

if ndims(Apoly)==3,  % (MIMO, MISO, or SIMO)
     % Submatrix A_{11}:
     A11 = zeros(m*(n+1),m*(n+1));
     A11(1:m,1:m) = eye(m)+Apoly(:,:,1);
     A11(m+1:2*m,1:m) = eye(m);
     for k=2:n,
         A11(1:m,(k-1)*m+1:k*m) = Apoly(:,:,k)-Apoly(:,:,k-1);
         A11(k*m+1:(k+1)*m,(k-1)*m+1:k*m) = eye(m);
     end
     A11(1:m,n*m+1:(n+1)*m) = -Apoly(:,:,n);
     % End of building A11.
     
     % Submatrix A_{12}:
     A12 = zeros(m*(n+1),p*ell);
     for k=1:p-1,
         A12(1:m,(k-1)*ell+1:k*ell) = Bpoly(:,:,k+1)-Bpoly(:,:,k);
     end
     A12(1:m,(p-1)*ell+1:p*ell) = -Bpoly(:,:,p);
     %End of building A12.
     
     % Submatrix A_{13}:
     A13 = zeros(m*(n+1),(q-1)*m);
     for k=2:q,
         A13(1:m,(k-2)*m+1:(k-1)*m) = Cpoly(:,:,k);
     end
     %End of building A13.
     
	% First block row of A:
	A(1:m*(n+1),:) = [A11,A12,A13];
	% Insert A_{22}:
	for k=1:p-1,
        A22rowrange = m*(n+1)+[k*ell+1:(k+1)*ell];
        A22colrange = m*(n+1)+[(k-1)*ell+1:k*ell];
        A(A22rowrange,A22colrange) = eye(ell);
	end
	% Insert A_{33}:
	for k=2:q-1,   % This only needed if q > 2.
        A33rowrange = m*(n+1)+p*ell+[(k-1)*m+1:k*m];
        A33colrange = m*(n+1)+p*ell+[(k-2)*m+1:(k-1)*m];
        A(A33rowrange,A33colrange) = eye(m);
	end
	
elseif length(find(size(Apoly)>1))==1,   % SISO
    
    % Build A_{11}:
    A11 = zeros(n+1,n+1);
    A11(1,1) = 1+Apoly(1);
    A11(2,1) = 1;
    for k=2:n,
        A11(1,k) = Apoly(k)-Apoly(k-1);
        A11(k+1,k) = 1;
    end
    A11(1,n+1) = -Apoly(n);
    % End of building A11.
    
    % Build A_{12}:
    A12 = zeros(n+1,p);
    for k=1:p-1,
        A12(1,k) = Bpoly(k+1)-Bpoly(k);
    end
    A12(1,p) = -Bpoly(p);
    % End of building A12.
    
    % Build A_{13}:
    A13 = zeros(n+1,q-1);
    for k=1:q-1,
        A13(1,k) = Cpoly(k+1);
    end
    % End of building A13.
    
    A(1:n+1,:) = [A11,A12,A13]; % First n+1 rows of A.
    for k=n+3:n+p+1,
        A(k,k-1) = 1;  % Inserts A_{22}.
    end
    for k=n+p+3:ns,
        A(k,k-1) = 1;  % Inserts A_{33}.
    end
    
else
    error('GPC2SS Apoly error: Should never get here!')
end
% End of building matrix A.

% Build matrices Bu and Bv:
Bu = zeros(ns,ell);
Bv = zeros(ns,m);
if ndims(Bpoly)==3,
    Bu(1:m,:) = Bpoly(:,:,1);
    Bv(1:m,:) = Cpoly(:,:,1);
elseif length(find(size(Bpoly)>1))==1,
    Bu(1,1) = Bpoly(1);
    Bv(1,1) = Cpoly(1);
else
    error('GPC2SS Bpoly error: Should never get here!')
end
Bu(m*(n+1)+1:m*(n+1)+ell,:) = eye(ell);
Bv(m*(n+1)+ell*p+1:m*(n+1)+ell*p+m,:) = eye(m);
% End of building matrices Bu and Bv.

if verbose,
    if Bv(1:m,:)~=eye(m),
        disp('Warning: The first coefficient in the C(1/z) polynomial is')
        disp('         not 1 or an identity matrix, which is unusual.')
    end
end

% Build matrix C:
C = [eye(m),zeros(m,ns-m)];

% Build observer gain matrix:
L = zeros(ns,m);
L(1:m,:) = eye(m);
if ndims(Cpoly)==3,
    L(m*(n+1)+ell*p+[1:m],:) = Cpoly(:,:,1);
elseif length(find(size(Cpoly)>1))==1,
    L(n+p+2,:) = Cpoly(1);
else
    error('GPC2SS Cpoly error: Should never get here!')
end
% End of building L.

% Observer eigenvalues:
evalues = eig(A*(eye(ns)-L*C));
nzeros = n*m+p*ell;
if verbose,
    disp(' ')
    disp(['Expected number of zero eigenvalues: ',int2str(nzeros)])
    disp(['Remaining ',int2str(ns-nzeros),' eigenvalues should be roots of det(C(1/z))'])
    disp([' (of which ',int2str(m),' also zero).'])
    disp('Observer eigenvalues are:')
    disp(evalues')
end