function w = freqgrid(z,p,Ts,Grade,OPTS)
%FREQGRID  Generates frequency grid for frequency response plots.
%
%   W = FREQGRID(Z,P,TS,GRADE,FRANGE) generates a frequency grid W 
%   given:
%     * the zeros Z and poles P for each I/O pair (cell arrays)
%     * the sampling time TS.
%
%   GRADE is an integer between 1 and 4 that controls the grid density 
%   for different plot types (1=finest, 4=coarsest). 
%
%   OPTS is a struct with fields
%     Focus     Desired frequency range [FMIN,FMAX]. FMIN and FMAX
%               can be set to NaN to indicate unspecified limits (0 and
%               Inf are interpreted as NaN). When FRANGE is unspecified, 
%               the grid W spans the frequency range specified by
%               UTMAXFREQRANGE and is most dense around the frequencies 
%               of the system dynamics. When FMIN,FMAX are specified, the
%               grid is clipped to fit this range.
%     Negative  true/false. Generates both negative and positive
%               frequencies when true (for complex system support).
%     Decades   true/false. Includes 10^k points when true.
%
%   Note: FREQGRID never generates frequencies past pi/Ts. It returns an
%   empty w when FOCUS(1)>=pi/Ts.

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

% RE: Expects data for a single SISO or MIMO LTI model

% Parameters
if nargin<5
   OPTS = struct('Focus',[],'Negative',false,'Decades',false);
end

% Handle complex case
if OPTS.Negative
   OPTS.Negative = false;
   wpos = freqgrid(z,p,Ts,Grade,OPTS);
   wneg = freqgrid(cellfun(@conj,z,'UniformOutput',false),...
      cellfun(@conj,p,'UniformOutput',false),Ts,Grade,OPTS);
   w = unique([-wneg;wpos]);
   return
end

% threshold for selection of resonant modes:
if Grade==1
   dampfact = 0.7;  % Nyquist
else
   dampfact = 0.4;
end
SysOrder = max(cellfun('length',p(:)));  % system order
Focus = OPTS.Focus;
if isempty(Focus)
   Focus = NaN(1,2);
else
   Focus(Focus<=0 | Focus==Inf) = NaN;
end
% Nyquist freq.
nf = pi/Ts;
if Focus(1)>=nf
   % FREQGRID does not generate frequencies beyond pi/Ts
   w = zeros(0,1);  return
end

% Gather poles and zeros
Poles = cat(1,p{:});
hasIntegrator = (Ts==0 && any(Poles==0)) || (Ts>0 && any(Poles==1));
zp = unique(cat(1,z{:},Poles));
if Ts>0
   % Get equivalent continuous-time roots
   zp = map2s(zp,Ts);
end

% System dynamics and their frequency
% 1) Do not use any zero tolerance (g350339)
% 2) For systems with complex coeffs, required density depends on both
%    positive and negative frequencies.
zp = zp(zp~=0);
[fzp,zeta] = damp(zp);
zeta = abs(zeta);

% Determine dynamics range [FMIN,FMAX]
[flb,fub] = utMaxFreqRange(Ts);  % ultimate range
fzpRange = fzp(fzp>flb & fzp<1.01*fub);
if all(isnan(Focus))
   % Get dynamics in ultimate range (see g352625). Watch for fub=Nyquist freq.
   if isempty(fzpRange)
      fmax = min(10,nf);
      fmin = fmax/100;
   else
      fmin = max(min(fzpRange)/50,flb);
      fmax = min(max(fzpRange)*50,fub);
   end
else
   % Fully or partially specified range
   fmin = Focus(1);  % note: NaN or <nf
   fmax = Focus(2);
   if fmax>nf
      fmax = nf;
   end
   if isnan(fmin)
      fmin = min([fzpRange/50 ; fmax/100]);
   end
   if isnan(fmax)
      fmax = max([fzpRange*50 ; 100*fmin]);
   end
end

% Decade metrics
logfmin = log10(fmin);
logfmax = log10(fmax);
lognf = log10(nf);

% Set minimum number DNPTS of points/decade
MinTotal = 40 + 20 * (Grade==1);   % min number of points overall
MinDec = 15;                       % min number of points per decade
dnpts = max(MinDec,ceil(MinTotal/max(1,logfmax-logfmin)));

