function [x_hat, numiter, x_path] = infty_ALPS(y, Phi, K, params)
% =========================================================================
%               infty-ALPS(#) algorithm - Beta Version
% =========================================================================
% Algebraic Pursuit (ALPS) algorithm with infty-memory acceleration. 
% 
% Detailed discussion on the algorithm can be found in 
% [1] "On Accelerated Hard Thresholding Methods for Sparse Approximation", written
% by Volkan Cevher, Technical Report, 2011.
% =========================================================================
% INPUT ARGUMENTS:
% y                         M x 1 undersampled measurement vector.
% Phi                       M x N regression matrix.
% K                         Sparsity of underlying vector x* or desired
%                           sparsity of solution.
% params                    Structure of parameters. These are:
%
%    tol,...                Early stopping tolerance. Default value: tol =
%                           1-e5
%    ALPSiters,...          Maximum number of algorithm iterations. Default
%                           value: 300. 
%    solveNewtonb,...       Value: solveNewtonb = 0. Not used in infty
%                           methods.
%    gradientDescentx,...   If gradientDescentx == 1: single gradient
%                           update of x_{i+1} restricted ot its support with 
%                           line search. Default value: gradientDescentx =
%                           1.
%    solveNewtonx,...       If solveNewtonx == 1: Akin to Hard Thresholding Pursuit
%                           (c.f. Simon Foucart, "Hard Thresholding Pursuit," 
%                           preprint, 2010). Default vale: solveNewtonx = 0
%    tau,...                Variable that controls the momentum in
%                           non-memoryless case. Ignored in memoryless
%                           case. Default value: tau = 1/2.
%                           Special cases:
%                               - tau = -1: momentum step size is automatically
%                               optimized in every step.
%                               - tau as a function handle: user defined
%                               behavior of tau momentum term.
%    mu,...                 Variable that controls the step size selection. 
%                           When mu = 0, step size is computed adaptively 
%                           per iteration. Default value: mu = 0. 
%    cg_maxiter,...         Maximum iterations for Conjugate-Gradients method.
%    cg_tol                 Tolerance variable for Conjugate-Gradients method.    
% =========================================================================
% OUTPUT ARGUMENTS:
% x_hat                     N x 1 recovered K-sparse vector.
% numiter                   Number of iterations executed.
% x_path                    Keeps a series of computed N x 1 K-sparse vectors 
%                           until the end of the iterative process.
% =========================================================================
% 01/04/2011, by Anastasios Kyrillidis. anastasios.kyrillidis@epfl.ch, EPFL.
% =========================================================================
% cgsolve.m is written by Justin Romberg, Caltech, Oct. 2005.
%                         Email: jrom@acm.caltech.edu
% =========================================================================
% This work was supported in part by the European Commission under Grant 
% MIRG-268398 and DARPA KeCoM program #11-DARPA-1055. VC also would like 
% to acknowledge Rice University for his Faculty Fellowship.
% =========================================================================

[~,N] = size(Phi);

%% Initialize transpose of measurement matrix

Phi_t = Phi';

%% Initialize to zero vector
x_cur = zeros(N,1);
y_cur = zeros(N,1);
X_i = [];

x_path = zeros(N, params.ALPSiters);

%% CG params
if (params.solveNewtonx == 1 || params.solveNewtonb == 1)
    cg_verbose = 0;
    cg_A = Phi_t*Phi;
    cg_b = Phi_t*y;
end;

%% Determine momentum step size selection strategy
optimizeTau = 0;
function_tau = 0;

if (isa(params.tau,'float'))
    if (params.tau == -1)
        optimizeTau = 1;
    end;
elseif (isa(params.tau, 'function_handle'))
    function_tau = 1;
end;

%% Determine step size selection strategy
function_mu = 0;
adaptive_mu = 0;

if (isa(params.mu,'float'))
    function_mu = 0;
    if (params.mu == 0)
        adaptive_mu = 1;
    else
        adaptive_mu = 0;
    end;
elseif (isa(params.mu,'function_handle'))
    function_mu = 1;
end;

%% Help variables
complementary_Xi = ones(N,1);
setXi = zeros(N,1);
setYi = zeros(N,1);

i = 1;
%% infty-ALPS(#)
while (i <= params.ALPSiters)
    x_path(:,i) = x_cur;
    x_prev = x_cur;
    
    % Compute the residual
    if (i == 1)
        res = y;
    else
        Phi_x_cur = Phi(:,X_i)*x_cur(X_i);
        res = y - Phi_x_cur;
    end;
    
    % Compute the derivative
    der = Phi_t*res;
    
    % Determine S_i set via eq. (11)
    complementary_Xi(X_i) = 0;    
    [~, ind_der] = sort(abs(der).*complementary_Xi, 'descend');
    complementary_Xi(X_i) = 1;    
    S_i = [X_i; ind_der(1:K)];    
    
    ider = der(S_i);    
    
    setder = zeros(N,1);
    setder(S_i) = 1;
    if (adaptive_mu)
        % Step size selection via eq. (12) and eq. (13)        
        Pder = Phi(:,S_i)*ider;
        mu_bar = ider'*ider/(Pder'*Pder);        
    end;
    
    iy_cur = y_cur.*setYi;    
    if (~function_tau)      % If tau is not a function handle...
        if (optimizeTau)    % Compute optimized tau
            
            % tau = argmin || u - Phi(x_i + y_i) ||
            %     = <Phi*y_i, u - Phi(x_i - mu/2 * grad_Si f(xi))>/||Phi*y_i||^2
            
            if (i == 1)
                params.tau = 0;
            else
                % u - Phi*(x_i - mu/2 grad_Si f(xi)) = u - Phi*b
                if (adaptive_mu)
                    b = x_cur(S_i) + mu_bar*ider;        % Non-zero elems: S_i
                elseif (function_mu)
                    b = x_cur(S_i) + params.mu(i)*ider;
                else b = x_cur(S_i) + params.mu*ider;
                end;
                
                y_Phi_b = y - Phi(:,S_i)*b;               
                Phi_y_prev = Phi(:,Y_i)*y_cur(Y_i);     % Phi * y_i
                params.tau = y_Phi_b'*Phi_y_prev/(Phi_y_prev'*Phi_y_prev);
            end;            
            
            if (adaptive_mu)
                y_cur = params.tau*iy_cur + mu_bar*der.*setder;
            elseif (function_mu)
                y_cur = params.tau*iy_cur + params.mu(i)*der.*setder;
            else y_cur = params.tau*iy_cur + params.mu*der.*setder;
            end;
            
            Y_i = ne(y_cur,0);
            setYi = zeros(N,1);
            setYi(Y_i) = 1;
        else    % Tau fixed and user-defined
            if (adaptive_mu)
                y_cur = params.tau*iy_cur + mu_bar*der.*setder;
            elseif (function_mu)
                y_cur = params.tau*iy_cur + params.mu(i)*der.*setder;
            else y_cur = params.tau*iy_cur + params.mu*der.*setder;
            end;
            
            Y_i = ne(y_cur,0);
            setYi = zeros(N,1);
            setYi(Y_i) = 1;
        end;
    else
        if (adaptive_mu)
            y_cur = params.tau(i)*iy_cur + mu_bar*der.*setder;
        elseif (function_mu)
            y_cur = params.tau(i)*iy_cur + params.mu(i)*der.*setder;
        else y_cur = params.tau(i)*iy_cur + params.mu*der.*setder;
        end;
        
        Y_i = ne(y_cur,0);
        setYi = zeros(N,1);
        setYi(Y_i) = 1;
    end;
       
    % Hard-threshold b and compute X_{i+1}
    set_Xi_Yi = setXi + setYi;
    ind_Xi_Yi = find(set_Xi_Yi > 0);
    z = x_cur(ind_Xi_Yi) + y_cur(ind_Xi_Yi);
    [~, ind_z] = sort(abs(z), 'descend');
    x_cur = zeros(N,1);
    x_cur(ind_Xi_Yi(ind_z(1:K))) = z(ind_z(1:K));
    X_i = ind_Xi_Yi(ind_z(1:K));
    
    setXi = zeros(N,1);
    setXi(X_i) = 1;
    
    if (params.gradientDescentx == 1)
        % Calculate gradient of estimated vector x_cur
        Phi_x_cur = Phi(:,X_i)*x_cur(X_i);        
        res = y - Phi_x_cur;
        der = Phi_t*res;
        
        ider = der(X_i);

        if (adaptive_mu)
            Pder = Phi(:,X_i)*ider;
            mu_bar = ider'*ider/(Pder'*Pder);
            x_cur(X_i) = x_cur(X_i) + mu_bar*ider;
        elseif (function_mu)
            x_cur = x_cur(X_i) + params.mu(i)*ider;
        else x_cur = x_cur(X_i) + params.mu*ider;
        end;
    elseif (params.solveNewtonx == 1)                
        % Similar to HTP
        if (params.useCG == 1)
            [v, ~, ~] = cgsolve(cg_A(X_i, X_i), cg_b(X_i), params.cg_tol, params.cg_maxiter, cg_verbose);
        else
            v = cg_A(X_i,X_i)\cg_b(X_i);
        end;
        x_cur(X_i) = v;
    end;
   
    % Test stopping criterion
    if (i > 1) && (norm(x_cur - x_prev) < params.tol*norm(x_cur))
        break;
    end;
    i = i + 1;
end;

x_hat = x_cur;
numiter = i;

if (i > params.ALPSiters)
    x_path = x_path(:,1:numiter-1);
else
    x_path = x_path(:,1:numiter);
end;
