function yp = vfdiagsp(u, h, p, W, spiopts )
%VFDIAGSP Computes the output of a homogeneous pth-order Volterra filter 
%in the diagonal coordinate representation for a given input signal.
%
%   YP = VFDIAGSP(u, h, p, W) returns the output of the homogeneous
%   pth-order subsystem defined by the kernel h, stored in the diagonal
%   coordinate representation, for the input signal u. W specifies the
%   width of the pth-order kernel representation, that is, the maximum 
%   distance around the main kernel diagonal up to which all diagonals are kept.
%
%   YP = VFDIAGSP(u, h, p, W, spiopts) allows applying a sparse-interpolated
%   strategy where the kernel diagonals are sparsified and the output is
%   filtered by a reconstruction filter spiopts.g, which mitigates the
%   effect of zeroing kernel coefficients. In this case, spiopts.L contains 
%   the interpolation factor, and only the nonzero coefficients are stored 
%   in the diagonals contained by the cell array h.
%
%   The diagonal coordinate representation h must be computed from the 
%   vectorized pth-order Volterra kernel by means of the function kvdiags.
%
%   See also VFDIAGS, KVDIAGS
%
%   CBVK Toolbox
%   JHMG, PMSB 2016

if nargin < 5
    spiopts = [];
end

% Memory of kernel
N = length(h{1});

% If interpolation options are specified, input is padded so that its
% length is divisible by the interpolation factor, L
if ~isempty(spiopts)
    N = spiopts.L*N;
    pad = rem(spiopts.L - rem(length(u),spiopts.L), spiopts.L);
    u = [u; zeros(pad,1)];
end

if nargin < 4
    W = N;
end

% Input signal length
M = length(u);

% Input signal of pth-order subsystem
MM = M + N - 1;
yp = zeros(MM,1);

us = zeros(M,p);
us(:,1) = u;

% Loops over all diagonals of pth-order kernel
wp = zeros(1,p-1);
k = 1;
while ~isempty(wp)

    % Indices of corresponding diagonal
    ii = wp+1;
    
    % Corresponding product of input samples
    for q=2:p
        us(:,q) = [zeros(ii(q-1)-1,1); u(1:end-ii(q-1)+1)];
    end
    up = prod(us,2);
    
    % Computes output due to current diagonal
    ypartial = convsi( h{k} , up, spiopts);
    yp = yp + [ypartial; zeros(MM-length(ypartial),1)];

    % Moves on to next diagonal
    wp = nextinds(wp,W);
    k = k+1;
end;

% Removes added zeros from output, if they have been added (when
% interpolation is active)
if ~isempty(spiopts)
    yp = conv(yp,spiopts.g);
    yp = yp(spiopts.L:end-spiopts.L+1);    
    yp = yp(1:end-pad);
end