function [h,InfResp] = fresp(D,w)
% Frequency response of MECHSS model.

%	 Author(s): P.Gahinet
%   Copyright 2020 The MathWorks, Inc.

% Form vector of complex frequencies
[s,sn] = ltipack.utGetComplexFrequencies(w,D.Ts);

% Seek arrow structure
D = xsort(D);

% Scaling (makes a big difference, e.g., for ASML1 model)
if D.Ts==0
   % Scale based on mid-range frequency
   f = sort(abs(w(w>0 & isfinite(w))));
   if ~isempty(f)
      fmid = sqrt(f(1)*f(end));
      D = scale(D,[],abs(D.k)+fmid*abs(D.c)+fmid^2*abs(getM(D)));
   end
else
   D = scale(D,[],abs(D.k)+abs(D.c)+abs(getM(D)));
end

% Extract data
m = getM(D);
k = D.k;
c = D.c;
b = D.b;
f = D.f;
g = D.g;
d = full(D.d);
nf = numel(w);
[ny,nu] = size(d);

% Configure solver and parallelism
OPTS = getSolverConfig(D);
UseParallel = ltipack.util.checkParallel(OPTS.UseParallel);
if UseParallel
   ParPool = gcp;
else
   ParPool = matlab.internal.threadPool;
end

% Performance optimization to minimize number of backsolves
if ~any(g)
   g = [];
end
if ny<nu
   ff = full(f)';  gg = full(g)';
else
   ff = f';  gg = g';  b = full(b);
end

% Compute response
h = zeros(ny,nu,nf,'like',1i);
parfor (ct=1:nf, ParPool)
   h(:,:,ct) = localFRESP(m,c,k,b,ff,gg,d,s(ct));
end
if allfinite(s)
   InfResp = ~allfinite(h);
else
   InfResp = ~allfinite(h(:,:,isfinite(s)));
end

% Factor in delays
Delay = D.Delay;
if ~isempty(Delay.Internal)
   h = ltipack.util.addInternalDelayResp(h,Delay.Internal,sn);
end
if any(Delay.Input) || any(Delay.Output)
   [ny,nu,~] = size(h);
   Tdio = Delay.Input(:,ones(1,ny)).' + Delay.Output(:,ones(1,nu));
   h = ltipack.util.addIODelayResp(h,Tdio,sn);
end      


%--------------------------------------------
function h = localFRESP(m,c,k,b,ff,gg,d,s)
% Computes frequency response. Here FF=F' and GG=G'
[ny,nu] = size(d);
if isfinite(s)
   if nnz(c)>0
      aux = k + s * c + s^2 * m;
   else
      aux = k + s^2 * m;
   end
   if ~isempty(gg)
      ff = ff + conj(s) * gg;  % (F+s*G)'
   end
   % Note: Evaluating L*x is most efficient when L is sparse and x full
   if ny<nu
      % Evaluate as D+(B'*(AUX'\FF))'
      x = matlab.internal.math.nowarn.mldivide(aux,ff,true);
      if allfinite(x)
         h = d + (b'*x)';
      else
         h = inf(ny,nu);
      end
   else
      x = matlab.internal.math.nowarn.mldivide(aux,b,false);
      if allfinite(x)
         h = d + ff'*x;
      else
         h = inf(ny,nu);
      end
   end
else
   h = nan(ny,nu);
end
