%
% lowpassResample.m
%
% Function to interpolate a set of samples using an anti-aliasing lowpass
% filter. The behavior matches the behavior of the Lowpass Resampler block
% from the Mixed-Signal Blockset Utilities library.
%
% Inputs:
% t: A vector of input sample times.
% v: A vector of input signal sample values associated with the input
% sample times.
% tq: A vector of sample times for which an interpolated output value is
% required.
% config: A struct to configure the interpolation process. The fields of
% the struct are:
%   OutputRiseFall: The 0% to 100% rise/fall time of the interpolated
%   output. This can also be viewed as the sample time for the 
%   interpolation process, with an anti-aliasing Nyquist cutoff frequency 
%   of 1/(2*OutputRiseFall). Default: 1e-10.
%   NDelay: An integer number of rise/fall times after the interpolation
%   time for which input samples will be considered in the interpolation.
%   See also CausalMode below. Default: 1.
%   SampleMode: Input sampling mode. 'fixed' for fixed step discrete sample
%   time and 'variable' for variable step discrete sample time. Default:
%   'variable'.
%   CausalMode: Set whether or not the interpolation process is to
%   introduce a delay or not. 'off' or 'on'. Default 'off'.
%   When config.CausalMode == 'off', the interpolation process does not
%   introduce any delay between input and output samples.
%   When config.CausalMode == 'on', the interpolation process introduces
%   enough delay between input and output samples that the process is
%   strictly causal.
%
% Outputs:
% vq: A vector of interpolated samples corresponding to the sample times in
% the tq input vector.
% dq: A vector of derives of the interpolated samples corresponding to the
% sample times in the tq input vector.
%
% Choosing config.NDelay:
% The default value of config.NDelay = 1 produces an interpolation that
% has no ringing due to the Gibbs phenomenon, but also has only modest
% reject of out of band numerical artifacts.
% A value of config.NDelay = 5 introduces enough anti-aliasing filtering to
% satisfy most applications but introduces ringing due to the Gibbs
% phenomenon.
% A value of config.NDelay = 10 introduces enough anti-aliasing filtering
% to satisfy demanding applications, at the cost of additional delay and
% computation.
% Values of config.NDelay > 10 are supported, but are not normally needed.
%
% Choosing config.SampleMode:
% The default value of config.SampleMode = 'variable' assumes that the
% input values are the result of a zero order hold process, such that the 
% signal value is always equal to the value of the most recent sample. 
% This choice is appropriate for saturated signals for which the transition
% times are the most important consideration.
% A value of config.SampleMode = 'fixed' assumes that the input values are
% instantaneous samples of a mathematically continuous signal at uniformly
% spaced sample times. This choice is appropriate for signals that are
% subject to a significant amount of analog filtering.
%
% Choosing config.CausalMode:
% The default value of config.CausalMode = 'off' aligns the time scale of
% the interpolation with the time scale of the input. This is appropriate
% when all of the necessary input values are available in a single vector, 
% as is often the case in a MATLAB analysis.
% A value of config.CausalMode = 'on' reproduces the behavior of the
% Lowpass Resampler block in Simulink exactly. In Simulink one must
% wait until the required sample values are available before performing an 
% interpolation. The interpolated sample times are therefore always delayed
% by a constant value with respect to the input sample times. The required
% additional delay in this mode is OutputRiseFall*NDelay.

% copyright 2020, The MathWorks, Inc.

%#codegen

function [vq,dq] = lowpassResample(t,v,tq,config)
% 1. Validate the input variables.
%   Input time and value sizes must be consistent.
[nr ,nc ] = size(t);
[nr1,nc1] = size(v);
[nrq,ncq] = size(tq);
if nr<1 || nc<1 || nr1<1 || nc1<1 || nrq<1 || ncq<1
    error(message('msblks:msblksMessages:lowpassResampleEmptyArray'));
end
if nr1 ~= nr || nc1 ~= nc
    error(message('msblks:msblksMessages:lowpassResampleArraysSameSize'));
end
if nr == 1  %Accept either row or column inputs.
    nin = nc;
elseif nc == 1
    nin = nr;
else
    error(message('msblks:msblksMessages:lowpassResampleOneD'));
end
%   If config fields are not defined, or are not valid, supply the default.
if isfield(config,'OutputRiseFall')
    if isscalar(config.OutputRiseFall) && config.OutputRiseFall > 0
        tau = config.OutputRiseFall;
    else
        tau = 1e-10; %codegen hack
        error(message('msblks:msblksMessages:lowpassResampleDefault',...
                      'config.OutputRiseFall','1e-10'));
    end
else
    tau = 1e-10;
end
if isfield(config,'NDelay')
    if isscalar(config.NDelay) && round(config.NDelay) > 0
        ndelay = round(config.NDelay);
    else
        ndelay = 1; %codegen hack
        error(message('msblks:msblksMessages:lowpassResampleDefault',...
                      'config.NDelay','1'));
    end
else
    ndelay = 1;
