% Lagrange interpolation for resampling
% References:
% [1] A digital signal processing approach to Interpolation
%     Ronald W. Schafer and Lawrence R. Rabiner
%     Proc. IEEE vol 61, pp.692-702, June 1973
% [2] https://ccrma.stanford.edu/~jos/Interpolation/Lagrange_Interpolation.html
% [3] Splitting the Unit delay: Tools for fractional delay filter design
%     T. I. Laakso, V. Valimaki, M. Karjalainen, and U. K. Laine
%     IEEE Signal Processing Magazine, vol. 13, no. 1, pp. 30..60, January 1996
function lagrangeResamplingDemo()
    originDefinition = 0; % see comment for LagrangeBasisPolynomial() below

    % Regarding order: From [1]
    % "Indeed, it is easy to show that whenever Q is odd, none of the
    % impulse responses corresponding to Lagrange interpolation can have
    % linear phase"
    % Here, order = Q+1 => odd orders are preferable
    order = 5;

    % *******************************************************************
    % Set up signals
    % *******************************************************************
    fun = inline('sin(2*pi*x*3) + sin(2*pi*x*5)');
    fs = 40; % sampling freq
    x = 0:1/fs:10-1/fs;
    inData = fun(x);

    nIn = length(inData);
    nOut = 500;

    % Get Lagrange polynomial coefficients
    c0 = lagrangeBasisPolynomial(order, 0, originDefinition);
    c1 = lagrangeBasisPolynomial(order, 1, originDefinition);
    c2 = lagrangeBasisPolynomial(order, 2, originDefinition);
    c3 = lagrangeBasisPolynomial(order, 3, originDefinition);
    c4 = lagrangeBasisPolynomial(order, 4, originDefinition);
    c5 = lagrangeBasisPolynomial(order, 5, originDefinition);

    % *******************************************************************
    % Plot
    % *******************************************************************
    figure(2), clf, hold on
    xr = [0:10/nOut:10-10/nOut]; % fsn = 50Hz
    yr = fun(xr);
    plot(xr,yr,'g')

    % *******************************************************************
    % Apply Lagrange interpolation to input data
    % *******************************************************************
    outData = zeros(1,nOut);
    for i=0:nOut-1

        % outTimeAtInputInteger
        temp = floor( (i/nOut)*nIn );

        % the value of the input sample that appears at FIR tap 'ixTap'
        d0 = inData( mod(temp + 0 - order, nIn) + 1 );
        d1 = inData( mod(temp + 1 - order, nIn) + 1 );
        d2 = inData( mod(temp + 2 - order, nIn) + 1 );
        d3 = inData( mod(temp + 3 - order, nIn) + 1 );
        d4 = inData( mod(temp + 4 - order, nIn) + 1 );
        d5 = inData( mod(temp + 5 - order, nIn) + 1 );

        % Evaluate the Lagrange polynomials, resulting in the time-varying FIR tap weight
        evalFracTime = -0.5 + mod(i,nOut/nIn) * (nIn/nOut);

        cTap0 = polyval(c0, evalFracTime);
        cTap1 = polyval(c1, evalFracTime);
        cTap2 = polyval(c2, evalFracTime);
        cTap3 = polyval(c3, evalFracTime);
        cTap4 = polyval(c4, evalFracTime);
        cTap5 = polyval(c5, evalFracTime);

        % FIR operation: multiply FIR tap weight with input sample
        outData(i+1) = d0*cTap0 + d1*cTap1 + d2*cTap2 + d3*cTap3 + d4*cTap4 + d5*cTap5;

        %plot( i/nOut, outData(i+1), 'b.');
        %pause(0.01)
    end

    plot( xr(1:end-4), outData(5:end), 'b.-');
    legend('Reference', 'Interpolated')
    axis([-0.1 1.2 -2 2])

    snr(yr(1:end-4), outData(5:end))
end

% Returns the coefficients of a Lagrange basis polynomial
% 1 <= order: Polynomial order
% 0 <= evalIx <= order: index of basis function.
%
% At the set of support points, the basis polynomials evaluate as follows:
% evalIx = 1: [1 0 0 ...]
% evalIx = 2: [0 1 0 ...]
% evalIx = 3: [0 0 1 ...]
%
% The support point are equally spaced.
% Example, using originDefinition=0:
% order = 1: [-0.5 0.5]
% order = 2: [-1 0 1]
% order = 3: [-1.5 -0.5 0.5 1.5]
%
% The area around the mid-point corresponds to -0.5 <= x <= 0.5.
% If a resampler implementation uses by convention 0 <= x <= 1 instead, set
% originDefinition=0.5 to offset
% the polynomial.
function polyCoeff = lagrangeBasisPolynomial(order, evalIx, originDefinition)
    assert(evalIx >= 0);
    assert(evalIx <= order);

    tapLocations = -0.5*(order) + (0:order) + originDefinition;

    polyCoeff = [1];
    for loopIx = 0:order
        if loopIx ~= evalIx
            % numerator: places a zero in the polynomial via (x-xTap(k)), with k != evalIx
            % denominator: scales to 1 amplitude at x=xTap(evalIx)
            term = [1 -tapLocations(loopIx+1)] / (tapLocations(evalIx+1)-tapLocations(loopIx+1));

            % multiply polynomials => convolve coefficients
            polyCoeff = conv(polyCoeff, term);
        end
    end

end