%% Copyright (C) 2021 Tony Richardson
%%
%% This program is free software: you can redistribute it and/or modify
%% it under the terms of the GNU General Public License as published by
%% the Free Software Foundation, either version 3 of the License, or
%% (at your option) any later version.
%%
%% This program is distributed in the hope that it will be useful,
%% but WITHOUT ANY WARRANTY; without even the implied warranty of
%% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%% GNU General Public License for more details.
%%
%% You should have received a copy of the GNU General Public License
%% along with this program.  If not, see <https://www.gnu.org/licenses/>.

%% -*- texinfo -*-
%% @deftypefn {} {@var{[bt, at]} =} lp2bs (@var{b}, @var{a}, @var{Wo}, @var{Bw})
%% @deftypefnx {}{@var{[At,Bt,Ct,Dt]} =} lp2bs (@var{A}, @var{B}, @var{C}, @var{D}, @var{Wo}, @var{Bw})
%%
%% [bt, at] = lp2bs(b, a, Wo, Bw) transforms an analog lowpass filter in transfer
%% function form to an analog bandstop filter with center frequency Wo and 
%% bandwidth Bw.
%%
%% [At,Bt,Ct,Dt] = lp2bs(A, B, C, D, Wo, Bw)  transforms an analog lowpass filter in state
%% space form to an analog bandstop filter with center frequency Wo and 
%% bandwidth Bw.
%%
%% @seealso{lp2lp, lp2hp, lp2bp}
%% @end deftypefn

%% Author: Tony Richardson <richardson.tony@gmail.com>
%% Created: 2021-04-09

function [At Bt Ct Dt] = lp2bs(A, B, C, D, Wo, Bw)
  if (nargin == 4)
    Wo = C; Bw = D;
    [A B C D] = tf2ss(A, B);
  elseif nargin!=6
    print_usage;
  end
  Q = Wo/Bw;
  [ma, m] = size(A); n = size(B, 2); mc = size(C, 1);
  At = [Wo/Q*inv(A) Wo*eye(ma); -Wo*eye(ma) zeros(ma)];
  Bt = -[Wo/Q*(A\B); zeros(ma,n)];
  Ct = [C/A zeros(mc,ma)];
  Dt = D - C/A*B;
  if (nargout == 2)
    [At Bt] = ss2tf(At, Bt, Ct, Dt);
  endif
end

%!demo
%! n = 10;
%! [z p k] = buttap(n);
%! [b a] = zp2tf(z, p, real(k));
%!
%! w = logspace(-1, 1, 501);
%! H = freqs(b, a, w);
%! figure(1)
%! clf
%! subplot(2, 1, 1)
%! loglog(w, abs(H))
%! ylim([1e-15 1]);
%! yticks(logspace(-15, 0, 4))
%! grid on
%! subplot(2, 1, 2)
%! semilogx(w, rad2deg(angle(H)))
%! grid on
%!
%! fl = 20; fh = 60;
%! Wo = 2*pi*sqrt(fl*fh);
%! Bw = 2*pi*(fh - fl);
%! [bt, at] = lp2bs(b, a, Wo, Bw);
%!
%! w = logspace(1, 3, 501);
%! H = freqs(bt, at, w);
%!
%! figure(2)
%! clf
%! subplot(2, 1, 1)
%! loglog(w, abs(H))
%! ylim([1e-20 1]);
%! yticks(logspace(-20, 0, 3))
%! grid on
%! subplot(2, 1, 2)
%! semilogx(w, rad2deg(angle(H)))
%! grid on
