function [Dr,PROJ] = modappx(D,Options)
% Computes modal data for modal truncation MOR.
% OPTIONS must be of class mor.SparseModalTruncationOptions.

%	 Author(s): P. Gahinet
%   Copyright 2023 The MathWorks, Inc.
Ts = abs(D.Ts);
if hasInternalDelay(D)
   error(message('Control:transformation:MODALROM13'))
end
nq = size(D.k,1);

% Treat first-order models (M=0) as such
if size(D.m,1)==nq && nnz(D.m)==0
   [Dr,PROJ] = modappx(sparss(D),Options);
   return
end

if Ts==0
   RD = ltipack.util.RayleighDamping(D.c,D.m,D.k);
   if isempty(RD)
      % C is not a combination of K,M
      HPSD = false;
   else
      % Check that K=K' and M=M' semi-definite (all eigenvalues are real)
      [K,M,HPSD,msign] = ltipack.util.checkSparsePSD(D.k,D.m);
   end
else
   % Use Tustin transform to map to continuous-time poles
   h = Ts/2;
   if isempty(D.m)
      M = (D.k+speye(nq)-D.c)/4;
      K = (D.k+speye(nq)+D.c)/(2*h)^2;
      C = (speye(nq)-D.k)/(2*h);
   else
      M = (D.k+D.m-D.c)/4;
      K = (D.k+D.m+D.c)/(2*h)^2;
      C = (D.m-D.k)/(2*h);
   end
   RD = ltipack.util.RayleighDamping(C,M,K);
   if isempty(RD)
      HPSD = false;
   else
      [K,M,HPSD,msign] = ltipack.util.checkSparsePSD(K,M);
   end
end
if ~HPSD
   error(message('Control:transformation:MODALROM21'))
end

% Compute modes in region of interest
Nmax = ceil(Options.MaxOrder/2);
if isempty(M)
   MM = speye(nq);
else
   MM = M;
end
% We're computing -s^2 where s are the modes of interest
fmin = Options.Focus(1)^2;
fmax = Options.Focus(2)^2;
if fmin>0 && fmax<Inf
   % Eigenvalues in [-fmax,fmin] and [fmin,fmax].
   % Use LANCZOS to stay clear of potentially large clusters near f=0 or f=Inf
   opts = struct('FrequencyRange',[fmin,fmax],'convTol',Options.ModeTol);
   [V1,p1] = matlab.internal.math.lanczos(K,MM,opts);
   opts = struct('FrequencyRange',[-fmax,-fmin],'convTol',Options.ModeTol);
   [V2,p2] = matlab.internal.math.lanczos(K,MM,opts);
   V = [V1,V2];  p = [p1;p2];
else
   eigsOptions = {'IsSymmetricDefinite',true,'Tolerance',Options.ModeTol};
   if fmin>0
      % Compute eigenvalues near infinity
      if ~isempty(M) && sprank(M)<nq
         % Can't perform M\x operations
         error(message('Control:transformation:MODALROM15'))
      end
      [~,~,~,~,nneg1] = ldl(K-fmin*MM);
      [~,~,~,~,nneg2] = ldl(K+fmin*MM);
      N = nq-nneg1+nneg2;
      [V,p] = eigs(K,M,min(N,Nmax),'LM',eigsOptions{:});
   elseif fmax<Inf
      % Compute eigenvalues near origin
      sigma = Options.DCFrequency; % to handle integrators
      if sigma==0 && sprank(K)<nq
         % Can't perform K\x operations
         error(message('Control:transformation:MODALROM17'))
      end
      [~,~,~,~,nneg1] = ldl(K-fmax*MM);
      [~,~,~,~,nneg2] = ldl(K+fmax*MM);
      N = nneg1-nneg2;
      [V,p] = eigs(K,M,min(N,Nmax),sigma,eigsOptions{:});
   else
      % Range=[0,Inf]
      % Compute the MaxOrder smallest-magnitude eigenvalues
      N = min(nq,Nmax);
      if ~isempty(M)
         N = min(N,sprank(M));  % no Inf eigenvalues
      end
      [V,p] = eigs(K,M,N,'SM',eigsOptions{:});
   end
   p = diag(p);
   if any(isnan(p))
      % Not all eigenvalues converged
      warning(message('Control:transformation:MODALROM16'))
      idx = find(isfinite(p));
      p = p(idx,:);
      V = V(:,idx);
   end
