function [ xc ] = LqFOCUSS( f, D, xo, q, tol, itmax )
% LqFOCUSS solves for x in f=D x  by minimizing the lq^q norm by the method FOCUSS   
%
% Usage:    [ xc ] = LqFOCUSS( f, D, xo, q )
%           [ xc ] = LqFOCUSS( f, D, xo, q, tol, itmax )
%
% Inputs: 
%   f       data  modelled as f= D xc
%   D       matrix in the model
%   xo      initial solution 
%   q       value for q in the lq norm like measure 
%   tol     tolerance for convergence (default tol = 1e-8)
%   itmax   maximum number of iterations (default itmax=30)
%
% Outputs:
%    xc     solution of minimum lq norm in the model f= D*xc 
%    
% Reference
%   [1] B.D. Rao and K. Kreutz-Delgado,  An Affine Scaling Methodology for Best Basis
%   Selection, IEEE Trans. Sig. Proc. 47 (1999) 187--200

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

  if nargin<6, itmax=30; end
  if(nargin<5) | (isempty(tol)==1); tol=1e-8; end
    [L,N]=size(D);
    Di=1:N;
    res=tol+1;
    it=0;
    while (res>tol & it<itmax)
      Wk=diag(abs(xo).^(1-q/2));
      xn=Wk*(pinv(D*Wk)*f);
      res=norm(xn-xo);
       xo=xn;
      it=it+1;
    end 
    xc=xo;
