function [Dd,ICMap] = utDiscretizeTustin(Dc,Ts,options)
%UTDISCRETIZETUSTIN is a utility to discretize SS models based on Tustin's
%method.

%   Author(s): P. Gahinet, Murad Abu-Khalaf, August 10, 2009
%   Copyright 1984-2011 The MathWorks, Inc.

% Handle prewarp
w = options.PrewarpFrequency;
if w == 0
   T = Ts;
else
   % Handle prewarping
   T = 2*tan(w*Ts/2)/w;
end
CONSISTENT = (nargin>2 && strcmp(options.Consistency,'on'));

% Get LFT's dimensions
nx = size(Dc.a,1);
nfd  = numel(Dc.Delay.Internal);   % nfd=nw=nz
[nyz,nuw] = size(Dc.d);
ny = nyz-nfd;
nu = nuw-nfd;

% Get Fractional Delays options
% Note: Always round delays when consistency required
nf = options.ThiranOrder;
roundOK = (nf == 0 || CONSISTENT);
delayOK = strcmpi(options.DelayModeling,'delay');

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

% Compute discrete matrices and offsets
% Note: Discretized state is w[k] = x(k*Ts) - (T/2) x'(k*Ts)
dx0 = Dc.dx0;  x0 = Dc.x0;   
if isempty(Dc.e)
   % Explicit SS: Return explicit model
   a = Dc.a;
   m = eye(nx)-(T/2)*a;
   [aux,cd] = ltipack.util.safeMdivide(m, [eye(nx)+(T/2)*a,T*Dc.b,T*dx0], Dc.c);
   if ~(allfinite(cd) && allfinite(aux))
      error(message('Control:transformation:c2d10'))
   end
   ad = aux(:,1:nx);      % (I - a*T/2)\(I + a*T/2)
   bd = aux(:,nx+1:nx+nuw);  % (I - a*T/2)\b*T
   dd = (Dc.c*bd)/2 + Dc.d;  % (T/2)*c*(I - a*T/2)\b + d
   ed = [];
   dx0d = aux(:,nx+nuw+1);
   y0d = Dc.y0 + (Dc.c*dx0d)/2;
   dx0d = x0 + dx0d;
else
   % Descriptor SS: Return descriptor model
   a = Dc.a;   e = Dc.e;
   m = e-(T/2)*a;
   ed = m;
   ad = e+(T/2)*a;
   bd = T*Dc.b;
   [~,aux] = ltipack.util.safeMdivide(m,[],Dc.c);  % c/(e - a*T/2)
   if ~allfinite(aux)
      error(message('Control:transformation:c2d10'))
   end
   cd = aux*e;
   dd = Dc.d + (aux*bd)/2;  % (T/2)*c*(e - a*T/2)\b + d
   dx0d = T*dx0 + ed*x0;
   y0d = Dc.y0 + (T/2)*(aux*dx0);
end

% Create discrete model
Dd = ltipack.ssdata(ad,bd,cd,dd,ed,Ts); % Note: Discard state names because w[k] ~= x(k*Ts)
Dd.dx0 = dx0d;
Dd.x0 = x0;
Dd.u0 = Dc.u0;
Dd.y0 = y0d;

% 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,G,nu+1:nuw,ny+1:nyz,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,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(G,Dd); % G*Dd
   Dd.Delay.Output = Delay.Output-appxOutputDelays;
   % Reorder states so that original states are in upper left corner.
   % Only applicable to DelayModeling = 'state'
   if ~delayOK
      nxDd = size(Dd.a,2);
      nxG  = size(G.a,2);
      perm = [nxG+1:nxDd 1:nxG];
      Dd.a = Dd.a(perm,perm);
      Dd.b = Dd.b(perm,:);
      Dd.c = Dd.c(:,perm);
      if ~isempty(Dd.e)
         Dd.e = Dd.e(perm,perm);
      end
   end
else
   Dd.Delay.Output = Delay.Output + round(fod);
end

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

% Eliminate zero internal delays (may be introduced by rounding)
% Note: Allow improper/augmented result since this is not used directly for simulation
if CONSISTENT
   Dd.Delay.Internal = max(1,Dd.Delay.Internal);
else
   Dd = elimZeroDelay(Dd);
end

% Compute IC Mapping:   w[0] = G * [x0;u0] = x0 - (T/2) * x0'
if nargout>1
   Gu = -(T/2)*Dc.b;
   % Zero columns of Gu when there are input delays
   if roundOK
      Gu(:,Dd.Delay.Input>0) = 0;
   else
      Gu(:,Dd.Delay.Input+fid>0) = 0;
   end
   ICMap = [m , Gu];
   if ~isempty(Dc.e)
      % May return Inf when E singular
      ICMap = matlab.internal.math.nowarn.mldivide(Dc.e,ICMap);
   end
   % Zero padding for extra states
   nxnew = size(Dd.a,1)-nx;
   ICMap = [ICMap;zeros(nxnew,size(ICMap,2))];
end
