function x = OMP(A,b,s)
%OMP Orthogonal Matching Pursuit
%   Solving Ax=b for sparse solution x with sparsity s

[m,N] = size(A);
kmax = s;
%
r = b;
x = zeros(N,1);
S = zeros(kmax,1);
%
Q = zeros(m,kmax);
R = zeros(m,kmax);
z = zeros(kmax,1); % z = R_S x_S

for k=1:kmax    
    Sc = setdiff((1:N)',S(1:k));
    [q,j_Sc] = max( abs(A(:,Sc)'*r) );
    S(k) = Sc(j_Sc);
    
%     % avoid neighboring
%     p=1; nhbr=min(abs(diff(S(1:k)))); 
%     [q,j_Sc] = sort( abs(A(:,Sc)'*r) , 'descend');
%     S(k) = Sc(j_Sc(1));
%     while ( k>=2 && nhbr < 15 )
%         p = p+1;
%         S(k) = Sc(j_Sc(p));
%         display(sprintf('choice of neighbor %d, p =%d', nhbr, p ))
%     end
       
    %[Q,R] = qr(A(:,S(1:k)),0);
    Q(:,k) = A(:,S(k));
    [Q(:,1:k),Rtemp] = qr(Q(:,1:k),0);
    R(1:k,k) = Rtemp(:,k);

    q = Q(:,k);
    z(k) = q'*b;
    
    r = r - z(k)*q;
    
    res = norm(r,2);
    if(res<1e-7)
        break;
    end
end

x(S(1:k)) = R(1:k,1:k)\z(1:k);

end % of function
%%
% 
% [m,N] = size(A);
% kmax = s;
% r = b;
% X = zeros(N,1);
% S = [];
% for k=1:kmax
%     %step1
%     mu = abs(A'*r);
%     [q,lambda] = max(mu);
%     
%     %step2
%     S = [S,lambda]; % display(Lda); %Lda = union(Lda,lambda);
%     A_S = A(:,S);
% 
%     %step3, pseudo inverse
%     x = (A_S'*A_S)\(A_S'*b);
%     
%     %step4, residual
%     r = b - A_S*x;
%     
%     %dump
%     res = norm(r,2);
%     %display(sprintf('k=%d, res=%g', k, res))
%     
%     X(S) = x;
%     if(res<1e-14) % || length(S)==s)
%         return;
%     end
% end

