function [h,bev,L2E,mL2,it,cs] = cl2mbr(n,mag,be,up,lo)
% [h,bev,L2E,mL2,it,cs] = cl2mbr(n,mag,be,up,lo)
% Constrained L2 MultiBand FIR filter design
% Author: Ivan Selesnick, Rice University, 1995
%  n   : filter length
%  mag : vector of magnitudes
%  be  : vector of band edges
%  up  : vector of upper bounds
%  lo  : vector of lower bounds
% output:
%   h   : filter of length n
%   bev : vector of bandeges
%   L2E : L2 error
%   mL2 : minimum L2 error achievable (L2 error of best L2 filter)
%   it  : number of iterations
%   cs  : constraint set at convergence
%
% example
%   n   = 30;
%   mag = [0 1 0];
%   be  = [0.3, 0.6]*pi;
%   up  = [0.02, 1.02, 0.02]; lo = [-0.02, 0.98, -0.02];
%   h   = cl2mbr(n,mag,be,up,lo);

PF = 0;		% PF : print flag

L = 2^ceil(log2(3*n));
r = sqrt(2);
be = [0; be(:); pi];
if rem(n,2) == 0
        parity = 0;
	m = n/2;
else
        parity = 1;
	m = (n-1)/2;
end

% ----- calculate Fourier coefficients and upper ---
% ----- and lower bound functions ------------------

num_bands = length(mag);
if parity == 1
   c = zeros(m+1,1);
   Z = zeros(2*L-1-2*m,1);
   v = [0:m];
   tt = 1-1/r;
else
   c = zeros(m,1);
   Z = zeros(4*L,1);
   v = [1:m]-1/2;
   tt = 0;
