function [Ad,Bd,Cd,Dd,Kd,X0,idd,NVd,ICmap,InputGroup] = ...
   utSSFastC2D(A,B,C,D,K,X0,id,iod,NV,Ts,InterSample,sys,c2dopt,NoiseAdjust)
%UTSSFASTC2D Discretization for state-space systems.
%  Works directly on system matrices (hence fast) for delay-free systems.
%
%  Input arguments:
%  A,B,C,D,K: Continuous-time state-space matrices (double values).
%  X0: Initial states of the continuous-time model. This is not
%      discretized, but is replicated in case of hybrid sampling.
%  id: Column vector of input delays 
%  iod: Matrix of I/O delays. Nonempty for process models only. They must
%       be absorbed into input delays and system matrices.
%  NV: Noise variance
%  Ts: Sample time
%  InterSample: Input intersample values (1-by-Nu cell array). Currently
%               only 'zoh' and 'foh' are supported. 'bl' is treated as 'foh'.
%  
%  sys: A template ltipack.ssdata object; passed as an input argument to
%       avoid construction on the fly. (A,B,C,D) are packed into it when
%       discretization of delay-containing systems has to be performed.
%
%  c2dopt: c2dOptions object.
%
%  NoiseAdjust: Apply Kalman correction for K in case predictor is
%               unstable. Default: TRUE.
%
%  Note: Even when NV=0, noise model is discretized.

% Copyright 2010-2024 The MathWorks, Inc.
if nargin<14, NoiseAdjust = true; end
NoiseComp = false;
[Ny,Nu] = size(D);

% Minimize IO delay, if applicable
if any(iod(:))
    id2 = min(iod,[],1);
    id = id+id2;
    iod = iod-repmat(id2,[Ny 1]);
end
if ~any(iod(:)), iod = []; end

if norm(K,1)>0 
   NoiseComp = true;
   %L = idpack.cholcov(NV,'lower');
   B = [B, K]; 
   D = [D, eye(Ny)];
   id = [id; zeros(Ny,1)];
   if ~isempty(iod), iod = [iod, zeros(Ny)]; end
   NoiseInterSample = {'zoh'};
   if size(B,2)>Ny
      NoiseInterSample = InterSample(1);
   end
   InterSample = [InterSample; NoiseInterSample(ones(Ny,1))];
else
   sys = getsubsys(sys,1:Ny,1:Nu);
end

if Nu==1 && strcmp(InterSample(1),'zoh')
   UniqueInters = InterSample(1);
else
   UniqueInters = unique(InterSample);
end
if isempty(UniqueInters), UniqueInters = {'zoh'}; end   
if isscalar(UniqueInters)
   % Uniform intersample. Discretize once.
   [Ad,Bd,Cd,Dd,idd,ICmap] = localDiscretizeOnce(UniqueInters{1},A,B,C,D,id,...
      iod,Ts,sys,c2dopt);
   InputGroup = {1};
else
   % Different intersample values for different inputs.
   InputGroup = cell(1,length(UniqueInters));
   for kint = 1:length(UniqueInters)
      Ind = find(strcmp(UniqueInters{kint},InterSample(:,1)));
      Method = UniqueInters(kint);  
      if ~isempty(iod)
         iod_ = iod(:,Ind);
      else
         iod_ = [];
      end
      [Ads,Bds,Cds,Dds,idds,ICmaps] = localDiscretizeOnce(Method,A,B(:,Ind),...
         C,D(:,Ind),id(Ind),iod_,Ts,sys,c2dopt);
      if kint>1
         %sysd = iocat(2,sysd,subsysd);
         Ad = blkdiag(Ad,Ads); Bd = blkdiag(Bd, Bds); Cd = [Cd, Cds]; %#ok<*AGROW>
         Dd = [Dd, Dds]; ICmap = blkdiag(ICmap, ICmaps);
         idd = [idd; idds];
      else
         Ad = Ads; Bd = Bds; Cd = Cds; Dd = Dds; ICmap = ICmaps; idd = idds;
      end
      InputGroup{kint} = Ind;
   end
   
   % Sort inputs. Does not change state orders.
   [~,InputOrder] = sort(cat(1,InputGroup{:}));
   Bd = Bd(:,InputOrder); Dd = Dd(:,InputOrder);
   X0 = repmat(X0,[length(UniqueInters),1]);