% Initialize W (frequencies expressed in DECADES)
logfzp = log10(fzp(fzp>fmin & fzp<fmax)); % ignore dynamics outside of [FMIN,FMAX]
if Ts
   % Sparse grid from FMIN to NF/2
   hnf = log10(nf/2);     % half the Nyquist frequency
   w = localInitGrid(logfmin,hnf,dnpts,logfzp);
   % Finer grid between NF/2 and NF
   if Grade<3
      extra = log10(linspace(nf/2,nf,20));
   else
      extra = linspace(hnf,lognf,6);
   end
   w = [w , extra(2:end)];
else
   % Sparse grid from FMIN to FMAX
   w = localInitGrid(logfmin,logfmax,dnpts,logfzp);
end
% Add sparse cover toward 0 and Inf
% Gradually increase sparsity away from focus range, see
%   bode(c2d(tf(1,[1 0 1e-14]),1e-3))
wlow = LocalSparseGrid([log10(flb),w(1)],2);
whigh = LocalSparseGrid([w(end),log10(fub)],1);
w = [wlow , w , whigh];
%numel(w)

% Find resonant modes inside [FMIN,FMAX] and refine grid W near these modes.
% Note: 
%   * Sort by increasing damping to process high-density refinements first.
%   * Only consider resonant modes at positive frequencies. For systems
%     with complex coeffs, separate call handles negative frequencies
[~,is] = sort(zeta);
zp = zp(is);  fzp = fzp(is);  zeta = zeta(is);
zp = zp(fzp>=fmin/2 & fzp<=2*fmax & zeta<dampfact & imag(zp)>=0);
nres = numel(zp);
wpeak = zeros(1,nres);
for ct=1:nres
   [w,wpeak(ct)] = localResGrid(w,zp(ct),Ts,dnpts,Grade);
end
% Make sure to include peak frequencies to accurately capture peaks and dips
w = [w , wpeak];

% Add phase extrema
if Grade<4 && Ts==0 && numel(z)==1 && SysOrder<20
   w = LocalAddPhaseExtrema(w,z{1},p{1});
end

% Add 10^k points
if OPTS.Decades
   w = [w , floor(min(w)):ceil(max(w))];
end

% Convert to frequencies in rad/sec
% RE: Remove duplicates and allow for roundoff so that conversions don't
% reintroduce dups
w = sort(w);
if Ts
   % Clip at Nyquist frequency
   w = w(w<=lognf);
end
w(:,w(2:end)-w(1:end-1)<1e3*eps) = [];
w = 10.^w(:);
%numel(w)

% Clip to user-specified range if any
gap = 1+sqrt(eps);
if Focus(1)<nf
   w = [Focus(1) ; w(w>gap*Focus(1))];
elseif Grade==1 && ~hasIntegrator
   % Add zero frequency for Nyquist
   w = [0;w];
end
if Focus(2)<=nf
   w = [w(w<Focus(2)/gap) ; Focus(2)];
end


%%%%%%%%%%%%%%%%%%%% Local Functions %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function w = localInitGrid(fmin,fmax,dnpts,fzp)
% Initializes grid with uniform density DNPTS (except in regions
% without any dynamics)
% Note: All frequency values in decade units

% Grid with uniform density DNPTS/decade
w = linspace(fmin,fmax,ceil((fmax-fmin)*dnpts));

% Reduce density in regions without dynamics (to avoid, e.g., high number
% of points for systems with quasi-integrators)
% RE: For smoothness do not leave large regions without points, cf
%       A = [-2 -1  0  0  0;1 0 0 0 0;-1 -1 -2 -1 0;0 0 1 0 0;-1 -1 -1 -1 -1];
%       B = [1 0 1 0 1]';   C = -ones(1,5);  D = 1;
%       bode(ss(A,B,C,D),{1e-6 1e2})
fzp = sort(fzp);
idx = find(diff(fzp)>3.5);  % 3.5 decade gaps in pole/zero distributions
for ct=1:length(idx)
   SparseGrid = fzp(idx(ct))+1.5:0.5:fzp(idx(ct)+1)-1.5;
   w = [w(:,w<SparseGrid(1)-0.1) , SparseGrid , w(:,w>SparseGrid(end)+0.1)];
end

%--------------------------
function [w,wpeak] = localResGrid(w,s,Ts,dnpts,Grade)
% Generates finer grid around peak frequency for resonant S-plane poles
% or zeros. This grid is determined based on the response of the
% second-order system with poles s and conj(s). The refined grid is merged
% with the ambient grid W so as to preserve the higher density portions of
% each grid and avoid unnecessary growth in the number of grid points.
w0 = abs(s);                   % natural frequency
zeta = abs(real(s))/w0;        % damping ratio (< 0.707)

