%% -*- mode: octave -*-
%%
%% *SpHEAR Project
%%
%% Common functions for A2B encoder design
%%
%% Copyright 2016-2017, 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/>.

%% make sure octave treats this as a script file
1;

pkg load signal;

%%
%% Read an A-Format signal for specified measurements
%%

global DATA = "../data/";
global CURRENT = "current/";

function [AF, FS] = sphear_af_read(measurements, raw = false);
  global DATA;
  global CURRENT;
  if (raw)
    type = "";
  else
    type = "cal-";
  end
  if (exist([DATA CURRENT "af/a-format-cal-001-5.wav"]))
    %% assume it is an eight capsule microphone
    capnames = cellstr(['1'; '2'; '3'; '4'; '5'; '6'; '7'; '8']);
    capsules = 8;
  else
    %% otherwise a tetrahedral microphone
    capnames = cellstr(['LF'; 'RF'; 'LB'; 'RB']);
    capsules = 4;
  end
  %% read all measurements
  for m = measurements
    %% read all capsules
    for c = 1:capsules
      %% read in calibrated capsule measurement
      [A, FS] = wavread([DATA CURRENT "af/a-format-" type sprintf("%03d", m) "-" char(capnames(c)) ".wav"]);
      AF(m, c, :) = A;
    end
  end
  %% read any up/down measurements
  if (exist([DATA CURRENT "af/a-format-cal-up-001-5.wav"]))
    for m = measurements
      %% read all capsules
      for c = 1:capsules
	%% read in calibrated capsule measurement
	[A, FS] = wavread([DATA CURRENT "af/a-format-" type sprintf("up-%03d", m) "-" char(capnames(c)) ".wav"]);
	AF(m + 16, c, :) = A;
      end
    end
  end
  if (exist([DATA CURRENT "af/a-format-cal-down-001-5.wav"]))
    for m = measurements
      %% read all capsules
      for c = 1:capsules
	%% read in calibrated capsule measurement
	[A, FS] = wavread([DATA CURRENT "af/a-format-" type sprintf("down-%03d", m) "-" char(capnames(c)) ".wav"]);
	AF(m + 16 + 16, c, :) = A;
      end
    end
  end
end

function [AF, FS] = sphear_af_read_capsule(measurements, raw = false);
  global DATA;
  global CURRENT;
  if (raw)
    type = "";
  else
    type = "cal-";
  end
  if (exist([DATA CURRENT "af/capsule_u-cal-001-1.wav"]))
    %% assume it is an eight capsule microphone
    capnames = cellstr(['1'; '2'; '3'; '4'; '5'; '6'; '7'; '8']);
    capsules = 1;
  else
    %% otherwise a tetrahedral microphone
    capnames = cellstr(['LF'; 'RF'; 'LB'; 'RB']);
    capsules = 4;
  end
  %% read all measurements
  for m = measurements
    %% read all capsules
    for c = 1:capsules
      %% read in calibrated capsule measurement
      printf("m:%d -> c:%d\n", m, c);
      [A, FS] = wavread([DATA CURRENT "af/capsule_u-" type sprintf("%03d", m) "-" char(capnames(c)) ".wav"]);
      AF(m, c, :) = A;
    end
  end
end

%%
%% Calculate power for signal in a frequency range
%%
%% cardioid == 1: replace Z with a cardioid pointing at horizontal angle "azim"
%% cardioid == 2: replace Z with a second order lobe pointing at horizontal angle "azim"

function [R] = sphear_signal_power(SIG, FS, fmin, fmax, fft_size, cardioid = 0, azim = 45)
  %% indexes in array for frequency bins
  imin = round((fft_size/2) * fmin / (FS/2));
  imax = round((fft_size/2) * fmax / (FS/2));
  measurements = size(SIG)(1);
  signals = size(SIG,2);
  %% calculate power in frequency range
  for m = (1:measurements)
    for s = (1:signals)
      if (s == 4 && cardioid > 0)
	if (cardioid < 2)
	  %% replace Z with a cardioid
	  spectrum = fft((SIG(m, 1, :) +
			  1.0 * SIG(m, 2, :) * cos(azim/180*pi) +
			  1.0 * SIG(m, 3, :) * sin(azim/180*pi)) / 2, fft_size);
	else
	  %% replace Z with a second order lobe
	  spectrum = fft((SIG(m, 1, :) +
			  (1.0 * SIG(m, 2, :) * cos(azim/180*pi)) +
			  (1.0 * SIG(m, 3, :) * sin(azim/180*pi)) +
			  (1.0 * SIG(m, 5, :) * cos((2 * azim)/180*pi)) +
			  (1.0 * SIG(m, 6, :) * sin((2 * azim)/180*pi))) / 2.5, fft_size);
	end
      else
	spectrum = fft(SIG(m, s, :), fft_size);
      end
      %% return total power of signal in frequency range
      R(m,s,:) = sqrt(sum(spectrum(imin:imax).*conj(spectrum(imin:imax)))/(imax - imin));
    end
  end
