function write_fds(b, varargin)
% WRITE_FDS Writes to NI .fds file for use with the Speedy-33
%   WRITE_FDS(Hd,Fs) writes the dfilt.df2sos object to the .fds file
%
%   WRITE_FDS(B,A,Fs) converts the numerator and denominator
%   coefficients to SOS Type-II Form and writes them to the .fds
%   file
%
%   WRITE_FDS(SOS,G,Fs) converts the SOS and gain matrices to the
%   National Instruments format and writes them to the .fds file
%
%   WRITE_FDS(Z,P,K,Fs) converts the ZPK form of a transfer
%   function to SOS Type-II Form and writes it to the .fds file
%
%   WRITE_FDS(TF,Fs) converts the transfter function form to SOS
%   Type-II Form and writes it to the .fds file
%
%   Author: William Lois 
%   Edited by Matthew D'Arcangelo
%   Version: 1.1    
    
    narginchk(2,4);
    
    % Check if it is a dfilt object
    if isa(b, 'dfilt.df2sos') == 1
        sosmat = b.sosMatrix;
        gmat   = b.Scalevalues; % repeat gains for each coeff
        Fs = varargin{1};
    
    % Check if it is already an SOS matrix
    elseif all(size(b)>=[1 1]) & numel(varargin{1}) == 1
        % Input is a matrix, check if it is a valid SOS matrix
        if size(b,2) ~= 6
            error(message('signal:signalanalysisbase:invalidinputsosmatrix'));
        end
        % Checks if SOS is a valid numeric data input
        signal.internal.sigcheckfloattype(b,'','freqz','SOS');
        
        sosmat = b;
        gmat = varargin{1};
        Fs = varargin{2};
        
    % Check if it is a numerator-denominator pair
    elseif nargin == 3
        a = varargin{1};
        Fs = varargin{2};
        [sosmat, gmat] = tf2sos(b, a)
    
    % Check if it is ZPK 
    elseif nargin == 4
        z = b;
        p = varargin{1};
        k = varargin{2};
        Fs = varargin{3};
        
        % convert to sos
        [sosmat, gmat] = zp2sos(z,p,k)
    
    % We will then assume it is a TF
    else
        [b, a] = tfdata(b);
        b = b{:};
        a = a{:};
        [sosmat, gmat] = tf2sos(b,a);
        Fs = varargin{1};
    end
        
    gain = prod(gmat); %Filter gain
    sosmat(1,1:3) = gain*sosmat(1,1:3); % have the first SOS take
                                       % care of the gain of the
                                       % system
    
    % convert the sos and g matrices to the correct format
    b_coefs = reshape(sosmat(:, 1:3).', [1 numel(sosmat(:, 1:3))]);
    a_coefs = reshape(sosmat(:, 5:6).', [1 numel(sosmat(:, 5:6))]);

    % write to the file
    coef2fds(b_coefs,a_coefs,gain,Fs);
    freqz(sosmat);
    title('Frequency Response of Filter');
    disp(['Compare this to the plot on the Speedy-33.' [' If they ' ...
                        'are not similar, there is something wrong.']]);
end


function coef2fds(b_coefs,a_coefs,gain,fs)
% COEF2FDS Perform the write to the fds file
    Num_A_coefs    = length(a_coefs);
    Num_B_coefs    = length(b_coefs);
    order          = Num_B_coefs;

    %The following will write to the output file 
    [FileName,PathName] = uiputfile('*.fds','Select the Output fds Coefficient');
    fid = fopen([PathName FileName], 'w', 'b');
    versionc= '1.0'; % Version (No Need to Change)
    type    = 11;    % Type in Cascaded Second Order Sections Form II
                     % format

    fwrite(fid, 256 ,'uint32');         %No idea what this word means, but data wil not load into NI otherwise
    fwrite(fid, fs ,'double');          %Print Sampling Frequency
    fwrite(fid, 3 ,'uint32');           %No idea what this word means, but data wil not load into NI otherwise
    fwrite(fid, versionc ,'char');      %Print Version Frequency
    fwrite(fid, 0 ,'uint32');           %No idea what this word means, seems to have no impact on load
    fwrite(fid, type ,'uint16');        %Print Filter Type for Original Filter
    fwrite(fid, gain ,'double');        %Print Gain for Original Filter
    fwrite(fid, Num_A_coefs ,'uint32'); %Print Number of A Coefficients for Original Filter
    fwrite(fid, a_coefs ,'double');     %Print A Coefficients for Original Filter
    fwrite(fid, Num_B_coefs ,'uint32'); %Print Number of B Coefficients for Original Filter
    fwrite(fid, b_coefs ,'double');     %Print B Coefficients for Original Filter
    fwrite(fid, type ,'uint16');        %Print A Coefficients for Current Filter
    fwrite(fid, gain ,'double');        %Print Gain for Current Filter
    fwrite(fid, Num_A_coefs ,'uint32'); %Print Number of A Coefficients for Current Filter
    fwrite(fid, a_coefs ,'double');     %Print A Coefficients for Current Filter
    fwrite(fid, Num_B_coefs ,'uint32'); %Print Number of B Coefficients for Current Filter
    fwrite(fid, b_coefs ,'double');     %Print B Coefficients for Current Filter
    fwrite(fid, 0 ,'bit1');             %haven't tested quantized coeficients
    for e = 1:12                    % 
        fwrite(fid, 0 ,'uint16');       %Unknown but necessary for file generation
    end
    fwrite(fid, order ,'uint16');       %Print Filter Order for Current Filter

    fclose(fid);
end
