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

%   Author(s): P. Gahinet
%   Copyright 2020 The MathWorks, Inc.

% Handle prewarp
w = options.PrewarpFrequency;
if w == 0
    T = Ts;
else
    % Handle prewarping
    T = 2*tan(w*Ts/2)/w;
end

% Get LFT's dimensions
a = Dc.a;   e = Dc.e;  c = Dc.c;  d = Dc.d;
nx = size(a,1);
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);

% Compute discrete matrices
% Note: Discretized state is w[k] = x(k*Ts) - (T/2) x'(k*Ts)
EXPLICIT = isempty(e);
if EXPLICIT
   e = speye(nx);
end
tau = T/2;
ed = e - tau*a;
ad = e + tau*a;
bd = T*Dc.b;
[~,aux] = ltipack.util.safeMdivide(ed,[],c);  % c/(e - a*T/2)
if ~allfinite(aux)
   error(message('Control:transformation:c2d10'))
end
cd = aux*e;
dd = d + (aux*bd)/2;
Dd = ltipack.spssdata(ad,bd,cd,dd,ed,Ts);
dx0 = Dc.dx0;  x0 = Dc.x0;
Dd.dx0 = T*dx0 + ed*x0;
Dd.x0 = x0;
Dd.u0 = Dc.u0;
Dd.y0 = Dc.y0 + (T/2)*(aux*dx0);
Dd.StateInfo = Dc.StateInfo;

% 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:   w[0] = G * [x0;u0]
if nargout > 1
   if EXPLICIT
      % Zero columns of Gu when there are input delays
      Gu = (-bd/2);
      if roundOK
         Gu(:,Dd.Delay.Input>0) = 0;
      else
         Gu(:,Dd.Delay.Input+fid>0) = 0;
      end
      ICMap = [ed , Gu];
      % Zero padding extra states
      nxnew = size(Dd.a,1)-nx;
      ICMap = [ICMap;sparse(nxnew,size(ICMap,2))];
   else
      % G = E\[E-T/2*A , -T/2*B] is dense + too expensive to compute
      ICMap = [];
   end
end