function [h,iwp,iws,L2E,mL2,it,cs] = cwl2lpr(n,wo,up,lo,wp,ws,K,L) 
% function [h,iwp,iws,L2E,mL2,it,cs] = cwl2lpr(n,wo,up,lo,wp,ws,K,L)
% Constrained WEIGHTED L2 Low Pass FIR filter design with Refinement
% (constraint frequncies are refined with Newton's Method)
% Author : Ivan Selesnick, October 94, Rice University
% input:  
%  n  : filter length
%  wo : cut-off frequency in (0,pi)
%  up : [upper bound in pass band, upper bound in stop band]
%  lo : [lower bound in pass band, lower bound in stop band]
%  wp : passband edge of L2 weight function
%  ws : stopband edge of L2 weight function
%  K  : stopband L2 weight / passband L2 weight
%  L  : grid size (optional)
% need
%  wp <= wo <= ws
% output:
%  h   : filter of length n
%  iwp : `induced' passband edge
%  iws : `induced' stopband edge
%  L2E : _unweighted_ L2 error
%  mL2 : minimum _unweighted_ L2 error achievable 
%           (L2 error of best unweighted L2 filter)
%  it  : number of iterations
%  cs  : constraint set at convergence
% subprograms called:
%  frefine : refines the extremal frequencies of an FIR filter
%  ffbe    : finds the band edges of an FIR filter
% EXAMPLE
%  wo = 0.3*pi; wp = 0.28*pi; ws = 0.32*pi; n = 55; K = 3;
%  dp = 0.02; ds = 0.004; up = [1+dp, ds]; lo = [1-dp, -ds];
%  [h,iwp,iws,L2E,mL2,it,cs] = cwl2lpr(n,wo,up,lo,wp,ws,K);

PF = 1;                          % flag : Plot Figures (1:plot figs, 0:don't)

if nargin < 8
   L = 2^ceil(log2(3*n));
end
r = sqrt(2);
w = [0:L]*pi/L;                  % w includes both 0 and pi
q = round(wo*L/pi);
u = [up(1)*ones(q,1); up(2)*ones(L+1-q,1)];
l = [lo(1)*ones(q,1); lo(2)*ones(L+1-q,1)];
if rem(n,2) == 1
	parity = 1;
	m = (n-1)/2;
	% c = 2*[wo/r; [sin(wo*[1:m])./[1:m]]']/pi;
	c = 2*[wp/r; [sin(wp*[1:m])./[1:m]]']/pi;
	Z = zeros(2*L-n,1);
	v = [0:m];
	tt = 1 - 1/r;
else
	parity = 0;
	m = n/2;
	Z = zeros(4*L,1);
	v = [1:m]-1/2;
        c = [2*sin(wp*v)./(v*pi)]';
	tt = 0;
end

% ----- construct R matrix --------------------
if rem(n,2) == 1
   % odd length symmetric filter
   v1 = 1:m;
   v2 = m:2*m;
   tp = [wp+K*(pi-ws), (sin(v1*wp)-K*sin(v1*ws))./v1]/pi;
   hk = ((sin(v2*wp)-K*sin(v2*ws))./v2)/pi;
   R = toeplitz(tp,tp) + hankel(tp,hk);
   R(1,:) = R(1,:)/r;
   R(:,1) = R(:,1)/r;
   Ri = inv(R); 
else
   % even length symmetric filter
   v1 = 1:(m-1);
   v2 = (m-1):2*(m-1);
   tp = [(wp+K*(pi-ws)), (sin(v1*wp)-K*sin(v1*ws))./v1]/pi;
   v1 = 1:m;
   v2 = m:2*m-1;
   tp2 = ((sin(v1*wp)-K*sin(v1*ws))./v1)/pi;
   hk2 = ((sin(v2*wp)-K*sin(v2*ws))./v2)/pi;
   R = toeplitz(tp,tp) + hankel(tp2,hk2);
   Ri = inv(R);
end

% how do you find L2 error for weighted L2 norm ???
% (without estimating it from grid points etc.)

mL2 = wo/pi - c'*c/2;
L2E = mL2;
a = Ri*c;                       % best L2 cosine coefficients
mu = [];                        % Lagrange multipliers
SN = 1e-7;                      % Small Number
it = 0;
% BEGIN LOOPING
while 1

   % calculate H 
   if parity == 1
      H = fft([a(1)*r; a(2:m+1); Z; a(m+1:-1:2)]); 
      H = real(H(1:L+1)/2);
   else
      Z(2:2:2*m) = a;
      Z(4*L-2*m+2:2:4*L) = a(m:-1:1);
      H = fft(Z);
      H = real(H(1:L+1)/2);
   end

   % find local maxima and minima
   kmax = local_max(H);
   kmin = local_max(-H);
   if parity == 0
        n1 = length(kmax);
        if kmax(n1) == L+1
                kmax(n1) = [];
                n1 = n1 - 1;
        end
        n2 = length(kmin);
        if kmin(n2) == L+1
                kmin(n2) = [];
                n2 = n2 - 1;
        end
   end

   % refine frequencies
   cmax = (kmax-1)*pi/L;      cmin = (kmin-1)*pi/L;
   cmax = frefine(a,v,cmax);  cmin = frefine(a,v,cmin);

   % evaluate H at refined frequencies
   Hmax = cos(cmax*v)*a - tt*a(1);
   Hmin = cos(cmin*v)*a - tt*a(1);

   % determine new constraint set
   v1   = Hmax > u(kmax)-10*SN;
   v2   = Hmin < l(kmin)+10*SN;
   kmax = kmax(v1);
   kmin = kmin(v2);
   cmax = cmax(v1);
   cmin = cmin(v2);
   Hmax = Hmax(v1);
   Hmin = Hmin(v2);
   n1   = length(kmax);
   n2   = length(kmin);

   % plot figures
   if PF 
   wv = [cmax; cmin];
   Hv = [Hmax; Hmin];
   figure(1),
	plot(w/pi,H),
	axis([0 1 -.2 1.2])
	hold on,
	plot(wv/pi,Hv,'o'),
	hold off
   figure(2)
	subplot(211)
	plot(w/pi,H-1),
	hold on,
	plot(wv/pi,Hv-1,'o'),
	hold off
	axis([0 wo/pi 2*(lo(1)-1) 2*(up(1)-1)])
	subplot(212)
	plot(w/pi,H),
	hold on,
	plot(wv/pi,Hv,'o'),
	hold off
	axis([wo/pi 1 2*lo(2) 2*up(2)])
   pause(.5)
   end

   % check stopping criterion
   E  = max([Hmax-u(kmax); l(kmin)-Hmin; 0]);
   fprintf(1,'    Bound Violation = %15.13f  L2E = %18.15f\n',E,L2E);
   if E < SN
      fprintf(1,'    I converged in %d iterations\n',it)
      break
   end

   % calculate new Lagrange multipliers
   if parity == 1
      G = [ones(n1,m+1); -ones(n2,m+1)].*cos([cmax; cmin]*[0:m]);
      G(:,1) = G(:,1)/r;
   else
      G = [ones(n1,m); -ones(n2,m)].*cos([cmax; cmin]*([1:m]-1/2));
   end
   d = [u(kmax); -l(kmin)];
   mu = (G*Ri*G')\(G*Ri*c-d);

   % iteratively remove negative multiplier
   [min_mu,K] = min(mu);
   while min_mu < 0
      G(K,:) = [];
      d(K) = [];
      mu = (G*Ri*G')\(G*Ri*c-d);            
      [min_mu,K] = min(mu);
   end

   % determine new cosine coefficients
   vv  = G'*mu;
   L2E = vv'*vv/2 + mL2;
   a   = Ri*(c-vv);

   it = it + 1;

end

if parity == 1
   h = [a(m+1:-1:2)/2; a(1)/r; a(2:m+1)/2];
else
   h = [a(m:-1:1); a]/2;
end

be = ffbe(a,v,[wo,wo],[lo(1),up(2)]+tt*a(1));
iwp = be(1);
iws = be(2);
cs = [cmax; cmin];