%%
%% *SpHEAR Project
%%
%% sphear_fir_design: design a matrix of FIR filters that equalizes the microphone
%%
%% usage:
%%   [FIR] = sphear_fir_design(F, A2BF, n, Fs, Fl, W, XYZ)
%%
%% F: array of filter section center frequencies
%% A2BF: vector of A2B matrixes, one for each frequency
%% n: order of the FIR filter being created
%% Fs: sampling rate
%%
%% for the first run do not supply Fl/W/XYZ, render the filters and from inspection
%% determine the limit frequency, then run sphear_fir_extract to extract the
%% W and XYZ filter shapes and run this again with the extra parameters below
%%
%% Fl: frequency limit, below it use A2B matrixes, above it use WXYZ shapes
%% W: extracted shape of W filter (used above Fl)
%% XYZ: extracted shape of X, Y and Z filters (used above Fl)
%%
%% Copyright 2016, Fernando Lopez-Lezcano, All Rights Reserved
%% nando@ccrma.stanford.edu
%%
%%  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 <http://www.gnu.org/licenses/>.

function [FIR] = sphear_fir_design(F, A2BF, n, Fs, Fl, W, XYZ)

  FIR = zeros(4, 4, (n + 1));

  %% frequency range
  f = [0 F' Fs/2]' / (Fs/2);

  %% all b format components
  for b = 1:4
    %% all capsules
    for c = 1:4

      for d = 1:size(A2BF(:,1,1))(1)
	if (F(d) <= Fl)
	  %% below the limit frequency we use the A2B coefficients
	  vlast = A2BF(d, c, b);
	  dlast = d;
	  if (exist("W") == 1)
	    wlast = W(d);
	  end
	  if (exist("XYZ") == 1)
	    xyzlast = XYZ(d);
	  end
	else
	  %% above the limit frequency we interpolate the W and XYZ shapes
	  if (b == 1)
	    if (exist("W") == 1)
	      A2BF(d, c, b) = vlast * (W(d)/wlast);
	    else
	      A2BF(d, c, b) = vlast;
	    end
	  else
	    if (exist("XYZ") == 1)
	      A2BF(d, c, b) = vlast * (XYZ(d)/xyzlast);
	    else
	      A2BF(d, c, b) = vlast;
	    end
	  end
	end
      end
      
      %% interpolate last point at nyquist from the previous two points
      r = [ A2BF(1,c,b) A2BF(:,c,b)' (A2BF(end,c,b) + (A2BF(end,c,b) - A2BF(end-1,c,b)))]';

      %% get a spline fitted over the points to smooth the response
      %% endpoints have null first derivative
      fi = (0:200/(Fs/2):1)';
      ri = spline(f, [0 r' 0], fi);

      if (b == 1)
	semilogx(f, r,'*g');
      else
	semilogx(f, r,'*b');
      end
      hold on;
      if (b == 1)
	semilogx(fi,ri,'g');
      else
	semilogx(fi,ri,'r');
      end
      
      %% design the filter
      FIR(b, c, :) = fir2(n, fi, ri);
    end
  end
  
end