end

% Split measured and noise components.
if NoiseComp
   [Bd,Dd,Kd,NV2] = idpack.ssdata.utSplitNoiseMeas(Ad,Bd,Cd,Dd,Ts,NoiseAdjust);
   L = idpack.cholcov(NV2);
   NVd = L*(NV/Ts)*L';
   idd = idd(1:end-Ny);
   
   %  When noise model exists, ICMap contains nu+ny columns, the last ny
   %  columns corresponding to e0, where e0 is first sample of innovations
   %  with noise variance sys.NoiseVariance. Assuming zero mean for e0, we
   %  remove last ny columns of ICMap.  
   ICmap = ICmap(:,1:end-Ny); 
else
   NVd = NV/Ts;
   Kd = zeros(size(Ad,1),Ny);
end

%--------------------------------------------------------------------------
function [Ad,Bd,Cd,Dd,Inpd,ICmap] = localDiscretizeOnce(Method,varargin)
% Discretization for uniform intersample case.
% Syntax: 
%  localDiscretizeOnce(Method,A,B,C,D,InputDelay,iod,Ts,TemplateSys,c2dOptions)

if strcmpi(Method,'zoh')
   [Ad,Bd,Cd,Dd,Inpd,ICmap] = localc2dzoh(varargin{:});
else
   % REVISIT: Handle 'bl' differently.
   [Ad,Bd,Cd,Dd,Inpd,ICmap] = localc2dfoh(varargin{:});
end

%--------------------------------------------------------------------------
function [E,F,G,H,idd,ICmap] = localc2dzoh(a,b,c,d,id,iod,Ts,sys,opt)
% ZOH discretization.
% idd: integral part of input delay

idd = id;
[~,nu] = size(d); nx = size(a,1);
if ~any(id) && isempty(iod)
   s = utScaledExpm([[a b]*Ts; zeros(nu,nx+nu)]);
   E = s(1:nx,1:nx);
   F = s(1:nx,nx+1:nx+nu);
   G = c;
   H = d;
   
   nxtotal = size(E,1);
   ICmap = zeros(nxtotal,nx+nu);
   ICmap(1:nxtotal+1:nxtotal*nx) = 1;
else
   opt.Method = 'zoh';
   [E,F,G,H,idd,ICmap] = localSystemC2D(sys,a,b,c,d,Ts,id,iod,opt);
end

%--------------------------------------------------------------------------
function [E,F,G,H,idd,ICmap] = localc2dfoh(a,b,c,d,id,iod,Ts,sys,opt)
% FOH discretization.

nu = size(d,2); nx = size(a,1);
idd = id;
if ~any(id) && isempty(iod)
   M = [a , b , zeros(nx,nu)  ; ...
      zeros(nu,nx+nu)  eye(nu)/Ts ; ...
      zeros(nu,nx+2*nu)];
   s = utScaledExpm(M*Ts);
   F1 = s(1:nx,nx+1:nx+nu);
   F2 = s(1:nx,nx+nu+1:nx+2*nu);
   
   % Discrete-time matrices
   E = s(1:nx,1:nx);
   F = F1 + E*F2 - F2;
   G = c;
   H = d + c*F2;
   
   % Continuous to discrete initial condition map
   ICmap = [eye(nx) , -F2];
else
   opt.Method = 'foh';
   [E,F,G,H,idd,ICmap] = localSystemC2D(sys,a,b,c,d,Ts,id,iod,opt);
end

%--------------------------------------------------------------------------
function [E,F,G,H,idd,ICmap] = localSystemC2D(sys,a,b,c,d,Ts,id,iod,opt)
% Discretize using ltipack.ssdata/c2d.

sys.a = a; sys.b = b; sys.c = c; sys.d = d; sys.Delay.Input = id;
if ~isempty(iod), sys = setIODelay(sys, iod); end
[sys,ICmap] = c2d(sys,Ts,opt);
if ~isempty(iod)
   [sys, ICmap] = elimDelay(sys,[],sys.Delay.Output,sys.Delay.Internal,ICmap);
end
[E,F,G,H] = getABCD(sys);
idd = sys.Delay.Input;
