function [H,H0,INFO] = modsep(G,opt)
% Fine-grain modal decomposition.

%   Author(s): P. Gahinet
%   Copyright 2023 The MathWorks, Inc.
[ny,nu] = iosize(G);
if hasInternalDelay(G)
   throw(ltipack.utNoDelaySupport('modalsep',G.Ts,'internal'))
end

% Extract data
a = G.a;  b = G.b;  c = G.c;  d = G.d;  e = G.e;   Ts = G.Ts;
dx0 = G.dx0; x0 = G.x0;  u0 = G.u0;  y0 = G.y0;
EXPLICIT = isempty(e);
REAL = isreal(a) && isreal(e);
nx = size(a,1);
if nx>50
   % Look for (A,E) already block-diagonal with 1x1 or 2x2 blocks.
   [BDIAG,blksize] = localCheckBlockDiag(a,e,REAL);
else
   BDIAG = false;
end

if ~BDIAG
   % Scale A,B,C to minimize loss of accuracy in block diagonalization
   if G.Scaled
      % No scaling necessary
      sL = ones(nx,1);  sR = sL;
   else
      [a,b,c,e,sR,sL] = ltipack.xscale(a,b,c,d,e,Ts);
      dx0 = sL.*dx0;  x0 = sR.\x0;
   end
   % Block diagonalize
   CondMax = max(1,opt.SepTol/eps);
   try
      if EXPLICIT
         [tR,a,blksize] = bdschur(a,CondMax);
         aux = tR\[b dx0 x0];
         b = aux(:,1:nu);  dx0 = aux(:,nu+1);  x0 = aux(:,nu+2);
         c = c*tR;
      else
         [a,e,tL,tR,blksize] = bdqz(a,e,CondMax);
         b = tL'*b;   dx0 = tL'*dx0;
         c = c*tR;
         if any(x0)
            x0 = tR\x0;
         end
      end
   catch ME
      % Should not happen
      throw(ME)
   end
end

% Initialize outputs
H0 = ltipack.ssdata([],zeros(0,nu),zeros(ny,0),d,[],Ts);
H0.u0 = u0;  H0.y0 = y0;  
H0.Delay = G.Delay;
% nblks-by-1 array H
nblks = numel(blksize);
h = ltipack.ssdata([],zeros(0,nu),zeros(ny,0),zeros(ny,nu),[],Ts);
h.u0 = u0;  h.y0 = y0;  
h.Delay = G.Delay;
H = repmat(h,[nblks 1]);

% Store factors
is = 0;
for j=1:nblks
   % Store j-th component
   bs = blksize(j);
   ix = is+1:is+bs;
   H(j).a = a(ix,ix);
   H(j).b = b(ix,:);
   H(j).c = c(:,ix);
   if ~EXPLICIT
      H(j).e = e(ix,ix);
   end
   H(j).dx0 = dx0(ix,:);
   H(j).x0 = x0(ix,:);
   is = is+bs;
end

% Construct block diagonalization state transformation T and its inverse Ti
% RE: The system with decoupled dynamics is ss2ss(G,T)
if nargout>2
   if BDIAG
      TL = 1;  TR = 1;
   elseif EXPLICIT
      TL = inv(tR) .* sL';
      TR = sR .* tR;
   else
      TL = (sL .* tL)';
      TR = sR .* tR;
   end
   % Note: Use unstable s to minimize risk of singularity
   if Ts==0
      s = opt.DCFrequency;
   else
      s = exp(opt.DCFrequency*abs(Ts));
   end
   [su,sy] = getIOScaling(opt,nu,ny);
   sio = sy.*su';
   DCG = norm(evalfr(G,s).*sio);
   Modes = zeros(nblks,1);
   NDCG = zeros(nblks,1);
   for j=1:nblks
      if EXPLICIT
         p = ordeig(H(j).a);
      else
         p = ordeig(H(j).a,H(j).e);
      end
      if REAL && ~any(imag(p)==0)
         % No real poles in cluster (complex pair). Take mean of a+jb with b>0
         Modes(j) = mean(p(imag(p)>0,:));
      else
         % Take mean of cluster poles.
         Modes(j) = mean(p);
      end
      NDCG(j) = norm(evalfr(H(j),s).*sio)/DCG;
   end
   INFO = struct('Mode',Modes,'DCGain',NDCG,'TL',TL,'TR',TR);
end

%-------------------------------------------------------------------
function [BDIAG,blksize] = localCheckBlockDiag(a,e,REAL)
% Is (A,E) block diagonal with 1x1 or 2x2 blocks, the 2x2 blocks being
% consistent with complex pairs and ORDEIG(A,E) demanding that E be upper
% triangular?
[perm,blksize] = ltipack.util.blkdiagStructure(a,e);
BDIAG = isequal(perm,1:size(a,1)) && all(blksize<=1+REAL);
if BDIAG && any(blksize>1)
   % Check 2x2 blocks are not outliers
   try
      if isempty(e)
         p = ordeig(a);
      else
         % May fail if E is not upper triangular
         p = ordeig(a,e);
      end
      BDIAG = (nnz(imag(p)==0)==nnz(blksize==1));
   catch 
      BDIAG = false;
   end
end


