%%  Filter test Based on http://www.mathworks.com/help/dsp/ref/dsp.biquadfilter-class.html
% Biquad Direct Form 1
% For board F446RE
% Fd12 = 372 * 2; % for 2 sections, filter order 4; V2 of software
Fd12 = 314 * 2; % for 3 sections, filter order 6; V2 of software
%Fd12 = 272 * 2; % for 4 sections, filter order 8; V2 of software

FilterFs=Fd12*10^3;           % sampling frequency, taken from D12 pin
Fc = 20*10^3;                 % 20KHz cut off freq

% design a second order filter:
FilterOrder=6;
RipplePass=1;
RippleStop=80;
FilterType = 'butter';
% FilterType = 'cheby2';
% FilterType = 'ellip';

[MyHd, MySOS, MyG] = HighPassFilterDesign(FilterFs,Fc, FilterOrder, RipplePass, RippleStop, FilterType);

% [z,p,k] = butter(FilterOrder,Wn,'high'); FilterType = 'butter';
% [z,p,k] = cheby2(FilterOrder,RippleStop, Wn, 'high'); FilterType = 'cheby2';
% [z,p,k]=ellip(FilterOrder,RipplePass,RippleStop,Wn,'high'); FilterType = 'ellip';
% Convert to Coeficients matrix
%[SOS,G] = zp2sos(z,p,k);
%MyHd = dfilt.df1sos(SOS,G)

% Plot The filter response:

fvtool(MyHd,'Fs',FilterFs)

% Print filter in the form, suitable for copying into C code
columns = size(MySOS,2);
lines = size(MySOS,1);
fprintf('/*\n%s FILTER DESIGN, %s, %d sections, for sampling frequency %5.1f Hz \n', FilterType, datestr(now), lines, FilterFs);
fprintf('Filter Order %d, Cut Off Frequency %d Hz\n', FilterOrder, Fc);
fprintf('*/\n');
% fprintf('char FilterTypeHP[] = "%s";\n', FilterType);
fprintf('float SOSMatHP[%d][%d] = {\n', lines, columns);
for i =1:lines
    fprintf(' %f, ', MySOS(i,1:columns));
    fprintf('\n');
end
fprintf('\b\b\b\n};\n'); %\b\b\b  - erase <\n>, <space> and <,>
fprintf('float GscaleHP = %f;\n\n',MyG);

% Copy coeficients to the mbed code