end
if isfield(config,'SampleMode')
    switch config.SampleMode
        case 'fixed'
            fixedmode = true;
        case 'variable'
            fixedmode = false;
        otherwise
            fixedmode = false; %codegen hack
            error(message('msblks:msblksMessages:lowpassResampleDefault',...
                          'config.SampleMode','''variable'''));
    end
else
    fixedmode = 'variable';
end
if isfield(config,'CausalMode')
    switch config.CausalMode
        case 'on'
            causalmode = true;
        case 'off'
            causalmode = false;
        otherwise
            causalmode = false; %codegen hack
            error(message('msblks:msblksMessages:lowpassResampleDefault',...
                          'config.CausalMode','''off'''));
    end
else
    causalmode = false;
end
%   Implement the CausalMode feature.
if ~causalmode
    tq = tq+tau*ndelay;
end
%   The interpolation times should be within the valid range for
%   interpolation. This calculation depends on the configuration.
[t,sortorder] = sort(t);
v = v(sortorder);
if nrq == 1 % support row or column
    nout = ncq;
elseif ncq == 1
    nout = nrq;
else
    error(message('msblks:msblksMessages:lowpassResampleOneD').getString);    
end
vq = zeros(nrq,ncq);
dq = zeros(nrq,ncq);
% 2. Set up the anti-aliasing filter.
delaylookup = [1 7 11 10 18];
if ndelay <= 5
    nrange = delaylookup(ndelay);
else
    nrange = round(ndelay*3.6);
end
trange = nrange*tau;
a1 = 1.322857814499375-2.499111671862698j;
a2 = -0.8228578144993748+0.02530261334000322j;
pole1 = (-2.669568357387926+0.799338980797753j)/ndelay;
pole2 = (-1.939344414969149+2.449656871636641j)/ndelay;
anorm1 = a1*pole1/real(a1*pole1*exp(pole1*ndelay) + a2*pole2*exp(pole2*ndelay));
anorm2 = a2*pole2/real(a1*pole1*exp(pole1*ndelay) + a2*pole2*exp(pole2*ndelay));
% 3. Calculate the output.
for indx = 1:nout
    tnow = tq(indx);
    % Find the first and last incoming samples that must be included in the
    % interpolation.
    yndx = 1;
    while yndx+1 <= nin && t(yndx) < tnow-trange
        yndx = yndx+1;
    end
    nfirst = yndx;
    yndx = nin;
    while yndx > 1 && t(yndx) > tnow
        yndx = yndx-1;
    end
    nlast = yndx;
    if fixedmode %   Set up for fixed step mode
        vq(indx) = 0;
        dq(indx) = 0;
        for yndx = nfirst:nlast
            tovertau = (tnow-t(yndx))/tau;
            if abs(tovertau - ndelay) < 0.001 % Taylor series approximation
                x       = pi*(tovertau-ndelay);
                hsin    = 1 - x^2/6 + x^4/120;
               dhsindt = -x/2 + x^3/30 - x^5/840;
            else
                hsin    = sin(pi*(tovertau-ndelay))/pi/(tovertau-ndelay);
                dhsindt = cos(pi*(tovertau-ndelay))/(tovertau-ndelay) - ...
                          sin(pi*(tovertau-ndelay))/pi/(tovertau-ndelay)^2;
            end
            hbt = real(anorm1*exp(pole1*tovertau) + ...
                anorm2*exp(pole2*tovertau) );
            dhbtdt = real(anorm1*pole1*exp(pole1*tovertau) + ...
                      anorm2*pole2*exp(pole2*tovertau) );
            vq(indx) = vq(indx) + hsin*hbt*v(yndx);
            dq(indx) = dq(indx) + ...
                (hsin*dhbtdt + dhsindt*hbt)*v(yndx)/tau;
        end
    else %   Set up for variable step mode
        dq(indx) = 0;
        if t(nfirst) < tnow-trange
            vq(indx) = v(nfirst);
            if nfirst == nin % No more samples to be interpolated.
                continue; 
            end
        elseif nfirst > 1 % Starting value
            vq(indx) = v(nfirst-1);
        else
            vq(indx) = 0;
        end
        % Add in any step responses that occur after the starting value.
        for yndx = nfirst:nlast
            % Calculate sumhaa
            sumhaa = 0;
            for wndx = 0:nrange
                tovertau = (tnow-t(yndx))/tau - wndx;
                if tovertau > 0
                    if abs(tovertau - ndelay) < 0.001 % Taylor series approximation
                        x       = pi*(tovertau-ndelay);
                        hsin    = 1 - x^2/6 + x^4/120;
                    else
                        hsin    = sin(pi*(tovertau-ndelay))/pi/(tovertau-ndelay);
                    end
                    hbt = real(anorm1*exp(pole1*tovertau) + ...
                        anorm2*exp(pole2*tovertau) );
                    sumhaa = sumhaa + hsin*hbt;
                end
            end
            if yndx > 1
                vq(indx) = vq(indx) + sumhaa * (v(yndx)-v(yndx-1));
            else
                vq(indx) = vq(indx) + sumhaa * v(yndx);
            end
        end
    end
end
end