%% CCAF Bound Calculator
% This function calculates the constant upper and lower matrices phi
% and psi for CCAF's for the Blocking Matrix (BM) 
%
% *Syntax*
% |[phi, psi] = ccafbounds(m, fs, c, p, order)|
% *Inputs*
% *|mpos|-microphone positions in R^3 (meters)
% *|fs|-sample rate (Hertz)
% *|soundc|-Speed of sound (meters/sec)
% *|prop|-Estimated propagation time across the array in samples
% *|order|-Order of the adaptive filters
%
% *Outputs*
% * |phi| - Matrix of upper bounds, where each column is a vector of
%      bounds for a the adaptive filter of a single track in the BM.
% * |psi| - Matrix of lower bounds with the same structure as psi.
%
% *Notes*
%
% This code is based on equations derived for a linear microphone
% array where the effect of the coefficient bounds
% would be to expand the main lobe of the beamformer depending on the
% sine of a parameter delta-theta, the supposed error in steering
% angle of the beamformer.  However, here we consider a beamformer of
% arbitary geometry and thus the parameter "delta-theta" no longer
% makes sense.  Our current adaptation of the algorithm is
% 
% # Consider the center of the array as the centroid of array,
%   calculated as the arithmetic mean of the mic coordinates.
% # Hard-code a value for sin(delta-theta), which we know must be
%   bounded [-1 1].  At present we've selected .05
% 
% Note that these adaptations will still recreate the original
% performance of the algorithm for a linear array, where our selection
% of .05 for sin(delta-theta) corresponds to a steering angle error of
% about +/- 3 degrees.


%% Function Declaration
function [phi, psi] = ccafbounds(mpos, fs, soundc, prop, order)

%% Argument Error Checking
narginchk(5, 5);
if ~isreal(mpos) || ~all(all(isfinite(mpos))) || size(mpos,1) ~= 3
    error('mpos must be a real matrix with 3 rows')
elseif ~isscalar(fs) || ~isreal(fs) || ~isfinite(fs) || ...
        fs < 0 || abs(mod(fs, floor(fs))) > 0
    error('fs must be a positive integer')
elseif ~isreal(soundc) || ~isscalar(soundc) || ~isfinite(soundc) || soundc <= 0
    error('soundc must be positive real scalar');
elseif ~isscalar(prop) || ~isreal(prop) || ~isfinite(prop) || ...
        prop < 0 || abs(mod(prop, floor(prop))) > 0
    error('prop must be a positive integer')
elseif ~isscalar(order) || ~isreal(order) || ~isfinite(order) || ...
        order < 0 || abs(mod(order, floor(order))) > 0
    error('order must be a positive integer')
end

%% Calculate Bounds
% In the original paper, the bound vectors for each adaptive filter are
% calculated as
%
% $$\phi_{m,n} = \frac{1}{\pi\ max(.1, \ 
% (n\ ^\_ \ P)\ ^\_ \ T_m, \ ^\_ (n\ ^\_ \ P)\ ^\_ \ T_m)} \quad
% \psi = \ ^\_ \phi\ \forall\ m, n$$
%
% $$T_m = \frac{b_mf_s}{c}\sin\Delta\theta $$
%
% where bm is the distance of the mth microphone to the center of the
% array.  Remember that we must fudge for sin(delta-theta) in R^3.

M = size(mpos,2);  % number of microphones in the array
phi = zeros(order, M);  % initialze upper bound matrix for iteration

for mIter = 1:M   % iterate over all microphones
    sinDt = .05;  % kludge for 3-D (see Notes section above)
    arrayCentroid = mean(mpos,2);  % use centroid as "center" of array
    bm = sqrt(sum((mpos(:,mIter)-arrayCentroid).^2));  % Get mic distance
                                                    % from centroid
    Tm = bm*fs*sinDt/soundc;  % Hoshuyama equation
    for nIter = 1:order  % Set bound for each tap of this adaptive filter
        phi(nIter, mIter) = 1/(pi*max([.1, (nIter-prop)-Tm, -(nIter-prop)-Tm])); 
         % directly from Hoshuyama paper
    end
end
psi = -phi;  % psi is simply the opposite of phi

end