function [x, f, cost] = lpftvd(y, d, fc, lam, Nit)
% [x, f, cost] = lpftvd(y, d, fc, lam, Nit)
% Simultaneous low-pass filtering (LPF) and total variation denoising (TVD).
%
% INPUT
%   y - noisy data
%   d - degree of filter is 2d (use d = 1, 2, or 3)
%   fc - cut-off frequency (normalized frequency, 0 < fc < 0.5)
%   lam - regularization parameter (proporitional to noise sigma)
%   Nit - number of iterations
%
% OUTPUT
%   x - TV component
%   f - LPF component
%   cost - cost function history
%
% The algorithm is based on majorization-minimization and fast
% solvers for banded linear systems.

% Ivan Selesnick,  NYU-Poly, 2012

y = y(:);                                     % convert to column
cost = zeros(1, Nit);                         % cost function history
N = length(y);

[A, B, B1] = ABfilt(d, fc, N);                % banded filter matrices [sparse]
H = @(x) A\(B*x);                             % H : high-pass filter
S = @(x) [0; cumsum(x)];                      % S : cumulative sum 
Id = @(x) x(d+1:N-d);
AAT = A*A';                                   % A*A' : banded matrix [sparse]
b = (1/lam) * B1'*(AAT\(B*y));                % b : (1/lam) * S * H'*H*y
u = -diff(y);                                 % initialization
for k = 1:Nit
    Lam = spdiags(abs(u), 0, N-1, N-1);       % Lam : diag(abs(u)) [sparse]
    F = lam*AAT + B1*Lam*B1';                 % F : banded matrix [sparse]
    u = Lam * (b - (B1'*(F\(B1*(Lam*b)))));   % update
    cost(k) = 0.5*sum(abs(H(y-S(u))).^2) + lam * sum(abs(u));
end

x = S(u);                                     % x : tv (step) component
bn = nan + zeros(d, 1);                       % bn : nan's to extend f to length N
f = y - x - [bn; H(y-x); bn];                 % f : low-pass component
