% Convert output of filter design (transfer function) to n-Order Cascaded form.
%
% Outputs:
%   B_coefs = coefficients of numerator in cascaded second order form
%   A_coefs = coefficients of denominator in cascaded second order form
%   k = gain
%
% Inputs:
%   sysd = discrete transfer function in direct form.
%   n = order of cascaded blocks
%   variable argument in:
%       Since Matlab has trouble with repeated roots, there is an option to
%       provide the zeroes.  There are 2 ways to do this.  The first is to
%       provide an array of all the zeroes (e.g. cascade_form(b,a,[z1 z2 z3
%       z4])).  The second is to provide a single number and this number
%       will be used for all the zeroes (e.g. cascade_form(b,a,zero)).
%       This second form is useful for situations where all the zeroes are
%       the same.  This form can also be used for situations where all the
%       zeroes are the same complex conjugate pair.  All that needs to be
%       provided is one of the complex zeroes (not the pair).
%       
%       Providing zeroes is useful for certain filters.
%       Butterworth and Chebyshev I LPF have all zeroes at z=-1.
%       Butterworth and Chevyshev I HPF have all zeroes at z=1.
%       Butterworth and Chebyshev I BPF have half of the zeroes at z=1, half
%         at z=-1, with the number of zeroes equal to twice the order of
%         the filter.  Must use an array to explicity declare all zeroes.
%       Butterworth and Chevyshev I BSF have the same pair of complex
%         conjugate pair zeroes repeated, with the total number of zeroes
%         equal to twice the order of the filter.
%       
%       Chebyshev II and Elliptic filters do not have repeated roots.
%
%
% example:
%   Given the coefficients of a discrete filter's transfer function, the filter
%      in second order cascaded form:
%   
%   b=[coefficients of numerator of discrete filter TF];
%   a=[coeffcieints of denominator of discrete filter TF];
%   sysd=tf(b,a);
%   [B_coefs, A_coefs, k]=cascade_form(sysd, 2);
%
%   B_coefs, A_coefs, k can be used in Write_FDS.m as the coefs and gain
%
% example:
%   Create an 8th order Butterworth lowpass filter in second order casaced form with
%     Sampling frequency = 24kHz
%     Cutoff frequency = 2kHz
%     
%   [b,a,Ts]=butter(8, 2000/24000);
%   sysd=tf(b,a,Ts);
%   [B_coefs, A_coefs, k]=cascade_form(sysd,2,-1);
%     or
%   [B_coefs, A_coefs, k]=cascade_form(sysd,2,[-1 -1 -1 -1 -1 -1 -1 -1]);
%
% example:
%   Create an 8th order Butterworth bandpass filter in continuous, convert
%   to discrete using the Tustin Approximation, and change it to second
%   order cascaded form with
%     Sampling frequency = 24kHz
%     Passband frequencies = 2kHz-3kHz
%
%   [b,a]=butter(6, [2000 3000], 's');
%   sys=tf(b,a);
%   sysd=c2d(sys, 1/24000, 'tustin');
%   [B_coefs, A_coefs, k]=cascade_form(sysd, 2, [1 1 1 1 1 1 -1 -1 -1 -1 -1 -1]);
%

function [B_coefs,A_coefs,k] = cascade_form(sysd,n,varargin)

format short;

[z,p,k] = zpkdata(sysd);
disp([10, 'calculated zeroes: '])
disp(z{1,1})
if (nargin >= 3) %if additional argument added
    if (length(varargin{1,1})==1)
        %if only one term provided, replace all zeroes with that term,
        %where each zero is the conjugate of the previous (for real zeroes,
        %all zeroes will be the same; for complex zeroes, all zeroes will
        %be complex conjugate pairs)
        z{1,1}(1)=varargin{1,1}(1);
        for i=2:length(z{1,1})
            z{1,1}(i)=conj(z{1,1}(i-1));
        end
    elseif (length(varargin{1,1}) == length(z{1,1}))
        %if an array is provided, make sure it is the right length
        z{1,1} = varargin{1,1};
    else
        %if there is an error, display message and use calculated zeroes
        disp([10, 'Invalid array of zeroes. Using calculated zeroes. ', 10, ...
            'Must provide either a single number for all zeroes or ', 10, ...
            'an array containing the correct number of zeroes', 10]);
    end
    disp([10, 'provided zeroes: '])
    disp(z{1,1})
end

A_coefs=[];
B_coefs=[];

i=1; %track how many zeroes/poles used already
terms=0; %count how many terms in the final filter
while (i<=length(z{1,1})-n+1) %continue if there is enough left for full poly
    temp=poly(p{1,1}(i:i+n-1)); %create poly with next n poles
    A_coefs(end+1:end+n)=temp(2:n+1); %append poly to A_coefs, dropping first term
    temp=poly(z{1,1}(i:i+n-1)); %create poly with next n zeroes
    B_coefs(end+1:end+n+1)=temp(1:n+1); %append poly to B_coefs
    i=i+n; 
    terms=terms+1;
end

if (not(mod(length(z{1,1}),n) == 0)) %if there are some left over zeroes/poles
    temp=poly(p{1,1}(i:end)); %create poly with remaining poles
    A_coefs(end+1:end+length(temp)-1)=temp(2:end); %append poly, dropping first term
    A_coefs(end+1:end+1-length(temp)+n)=0; %pad missing terms with 0's
    temp=poly(z{1,1}(i:end)); %create poly with remaining zeroes
    B_coefs(end+1:end+length(temp))=temp(1:end); %append poly
    B_coefs(end+1:end+1-length(temp)+n)=0; %pad missing terms with 0's
    terms=terms+1;
end

format long;

k
B_coefs=B_coefs*k^(1/terms) %adjust with gain
A_coefs

format;

