function [h,L2E,mL2,it,cs] = adamslpr(n,up,lo,wp,ws,K,L) 
% [h,L2E,mL2,it,cs] = adamslpr(n,up,lo,wp,ws,K,L)
% An implementation of Adams' approach to constrained weighted 
% L2 Low Pass FIR filter design 
% (constraint frequncies are refined with Newton's Method)
% see John Adams' paper: FIR Digital Filters with
%    Least-Squares Stopbands Subject to Peak-Gain Constraints,
%    in IEEE Trans on Circuits and Systems,
%    vol 39, no 4, April 1991
%
% input:  
%  n  : filter length
%  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)
% output:
%  h   : filter of length n
%  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
%  local_max : finds local maxima
%
% EXAMPLE
%  wp = 0.27*pi; ws = 0.333*pi; n = 55; K = 3;
%  dp = 0.03; ds = 0.01; up = [1+dp, ds]; lo = [1-dp, -ds];
%  [h,L2E,mL2,it,cs] = adamslpr(n,up,lo,wp,ws,K);
%
%  % example from Adams' paper
%  wp = 0.0625*2*pi; ws = 0.0804*2*pi; n = 95;
%  ds = 10^(-45/20); 
%  up = [1 ds];
%  lo = [10^(-1/20) -ds];
%  K = 1000;
%  [h,L2E,mL2,it,cs] = adamslpr(n,up,lo,wp,ws,K);

% this program was coded by: Ivan Selesnick, October 95, Rice University

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
kp = round(L*wp/pi+1);
ks = round(L*ws/pi+1);
wo = (wp+ws)/2;
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

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 A (filter response amplitude) 
   if parity == 1
      A = fft([a(1)*r; a(2:m+1); Z; a(m+1:-1:2)]); 
      A = real(A(1:L+1)/2);
   else
      Z(2:2:2*m) = a;
      Z(4*L-2*m+2:2:4*L) = a(m:-1:1);
      A = fft(Z);
      A = real(A(1:L+1)/2);
   end

   % find local maxima and minima
   kmax = local_max(A);
   kmin = local_max(-A);
   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);

   % append band edges to constraint set
   kmax = [kmax; ks];
   kmin = [kmin; kp];
   cmax = [cmax; ws];
   cmin = [cmin; wp];

   % evaluate A at refined frequencies
   Amax = cos(cmax*v)*a - tt*a(1);
   Amin = cos(cmin*v)*a - tt*a(1);

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


   % if constraint set is too big, then trim it down 
   % to its maximum size (m+1).

        % identify w=0 and w=pi as local max or min
        if (n1+n2) > m+1
                [tmp1,i1] = min(kmin);
                [tmp2,i2] = min(kmax);
                if tmp1 == 1
                        zext_type = -1; zi = i1;
                        % (w=0 is a local minimum)
                else
                        zext_type = 1; zi = i2;
                        % (w=0 is a local maximum)
                end
                [tmp1,i1] = max(kmin);
                [tmp2,i2] = max(kmax);
                if tmp1 == L+1
                        piext_type = -1; pii = i1;
                        % (w=pi is a local minimum)
                else
                        piext_type = 1; pii = i2;
                        % (w=pi is a local maximum)
                end
        end
        if (n1+n2) == m+2
                % remove either w=0 or w=pi from
                % constraint set, not both.
                % decide which one to remove:
                if zext_type == 1
                        E0 = A(1) - up(1);
                else
                        E0 = lo(1) - A(1);
                end
                if piext_type == 1
                        Epi = A(L+1) - up(2);
                else
                        Epi = lo(2) - A(L+1);
                end
                if E0 > Epi
                        % remove w = 0 from constraint set
                        if zext_type == -1
                                kmin(zi) = []; n2 = n2 - 1;
				cmin(zi) = []; Amin(zi) = [];
                        else
                                kmax(zi) = []; n1 = n1 - 1;
				cmax(zi) = []; Amax(zi) = [];
                        end
                else % (E0 >= Epi)
                        % remove w = pi from constraint set
                        if piext_type == -1
                                kmin(pii) = []; n2 = n2 - 1;
				cmin(pii) = []; Amin(pii) = [];
                        else
                                kmax(pii) = []; n1 = n1 - 1;
				cmax(pii) = []; Amax(pii) = [];
                        end
                end
        elseif (n1+n2) == m+3
                % remove both w=0 and w=pi from constraint set
                if zext_type == -1
                        kmin(zi) = []; n2 = n2 - 1;
			cmin(zi) = []; Amin(zi) = [];
                        % update pii if nec.
                        if (piext_type == -1) & (zi < pii)
                                pii = pii-1;
                        end
                else
                        kmax(zi) = []; n1 = n1 - 1;
			cmax(zi) = []; Amax(zi) = [];
                        % update pii if nec.
                        if (piext_type == 1) & (zi < pii)
                                pii = pii-1;
                        end
                end
                % remove w = pi from constraint set
                if piext_type == -1
                        kmin(pii) = []; n2 = n2 - 1;
			cmin(pii) = []; Amin(pii) = []; 
                else
                        kmax(pii) = []; n1 = n1 - 1;
			cmax(pii) = []; Amax(pii) = [];
                end

        end

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

   % check stopping criterion
   E  = max([Amax-u(kmax); l(kmin)-Amin; 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

   if E > 20
        fprintf(1,'\nI think your specifications are unrealizable.\n\n')
   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

cs = [cmax; cmin];