
%
%   srs_function.m  ver 1.3
%
%   by Tom Irvine
%
%   Acceleration SRS using Smallwood ramp invariant digital recursive 
%   filtering relationship
%
%   Fast CPU-only SRS (Smallwood) using batched 2nd-order recursion
%
%    yy = input acceleration array - single column - accel (G)
%    dt = constant time step
%  damp = viscous damping ratio  (damp=0.05 for Q=10)
%     f = array of natural frequencies Hz (pick your own)
%
%  The highest natural frequency should be less than the Nyquist frequency.
%  
%  Ideally, the highest natural frequency should less that one-tenth of
%  the sample rate.
%
%
%  num_fn = number of natural frequencies
%   x_pos = peak positive response acceleration
%   x_neg = peak negative response acceleration
%
%   srs = [ natural frequency(Hz), peak pos(G), absolute value of peak neg(G)]
%
function [srs, cpu_time] = srs_function(yy, dt, damp, f)
%

tic;   % Start timing

% Ensure shapes
yy = yy(:);          % column
f  = f(:).';         % row
N  = length(yy);
num_fn = numel(f);

tpi   = 2*pi;
omega = tpi * f;
omegad = omega .* sqrt(1 - damp^2);

% --------------------------------------------
% 1) Compute Smallwood coefficients (vectorized)
% --------------------------------------------
E = exp(-damp .* omega * dt);
K = omegad * dt;

C  = E .* cos(K);
S  = E .* sin(K);
Sp = S ./ K;

a1 = 2 .* C;
a2 = -(E .^ 2);

b1 = 1 - Sp;
b2 = 2 .* (Sp - C);
b3 = (E .^ 2) - Sp;

% Make them column vectors for broadcasting with states
a1 = a1(:);
a2 = a2(:);
b1 = b1(:);
b2 = b2(:);
b3 = b3(:);

% --------------------------------------------
% 2) Batched recursion for SRS (forward sweep)
%    Equivalent to:
%      forward = [b1(j) b2(j) b3(j)];
%      back    = [1 -a1(j) -a2(j)];
%      resp    = filter(forward, back, yy);
%    but all freqs done in one pass.
% --------------------------------------------
x_pos = -inf(num_fn,1);
x_neg =  inf(num_fn,1);

% Second-order states y(n-1), y(n-2) per frequency
y1 = zeros(num_fn,1);
y2 = zeros(num_fn,1);

% Start at n=3 so we can access yy(n-2:n)
for n = 3:N
    a0  = yy(n);
    a1s = yy(n-1);
    a2s = yy(n-2);

    % y(n) for ALL frequencies at once:
    % y = a1*y(n-1) + a2*y(n-2) + b1*a(n) + b2*a(n-1) + b3*a(n-2)
    y = a1 .* y1 + a2 .* y2 + ...
        b1 .* a0 + b2 .* a1s + b3 .* a2s;

    % Track peaks
    x_pos = max(x_pos, y);
    x_neg = min(x_neg, y);

    % Shift states
    y2 = y1;
    y1 = y;
end

% --------------------------------------------
% 3) Assemble output
% --------------------------------------------
if size(f,2) > size(f,1)
    f = f.';
end

srs = [f, x_pos, abs(x_neg)];

cpu_time = toc;
% fprintf('CPU time for SRS computation (batched): %.6f seconds\n', cpu_time);

end
