%GPC2MOD Builds model in MOD format from GPC model (for MPC Toolbox).
%
% [mpcmod,minfo,Kest] = gpc2mod(Apoly,Bpoly,Cpoly,Ts,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.
%
%       Ts: Sampling time, for passing on to MPC Toolbox. (Default: 1)
%  verbose: 1 if output to terminal wanted, 0 otherwise. (Default: 1)
%
% Output arguments:
%  mpcmod: State-space model in MPC Toolbox's MOD format.
%   minfo: Information vector for use with functions scmpc, etc.
%    Kest: Observer gain for use in functions scmpc, etc.
%
% 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: GPC2SS

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

function [mpcmod,minfo,Kest] = gpc2mod(Apoly,Bpoly,Cpoly,Ts,verbose)

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

if nargin<5,
    verbose = 1;
end
if nargin<4,
    Ts = 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('GPC2MOD 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)])
    disp(['Sampling period: ',num2str(Ts)])
end

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

[A,Bu,Bv,C,L] = gpc2ss(Apoly,Bpoly,Cpoly,0);

minfo = [Ts,ns,ell,0,m,m,0];

mpcmod = ss2mod(A,[Bu,Bv],C,zeros(m,ell+m),minfo);

Kest = [L;eye(m)];