function [h,wp,ws,L2E,mL2,it,cs] = cl2lpr(n,wo,up,lo,L)
% [h,wp,ws,L2E,mL2,it,cs] = cl2lpr(n,wo,up,lo,L)
% Constrained 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]
%  L  : grid size (optional)
% output:
%  h   : filter of length n
%  wp  : passband edge
%  ws  : stopband edge
%  L2E : L2 error
%  mL2 : minimum L2 error achievable (L2 error of best 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
%  local_max : finds local maxima
% EXAMPLE
%  wo = 0.3*pi; n = 50; 
%  dp = 0.02; ds = 0.004; up = [1+dp, ds]; lo = [1-dp, -ds];
%  [h,wp,ws,L2E,mL2,it,cs] = cl2lpr(n,wo,up,lo);

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

if nargin < 5
   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;
	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(wo*v)./(v*pi)]';
	tt = 0;
end

mL2 = wo/pi - c'*c/2;
L2E = mL2;
a = c;                          % best L2 cosine coefficients
mu = [];                        % Lagrange multipliers
SN = 1e-8;                      % 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*G')\(G*c-d);            

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

   % determine new cosine coefficients
   vv  = G'*mu;
   L2E = vv'*vv/2 + mL2;
   a   = 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));
wp = be(1);
ws = be(2);
cs = [cmax; cmin];