function [h,rs] = firemb(M,mag,hmw,del);
% [h,rs] = firemb(M,w1,w2,del);
% multiband linear phase FIR filter design with specified ripple sizes
% - the derivative of the frequency response amplitude at the half-magnitude 
%   frequencies are set equal i
% parameters
%  h   : filter of length 2*M+1
%  rs  : reference set upon convergence
%  mag : magnitudes of the ideal
%  hmw : half-magnitude frequencies
%		hmw(i) should be in the interval (0,pi)
%  del : delta for the bands
% subprograms needed
%  localmax, frefine, dropw
%
% (this program has not been tested extensively and may
% have problems at times)
%
% % EXAMPLES
%
%  M   =  32;
%  mag = [0 1 0];
%  del = [0.05 0.05 0.05];
%  hmw = [.3 .7]*pi;
%  [h,rs] = firemb(M,mag,hmw,del);
%
%  M   =  22;
%  mag = [0 1 0 1];
%  del = [0.05 0.05 0.05 0.05];
%  hmw = [.25 .5 .75]*pi; 
%  [h,rs] = firemb(M,mag,hmw,del);
%
%  M   =  32;
%  mag = [0 1 0 1 0];
%  del = [0.05 0.05 0.05 0.05 0.05];
%  hmw = [.2 .4 .6 .8]*pi;
%  [h,rs] = firemb(M,mag,hmw,del);
%
%  M   =  42;
%  mag = [1 0 1 0 1 0];
%  del = [0.05 0.05 0.05 0.05 0.05 0.05];
%  hmw = [0.1667    0.3333    0.5000    0.6667    0.8333]*pi;
%  [h,rs] = firemb(M,mag,hmw,del);


% ------------------ initialize constants ---------------------------
mag = mag(:)';
del = del(:)';
hmw = hmw(:)';
K = length(mag);
L  = 2^ceil(log2(10*M));
w  = [0:L]*pi/L;                         % frequency axis
NR = M +4 - 2*K;                         % number of _nonspecified_ interpolation points
SN = 1e-8;                               % Small Number (stopping criterion)
Z  = zeros(2*(L+1-M)-3,1);
v  = [0, hmw, pi];

Ahm = (mag(1:K-1)+mag(2:K))/2;		% value of A at half-magnitude frequencies.

up = mag + del;
lo = mag - del;

PF = 1;					% PF : flag : Plot Figures

% ------------------ initialize reference set ----------------------------
% parity requirements ==> -1 : doesn't matter, 0 : even, 1 : odd
pv = [-1];
for k = 2:K-1
   if ((mag(k)>mag(k-1))&(mag(k)>mag(k+1))) | ((mag(k)<mag(k-1))&(mag(k)<mag(k+1)))
      pv = [pv, 1];
   else
      pv = [pv, 0];
   end
end
pv = [pv, -1];
% approximate initial no of ref freqs in each band
nb = ceil(NR*(v(2:K+1)-v(1:K))/pi);
f = find(nb==0);
nb(f) = ones(1,length(f));
% ensure correct parity by adding a freq to appropriate bands
for k = 2:K-1
   if pv(k) ~= rem(nb(k),2);
      nb(k) = nb(k)+1;
   end
end
% subtact ref points from each badn so that total number of ref set freqs
% is achieved, while maintaining correct parites.
t = sum(nb)-NR;
while t > 1
   [temp,l] = max(nb);
   nb(l(1)) = nb(l(1)) - 2;
   t = t - 2;
end
if t == 1
   if nb(1) > nb(K)
      nb(1) = nb(1)-1;
   else
      nb(K) = nb(K)-1;
   end
end
% calculate initial reference set
rs = [];
for k = 1:K
   if nb(k)>1
      rs = [rs, v(k)+(v(k+1)-v(k))*[1:nb(k)]/(nb(k)+1)];
   else
      rs = [rs, (v(k)+v(k+1))/2];
   end
   if k < K
      rs = [rs, hmw(k)];
   end
end

% ------------------ initialize interpolation values ---------------------
if mag(1) < mag(2)
   Y = lo(1)*(1-(-1).^(nb(1):-1:1))/2 + up(1)*((-1).^(nb(1):-1:1)+1)/2;
else
   Y = up(1)*(1-(-1).^(nb(1):-1:1))/2 + lo(1)*((-1).^(nb(1):-1:1)+1)/2;