end
u = [];
l = [];
for k = 1:num_bands
   if parity == 1
   	c = c + mag(k) * ...
        	(2*[be(k+1)/r; [sin(be(k+1)*[1:m])./[1:m]]']/pi - ...
         	 2*[be(k)/r;   [sin(be(k)*[1:m])./[1:m]]']/pi);
   else
   	c = c + mag(k) * ...
        	( [4*((sin(be(k+1)*[2*[1:m]-1]/2))./(2*[1:m]-1))/pi]' - ...
          	  [4*((sin(be(k) * [2*[1:m]-1]/2))./(2*[1:m]-1))/pi]' ) ;

   end
   q = round(L*(be(k+1)-be(k))/pi);
   u = [u; up(k)*ones(q,1)];
   l = [l; lo(k)*ones(q,1)];
end
flen = length(u);
if flen < L+1
   ov = ones(L+1-flen,1);
   u(flen+1:L+1) = up(num_bands)*ov;
   l(flen+1:L+1) = lo(num_bands)*ov;
elseif flen > L+1
   u = u(1:L+1);
   l = l(1:L+1);
end

w = [0:L]'*pi/L;
a = c;       % best L2 cosine coefficients
mL2 = sum([diff(be(:)) .* (mag(:).^2)])/pi - c'*c/2;
L2E = mL2;
mu = [];     % Lagrange multipliers
SN = 1e-9;   % Small Number
it = 0;
okmax = []; okmin = []; uvo = 0;
ocmax = []; ocmin = []; lvo = 0;
kmax = []; kmin = [];
cmax = []; cmin = [];

while 1

  if (uvo > SN/2) | (lvo > SN/2) 
     % ----- include old extremal ----------------
     if uvo > lvo
        kmax = [kmax; okmax(k1)]; okmax(k1) = [];
        cmax = [cmax; ocmax(k1)]; ocmax(k1) = [];
     else
        kmin = [kmin; okmin(k2)]; okmin(k2) = [];
        cmin = [cmin; ocmin(k2)]; ocmin(k2) = [];
     end
  else
     % ----- calculate A -------------------------
     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

     if PF
	cc = [cmax; cmin];
	occ = [ocmax; ocmin];
	if length(cc)>0
	   Acc = cos(cc*v)*a - tt*a(1);
        else
           Acc = [];
        end
	if length(occ)>0
	   Aocc = cos(occ*v)*a - tt*a(1);
        else
	   Aocc = [];
        end

        figure(1);
        plot(w/pi,A), hold on
        plot(cc/pi,Acc,'o')
        plot(occ/pi,Aocc,'x')
        hold off

        for k = 1:length(be)-1
         figure(k+1);
         plot(w/pi,A), hold on
         plot(cc/pi,Acc,'o')
         plot(occ/pi,Aocc,'x')
         hold off
         axis([be(k)/pi be(k+1)/pi ...
                mag(k)-1.5*(mag(k)-lo(k)) mag(k)+1.5*(up(k)-mag(k))])
        end
    end

     % ----- find extremals ----------------------
     okmax = kmax;              okmin = kmin;
     ocmax = cmax;              ocmin = cmin;
     kmax = local_max(A);       kmin = local_max(-A);
     if parity == 0
        n1 = length(kmax);
        if kmax(n1) == L+1, kmax(n1) = []; end
        n2 = length(kmin);
        if kmin(n2) == L+1, kmin(n2) = []; end
     end
     cmax = (kmax-1)*pi/L;      cmin = (kmin-1)*pi/L;
     cmax = frefine(a,v,cmax);  cmin = frefine(a,v,cmin);

     Amax = cos(cmax*v)*a - tt*a(1);
     Amin = cos(cmin*v)*a - tt*a(1);
     v1 = Amax > u(kmax)-SN/10;
     v2 = Amin < l(kmin)+SN/10;
     kmax = kmax(v1); kmin = kmin(v2);
     cmax = cmax(v1); cmin = cmin(v2);
     Amax = Amax(v1); Amin = Amin(v2);

     % ----- check stopping criterion ------------
     Eup = Amax-u(kmax); Elo = l(kmin)-Amin;
     E = max([Eup; Elo]);
     fprintf(1,'  Bound Violation = %15.13f  L2E = %18.15f\n',E,L2E);
     if E < SN, break, end
  end

  % ----- calculate new multipliers -----------
  n1 = length(kmax);         n2 = length(kmin);
  if parity == 1
     O  = [ones(n1,m+1); -ones(n2,m+1)];
     G  = O .* cos([cmax;cmin]*[0:m]);
     G(:,1) = G(:,1)/r;
  else
     O  = [ones(n1,m); -ones(n2,m)];
     G  = O .* cos([cmax;cmin]*v);
  end
  d  = [u(kmax); -l(kmin)];
  mu = (G*G')\(G*c-d);

  % ----- remove negative multiplier ----------
  [min_mu,K] = min(mu);
  while min_mu < 0
    G(K,:) = []; d(K) = [];
    mu = (G*G')\(G*c-d);
    if K > n1
       kmin(K-n1) = [];
       cmin(K-n1) = [];
       n2 = n2 - 1;
    else
       kmax(K) = [];
       cmax(K) = [];
       n1 = n1 - 1;
    end
    [min_mu,K] = min(mu);
  end

  % ----- determine new coefficients ----------
  vv  = G'*mu;
  L2E = vv'*vv/2 + mL2;
  a   = c-vv;

  if length(ocmax)>0
     oAmax = cos(ocmax*v)*a - tt*a(1);
     [uvo,k1] = max([oAmax-u(okmax); 0]);
  else
     uvo = 0;
  end
  if length(ocmin)>0
     oAmin = cos(ocmin*v)*a - tt*a(1);
     [lvo,k2] = max([l(okmin)-oAmin; 0]);
  else
     lvo = 0;
  end

  it = it + 1;
end

fprintf(1,'    I converged in %d iterations\n',it)

if parity == 1
   h = [a(m+1:-1:2); a(1)*r; a(2:m+1)]/2;
else
   h = [a(m:-1:1); a]/2;
end
be = be(2:length(be)-1);
Q = [be(:)'; be(:)'];
Q = Q(:);
S = [mag(:)'; mag(:)'];
S = S(:);
S = S(2:length(S)-1);
bev = ffbe(a,v,Q,S+tt*a(1));
cs  = [cmax; cmin];

	Acs = cos(cs*v)*a - tt*a(1);
        figure(1);
        plot(w/pi,A), hold on
        plot(cs/pi,Acs,'o')
        hold off