%%
%% *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, X, Y, Z)
%%
%% F: array of filter section center frequencies
%% A2BF: vector of A2B matrixes, one for each frequency
%% n: order of the FIR filter being created
%% maxboost: max high and low frequency boost in dB of the final filter
%% Fs: sampling rate
%% Fl: frequency limit, below it use A2B matrixes, above it use W, X, Y, Z shapes
%%
%% for the first run do not supply W/X/Y/Z, render the filters and then run
%% sphear_fir_extract to extract the W, X, Y and Z filter shapes, finally run
%% this again with the extra parameters below
%%
%% W: extracted shape of W filter (used above Fl)
%% X: extracted shape of X filter (used above Fl)
%% Y: extracted shape of Y filter (used above Fl)
%% Z: extracted shape of Z filter (used above Fl)
%%    Z is optional, if not supplied X and Y are averaged to obtain Z
%%
%% 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, maxboost, Fs, Fl, W, X, Y, Z)

  fig = figure;

  if (exist("W") == 1 && ! exist("Z"))
    %% Z was not supplied, just average X and Y data
    Z = (X + Y) / 2;
  end

  %% 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);
	    xlast = X(d);
	    ylast = Y(d);
	    zlast = Z(d);
	  end
	else
	  %% above the limit frequency we interpolate the W, X, Y and Z
	  %% filters if they exist, otherwise keep the last valid value
	  if (b == 1)
	    if (exist("W") == 1)
	      A2BF(d, c, b) = vlast * (W(d)/wlast);
	    else
	      A2BF(d, c, b) = vlast;
	    end
	  end
	  if (b == 2)
	    if (exist("X") == 1)
	      A2BF(d, c, b) = vlast * (X(d)/xlast);
	    else
	      A2BF(d, c, b) = vlast;
	    end
	  end
	  if (b == 3)
	    if (exist("Y") == 1)
	      A2BF(d, c, b) = vlast * (Y(d)/ylast);
	    else
	      A2BF(d, c, b) = vlast;
	    end
	  end
	  if (b == 4)
	    if (exist("Z") == 1)
	      A2BF(d, c, b) = vlast * (Z(d)/zlast);
	    else
	      A2BF(d, c, b) = vlast;
	    end
	  end
	end
      end

      %% fit a spline to the W, X, Y and Z segments and extrapolate
      %% highest frequency if it does not exist in the measurements
      %%
      %% the 200Hz spacing of the points is arbitrary, we should
      %% select something that makes more sense
      fh = (F(dlast-1)/(Fs/2):200/(Fs/2):1)';
      %% make the end really 1.0
      fh(end) = 1;
      rh = interp1(F(dlast-1:end)/(Fs/2), A2BF(dlast-1:end,c,b)', fh, "cubic", "extrap");

      %% we design the spline taking into account one additional point
      %% on the bottom edge so that it will fit better with the existing
      %% low frequency points, now we need to find the exact transition
      %% frequency in the interpolated array
      for idx = 1:size(fh(:))
	if (fh(idx) <= F(dlast)/(Fs/2))
	  istart = idx;
	end
      end
      
      %% extract the low frequency A2B coefficients
      fl = [0 F(1:dlast-1)']' ./ (Fs/2);
      rl = [A2BF(1,c,b) A2BF(1:dlast-1,c,b)']';

      %% concatenate low and high frequency arrays
      fi = cat(1, fl, fh(istart:end));
      ri = cat(1, rl, rh(istart:end));

      printf("low frequency boost %f, high frequency boost %f\n", 20*log10(ri(1)), 20*log10(ri(end)));
      %% force a max boost at low and high frequencies
      if (maxboost != 0)
	if (abs(ri(1)) > 10^(maxboost/20))
	  printf("limit low freq boost from %f to %f dB (%f)\n", 20*log10(ri(1)), maxboost, ri(1));
	  ri(1) = sign(ri(1)) * 10^(maxboost/20);
	  printf("new limit %f\n", ri(1));
	end
	if (abs(ri(end)) > 10^(maxboost/20))
	  printf("limit high freq boost from %f dB to %f dB (%f)\n", 20*log10(ri(end)), maxboost, ri(end));
	  ri(end) = sign(ri(end)) * 10^(maxboost/20);
	  printf("new limit %f\n", ri(end));
	end
      end
      
      %% plot real measured points
      axis([300, (Fs/2)]);
      tics('x', [300, 500, 1000, 2000, 3000, 5000, 7000, 10000, 15000, 20000]);
      grid on;
      title(sprintf("B format FIR filters (transition frequency %d Hz)", Fl));
      if (b == 1)
	dlabel = semilogx(F, 20*log10(abs(A2BF(:,c,b)))','.k');
      else
	dlabel = semilogx(F, 20*log10(abs(A2BF(:,c,b)))','.k');
      end
      hold on;
      %% plot extrapolated spline
      if (b == 1)
	wh = semilogx(fi*(Fs/2), 20*log10(abs(ri)), 'b');
      else
	if (b == 4)
	  %% Z
	  zh = semilogx(fi*(Fs/2), 20*log10(abs(ri)), 'g');
	else
	  %% XY
	  xyh = semilogx(fi*(Fs/2), 20*log10(abs(ri)), 'r');
	end
      end
      
      %% design the filter
      FIR(b, c, :) = fir2(n, fi, ri);
    end
  end
  
  l = legend([wh, zh, xyh], {'W', 'XY', 'Z'});
  legend(l, 'location', 'southwest');

  if (exist("W") == 1)
    print(fig, '-dpng','-color',["figures/w_xyz_filter_full.png"]);
  else
    print(fig, '-dpng','-color',["figures/w_xyz_filter_low.png"]);
  end
end