end
Y = [Y, Ahm(1)];
for k = 2:K
   if mag(k)>mag(k-1)
      Y = [Y, up(k)*(1-(-1).^(1:nb(k)))/2 + lo(k)*((-1).^(1:nb(k))+1)/2];
   else
      Y = [Y, lo(k)*(1-(-1).^(1:nb(k)))/2 + up(k)*((-1).^(1:nb(k))+1)/2];
   end
   if k < K
      Y = [Y, Ahm(k)];
   end
end

% ------ calculate derivative constraint vector ---------
V = ones(K-2,1)*(0:M) .* [sin((hmw(1:K-2))'*[0:M])-sin((((-1).^pv(2:K-1)).*hmw(2:K-1))'*[0:M])];


% begin looping
Err = 1;
while Err > SN

   % --------------- calculate cosine coefficients -----------------------
   a = [cos(rs(:)*[0:M]); V]\[Y'; zeros(K-2,1)];

   % --------------- calculate frequency response ------------------------
   H = real(fft([a(1);a(2:M+1)/2;Z;a(M+1:-1:2)/2])); H = H(1:L+1);

   % --------------- determine local max and min -------------------------
   v1 = localmax(H);
   v2 = localmax(-H);
   if v1(1) < v2(1)
      s = 0;
   else
      s = 1;
   end
   ri = sort([v1; v2]);
   rs = (ri-1)*pi/L;
   rs = frefine(a,rs);
   nb = [];         
   f = find(rs<v(2));
   nb(1) = length(f);
   for k = 2:K-1
      f = find((v(k)<=rs)&(rs<=v(k+1)));
      nb(k) = length(f);
   end
   f = find(v(K)<rs);
   nb(K) = length(f);

   % --------------- calculate frequency response at local max and min ---
   Hr = cos(rs*[0:M]) * a;
   Id = [];
   Dr = [];
   for k = 1:K
      Id = [Id; mag(k)*ones(nb(k),1)];
      Dr = [Dr; del(k)*ones(nb(k),1)];
   end 
   Er = (Hr - Id)./Dr;
   if s == 1 
      Er = - Er;
   end

   % Plot Figures if PF == 1
   if PF
   	figure(1),
	plot(w/pi,H),
	hold on,
	plot(rs/pi,Hr,'o'),
	hold off, 
	axis([0 1 -.2 1.2]),
	% pause
   	figure(2),
        plot(Er),
        hold on,
        plot(Er,'o'),
        hold off
   end

   % ------------------ calculate interpolation values ---------------------
   if mag(1) < mag(2)
      Y = lo(1)*(1-(-1).^(nb(1):-1:1))/2 + up(1)*((-1).^(nb(1):-1:1)+1)/2;
   else
      Y = up(1)*(1-(-1).^(nb(1):-1:1))/2 + lo(1)*((-1).^(nb(1):-1:1)+1)/2;
   end
   for k = 2:K
      if mag(k)>mag(k-1)
         Y = [Y, up(k)*(1-(-1).^(1:nb(k)))/2 + lo(k)*((-1).^(1:nb(k))+1)/2];
      else
         Y = [Y, lo(k)*(1-(-1).^(1:nb(k)))/2 + up(k)*((-1).^(1:nb(k))+1)/2];
      end
   end


   % --------------- slim down the set of local max and min --------------
   % --------------- to obtain new reference set of correct size ---------
   if length(rs) > NR
      [rs,Y,Er] = dropw(rs,Y,Er,length(rs)-NR);
   end

   % calculate error
   Err = max(abs(Er))-1;
   fprintf(1,'    Err = %20.15f\n',Err);

   % --------------- update no of ref freq in each band ----------------------------
   nb = [];
   f = find(rs<v(2));
   nb(1) = length(f);
   for k = 2:K-1
      f = find((v(k)<=rs)&(rs<=v(k+1)));
      nb(k) = length(f);
   end
   f = find(v(K)<rs);
   nb(K) = length(f);
   % --------------- update new interpolation points ---------------------
   oldY = Y; % oldY does not include half-magnitude frequencies.
   Y = oldY(1:nb(1));
   csnb = cumsum(nb);
   for k = 2:K
      Y = [Y, Ahm(k-1), oldY(csnb(k-1)+1:csnb(k))];   
   end

   % --------------- update new reference set ----------------------------
   rs = sort([rs(:); hmw(:)]);
end

   
% ------------------ calculate band edges ----------------------------

% ------------------ calcuate filter coefficients ----------------------------
h = [a(M+1:-1:2)/2; a(1); a(2:M+1)/2];

figure(1), plot(w/pi,H), hold on, plot(rs/pi,Y,'o'), hold off
ax = axis;
axis([0 1 -.2 1.2]);
xlabel('w'), ylabel('H'), title('Frequency Response Amplitude')