rwlsmpq(A,y,group,m,p,q)

function x = rwlsmpq(A,y,group,m,p,q)

% Solution to the non-convex optimization problem min||x||_m,p subject to 
% ||y - Ax||_q < eps
% This algorithm is based upon the Reweighted Least-squares algorithm 
% 
% Copyright (c) Angshul Majumdar 2009

% Input
% A = N X d dimensional measurment matrix
% y = N dimensional observation vector
% m - inner norm for group (default 2)
% p - norm for sparsity of group (default 1)
% q - norm for model fitting (default 2)

% Output
% x = estimated sparse signal

if nargin < 4
    m = 2; p = 1; q = 2;
end
if nargin < 5
    p = 1; q = 2;
end
if nargin < 6
    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;
NGroup = max(group);
for i = 1:NGroup
    GInd{i} = find(group == i);
end
% 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;
    for i = 1:NGroup
        tw1(GInd{i}) = norm(u_old(GInd{i})).^(p-m);
    end
	tw2 = abs(u_old).^(m-2);
    wm = tw1'.*tw2 + epsilon;
    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