end

%%
%% Calculate power for capsule in a frequency range
%%

function [R] = sphear_capsule_power(SIG, FS, fmin, fmax, fft_size)
  %% indexes in array for frequency bins
  imin = round((fft_size/2) * fmin / (FS/2));
  imax = round((fft_size/2) * fmax / (FS/2));
  measurements = size(SIG)(1);
  signals = size(SIG,2);
  %% calculate power in frequency range
  for m = (1:measurements)
    for s = (1:signals)
      spectrum = fft(SIG(m, s, :), fft_size);
      %% return total power of signal in frequency range
      R(m,s,:) = sqrt(sum(spectrum(imin:imax).*conj(spectrum(imin:imax)))/(imax - imin));
    end
  end
end

%%
%% Calculate A to B matrix from matrix of power measurements
%%

function [A2B PV COND ERROR] = sphear_calculate_a2b(R, M)
  if (exist("M"))
    [A2B PV ERROR] = A2B_matrix(R, M);
  else
    [A2B PV ERROR] = A2B_matrix(R);
  end
  %% normalize matrix
  %%
  %% normalize first order components of the matrix first
  A2B = A2B ./ mean(mean(abs(A2B(:,1:4))));
  if (size(A2B, 2) > 4)
    %% S/T/U/V are difference microphones, so at the reference
    %% frequency range their signal is much lower than the
    %% first order components, so normalize separately
    ST_avg = mean(mean(abs(A2B(:,5:6))));
    if (ST_avg != 0)
      A2B(:,5:6) = A2B(:,5:6) ./ ST_avg;
    end
    UV_avg = mean(mean(abs(A2B(:,7:8))));
    if (UV_avg != 0)
      A2B(:,7:8) = A2B(:,7:8) ./ UV_avg;
    end
  end
  %% return condition number
  COND = cond(A2B);
end

%% Render a calibrated capsule impulse response through an A2B matrix
%%
%% m: number of measurement to render
%% A2B: A to B matrix
%% FIR: optional FIR filters
%% fft_size: FFT size for filtering

function [BF, BFF] = sphear_af_to_bf(AF, A2B, FIR, fft_size);
  measurements = size(AF)(1);
  BF = zeros(measurements, size(A2B, 2), size(AF, 3));
  for m = (1:measurements)
    for c = (1:size(AF,2))
      bfc(1,:,:) = reshape(AF(m,c,:), 1, []) .* reshape(A2B(c,:), 1, [])';
      BF(m,:,:) = BF(m,:,:) .+ bfc(1,:,:);
    end
  end
  if exist("FIR")
    if (size(FIR, 1) <= 8)
      %% filter the B format signal through the common FIR filters
      for m = (1:measurements)
	for b = 1:size(FIR, 1)
	  %% we add trailing zeros to let the filter ring
	  BFF(m,b,:) = fftfilt(reshape(FIR(b,:), 1, [])', cat(1, reshape(BF(m,b,:), 1, [])', zeros(2*size(FIR(1,:))(2),1))', fft_size);
	end
      end
    else
      %% first four components use common filters, U/V use filter arrays
      for m = (1:measurements)
	for b = 1:4
	  BFF(m,b,:) = fftfilt(reshape(FIR(b,:), 1, [])', cat(1, reshape(BF(m,b,:), 1, [])', zeros(2*size(FIR(1,:))(2),1))', fft_size);
	end
      end
      capsules = 8;
      for m = (1:measurements)
	for b = 5:6
	  for c = 1:capsules
	    fi = 4 + (b - 5)*capsules + c;
	    if (c == 1)
	      BFF(m,b,:) = fftfilt(reshape(FIR(fi,:), 1, [])', cat(1, reshape(BF(m,b,:), 1, [])', zeros(2*size(FIR(1,:))(2),1))', fft_size);
	    else
	      BFF(m,b,:) = reshape(BFF(m,b,:), size(BFF, 3), 1)' .+ fftfilt(reshape(FIR(fi,:), 1, [])', cat(1, reshape(BF(m,b,:), 1, [])', zeros(2*size(FIR(1,:))(2),1))', fft_size);
	    end
	  end
	end
      end
    end
  else
    BFF = BF;
  end
