function [xf, nround] = softthreshold2(x, epsilon)
% Hard threshold filtrering
%           [xf, nround] = softhtreshold2(x, epsilon)
%
%       Input:
% x         Signal of length k * 2^J (J = number of subbands.)
% epsilon   Treshold value. If epsilon is a scalar, the same value is used
%           for all frequency bands. If epsilon is a vector of length J+1,
%           the signal is assumed to be divided to J subbands. The different
%           epsilon vector components are used for different subbands.
%       Output:
% xf        The thresholded signal.
% nround    Number of signal values equal to 0.

nround = 0;
[nrows, ncols] = size(x);

% Specify number of subbands
if length( epsilon ) == 1
    % epsilon is a scalar
    J = 0;
else
    % J subbands
    J = length( epsilon ) - 1;
end

% Verify that x can be divided by 2^J in both dimensions
if (rem(nrows, 2^J) ~= 0) || (rem(ncols, 2^J) ~= 0)
    error('The dimensions of x must be multiples of 2^J.');
end

% Make epsilon vector to a symmetric matrix
e = zeros(J+1,J+1);
for n=1:J+1
    for m=n:J+1
        e(n,m) = epsilon(m);
        e(m,n) = epsilon(m);
    end
end


% Create vectors containing the lengths of subbands row- and columnwise.
rowlen = zeros(J+1,1);
collen = zeros(J+1,1);
rowlen(1) = nrows / 2^J;
collen(1) = ncols / 2^J;
% When k=2, exponent of 2 is J = J+2-2 = J+2-k
% When k=J+1, exponent of 2 is 1 = 1+J-J+1-1 = J+2-k
for k=2:(J+1)
    rowlen(k) = nrows / 2^(J+2-k);
    collen(k) = ncols / 2^(J+2-k);
end

prevrowix = 1;
% krow and kcol specify the partial matrix to be thresholded with epsilon
% value e(krow,kcol). rowixs and colixs give the corresponding row and
% column indices.
for krow=1:(J+1)
    % Row index for this subband
    rowixs = prevrowix:( prevrowix - 1 + rowlen(krow) );
    % update prevrowix
    prevrowix = prevrowix + rowlen(krow);
    % Initialize prevcolix
    prevcolix = 1;
    for kcol=1:(J+1)
        % Column index for this subband
        colixs = prevcolix:( prevcolix - 1 + collen(kcol) );
        prevcolix = prevcolix + collen(kcol);
        % Epsilon for this subband
        epsilonnm = e(krow,kcol);
        for n=rowixs
            for m=colixs
                % Treshold function
                if abs( x(n,m) ) < epsilonnm
                    x(n,m) = 0;
                    nround = nround + 1;
                else
                    x(n,m) = sign( x(n,m) ) * ( abs(x(n,m)) - epsilonnm );
                end         
            end
        end
    end
end

xf = x;