% Compute frequency WPEAK (in DECADES) where gain peaks
if Ts==0
   % Continuous mode
   wpeak = w0 * sqrt(1-2*zeta^2);
else
   % S-plane equivalent of discrete mode. For s=a+jb, gain of
   %      1/(z-exp((a+jb)*Ts))/(z-exp((a-jb)*Ts)
   % peaks at either at 0, pi/Ts, or phi/Ts where
   %      cos(phi) = cos(b*Ts) cosh(a*Ts)
   h = cos(imag(s)*Ts)*cosh(real(s)*Ts);
   h = min(1-10*eps,max(h,-1));  % ensure wpeak>0
   wpeak = acos(h)/Ts;
end
wpeak = log10(wpeak);

% Handle various plot types
if Grade<3
   % Nyquist or Nichols: generate frequencies WT for which the phase
   % is evenly spaced
   offset = pi/90;
   if Grade==1  % Nyquist
      spacing = -pi/45;
      angles = -offset:spacing:-pi+offset;
   else          % Nichols
      spacing = -pi/60;
      angles = [-offset:spacing:-pi/6-spacing ,...
         -pi/6:2*spacing:-5*pi/6 , ...
         -5*pi/6+spacing:spacing:-pi+offset];
   end
   ct = -cot(angles);
   
   % Compute frequencies WFINE associated with ANGLES. Beware of running
   % out of frequency grid resolution when ZETA approaches zero.
   % RE: The max. relative separation is of order |CT(1)|*ZETA
   % see: nyquist(tf(1,[1 1]) + tf(10,[1 10]) + tf(1e16,[1 10 1e20]))
   zct = zeta*ct;
   pos = (zct>0);
   zctpos = zct(:,pos);
   zctneg = zct(:,~pos);
   wRefine = unique([log10(w0 * (sqrt(1+zctneg.^2)-zctneg)) , ...
      log10(w0 ./ (sqrt(1+zctpos.^2)+zctpos)) , wpeak]);
   if length(wRefine)==1
      wRefine = wpeak + [-sqrt(eps) 0 sqrt(eps)];  % note: wpeak is in log units
   end
   
else
   % Bode or sigma
   Extent = 0.30103;  % log10(2) (set grid extent to [WPEAK/2,2*WPEAK])
   % Generate refined grid WT with points accumulating exponentially
   % around WPEAK.  The frequency points (in DECADES) are generated by
   %    w(k+1) = w(k) + a * exp(b*k),   w(0) = wpeak
   % where a,b are determined by the constraints:
   %    (1) the log. spacing at WPEAK is of order GAP
   %    (2) the spacing matches the default spacing of DNPTS/decade
   %        at the grid bounds WPEAK+/-EXTENT
   delta = 1/max(1,dnpts);
   a = (0.7*delta) * sqrt(max(5e-3,zeta));  % adhoc initial spacing near WPEAK
   b = log(1+(delta-a)/Extent);
   npts = round(log(delta/a)/b);  % number of points left/right of WPEAK
   theta = a/(1-exp(b));
   grid = 1-exp(b*(1:npts));
   wl = wpeak - theta * grid(end:-1:1);
   wr = wpeak + theta * grid;
   % If zeta=0, perturb wpeak to avoid singularity (no point drawn, e.g.,
   % 1/(s^2+1))
   zerotol = 1e3*eps;
   wpeak = wpeak-zerotol*(zeta<zerotol);
   wRefine = [wl , wpeak , wr];
end

% Merge grids
w = localMergeGrids(w,wRefine);

%--------------------------
function G = localMergeGrids(G,Gnew)
% Adds frequencies of Gnew to G where density of Gnew is higher.
% Note: Both G and Gnew are in decades (log10 of frequencies).

% Compute density of G
dG = 1./diff(G);

% Combine and sort
nG = numel(G);
[G,is] = sort([G , Gnew]);
IND = (is>nG);
jG = find(~IND);    % G frequencies
jnew = find(IND);   % Gnew frequencies
jnew = jnew(jnew>jG(1) & jnew<jG(end));  % portion of Gnew in G range
if isempty(jnew)
   % Gnew entirely outside G range: just combine
   return
end
js = jnew(1);
je = jnew(end);

% Prune
nf = numel(G);
KEEP = true(1,nf);  
KEEP(jnew) = false;
kG = js-1;  % tracks position in G
j0 = js;
dGav = 0;
for j=js+1:je  % start past first frequency in Gnew
   % Update average density of G on current interval [Gnew(k),Gnew(k+1)]
   dGav = dGav + (G(j)-G(j-1)) * dG(kG);
   if IND(j)
      % Hit Gnew frequency. Compare average density of G and density of
      % Gnew over the interval [G(j0),G(j)]=[Gnew(k),Gnew(k+1)]. Keep
      % grid of highest density.
      % Note:
      %   * dGnew=1 (density of Gnew is normalized)
      %   * Use 0.75 instead of 1 to avoid adding points for only marginal
      %     density gain
      if dGav<0.75
         % Replace G by higher-density Gnew
         KEEP(:,[j0 j]) = true;
         if j>j0+1 % typically false
            KEEP(:,j0+1:j-1) = false;
         end
      end
      % Advance to next Gnew interval
      dGav = 0;  j0 = j;
   else
      % Advance to next G frequency
      kG = kG+1;
   end
end
G = G(:,KEEP);
G(:,G(2:end)==G(1:end-1)) = []; % remove duplicates

%--------------------------
function w = LocalAddPhaseExtrema(w,z,p)
% Computes frequencies of phase extrema for continuous-time systems

% Get NUM and DEN
num = poly(z);
den = poly(p);

% Normalize to avoid overflow (see tbode for example)
num = num/(1+max(abs(num)));
den = den/den(1);
% Avoid empty N2,D2 below
num = [zeros(1,length(num)==1) , num];
den = [zeros(1,length(den)==1) , den];

% Write NUM(jw) = N1(x) + jw N2(x)
%       DEN(jw) = D1(x) + jw D2(x)  with x = w^2
altsgn = ones(1,max(length(num),length(den)));
altsgn(2:2:end) = -1;
n1 = num(end:-2:1);     n1 = fliplr(n1 .* altsgn(1:length(n1)));
n2 = num(end-1:-2:1);   n2 = fliplr(n2 .* altsgn(1:length(n2)));
d1 = den(end:-2:1);     d1 = fliplr(d1 .* altsgn(1:length(d1)));
d2 = den(end-1:-2:1);   d2 = fliplr(d2 .* altsgn(1:length(d2)));

% Extrema are solutions x>0 of
%    [N1*N2+2x*(N2'N1-N1'N2)] * [D1^2+x*D2^2] =
%    [D1*D2+2x*(D2'D1-D1'D2)] * [N1^2+x*N2^2]
lhs1 = psum(conv(n1,n2),2*[psum(conv(polyder(n2),n1),-conv(polyder(n1),n2)) 0]);
lhs2 = psum(conv(d1,d1),[conv(d2,d2) 0]);
rhs1 = psum(conv(d1,d2),2*[psum(conv(polyder(d2),d1),-conv(polyder(d1),d2)) 0]);
rhs2 = psum(conv(n1,n1),[conv(n2,n2) 0]);
p = psum(conv(lhs1,lhs2),-conv(rhs1,rhs2));
try % overflow protection
   x = roots(p);
catch
   x = zeros(0,1);
end

% wp = frequencies of phase extrema (in decades)
wp = log10(real(x(real(x)>0 & abs(imag(x))<10*sqrt(eps)*real(x))))/2;
if ~isempty(wp)
   w = [w , wp'];
end

%--------------------------------------
function w = LocalSparseGrid(wrange,idx)
% Generates an increasingly sparse log-scale grid starting at range(idx)
% and contained in the range interval
inc = cumsum(1:ceil(sqrt(max(0,wrange(2)-wrange(1)))));
if isempty(inc)
   w = zeros(1,0);
elseif idx==1
   w = wrange(1) + inc;
   w = [w(:,w<wrange(2)) , wrange(2)];
else
   w = wrange(2) - fliplr(inc);
   w = [wrange(1) , w(:,w>wrange(1))];
end

function  p = psum(p1,p2)
%PSUM   Sums two polynomials
l1 = length(p1);
l2 = length(p2);
p = [zeros(1,l2-l1) p1] + [zeros(1,l1-l2) p2];

function r = map2s(r,Ts)
% Map to S plane if system is discrete
zr = (abs(r)<1e-2);  % roots near z=0
r(zr,:) = pi/Ts;     % show phase shift up to Nyquist freq. for pseudo delays
r(~zr,:) = log(r(~zr,:))/Ts;