end

%% Render a calibrated capsule impulse response through UV matrix filters
%%
%% m: number of measurement to render
%% A2B: A to B matrix
%% FIR: optional FIR filters
%% fft_size: FFT size for filtering

function [UV, UVF] = sphear_af_to_bf_new(AF, A2B, FIR, fft_size);
  measurements = size(AF)(1);
  BF = zeros(measurements, size(A2B, 2), size(AF, 3));
  for m = (1:measurements)
    for c = (1:size(AF,2))
      bfc(1,:,:) = reshape(AF(m,c,:), 1, []) .* reshape(A2B(c,:), 1, [])';
      BF(m,:,:) = BF(m,:,:) .+ bfc(1,:,:);
    end
  end
  if exist("FIR")
    if (size(FIR, 1) <= 6)
      %% filter the B format signal through four or six FIR filters
      for m = (1:measurements)
	for b = 1:size(FIR, 1)
	  %% we add trailing zeros to let the filter ring
	  BFF(m,b,:) = fftfilt(reshape(FIR(b,:), 1, [])', cat(1, reshape(BF(m,b,:), 1, [])', zeros(2*size(FIR(1,:))(2),1))', fft_size);
	end
      end
    else
      %% filter the B format signal through single filters for first order
      %% and array of filters for second order
    end
  else
    BFF = BF;
  end
end

%%
%% Calculate B format power in logarithmic frequency bands
%%

