function h = adamsfir(m,up,lo,wp,ws,K,L)
% h = adamsfir(m,up,lo,wp,ws,K,L)
% An implementation of Adams' approach to constrained weighted
% L2 Low Pass FIR filter design 
% 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
%
%   h  : 2*m+1 filter coefficients
%   m  : degree of cosine polynomial
%   up : [upper bound in passband, stopband]
%   lo : [lower bound in passband, stopband]
%   wp : passband edge 
%   ws : stopband edge 
%   K  : stopband L2 weight / passband L2 weight
%   L  : grid size
% need
%   0 < wp < ws < pi
% example
%   up = [1.03, 0.02]; lo = [0.97, -0.02];
%   wp = 0.27*pi; ws = 0.33*pi;
%   h = adamsfir(25,up,lo,wp,ws,2,2^11);

r = sqrt(2);              
w = [0:L]'*pi/L;
Z = zeros(2*L-1-2*m,1);   
kp = round(L*wp/pi+1);    % index into grid for wp
ks = round(L*ws/pi+1);    % index into grid for ws
q = round((wp+ws)*L/(2*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)];
c = 2*[wp/r; [sin(wp*[1:m])./[1:m]]']/pi;
% ----- construct R matrix --------------------
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); 
a = Ri*c;    % best L2 cosine coefficients
mu = [];     % Lagrange multipliers
SN = 1e-6;   % Small Number
while 1
  % ----- calculate A -------------------------
  A = fft([a(1)*r;a(2:m+1);Z;a(m+1:-1:2)]);
  A = real(A(1:L+1))/2;
  % ----- find extremals ----------------------
  kmax = local_max(A);    kmin = local_max(-A);

  % ---- append bandedges to constraint set ---
  kmin = [kmin; kp];      kmax = [kmax; ks];

  kmax = kmax( A(kmax) > u(kmax)-10*SN );
  kmin = kmin( A(kmin) < l(kmin)+10*SN );
  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;
                        else
                                kmax(zi) = []; n1 = n1 - 1;
                        end
                else % (E0 >= Epi)
                        % remove w = pi from constraint set
                        if piext_type == -1
                                kmin(pii) = []; n2 = n2 - 1;
                        else
                                kmax(pii) = []; n1 = n1 - 1;
                        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;
			% update pii if nec.
			if (piext_type == -1) & (zi < pii)
				pii = pii-1;
			end
		else
			kmax(zi) = []; n1 = n1 - 1;
			% 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;
		else
			kmax(pii) = []; n1 = n1 - 1;
		end

        end

        figure(1)
	plot(w/pi,A)
        hold on
        plot(w(kmax)/pi,A(kmax),'o')
        plot(w(kmin)/pi,A(kmin),'o')
        hold off
	pause(.5)


  % ----- check stopping criterion ------------
  Eup = A(kmax)-u(kmax); Elo = l(kmin)-A(kmin);
  E = max([Eup; Elo; 0]); if E < SN, break, end
	E
  % ----- calculate new multipliers -----------
  O  = [ones(n1,m+1); -ones(n2,m+1)];
  G  = O .* cos(w([kmax;kmin])*[0:m]);
  G(:,1) = G(:,1)/r;
  d  = [u(kmax); -l(kmin)];
  mu = (G*Ri*G')\(G*Ri*c-d);
  % ----- 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 coefficients ----------
  a = Ri*(c-G'*mu);
end
h = [a(m+1:-1:2); a(1)*r; a(2:m+1)]/2;

        E
        figure(1)
        plot(w/pi,A)
        hold on
        plot(w(kmax)/pi,A(kmax),'o')
        plot(w(kmin)/pi,A(kmin),'o')
        hold off