function [A,B,C,D] = idModelTypeConverterTF2SS(num,den)
% idModelTypeConverterTF2SS Convert a MISO transfer function described with
% variable z^-1 to state-space representation
%
%    [A,B,C,D] = idModelTypeConverterTF2SS(num,den)
%
%    Inputs:
%      num  - row vector or matrix, numerators of the tf. If specified
%             as a matrix, the numerator coefficients are read from each
%             row
%      den  - row vector, denominator of the tf
%
%    Outputs: 
%      A, B, C, D - State-space matrices
%
%    This is a stripped down, codegen supporting version of 
%    idpack.polydata.armaxss

%   Copyright 2013-2015 The MathWorks, Inc.

%#codegen

% Pre-processing: Calculate denLength, numLength, Ns (# of states)
[denLength,numLength,nStates] = localGetSSMatrixDimensions(num,den);

% Check if den(1)==1. If no, scale both numerator and denominator
if den(1)==0
    den(1) = eps;
end    
if den(1)~=1
    num = num / den(1);
    den = den / den(1);
end

% Convert Ay = Bu into SS form using MFD: Y(z^-1) = inv(A(z^-1))*B(z^-1) U(z^-1)
% D matrix
D = num(:,1).';
% A matrix
if nStates>0
    A = diag(ones(1,nStates-1,'like',den),1);
else
    A = cast(0,'like',den);
end
for kk=1:nStates
    if denLength>kk
        A(kk,1) = -den(kk+1);
    else
        A(kk,1) = cast(0,'like',den);
    end
end
% B matrix
B = zeros(max(nStates,1),size(num,1),'like',den);
for kk=1:nStates
    if numLength>kk
        B(kk,:) = num(:,kk+1).' + A(kk,1)*D;
    else
        B(kk,:) = A(kk,1)*D;
    end
end
% C matrix
C = zeros(1,max(nStates,1),'like',den);
C(1) = 1;
end

function [denLength,numLength,nStates] = localGetSSMatrixDimensions(num,den)
% getSSMatrixDimensions get the number of states and inputs from the
% numerator and denominator polynomials
%
% This function assumes that the trailing zeros in num and den are already
% trimmed
denLength = uint32(numel(den));
numLength = uint32(size(num,2));
if denLength>=numLength
    nStates = denLength-1;
else
    nStates = uint32(numLength-1);
end
end