function [SP, FC, FE] = sphear_signal_power_all(SIG, FS, fmin, fmax, frefmin, frefmax, Noct, fft_size)
  %% calculate number of frequency bands spaced Noct
  n = floor(log(fmax/fmin)/log(2^(1/Noct)));
  %% adjust width of band so they fit exactly in frequency range
  N = log(2)/(log(fmax/fmin)/n);
  printf("%d frequency bands, %f bands per octave\n", n, N);

  %% first center frequency
  fcmin = fmin * 2^(1/(N * 2));
  %% all center and edge frequencies
  FC = fcmin * 2.^((0:n - 1)/N)';
  FE = fmin * 2.^((0:n)/N)';

  %% normalize power with respect to reference frequency band
  %% (measurement,signal,power)
  ref = sphear_signal_power(SIG, FS, frefmin, frefmax, fft_size);
  norm = mean(ref(:,:));

  %% we only normalize with respect to WXYZ, STUV in the frequency range
  %% where WXYZ are well behaved has low power (high pass filter due
  %% to the difference microphones) so we don't want to average that.
  %% If we do that the A2B coefficients are much higher than the ones
  %% for WXYZ and we get clipping for signals close to full scale (and
  %% then attenuation in the filtering stage after that)
  WXYZnorm = mean(ref(:,[1,2,3,4]));
  XYavg = mean(WXYZnorm([2,3]));
  %% use XY average for STUV
  norm = cat(1, WXYZnorm', [XYavg], [XYavg], [XYavg], [XYavg])';

  for i = (1:size(FC))
    %% (measurement,signal,band,power)
    SP(:,:,i) = sphear_signal_power(SIG, FS, FE(i), FE(i + 1), fft_size) ./ norm;
  end
end

%%
%% Extract average shapes of B format filters
%%

%% measurements:
%% 0 -> use defaults
%% 1 -> use all measurements
%% 2 -> use cardinal measurements
%% 3 -> use diagonal measurements

function [W X Y Z S T U V] = sphear_bf_extract_filter_shapes(B, measurements)
  %% cardinal and diagonal directions for four capsule array
  TETRA_CARDINALS = [1, 5, 9, 13];
  TETRA_DIAGONALS = [3, 7, 11, 15];
  %% cardinal and diagonal directions for eight capsule array
  OCTA_CARDINALS = [1, 3, 5, 7, 9, 11, 13, 15];
  OCTA_DIAGONALS = [2, 4, 6, 8, 10, 12, 14, 16];
  %% best directions for octa WXY
  OCTA_FLAT = OCTA_DIAGONALS;
  OCTA_BRIGHT = OCTA_CARDINALS;
  %% best directions for octa Z
  OCTA_FLAT_Z = [OCTA_FLAT + 16, OCTA_FLAT + 32];
  %% best directions for octa UV
  U_DIRS = [1, 5, 9, 13];
  V_DIRS = [3, 7, 11, 15];
  %% best directions for octa ST (up and down layers)
  S_DIRS = [U_DIRS + 16, U_DIRS + 32];
  T_DIRS = [V_DIRS + 16, V_DIRS + 32];
  %% all directions
  ALL_DIRS = [1:16];

  if (measurements == 0)
    %% cardinal directions
    if (size(B, 2) > 4)
      %% 8 capsules
      %% averaging all measurements for W seems to do better
      %%W_m = [OCTA_FLAT, OCTA_FLAT + 16, OCTA_FLAT + 32];
      %%Y_m = [OCTA_FLAT, OCTA_FLAT + 16, OCTA_FLAT + 32];
      %%X_m = [OCTA_FLAT, OCTA_FLAT + 16, OCTA_FLAT + 32];
      W_m = OCTA_FLAT;
      Y_m = OCTA_FLAT;
      X_m = OCTA_FLAT;
      if (size(B, 1) > 16)
	Z_m = OCTA_FLAT_Z;
      else
	Z_m = W_m;
      end
      S_m = S_DIRS;
      T_m = T_DIRS;
      U_m = U_DIRS;
      V_m = V_DIRS;
    else
      %% four capsules
      W_m = TETRA_CARDINALS;
      X_m = TETRA_CARDINALS;
      Y_m = TETRA_CARDINALS;
      Z_m = W_m;
    end
  else
    if (measurements == 1)
      %% diagonal directions
      if (size(B, 2) > 4)
	%% 8 capsules
	W_m = OCTA_BRIGHT;
	Y_m = OCTA_BRIGHT;
	X_m = OCTA_BRIGHT;
	Z_m = W_m;
	U_m = U_DIRS;
	V_m = V_DIRS;
      else
	%% four capsules
	W_m = TETRA_DIAGONALS;
	X_m = TETRA_DIAGONALS;
	Y_m = TETRA_DIAGONALS;
	Z_m = W_m;
      end
    else
      if (measurements == 2)
	%% average all measurements for all components
	W_m = ALL_DIRS;
	X_m = ALL_DIRS;
	Y_m = ALL_DIRS;
	Z_m = ALL_DIRS;
	U_m = ALL_DIRS;
	V_m = ALL_DIRS;
      end
    end
  end
  %% extract average response shape for W
  W = 1.0 ./ reshape(mean(B(W_m,1,:)), size(B(:,:,:))(3), 1);
  %% extract average response shape for XYZ
  X = 1.0 ./ reshape(mean(B(X_m,2,:)), size(B(:,:,:))(3), 1);
  Y = 1.0 ./ reshape(mean(B(Y_m,3,:)), size(B(:,:,:))(3), 1);
  Z = 1.0 ./ reshape(mean(B(Z_m,4,:)), size(B(:,:,:))(3), 1);
  if (size(B, 2) > 4)
    if (size(B, 1) > 16)
      %% full 3d measurements
      S = 1.0 ./ reshape(mean(B(S_m,5,:)), size(B(:,:,:))(3), 1);
      T = 1.0 ./ reshape(mean(B(T_m,6,:)), size(B(:,:,:))(3), 1);
    else
      %% flat 2d measurements, no S/T filters (ie: flat 1.0 filter)
      S = 1.0 ./ ones(size(B, 3), 1);
      T = 1.0 ./ ones(size(B, 3), 1);
    end
    U = 1.0 ./ reshape(mean(B(U_m,7,:)), size(B(:,:,:))(3), 1);
    V = 1.0 ./ reshape(mean(B(V_m,8,:)), size(B(:,:,:))(3), 1);
  end
end

%%
%% Extract average shapes of capsule filters
%%
%% (using for now only the horizontal plane measurements)

function [SHAPE] = sphear_extract_capsule_filter_shapes(A)
  %% for each capsule
  for c = (1:size(A,2))
    %% get indexes of measurements to average for each capsule
    mlll = mod(16 - (c * 2) + 0, 16) + 1;
    mll = mod(16 - (c * 2) + 1, 16) + 1;
    ml = mod(16 - (c * 2) + 2, 16) + 1;
    m = mod(16 - (c * 2) + 3, 16) + 1;
    mr = mod(16 - (c * 2) + 4, 16) + 1;
    mrr = mod(16 - (c * 2) + 5, 16) + 1;
    mrrr = mod(16 - (c * 2) + 6, 16) + 1;
    for n = (1:5)
      mm(n,:) = mod(16 - (c * 2) - n + 2, 16) + 1;
      mm(n + 5,:) = mod(16 - (c * 2) + n + 2, 16) + 1;
    end
    printf("c:%d -> %d %d %d %d %d %d %d\n", c, mlll, mll, ml, m, mr, mrr, mrrr);
    SHAPE(c,:) = (1.0 ./ reshape(mean(A([mll,ml,m,mr,mrr],c,:)), size(A,3), 1));
    %% average all directions
    %% SHAPE(c,:) = (1.0 ./ reshape(mean(A(:,c,:)), size(A,3), 1));
  end
end

%%
%% Filter signals through capsule correction filters
%%

function [AFF] = sphear_filter_capsule_signals(AF, FIR, fft_size);
  measurements = size(AF, 1);
  capsules = size(AF, 2);
  samples = size(AF,3);
  AFF = zeros(measurements, capsules, samples);
  for m = (1:measurements)
    for c = (1:capsules)
      %% we add trailing zeros to let the filter ring
      AFF(m,c,:) = fftfilt(reshape(FIR(c,:), 1, [])', cat(1, reshape(AF(m,c,:), 1, [])', zeros(2*size(FIR(1,:))(2),1))', fft_size)(1:samples);
    end
  end
end

%%
%% Calculate average power for each B format component and normalize A2B matrix
%%

function [A2B XY_gain, S_gain, T_gain, U_gain, V_gain, B_power] = sphear_bf_normalize_gain(A2B, B, FS, fmin, fmax, fft_size)
  do_Z = false;
  if (size(B,1)/3 > 8)
    %% we assume we have three sets of measurements and can do Z averaging
    do_Z = true;
  end
  %% (measurement,signal,band,power)
  B_power = sphear_signal_power(B, FS, fmin, fmax, fft_size);
  %% W, all measurements
  W_avg = mean(B_power(:,1));
  %% XY, peak of first order lobes
  XY_avg = mean([mean(B_power([1,9], 2)), mean(B_power([5, 13], 3))]);
  XY_gain = XY_avg/W_avg;
  if (do_Z)
    %% Z, +-elev weighted by elevation angle
    %% FIXME: we just hardwire the elevation angle for now
    Z_avg = mean(B_power([17:32,33:48],4)) / sin(43/180*pi);
    Z_gain = Z_avg/W_avg;
  else
    Z_gain = 0.0;
  end
  if (size(A2B, 2) > 4)
    %% UV, peak of second order lobes
    U_avg = mean(mean(B_power([1,5,9,13], 7)));
    U_gain = U_avg/W_avg;
    V_avg = mean(mean(B_power([3,7,11,15], 8)));
    V_gain = V_avg/W_avg;
    if (abs(mean(mean(A2B(:,[5,6])))) > 0)
      %% calculate peak value of s/t at measured elevation
      el = 43.0;
      s_t_val = 1.0 * sin((2*el)/180*pi);
      %% ST, peaks of lobes at +-elevation
      S_avg = mean([mean(B_power([1,9] + 16, 5)), mean(B_power([1,9] + 32, 5))]);
      S_gain = (S_avg/W_avg) * s_t_val;
      T_avg = mean([mean(B_power([5,13] + 16, 6)), mean(B_power([5, 13] + 32, 6))]);
      T_gain = (T_avg/W_avg) * s_t_val;
    else
      S_gain = 0.0;
      T_gain = 0.0;
    end
  else
    U_gain = 0.0;
    V_gain = 0.0;
  end
  %% normalize the A2B matrix
  if (do_Z)
    A2B(:,[2,3]) = A2B(:,[2,3]) / XY_gain;
    A2B(:,[4]) = A2B(:,[4]) / Z_gain;
  else
    A2B(:,[2,3,4]) = A2B(:,[2,3,4]) / XY_gain;
  end
  if (size(A2B, 2) > 4)
    if (abs(mean(mean(A2B(:,[5,6])))) > 0)
      printf("correct ST %f %f\n", S_gain, T_gain);
      A2B(:,[5]) = A2B(:,[5]) / S_gain;
      A2B(:,[6]) = A2B(:,[6]) / T_gain;
    end
    A2B(:,[7]) = A2B(:,[7]) / U_gain;
    A2B(:,[8]) = A2B(:,[8]) / V_gain;
  else
  end
  %% we want the W contribution of each capsule to be 1/8,
  %% so scale the whole matrix relative to mean W
  A2B(:,:) = A2B(:,:) / (mean(A2B(:, 1)) * 8);
  %% print gain scaling coefficients
  printf("XY gain: %f, Z gain: %f, ST: %f|%f, UV: %f|%f\n", XY_gain, Z_gain, S_gain, T_gain, U_gain, V_gain);
end

%%
%% Calculate A2B coefficients in logarithmic frequency bands
%%

function [A2BF] = sphear_calculate_a2b_all(SIG, FS, fmin, fmax, frefmin, frefmax, Noct, fft_size)
  %% calculate number of frequency bands spaced Noct
  n = floor(log(fmax/fmin)/log(2^(1/Noct)));
  %% adjust width of band so they fit exactly in frequency range
  N = log(2)/(log(fmax/fmin)/n);
  printf("%d frequency bands, %f bands per octave\n", n, N);

  %% first center frequency
  fcmin = fmin * 2^(1/(N * 2));
  %% all center and edge frequencies
  FC = fcmin * 2.^((0:n - 1)/N)';
  FE = fmin * 2.^((0:n)/N)';

  band = 1;
  for i = (1:size(FC))
    %% get power in band
    R = sphear_signal_power(SIG, FS, FE(i), FE(i + 1), fft_size);
    %% calculate a2b matrix in band
    [a2b, pv, cond] = sphear_calculate_a2b(R);
    A2BF(band,:,:) = a2b;
    band = band + 1;
  end
end

%%
%% Plot coefficients of A2B matrices as a function of frequency
%%

function sphear_plot_a2b_all(FC, A2B, fmin, fmax)
  hold on;
  grid on;
  axis([fmin, fmax]);
  xticks = [300, 500, 1000, 2000, 3000, 5000, 7000, 10000, 15000, 20000];
  set(gca, 'XTick', xticks);
  set(gca, 'XTickLabel', sprintf('%d|', xticks));
  ca=get (gcf, "currentaxes");
  set(ca,"fontweight","bold","linewidth",1.5)
  cmap = jet(size(A2B, 2));
  %% find the max frequency bin
  ifmin = 0;
  ifmax = 0;
  for i = (1:size(A2B, 1))
    if (FC(i) <= fmax)
      ifmax = i;
    end
  end
  ifmax
  FC(ifmax)
  size(A2B, 1)
  %% plot
  semilogx(FC([1:ifmax]), 10*log10(abs(A2B([1:ifmax],:,:))), ".-", "linewidth", 1.5);
  l = legend("C1", "C2", "C3", "C4", "C5", "C6", "C7", "C8");
  %%legend(l, 'location', 'west', 'outside');
  hold off;
end

%%%
%%% Merge 2nd order low frequency filter shape with overall filter shape
%%%

function [UC, VC] = sphear_merge_filters(FC, FE, U, V, A2BF, fmax)
  %% find max frequency index
  imax = 0;
  for f = 1:size(FC, 1)
    if (FC(f) < fmax)
      imax = f;
    end
  end
  %% merge filters below the maximum frequency
  for f = 1:size(FC, 1)
    if (FC(f) < fmax)
      for c = 1:size(A2BF, 2)
	%%UC(f,c,:) = U(f) + real(10 * log10(abs(A2BF(f,c,5) / A2BF(imax,c,5))));
	%%VC(f,c,:) = V(f) + real(10 * log10(abs(A2BF(f,c,6) / A2BF(imax,c,6))));
	UC(f,c,:) = U(f) + real(10 * log10(abs(A2BF(imax,c,5) / A2BF(f,c,5))));
	VC(f,c,:) = V(f) + real(10 * log10(A2BF(imax,c,6) / abs(A2BF(f,c,6))));
      end
    else
      for c = 1:size(A2BF, 2)
	UC(f,c,:) = U(f);
	VC(f,c,:) = V(f);
      end
    end
  end
end

%%%
%%% Plot component filters
%%%

function sphear_plot_all_filters(FC, UC, fmin, fmax)
  hold on;
  grid on;
  axis([fmin, fmax]);
  xticks = [300, 500, 1000, 2000, 3000, 5000, 7000, 10000, 15000, 20000];
  set(gca, 'XTick', xticks);
  set(gca, 'XTickLabel', sprintf('%d|', xticks));
  ca=get (gcf, "currentaxes");
  set(ca,"fontweight","bold","linewidth",1.5)
  %% find the max frequency bin
  ifmin = 0;
  ifmax = 0;
  for i = (1:size(UC, 1))
    if (FC(i) <= fmax)
      ifmax = i;
    end
  end
  %% plot
  semilogx(FC([1:ifmax]), UC([1:ifmax],:,:), ".-", "linewidth", 1.5);
  l = legend("C1", "C2", "C3", "C4", "C5", "C6", "C7", "C8");
  %%legend(l, 'location', 'west', 'outside');
  hold off;
end

%%%
%%% Plot B format signal for three elevation measurement angles
%%%

function sphear_plot_measurements_3d(SIG, FC, FE, FS, N, freq)
  figure;
  hold on;
  axis([1, 16]);
  grid on;
  ca=get (gcf, "currentaxes");
  set(ca,"fontweight","bold","linewidth",1.5)
  %% find the approximate desired frequency band
  for f = [1:size(FC)]
    if (FC(f) <= freq)
      band = f;
    end
  end
  title(["B format power in frequency band (" sprintf("%.0f", FE(band)) " to " sprintf("%.0f", FE(band+1)) " Hz)"]);
  %% and plot the powers
  for i = [1,17,33]
    if (i < 17)
      wstyle = "ko-;W0;";
      color = "r";
      Xname = "X 0elev";
      Yname = "Y 0elev";
    else
      if (i < 33)
	wstyle = "ko--;WU;";
	color = "b";
	Xname = "X +elev";
	Yname = "Y +elev";
      else
	wstyle = "ko:;WD;";
	color = "g";
	Xname = "X -elev";
	Yname = "Y -elev";
      end
    end
    %% W
    plot(SIG((i:i+15),1,band), wstyle, "linewidth", 1.5);
    %% X/Y
    plot(SIG((i:i+15),[2],band), "color", color, sprintf("o-;%s;", Xname), "linewidth", 1.5);
    plot(SIG((i:i+15),[3],band), "color", color, sprintf("o-;%s;", Yname), "linewidth", 1.5);
    %% Z
    pZ = plot(SIG((i:i+15),4,band), "co-;Z;", "linewidth", 1.5);
  end
  legend('location', 'southwestoutside');
  hold off;
end

%%%
%%% create X matrix for given elevation angles
%%%

function [M x y z s t u v az el] = sphear_X_matrix(n, up, down)
  eps = 0.01;
  %% create arrays of angles
  az = (0:360/n:360-eps)';
  %% map zero elevation into spherical harmonic components
  el = 0;
  for i = 1:size(az)
    x(i) = cos(az(i)/180*pi) * cos(el/180*pi);
    y(i) = sin(az(i)/180*pi) * cos(el/180*pi);
    z(i) = sin(el/180*pi);
    s(i) = cos(az(i)/180*pi) * sin((2*el)/180*pi);
    t(i) = sin(az(i)/180*pi) * sin((2*el)/180*pi);
    u(i) = cos((2*az(i))/180*pi) * cos(el/180*pi) * cos(el/180*pi);
    v(i) = sin((2*az(i))/180*pi) * cos(el/180*pi) * cos(el/180*pi);
  end
  %% map positive elevation measurements
  el = up;
  for i = 1:size(az)
    x(i + 16) = cos(az(i)/180*pi) * cos(el/180*pi);
    y(i + 16) = sin(az(i)/180*pi) * cos(el/180*pi);
    z(i + 16) = sin(el/180*pi);
    s(i + 16) = cos(az(i)/180*pi) * sin((2*el)/180*pi);
    t(i + 16) = sin(az(i)/180*pi) * sin((2*el)/180*pi);
    u(i + 16) = cos((2*az(i))/180*pi) * cos(el/180*pi) * cos(el/180*pi);
    v(i + 16) = sin((2*az(i))/180*pi) * cos(el/180*pi) * cos(el/180*pi);
  end
  %% map negative elevation measurements
  el = down;
  for i = 1:size(az)
    x(i + 32) = cos(az(i)/180*pi) * cos(el/180*pi);
    y(i + 32) = sin(az(i)/180*pi) * cos(el/180*pi);
    z(i + 32) = sin(el/180*pi);
    s(i + 32) = cos(az(i)/180*pi) * sin((2*el)/180*pi);
    t(i + 32) = sin(az(i)/180*pi) * sin((2*el)/180*pi);
    u(i + 32) = cos((2*az(i))/180*pi) * cos(el/180*pi) * cos(el/180*pi);
    v(i + 32) = sin((2*az(i))/180*pi) * cos(el/180*pi) * cos(el/180*pi);
  end
  %% return big matrix of projections
  M = [x' y' z' s' t' u' v'];
end

%%%
%%% Fractional delay
%%%
%%% based on:
%%% http://www.labbookpages.co.uk/audio/beamforming/fractionalDelay.html
%%%
%%% Also:
%%% http://users.spa.aalto.fi/vpv/publications/vesan_vaitos/ch3_pt1_fir.pdf
%%%
%%% N = length of sinc function (should be odd)

function [SINC] = sphear_delayed_sinc(N, delay)
  center = floor(N / 2);
  for t = (1:N)
    x = (t - 1) - delay;
    window = 0.54 - 0.46 * cos(2.0 * pi * (x + 0.5) / N);
    SINC(t) = sinc(x - center) * window;
  end
end

function [DSIG FIR] = sphear_delay_signal(SIG, delay, N, fft_size)
  FIR = sphear_delayed_sinc(N, delay);
  for m = [1:size(SIG, 1)]
    for c = [1:size(SIG, 2)]
      %% we add trailing zeros to let the filter ring
      DSIG(m, c, :) = fftfilt(FIR, cat(1, reshape(SIG(m, c, :), 1, [])', zeros(2*size(FIR, 2), 1))', fft_size)(round(N/2):end);
    end
  end
end

%%%
%%% Fix wobble delays in a set of measurements
%%%

function [FIXED] = sphear_fix_wobble(SIG, DELAY);
  sinc_size = 255;
  min_delay = min(DELAY);
  DELAY = DELAY - min_delay;
  max_delay = max(DELAY);
  for m = [1:size(DELAY, 1)]
    %% delay = max_delay - DELAY(m);
    delay = DELAY(m);

%%    if (delay >= 1)
%%      i_delay = floor(delay);
%%      f_delay = mod(delay, 1.0);
%%      printf("m: %d, delay: %f, integer %f, frac %f\n", m, delay, i_delay, f_delay);
%%      FIXED(m,:,:) = zeros(size(SIG, 3));
%%      FIXED(m,:,[i_delay:end]) = SIG(m,:,[1:end - i_delay])
%%      FIXED(m,:,:) = sphear_delay_signal(FIXED(m,:,:), f_delay, 255, 8192);
%%    else
      printf("m: %d, delay: %f\n", m, delay);
      FIXED(m,:,:) = sphear_delay_signal(SIG(m,:,:), delay, 255, 8192);
%%    end
  end
end

%%%
%%% Do a basic encoder design, static A2B matrix plus FIR filters
%%%

function [BFF A2B FIR W X Y Z U V] = sphear_calculate_design(fref_min, fref_max)
  fft_size = 8192;
  N_oct = 16;
  %% read in data
  [AF, FS] = sphear_af_read((1:16));
  %% power in reference band
  [R] = sphear_signal_power(AF, FS, fref_min, fref_max, fft_size);
  %% static A2B matrix
  [A2B, PV, COND] = sphear_calculate_a2b(R);
  %% generate a B-format signal
  [BF] = sphear_af_to_bf(AF, A2B);
  %% calculate power of B-format signals
  [B, FC, FE] = sphear_signal_power_all(BF, FS, 300, 20000, fref_min, fref_max, N_oct, fft_size);
  %% extract the shapes of wxyz[uv] filters
  %% average response in cardinal directions
  [W X Y Z U V] = sphear_bf_extract_filter_shapes(B, 0);
  %% design FIR filters
  [FIR IR MPS] = sphear_design_fir_filters(FC, 512, FS, W, X, Y, Z, U, V, 400, 18, -10);
  %% re-calculate the full B-format signal with the filters
  [BF, BFF] = sphear_af_to_bf(AF, A2B, FIR, fft_size);
  %% get the average power in the directions of the lobes for W, XY and UV
  %% and normalize the A2B matrix so that ZY and UV have the same peak amplitude
  %% as W
  [A2B XY_gain UV_gain] = sphear_bf_normalize_gain(A2B, BFF, FS, fref_min, fref_max, fft_size);
  %% re-calculate the B-format signal
  [BF, BFF] = sphear_af_to_bf(AF, A2B, FIR, fft_size);
end