end

% REVISIT: Normalize msign*V'*M*V=I (g3110723)
p = msign*p;
np = numel(p);
if ~isempty(M)
   mu = zeros(np,1);
   for ct=1:np
      v = V(:,ct);
      mu(ct) = msign*(v'*M*v);
   end
   if norm(mu-1)>1e-8
      V = V ./ sqrt(mu)';
   end
end
% end REVISIT

% Build modal approximation
% Note: 
%  * V'*M*V=msign*I, V'*K*V = diag(p), V'*C*V = diag(msign*RD(1)+RD(2)*p)
%  * With W=msign*V, rewrite as
%    W'*M*V=I, W'*K*V = diag(msign*p), W'*C*V = diag(RD(1)+RD(2)*msign*p)
%  * q = V*qr and reduced state is xr = [qr;qr'] because Mr=I
%  * Reorder to q1,q1',q2,q2',... to make A tridiagonal
%  * WL,WR are the transformations for the first-order equivalent:
%        WL'*[0 I;-K -C]*WR=AR,   WL'*[I 0;0 M]*WR=I=ER
CondMax = max(1,Options.SepTol/eps);
d = full(D.d);
[ny,nu] = size(d);
WR = zeros(2*nq,2*np);
WR(1:nq,1:2:2*np) = V;
WR(nq+1:2*nq,2:2:2*np) = V;   % [q;q'] = WR*[qr1;qr1';qr2;qr2';...] = WR*xr
WL = zeros(2*nq,2*np);
[Q,R] = qr(V,0);
WL(1:nq,1:2:2*np) = Q/R';   % V/(V'*V);
WL(nq+1:2*nq,2:2:2*np) = msign*V;
b = D.b;  f = D.f;  g = D.g;
if Ts==0
   k = p;   c = RD(1)+RD(2)*p;
   ar = zeros(2*np);  br = zeros(2*np,nu);  cr = zeros(ny,2*np);
   for ct=1:np
      idx = [2*ct-1 2*ct];
      v = V(:,ct);
      aa = [0 1;-k(ct) -c(ct)];
      bb = [zeros(1,nu) ; msign*(v'*b)];
      cc = [f*v g*v];
      if c(ct)^2>=4*k(ct)
         % Block has real eigenvalues
         [t,aa] = bdschur(aa,CondMax);
         bb = t\bb;  WL(:,idx) = WL(:,idx)/t';
         cc = cc*t;  WR(:,idx) = WR(:,idx)*t;
      end
      ar(idx,idx) = aa;
      br(idx,:) = bb;
      cr(:,idx) = cc;
   end
   Dr = ltipack.ssdata(ar,br,cr,d,[],Ts);
else
   m = 1 + h^2*p + h*(RD(1)+RD(2)*p);  % Md = M+h^2*K+h*C
   k = 1 + h^2*p - h*(RD(1)+RD(2)*p);  % Kd = M+h^2*K-h*C
   c = 2*(h^2*p-1);                    % Cd = 2*(h^2*K-M)
   %[norm(msign*V'*D.m*V-diag(m)) norm(msign*V'*D.k*V-diag(k)) norm(msign*V'*D.c*V-diag(c))]
   ar = zeros(2*np);  er = zeros(2*np);  br = zeros(2*np,nu);  cr = zeros(ny,2*np);
   for ct=1:np
      idx = [2*ct-1 2*ct];
      v = V(:,ct);
      aa = [0 1;-k(ct) -c(ct)];
      ee = [1 0;0 m(ct)];
      bb = [zeros(1,nu) ; msign*(v'*b)];
      cc = [f*v g*v];
      if c(ct)^2>=4*k(ct)*m(ct)
         % Block has real eigenvalues
         [aa,ee,tL,tR] = bdqz(aa,ee,CondMax);
         bb = tL*bb;  WL(:,idx) = WL(:,idx)*tL';
         cc = cc*tR;  WR(:,idx) = WR(:,idx)*tR;
      end
      ar(idx,idx) = aa;
      er(idx,idx) = ee;
      br(idx,:) = bb;
      cr(:,idx) = cc;
   end
   Dr = ltipack.ssdata(ar,br,cr,d,er,Ts);
end
Dr.Delay = D.Delay;
PROJ = struct('TL',WL,'TR',WR);  % q = W*xr
