function [Dd,ICMap] = utDiscretizeTRBDF2(Dc,Ts,options)
% Discretize SPARSS models using Tustin method.

%   Author(s): P. Gahinet
%   Copyright 2020 The MathWorks, Inc.
if hasOffset(Dc)
   error(message('Control:transformation:c2d27'))
end
% Get dimensions
a = Dc.a;   e = Dc.e;  b = Dc.b;  c = Dc.c;  d = Dc.d;
nx = size(a,1);
EXPLICIT = isempty(e);
if EXPLICIT
   e = speye(nx);
end
nfd  = numel(Dc.Delay.Internal);   % nfd=nw=nz
ny = size(d,1) - nfd;
nu = size(d,2) - nfd;

% Get Fractional Delays options
nf = options.ThiranOrder;
roundOK = (nf == 0);
delayOK = strcmpi(options.DelayModeling,'delay');

% Extract discrete external delays
[Delay,fid,fod,ffd] = ltipack.AbstractSS.discretizeDelay(Dc.Delay,Ts);

% The damped Tustin appx has state
%   xd[k] = [ x[k]-b*h*x'[k]+M*u[k] ; x[k-1+alpha]-b*h*x'[k-1+alpha] ];
bp = 0.252;  % should be same as in TRBDF2
alpha = 2*bp;
g1 = -1+1.5/bp-0.25/bp^2;
g2 = bp-2+0.5/bp;
g3 = 1-g1;
bh = bp*Ts;
g2h = g2*Ts;
aux = e-bh*a;
ed = [aux -(g1*e+g2h*a) ; sparse(nx,nx) aux];
ad = [[g3*e ; e+bh*a] , sparse(2*nx,nx)];
tau = g1*bh+g2h;
[M,caux] = ltipack.util.safeMdivide(aux,-(alpha*tau)*b,c);
if ~(allfinite(M) && allfinite(caux))
   error(message('Control:transformation:c2d10'))
end
bhAM = bh*(a*M);
bd = [(g3*bh+(1-alpha+g3*alpha)*tau)*b-g3*bhAM ; (2*bh+alpha*tau)*b-2*bhAM];
cd = [caux*e , sparse(ny+nfd,nx)];
dd = d + caux * ((bh+alpha*tau)*b-bhAM);
Dd = ltipack.spssdata(ad,bd,cd,dd,ed,Ts);
Dd.StateInfo = struct('Type',1,'Name',{"x[k]";"x[k-1+alpha]"},'Size',nx);

% Handle fractional delays
if any(ffd) && ~roundOK
    % Approximate using Thiran filters
    appxInternalDelays = min(Delay.Internal,nf-1) .* (ffd>0);
    G = ltipack.ssdata.getThiranAppx(appxInternalDelays+ffd,Ts,delayOK);
    G.Delay.Output = Delay.Internal - appxInternalDelays;
    Dd = lft(Dd,sparss(G),nu+(1:nfd),ny+(1:nfd),1:nfd,1:nfd); % nfd size may decrease
else
    % Round delays to nearest multiple of Ts
    Dd.Delay = ltipack.utDelayStruct(ny,nu,true);
    Dd.Delay.Internal = Delay.Internal + round(ffd);    
end
% Note: At this point Dd is well formed with correct dimensions for
% Dd.Delay for the MTIMES operations below.

if any(fid) && ~roundOK
    % Approximate using Thiran filters
    appxInputDelays = min(Delay.Input,nf-1) .* (fid>0);
    G = ltipack.ssdata.getThiranAppx(appxInputDelays+fid,Ts,delayOK);
    Dd = mtimes(Dd,sparss(G)); % Dd*G
    Dd.Delay.Input = Delay.Input-appxInputDelays;
else
    % Round delays to nearest multiple of Ts
    Dd.Delay.Input = Delay.Input + round(fid);
end

if any(fod) && ~roundOK
   % Approximate using Thiran filters
   appxOutputDelays = min(Delay.Output,nf-1) .* (fod>0);
   G = ltipack.ssdata.getThiranAppx(appxOutputDelays+fod,Ts,delayOK);
   % Multiply by the Output Delays. This results in the first states being those of G
   Dd = mtimes(sparss(G),Dd); % G*Dd
   Dd.Delay.Output = Delay.Output-appxOutputDelays;
else
   Dd.Delay.Output = Delay.Output + round(fod);
end

if roundOK && (any(fid) || any(fod) || any(ffd))
   warning(message('Control:transformation:RoundedDelay'))
end

% Eliminate zero internal delays (may be introduced by rounding)
Dd = elimZeroDelay(Dd);

% Compute IC Mapping:
%   xd[0] = [x0-b*h*x0'+M*u0;0]
% (result independent of portion set to zero)
if nargout > 1
   if EXPLICIT
      Gx = aux;  Gu = M-bh*b;
      % Zero columns of Gu when there are input delays
      if roundOK
         iud = find(Dd.Delay.Input);
      else
         iud = find(Dd.Delay.Input+fid);
      end
      Gu(:,iud) = 0;
      ICMap = [Gx , Gu];
      % Zero-pad extra states
      nxnew = size(Dd.a,1)-nx;
      ICMap = [ICMap;sparse(nxnew,size(ICMap,2))];
   else
      ICMap = [];  % large and dense
   end
end
