function [ xo ] = RegFOCUSS( f, D, q, lam, tol, itmax, xo )
% RegFOCUSS solves for xc in f=D xc  by minimizing  ||xc||_q^q + lam(f- Dxc)
%
% It applies  the algorithm given in FOCUSS [1]
%
% Usage:    [xc] = RegFOCUSS( f, D, q )
%           [xc] = RegFOCUSS( f, D, q, lam, tol, itmax, xo )
%
% Inputs:
%   f      data  modelled as f= D xc
%   D      matrix in the model
%   q      value for q in the lq norm like measure
%   lam    regularization parameter (default lam = 1e-8)
%   tol    tolerance for convergence (default tol = 1e-8)
%   itmax  maximum number of iterations (default itmax=30)
%   xo     initial solution (default all the entris equal to 1)
%   []     can be used for itmax, tol and lam
%
% Outputs
%   xo     regularized solution of f= D xc
%
% References
%   [1] B.D. Rao, K. Engan, S. F. Cotter,  J. Palmer, K. Kreutz-Delgado, Subset selection
%   in noise based on diversity measure minimization, IEEE Transactions on Signal
%   Processing, 51, 3  (2003) 760-- 770, 10.1109/TSP.2002.808076. 

% See   http://www.ncrg.aston.ac.uk/Projects/HNLApprox/
%       http://www.nonlinear-approx.info/

%-----------------------------------------------------------------------
   if nargin<7,   xo=ones(size(D,2),1); end
   if nargin<6  | (isempty(itmax)==1); itmax=30; end
   if(nargin<5) | (isempty(tol==1)); tol=1e-8;end
   if(nargin<4) | (isempty(lam==1)); tol=1e-8;end
   tic;
    [L,N]=size(D);Di=1:N;
    xo=ones(N,1);
    res=tol+1; it=0;
    while (res>tol & it<itmax)
      Wk=diag(abs(xo).^(1-q/2));
      DTD=Wk*(D'*D)*Wk+lam*eye(size(D,2));
      xn=Wk*(DTD\(Wk*D'*f));
      res=norm(xn-xo);
      xo=xn;
      it=it+1;
    end
    normf=norm(f-D*xo);
    fprintf(['FOCUSS lasted %g seconds error norm ||(f-f_app)||=' ...
             '%3.8f.\n'],toc,normf);
