function [D,blks,TL,TR] = modreal(D,Options)
% Computes modal form of state-space model.

%	Author(s): P. Gahinet
%   Copyright 2023 The MathWorks, Inc.
if hasInternalDelay(D)
   throw(ltipack.utNoDelaySupport('modalreal',D.Ts,'internal'))
end
a = D.a; b = D.b; c = D.c; e = D.e; Ts = D.Ts; dx0 = D.dx0; x0 = D.x0;
EXPLICIT = isempty(e);

% Scale 
if D.Scaled
   sL = 1;  sR = 1;
else
   [a,b,c,e,sR,sL] = ltipack.xscale(a,b,c,D.d,e,Ts);
   dx0 = sL.*dx0;  x0 = sR.\x0;
end
   
% Block diagonalize
CondMax = max(1,Options.SepTol/eps);
if EXPLICIT
   [TR,a,blks] = bdschur(a,CondMax);
   nu = size(b,2);
   aux = TR\[b dx0 x0];
   b = aux(:,1:nu);  dx0 = aux(:,nu+1);  x0 = aux(:,nu+2);
   c = c*TR;  TL = inv(TR);
else
   [a,e,TL,TR,blks] = bdqz(a,e,CondMax);
   b = TL'*b;  c = c*TR;  dx0 = TL'*dx0;  
   if any(x0)
      x0 = TR\x0;
   end
   TL = TL';
end

% Incorporate scaling
TL = TL .* sL.';
TR = sR .* TR;
   
% Normalize
if Options.Normalize && isreal(a) && isreal(e)
   % Rescale 2x2 diagonal blocks to look like Aj=[a b;-b a] and Ej=[1 0;0 1]
   % (a and b give the real and imaginary parts of the complex poles)
   i = 1;
   if EXPLICIT
      for ct=1:numel(blks)
         if blks(ct)==2 && a(i+1,i)~=0 % watch for real eig of multiplicity 2
            % Scale 2x2 block
            %    a(i:i+1,i:i+1) = [ a  q ] to  [  a  b ]
            %                     [ r  a ]     [ -b  a ]
            %    b = sqrt(abs(q*r))
            % Scaling factor = sign(q) * sqrt(q/r)
            a12 = a(i,i+1);  a21 = a(i+1,i);
            sfactor = sign(a12) * sqrt(abs(a21/a12));
            beta = sqrt(abs(a12*a21));
            a(i,i+1) = beta;
            a(i+1,i) = -beta;
            % Absorb scaling factor
            b(i+1,:) = b(i+1,:)/sfactor;
            TL(i+1,:) = TL(i+1,:)/sfactor;
            c(:,i+1) = c(:,i+1)*sfactor;
            TR(:,i+1) = TR(:,i+1)*sfactor;
            dx0(i+1) = dx0(i+1)/sfactor;
            x0(i+1) = x0(i+1)/sfactor;
         end
         i = i+blks(ct);
      end
   else
      for ct=1:numel(blks)
         if blks(ct)==1 && e(i,i)>0
            % Normalize Ej=1
            s1 = sqrt(e(i,i));
            a(i,i) = a(i,i)/e(i,i);
            e(i,i) = 1;
            b(i,:) = b(i,:)/s1;
            TL(i,:) = TL(i,:)/s1;
            c(:,i) = c(:,i)/s1;
            TR(:,i) = TR(:,i)/s1;
            dx0(i) = dx0(i)/s1;
            x0(i) = x0(i)*s1;
         elseif blks(ct)==2 && a(i+1,i)~=0 && e(i,i)>0 && e(i+1,i+1)>0
            % Normalize Ej = [e11 0;0 e22] with e11,e22>0 to Ej=I
            % Then normalize Aj to [a q;r a] (reference: LAPACK's DLANV2)
            s1 = sqrt([e(i,i);e(i+1,i+1)]);
            e(i,i) = 1;   e(i+1,i+1) = 1;
            [u,a(i:i+1,i:i+1)] = schur(s1 .\ a(i:i+1,i:i+1) ./ s1');
            % Normalize to Aj=[a b;-b a]
            a12 = a(i,i+1);  a21 = a(i+1,i);
            s2 = sign(a12) * sqrt(abs(a21/a12));
            beta = sqrt(abs(a12*a21));
            a(i,i+1) = beta;
            a(i+1,i) = -beta;
            % Absorb transforms
            u = s1.\u;  XL = u./[1 s2];  XR = u.*[1 s2];
            b(i:i+1,:) = XL' * b(i:i+1,:);
            TL(i:i+1,:) = XL' * TL(i:i+1,:);
            c(:,i:i+1) = c(:,i:i+1)*XR;
            TR(:,i:i+1) = TR(:,i:i+1) * XR;
            dx0(i:i+1,:) = XL' * dx0(i:i+1,:);
            x0(i:i+1,:) = XR \ x0(i:i+1,:);
         end
         i = i+blks(ct);
      end
   end
end

% Write output
D.a = a;  D.b = b;  D.c = c;
if EXPLICIT || (Options.Normalize && isequal(e,eye(size(e))))
   D.e = [];
else
   D.e = e;
end
D.dx0 = dx0;  D.x0 = x0;
D.StateName = strings(0,1);
D.StatePath = strings(0,1);
D.StateUnit = strings(0,1);
D.Scaled = false;
