function [accel,veloc,displ] = accel2displ(accel,t,method,aa,t0,tt)
% [accel,displ] = accel2displ(accel,t,method,aa,t0,tt)
% remove bias and drift from an acceleration time series 
% 
% INPUT    DESCRIPTION
% accel    a single acceleration time series as a row-vector    (1 x n)
% t        time vector                                          (1 x n)
% method   'SRA' : subtract running average from accel
%          'ZDV' : correct accel for nearly zero final displ and velocity
%  aa      forgetting factor for SRA - default 0.5*dt
%  t0      midpoint for ZRA, default T/2;
%  tt      timespan for ZRA, default T/20;
%
% 
% OUTPUT   DESRIPTION
% accel    acceleration record with some bias and drift removed  (1 x n)
% veloc    velocity     computed by trapezoidal rule from accel  (1 x n)
% displ    displacement computed by trapezoidal rule from accel  (1 x n)

n   = length(accel);       % number of data points
dt  = t(2)-t(1);           % time step value
T   = t(end);              % time duration of signal

if (nargin < 3) method = 'ZDV';  end
if (nargin < 4) aa = 0.5*dt;     end
if (nargin < 5) t0 = T/2;        end
if (nargin < 6) tt = T/20;       end

switch method

  case 'SRA' % subtract a running average of the acceleration 

    avgA = 0.0;                         % initialize running average

    for k=2:n
      avgA = (1-aa)*avgA + aa*accel(k); % update running average
      accel(k) = accel(k) - avgA;       % subtract running average
    end

  otherwise  % 'ZDV' correct acceleration so final displ and veloc are near zero

    exp_ts = exp(-(t-t0)/tt);   % negative exponent of scaled time

    ac = 1./(1+exp_ts);                   % sigmoidal logistic function
    vc = exp_ts .* (1+exp_ts).^(-2.0)/tt;                    % d(ac)/dt
    dc = exp_ts .* (1+exp_ts).^(-3.0).*(exp_ts-1)/tt^2;      % d(vc)/dt

    for iter = 1:50       % iteratively converge on acceleration correction
      velT  = sum(accel)*dt;
      dspT  = [n:-1:1]*accel'*dt^2;
      accel = accel - velT*vc - dspT*dc;
      if ( abs(velT) < 0.01 && abs(dspT) < 0.01 ) break; end
    end

end

if (nargout > 1) veloc = cumtrapz(accel)*dt; end % veloc from accel via cumtrapz
if (nargout > 2) displ = cumtrapz(veloc)*dt; end % displ from veloc via cumtrapz

% ------------------------------------------------------------------------
% HP Gavin 2023-01-23
