function [h,rs,be] = firebp2(n,w1,w2,up,lo);
% [h,rs,be] = firebp2(n,w1,w2,up,lo);
% Bandpass linear phase FIR filter design with specified ripple sizes.
% - the derivative of the frequency response amplitude at w1 is 
%   set equal to the negative derivative of the amplitude at w2 - 
% author : Ivan Selesnick, Rice University, Dec 94
% parameters
%  h   : filter of length 2*n+1
%  rs  : reference set upon convergence
%  be  : band edges of h
%  w1  : first half-magnitude frequency
%  w2  : second half-magnitude frequency
%		w1 < w2,  w1,w2 should be in the interval (0,pi)
%  up, lo : upper and lower bounds in respective bands
% subprograms needed
%  localmax, frefine, firfbe
% % EXAMPLE
%  n   =  61;
%  w1  = 0.15 * 2*pi; 
%  w2  = 0.25 * 2*pi;
%  up = [0.0001 1 0.01];
%  lo = [0 0.99 0];
%  [h,rs,be] = firebp2(n,w1,w2,up,lo);

% ------------------ initialize constants ---------------------------
L = 2^ceil(log2(10*n));
w = [0:L]*pi/L;                         % frequency axis
N = n - 2;                              % number of _nonspecified_ interpolation points
SN = 1e-8;                              % Small Number (stopping criterion)
Z  = zeros(2*(L+1-n)-3,1);
V  = (0:n) .* [sin(w1*[0:n])+sin(w2*[0:n])];

ideal = (up+lo)/2;
del = (up-lo)/2;
D1 = 0.5;				% half-magnitude value at w1
D2 = 0.5;				% half-magnitude value at w2

PF = 1;					% PF : flag : Plot Figures

% ------------------ initialize reference set ----------------------------
% n1 : number of reference frequencies in first stopband
% n2 : number of reference frequencies in passband
% n3 : number of reference frequencies in second stopband
n2 = round(N*(w2-w1)/pi);
if rem(n2,2) == 0, n2 = n2 - 1; end
n1 = floor((N-n2)*w1/(pi-(w2-w1)));
n3 = N-n2-n1;
R1 = w1*[0:n1-1]/n1;
R2 = w1+(w2-w1)*[1:n2]/(n2+1);
R3 = w2+(pi-w2)*[1:n3]/n3;
rs  = [R1, w1, R2, w2, R3]';

% ------------------ initialize interpolation values ---------------------
Y1 = lo(1)*(1-(-1).^(n1:-1:1))/2 + up(1)*((-1).^(n1:-1:1)+1)/2;
Y2 = up(2)*(1-(-1).^(1:n2))/2 + lo(2)*((-1).^(1:n2)+1)/2;
Y3 = lo(3)*(1-(-1).^(1:n3))/2 + up(3)*((-1).^(1:n3)+1)/2;
Y  = [Y1, D1, Y2, D2, Y3];

% begin looping
Err = 1;
while Err > SN
   % --------------- calculate cosine coefficients -----------------------
   a = [cos(rs*[0:n]); V]\[Y'; 0];

   % --------------- calculate frequency response ------------------------
   H = real(fft([a(1);a(2:n+1)/2;Z;a(n+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);
   n1 = sum(rs<w1);
   n2 = sum((w1<rs)&(rs<w2));
   n3 = sum(rs>w2);

   % --------------- calculate frequency response at local max and min ---
   Hr = cos(rs*[0:n]) * a;
   Id = [ideal(1)*ones(n1,1); ideal(2)*ones(n2,1); ideal(3)*ones(n3,1)];
   Dr = [del(1)*ones(n1,1);   del(2)*ones(n2,1);   del(3)*ones(n3,1)];
   Er = (Hr - Id)./Dr;

   % 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]),
   	figure(2),
        plot(Er),
        hold on,
        plot(Er,'o'),
        hold off
	pause(0.05)
   end

   % --------------- calculate new interpolation points  -----------------
   Y1 = lo(1)*(1-(-1).^(n1:-1:1))/2 + up(1)*((-1).^(n1:-1:1)+1)/2;
   Y2 = up(2)*(1-(-1).^(1:n2))/2 + lo(2)*((-1).^(1:n2)+1)/2;
   Y3 = lo(3)*(1-(-1).^(1:n3))/2 + up(3)*((-1).^(1:n3)+1)/2;
   Y = [Y1, Y2, Y3];

   % --------------- slim down the set of local max and min --------------
   % --------------- to obtain new reference set of correct size ---------
   lr = length(rs);
   if (lr-N) == 1
      % remove one frequency from the reference set
      if abs(Er(1)-Er(2)) < abs(Er(lr)-Er(lr-1))
         I = 1; s = s + 1;
      else
         I = lr;
      end
      rs(I) = []; Y(I) = []; Er(I) = []; lr = lr - 1;
   elseif (lr-N) == 2
       % remove two frequencies 
       % remove either the two endpoints or remove two adjacent points in the ref. set
       [temp,k] = min(diff(Er).*((-1).^([1:lr-1]'+s)));
       if (k == 1) | (k == (lr-1))
          if k == 1
              I = 1; s = s + 1;
          else	
              I = lr;
          end
          rs(I) = []; Y(I) = []; Er(I) = []; lr = lr - 1;
          if abs(Er(1)-Er(2)) < abs(Er(lr)-Er(lr-1))
             I = 1; s = s + 1;
          else 
             I = lr;
          end
          rs(I) = []; Y(I) = []; Er(I) = []; lr = lr - 1;
      else
         I = k;
         rs(I) = []; Y(I) = []; Er(I) = []; lr = lr - 1;
         rs(I) = []; Y(I) = []; Er(I) = []; lr = lr - 1;
      end
   elseif (lr-N) == 3
      % remove three frequencies 
      % just use the code for (lr-N)==1, followed by the code for (lr-N)==2
      if abs(Er(1)-Er(2)) < abs(Er(lr)-Er(lr-1))
         I = 1; s = s + 1;
      else
         I = lr;
      end
      rs(I) = []; Y(I) = []; Er(I) = []; lr = lr - 1;
      [temp,k] = min(diff(Er).*((-1).^([1:lr-1]'+s)));
      if (k == 1) | (k == (lr-1))
         if k == 1
            I = 1; s = s + 1;
         else
            I = lr;
         end
         rs(I) = []; Y(I) = []; Er(I) = []; lr = lr - 1;
         if abs(Er(1)-Er(2)) < abs(Er(lr)-Er(lr-1))
            I = 1; s = s + 1;
         else 
            I = lr;
         end
         rs(I) = []; Y(I) = []; Er(I) = []; lr = lr - 1;
      else
         I = k;
         rs(I) = []; Y(I) = []; Er(I) = []; lr = lr - 1;
         rs(I) = []; Y(I) = []; Er(I) = []; lr = lr - 1;
      end
   end

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

   if Err > SN

   % --------------- update new interpolation points ---------------------
   n1 = sum(rs<w1);
   n2 = sum((w1<rs)&(rs<w2));
   n3 = sum(rs>w2);
   Y1 = Y(1:n1);
   Y2 = Y(n1+1:n1+n2);
   Y3 = Y(n1+n2+1:n1+n2+n3);
   Y  = [Y1, D1, Y2, D2, Y3];

   % --------------- update new reference set ----------------------------
   rs = [rs(1:n1); w1; rs(n1+1:n1+n2); w2; rs(n1+n2+1:n1+n2+n3)];

   end

end

   
% ------------------ calculate band edges ----------------------------
be = firfbe(a,[w1,w1,w2,w2],[up(1),lo(2),lo(2),up(3)]);

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

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



