function [a,numIts]=cosamp_m(S, Phi, u, varargin)
% Cosamp algorithm
%   Input
%       S : sparsity of the solution
%       Phi : measurement matrix
%       u : measured vector
%       varargin{1} : tolerance for residual.
%   Output
%       a : Solution found by the algorithm
%
% Algorithm as described in "CoSaMP: Iterative signal recovery from 
% incomplete and inaccurate samples" by Deanna Needell and Joel Tropp.
% 
% This implementation was written by David Mary
% --- and corrected by Tomoya Sakai (Chiba University, Japan)
%
% This script/program is released under the Commons Creative Licence
% with Attribution Non-commercial Share Alike (by-nc-sa)
% http://creativecommons.org/licenses/by-nc-sa/3.0/
% Short Disclaimer: this script is for educational purpose only.
% Longer Disclaimer see  http://igorcarron.googlepages.com/disclaimer

eps = 1e-6;
if nargin > 3,
    eps = varargin{1};
end

% Initialization
spzeros = sparse(zeros(size(Phi,2), 1));
a = spzeros;
v = u;
suppa = [];
normq = norm(u);

numIts = 0;
while numIts <= 6*(S+1) && norm(v) > eps * normq
    numIts = numIts + 1;
    v = u - Phi * a;
    [y, yidx] = sort(abs(Phi'*v), 'descend');
    T = sort(union(yidx(1:2*S), suppa));
    Phit = Phi(:,T);
    % CG algorithm would accelerate the next step
    b = lscov(Phit, u);
    [bs, bidx] = sort(abs(b), 'descend');
    a = spzeros;
    suppa = T(bidx(1:S));
    a(suppa) = b(bidx(1:S));
end