function [A, B, C, D, K, X0, TD, ULEV, XNR] = procmod(par, ~, Type, DMOrder)
% Process model parameterization.
%
% Input arguments:
%  par: column vector of model parameters.
%  Ts: requested sample time of output matrices (ignored; always return CT
%      values)
%  Type: model structure type (NY-by-NU cell array)
%  ISP: Intersample behavior required for sampling (NY-by-1 cell)
%  DMOrder: Order of disturbance model (NY-by-1 vector). Disturbance model
%           is a parameter even if it represents 'none' (0:th order model)
%           or 'fixed' (0, 1 or 2 order but fixed).
%
% Output arguments:
%  A,B,C,D,K,X0: state space parameters
%  TD: I/O delays
%  ULEV: Input offset levels. Note that parameter vector always contains
%        input level values (even if they are not estimated)
%  XNR: Number of states for each I/O pair (NY-by-NU matrix)

%  Copyright 1993-2020 The MathWorks, Inc.

% NOTES:
% 1. Parameter order is:
%   [Kp ; 1/Tp1 ; 1/Tp2 ; 1/Tw ; Zeta; 1/Tp3 ; Td ; Tz] for each I/O
%    + [Cpoly; Dpoly]  for each output
%    + u0
%
% 2. Model is prameterized using pole/zero locations to avoid drop in
%    system dimension for zero values of time constants (in contrast, poles
%    are never pushed to +/-Inf by optimizer). Note that original "Kp" is
%    retained as a parameter as is. Hence the dc gain of model is normalized
%    by time constants (e.g., Kp*Tz/Tp1/Tp2/Tp3 for a 3 pole model)
%
% 3. This ODE files returns zeros for initial states. Hence init = 'model'
%    is same as init = 'zero' or init = 'fixed' with sys.X0 = 0;

A = []; B = []; C = []; D = []; K = []; X0 = []; XNR = [];  
[ny, nu] = size(Type);
L = 0; % Offset tracker
TD = zeros(ny,nu);

for ky = 1:ny
   nc = DMOrder(ky);
   Bky = []; Cky = []; Dky = []; Xnrky = [];
   for ku = 1:nu
      Type_ = Type{ky,ku};
      np = str2double(Type_(2)) + any(Type_=='Z') + any(Type_=='D')+1;
      [a,b,c,d,x0,TD(ky,ku),xnr] = localSISOData(par(L+1:L+np),Type_);
      L = L+np;
      
      A = blkdiag(A,a); 
      Bky = blkdiag(Bky,b);
      Cky = [Cky, c]; 
      Dky = [Dky, d]; 
      X0 = [X0; x0]; %#ok<*AGROW>
      Xnrky = [Xnrky, xnr];
   end % ku
   
   % Add noise model ("k")
   Kky = zeros(size(Bky,1),1);
   if nc>0
      % noise parameters: [num(2:end), den(2:end)]
      nnum = par(L+1:L+nc).';
      nden = par(L+nc+1:L+2*nc).';
      L = L+2*nc;
      
      if length(nnum)==1
         nnum = abs(nnum);
         nden = abs(nden);
         an = -nden;
         bn = nnum-nden;         
      else
         nden = fstab([1 nden],0,0,false);
         nnum = fstab([1 nnum],0,0,false);
         an = [-nden(2) 1; -nden(3) 0];
         bn = [nnum(2)-nden(2); nnum(3)-nden(3)];
      end
      A = blkdiag(A,an);
      
      Kky = [Kky; bn];      
      Cky = [Cky, 1, zeros(1,nc-1)];
      Bky = [Bky; zeros(nc,nu)];
      X0 = [X0; zeros(nc,1)];
   end
   Xnrky = [Xnrky, 2*nc];
   
   B = [B; Bky];
   C = blkdiag(C, Cky);
   D = [D; Dky];
   K = blkdiag(K, Kky);
   XNR = [XNR; Xnrky];
end %ky

ULEV = par(L+1:end);
if numel(ULEV)>nu
   % multi-experiment data
   Ne = round(numel(ULEV)/nu);
   ULEV = reshape(ULEV,[nu, Ne]);
end

%--------------------------------------------------------------------------
function [a,b,c,d,x0,Td,xnr] = localSISOData(p,Type)
% Compute state-space data for one I/O pair.
% Parameter order is:
%  [Kp ; 1/Tp1 ; 1/Tp2 ; 1/Tw ; Zeta ; 1/Tp3 ; Td ; Tz]
Td = 0; 
num = p(1); 
np = str2double(Type(2)); L = 1;
if np>0
   if ~any(Type=='U')
      iTp1 = p(2); L = L+1;
      if np>1, iTp2 = p(3); L = L+1; end
   else
      iTw = p(2);
      Zeta = p(3);
      L = L+2;
   end
   if np>2, iTp3 = p(4); L = L+1; end
end

if any(Type=='D')
   Td = p(L+1);
   L = L+1;
end

if any(Type=='Z')
   Tz = p(L+1);
   L = L+1;
end

den = 1;
if any(Type=='U')
   den = [1, 2*Zeta*iTw, iTw^2];
   num = num*iTw^2;
elseif np>0
   den = [1, iTp1];
   num = num*iTp1;
   if np>1
      den = conv(den,[1, iTp2]);
      num = num*iTp2;
   end
end

if np>2
   den = conv(den,[1, iTp3]);
   num = num*iTp3;
end

if any(Type=='I')
   den = [den, 0];
end

if any(Type=='Z')
   num = num*[Tz, 1];
end

% zero pad numerator
num2 = zeros(size(den));
num2(end-length(num)+1:end) = num;
num = num2;

nx = length(den)-1;
a = [-den(2:end); [eye(nx-1),zeros(nx-1,1)]];

if nx>0
   b = [1; zeros(nx-1,1)];
else
   b = zeros(0,1);
end

d = num(1);
c = num(2:end)-den(2:end)*num(1);
xnr = size(a,1);
x0 = zeros(xnr,1);
