function [Dm,Wcp] = utComputeDelayMargins(Pm,Wcp,Ts,Td,rtol)
% Utility to calculate delay margins
% Pm is in rads
% Wcp is in rad/s
% Ts is sample time
% Td is total delay
% rtol is relative accuracy on computed crossings/margins

%   Copyright 1986-2021 The MathWorks, Inc.
if nargin < 5
   rtol = 0;
end

% Delay margins: contributions from jw-axis or unit circle
Dm = inf(size(Pm));
nzf = (Wcp~=0);
Dm(:,~nzf & abs(Pm)<rtol) = 0;   % for Pm=0 at Wcp=0
Wcp = Wcp(nzf);
Pm = Pm(nzf);
% Enforce Dm>=-Td (Pm>=-Wcp*Td for Wcp>0, Pm<=-Wcp*Td for Wcp<0)
ix = find(Wcp>0 & Pm<-Td*Wcp-rtol);
Pm(ix) = Pm(ix)+2*pi;
ix = find(Wcp<0 & Pm>-Td*Wcp+rtol);
Pm(ix) = Pm(ix)-2*pi;
Dm(:,nzf) = max(-Td,Pm./Wcp,'includenan');
if Ts
   % Express Dm has a (fractional) multiple of the sample period
   Dm = Dm/Ts;
end