rwlspq(A,y,p,q)

function x = rwlspq(A,y,p,q)

% Solution to the non-convex optimization problem min||x||_p subject to 
% ||y - Ax||_q < eps
% 
% Copyright (c) Angshul Majumdar 2009

% Input
% A = N X d dimensional measurment matrix
% y = N dimensional observation vector
% p - sparsity of data
% q - model fitting

% Output
% x = estimated sparse signal

if nargin < 3
    p = 1; q = 2;
end
if nargin < 4
    q = 2;
end

explicitA = ~(ischar(A) || isa(A, 'function_handle'));
if (explicitA)
    AOp = opMatrix(A); clear A
else
    AOp = A;
end

% Set LSQR parameters
damp   = 0;
atol   = 1.0e-6;
btol   = 1.0e-6;
conlim = 1.0e+10;
itnlim = length(y);
show   = 0;
OptTol = 1e-6;

MaxIter = 500;
epsilon = 1;

% u_0 is the L_2 solution which would be exact if m = n,
% but in Compressed expectations are that m is less than n

[u1,u_0] = lsqr(@lsqrAOp,y,OptTol,20);
u_old = u_0; % use regularized estimate
j=0;
while (epsilon > 1e-5) && (j < MaxIter)
	j = j + 1;
    wm = (2/p)*((u_old.^(2) + epsilon).^(p/2-1)); % regularization term
    vm = 1./sqrt(wm);
    Wm = opDiag(vm);
    wd = (2/q)*(((AOp(u_old,1)-y).^(2) + epsilon).^(q/2-1)); % data fidelity term
    vd = 1./sqrt(wd);
    Wd = opDiag(vd);
    
	MOp = opFoG(Wd,AOp,Wm);
    ybar = Wd(y,1);
	[t1,t] = lsqr(@lsqrMOp,ybar,OptTol,20); % use regularized estimate
    u_new = Wm(t,1);
	if lt(norm(u_new - u_old,2),epsilon^(1/2)/100)
		epsilon = epsilon /10;
	end
	u_old = u_new;
end
x = u_new;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function y = lsqrAOp(x,transpose)
        switch transpose
            case 'transp'
                y = AOp(x,2);
            case 'notransp'
                y = AOp(x,1);
        end
    end
    function y = lsqrMOp(x,transpose)
        switch transpose
            case 'transp'
                y = MOp(x,2);
            case 'notransp'
                y = MOp(x,1);
        end
    end
end