function D = utSISOLFT(D,C)
% Computes LFT(D,DIAG(C1,...,CN)) where each Cj is SISO.
% Optimized for SISO Tool use.

%   Author(s): P. Gahinet
%   Copyright 1986-2010 The MathWorks, Inc.

% Number of C's
nC = length(C);

try
   if localHasDelay(C)
      % Delays in C use ssdata/lft command to close LFT
      D = localComputeLFT(D,C);
   else
      % Assumes no delays in C's
      [nYandC,nUandC] = iosize(D);
      ny = nYandC-nC;
      nu = nUandC-nC;

      % Fold in I/O delays associated with I/Os which will be closed
      Din = D.Delay.Input;
      Din(1:nu,:) = 0;
      Dout = D.Delay.Output;
      Dout(1:ny,:) = 0;
      D = utFoldDelay(D,Din,Dout);

      [rs,cs] = size(D.d);
      nd = length(D.Delay.Internal);% rs-nYandC or rc-nUandC;

      % Shift IO channels so that C's are at the bottom
      idxIn = [1:nu,nUandC+(1:nd),nu+(1:nC)];
      idxOut = [1:ny,nYandC+(1:nd),ny+(1:nC)];

      a = D.a;
      b = D.b(:,idxIn);
      c = D.c(idxOut,:);
      d = D.d(idxOut,idxIn);
      e = D.e;
      nx = size(D.a,1);

      % Close each SISO loop successively
      for ct=nC:-1:1
         ac = C(ct).a;
         bc = C(ct).b;
         cc = C(ct).c;
         dc = C(ct).d;
         ec = C(ct).e;
         t = 1-d(rs,cs)*dc;
         if t==0
            error(message('Controllib:general:UnexpectedError','Algebraic loop'))
         end
         b2 = b(:,cs);
         c2 = c(rs,:);
         d21 = d(rs,1:cs-1);
         d12 = d(1:rs-1,cs);
         dct = dc/t;
         e = ltipack.util.blkdiagE(e,ec,size(a,1),size(ac,1));
         a = [a+b2*(dct*c2) , b2*(cc/t) ; bc*(c2/t) , ac+bc*((d(rs,cs)/t)*cc)];
         b = [b(:,1:cs-1) + b2*(dct*d21) ; bc*(d21/t)];
         c = [c(1:rs-1,:) + (d12*dct)*c2 , (d12/t)*cc];
         d = d(1:rs-1,1:cs-1) + d12*dct*d21;

         % Decrement sizes
         rs = rs-1;
         cs = cs-1;
      end

      % Return closed-loop model
      nxnew = size(a,1);
      D.a = a;
      D.b = b;
      D.c = c;
      D.d = d;
      D.e = e;
      D.dx0 = zeros(nxnew,1);  D.x0 = zeros(nxnew,1);
      D.u0 = zeros(nu+nd,1);  D.y0 = zeros(ny+nd,1);
      D.Delay.Input = D.Delay.Input(1:nu);
      D.Delay.Output = D.Delay.Output(1:ny);
      % Note: Internal Delay did not change.
      if ~isempty(D.StateName)
         D.StateName = [D.StateName ; strings(nxnew-nx,1)];
      end
      if ~isempty(D.StatePath)
         D.StatePath = [D.StatePath ; strings(nxnew-nx,1)];
      end
      if ~isempty(D.StateUnit)
         D.StateUnit = [D.StateUnit ; strings(nxnew-nx,1)];
      end
      D.Scaled = false;
   end

catch ME
   % Algebraic loop encountered, use ssdata/lft command to close LFT
   D = localComputeLFT(D,C);
end


function D = localComputeLFT(D,C)
% Use ssdata/lft command to close LFT
% Create diagonal compensator block because compensators are passed in as a
% vector
nC = length(C);
Cdiag = C(1);
for ct = 2:nC
   Cdiag = append(Cdiag,C(ct));
end
[Dout,Din]=iosize(D);
D = lft(D,Cdiag,Din-nC+1:Din,Dout-nC+1:Dout,1:nC,1:nC);


function boo = localHasDelay(D)
% Check if any D has delay
if isscalar(D)
   boo = hasdelay(D);
else
   boo = false;
   for ct=1:numel(D)
      if hasdelay(D(ct))
         boo = true; break
      end
   end
end
