function adj = tet_mesh_order4_adj_set ( node_num, tetra_num, tetra_node, ...
  adj_num, adj_row )

%*****************************************************************************80
%
%% TET_MESH_ORDER4_ADJ_SET sets the nodal adjacency matrix.
%
%  Discussion:
%
%    A compressed format is used for the nodal adjacency matrix.
%
%    It is assumed that we know ADJ_NUM, the number of adjacency entries
%    and the ADJ_ROW array, which keeps track of the list of slots
%    in ADJ where we can store adjacency information for each row.
%
%    We essentially repeat the work of TET_MESH_ORDER4_ADJ_COUNT, but
%    now we have a place to store the adjacency information.
%
%    A copy of the ADJ_ROW array is useful, as we can use it to keep track
%    of the next available entry in ADJ for adjacencies associated with
%    a given row.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 November 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, integer TETRA_NUM, the number of tetrahedrons.
%
%    Input, integer TETRA_NODE(4,TETRA_NUM), the nodes that make up the
%    tetrahedrons.
%
%    Input, integer ADJ_NUM, the total number of adjacency relationships,
%
%    Input, integer ADJ_ROW(NODE_NUM+1), the ADJ pointer array.
%
%    Output, integer ADJ(ADJ_NUM), the adjacency information.
%

%
%  Each order 4 tetrahedron defines 6 adjacency pairs.
%
  pair(1,            1:  tetra_num) = tetra_node(1,1:tetra_num);
  pair(2,            1:  tetra_num) = tetra_node(2,1:tetra_num);

  pair(1,  tetra_num+1:2*tetra_num) = tetra_node(1,1:tetra_num);
  pair(2,  tetra_num+1:2*tetra_num) = tetra_node(3,1:tetra_num);

  pair(1,2*tetra_num+1:3*tetra_num) = tetra_node(1,1:tetra_num);
  pair(2,2*tetra_num+1:3*tetra_num) = tetra_node(4,1:tetra_num);

  pair(1,3*tetra_num+1:4*tetra_num) = tetra_node(2,1:tetra_num);
  pair(2,3*tetra_num+1:4*tetra_num) = tetra_node(3,1:tetra_num);

  pair(1,4*tetra_num+1:5*tetra_num) = tetra_node(2,1:tetra_num);
  pair(2,4*tetra_num+1:5*tetra_num) = tetra_node(4,1:tetra_num);

  pair(1,5*tetra_num+1:6*tetra_num) = tetra_node(3,1:tetra_num);
  pair(2,5*tetra_num+1:6*tetra_num) = tetra_node(4,1:tetra_num);

  pair_num = 6 * tetra_num;
%
%  Force the nodes of each pair to be listed in ascending order.
%
  pair = i4col_sort2_a ( 2, pair_num, pair );
%
%  Rearrange the columns in ascending order.
%
  pair = i4col_sort_a ( 2, pair_num, pair );
%
%  Mark all entries of ADJ so we will know later if we missed one.
%
  adj(1:adj_num) = -1;
%
%  Copy the ADJ_ROW array and use it to keep track of the next
%  free entry for each row.
%
  adj_row_copy(1:node_num) = adj_row(1:node_num);
%
%  Now set up the ADJ_ROW counts.
%
  for k = 1 : pair_num

    if ( 1 < k )
      if ( pair(1,k-1) == pair(1,k) && pair(2,k-1) == pair(2,k) )
        continue
      end
    end

    i = pair(1,k);
    j = pair(2,k);

    adj(adj_row_copy(i)) = j;
    adj_row_copy(i) = adj_row_copy(i) + 1;
    adj(adj_row_copy(j)) = i;
    adj_row_copy(j) = adj_row_copy(j) + 1;

  end

  return
end


function vol = ft_headmodel_bemcp(geom, varargin)

% FT_HEADMODEL_BEMCP creates a volume conduction model of the head
% using the boundary element method (BEM) for EEG. This function
% takes as input the triangulated surfaces that describe the boundaries
% and returns as output a volume conduction model which can be used
% to compute leadfields.
%
% The implementation of this function is based on Christophe Phillips'
% MATLAB code, hence the name "bemcp".
%
% Use as
%   vol = ft_headmodel_bem_cp(geom, ...)
%
% See also FT_PREPARE_VOL_SENS, FT_COMPUTE_LEADFIELD

ft_hastoolbox('bemcp', 1);

% get the optional arguments
hdmfile         = keyval('hdmfile', varargin);
conductivity    = keyval('conductivity', varargin);

% start with an empty volume conductor
vol = [];

if ~isempty(hdmfile)
  hdm = ft_read_vol(hdmfile);
  % copy the boundary of the head model file into the volume conduction model
  vol.bnd = hdm.bnd;
  if isfield(hdm, 'cond')
    % also copy the conductivities
    vol.cond = hdm.cond;
  end
else
  % copy the boundaries from the geometry into the volume conduction model
  vol.bnd = geom.bnd;
end

% determine the number of compartments
numboundaries = length(vol.bnd);

if ~isfield(vol, 'cond')
  % assign the conductivity of each compartment
  vol.cond = conductivity;
end

% determine the nesting of the compartments
nesting = zeros(numboundaries);
for i=1:numboundaries
  for j=1:numboundaries
    if i~=j
      % determine for a single vertex on each surface if it is inside or outside the other surfaces
      curpos = vol.bnd(i).pnt(1,:); % any point on the boundary is ok
      curpnt = vol.bnd(j).pnt;
      curtri = vol.bnd(j).tri;
      nesting(i,j) = bounding_mesh(curpos, curpnt, curtri);
    end
  end
end

if sum(nesting(:))~=(numboundaries*(numboundaries-1)/2)
  error('the compartment nesting cannot be determined');
end

% for a three compartment model, the nesting matrix should look like
%    0 1 1     the first is nested inside the 2nd and 3rd, i.e. the inner skull
%    0 0 1     the second is nested inside the 3rd, i.e. the outer skull
%    0 0 0     the third is the most outside, i.e. the skin
[~, order] = sort(-sum(nesting,2));

fprintf('reordering the boundaries to: ');
fprintf('%d ', order);
fprintf('\n');

% update the order of the compartments
vol.bnd    = vol.bnd(order);
vol.cond   = vol.cond(order);
vol.skin_surface   = numboundaries;
vol.source = 1;

% do some sanity checks
if length(vol.bnd)~=3
  error('this only works for three surfaces');
end
if vol.skin_surface~=3
  error('the third surface should be the skin');
end
if vol.source~=1
  error('the first surface should be the inside of the skull');
end

% Build Triangle 4th point
vol = triangle4pt(vol);

% 2. BEM model estimation, only for the scalp surface

defl =[ 0 0 1/size(vol.bnd(vol.skin_surface).pnt,1)];
% ensure deflation for skin surface, i.e. average reference over skin

% NOTE:
% Calculation proceeds by estimating each submatrix C_ij and combine them.
% There are 2 options:
% - calculating the matrices once, as it takes some time, keep them in
%   memory and use them the 2-3 times they're needed.
% - calculating the matrices every time they're needed, i.e. 2-3 times
% The former option is faster but requires more memory space as up to *8*
% square matrices of size C_ij have to be kept in memory at once.
% The latter option requires less memory, but would take much more time to
% estimate.
% This faster but memory hungry solution is implemented here.

% Deal first with surface 1 and 2 (inner and outer skull
%--------------------------------

% NOTE:
% C11st/C22st/C33st are simply the matrix C11/C22/C33 minus the identity
% matrix, i.e. C11st = C11-eye(N)

weight = (vol.cond(1)-vol.cond(2))/((vol.cond(1)+vol.cond(2))*2*pi);
C11st  = bem_Cii_lin(vol.bnd(1).tri,vol.bnd(1).pnt, weight,defl(1),vol.bnd(1).pnt4);
weight = (vol.cond(1)-vol.cond(2))/((vol.cond(2)+vol.cond(3))*2*pi);
C21    = bem_Cij_lin(vol.bnd(2).pnt,vol.bnd(1).pnt,vol.bnd(1).tri, weight,defl(1));
tmp1   = C21/C11st;

weight = (vol.cond(2)-vol.cond(3))/((vol.cond(1)+vol.cond(2))*2*pi);
C12    = bem_Cij_lin(vol.bnd(1).pnt,vol.bnd(2).pnt,vol.bnd(2).tri, weight,defl(2));
weight = (vol.cond(2)-vol.cond(3))/((vol.cond(2)+vol.cond(3))*2*pi);
C22st  = bem_Cii_lin(vol.bnd(2).tri,vol.bnd(2).pnt, weight,defl(2),vol.bnd(2).pnt4);
tmp2   = C12/C22st;

% Try to spare some memory:
tmp10 = - tmp2 * C21 + C11st;
clear C21 C11st
tmp11 = - tmp1 * C12 + C22st;
clear C12 C22st

% Combine with the effect of surface 3 (scalp) on the first 2
%------------------------------------------------------------
weight = (vol.cond(1)-vol.cond(2))/(vol.cond(3)*2*pi);
C31    = bem_Cij_lin(vol.bnd(3).pnt,vol.bnd(1).pnt,vol.bnd(1).tri, weight,defl(1));
%   tmp4   = C31/(- tmp2 * C21 + C11st );
%   clear C31 C21 C11st
tmp4 = C31/tmp10;
clear C31 tmp10

weight = (vol.cond(2)-vol.cond(3))/(vol.cond(3)*2*pi);
C32    = bem_Cij_lin(vol.bnd(3).pnt,vol.bnd(2).pnt,vol.bnd(2).tri, weight,defl(2));
%   tmp3   = C32/(- tmp1 * C12 + C22st );
%   clear  C12 C22st C32
tmp3 = C32/tmp11;
clear C32 tmp11

tmp5 = tmp3*tmp1-tmp4;
tmp6 = tmp4*tmp2-tmp3;
clear tmp1 tmp2 tmp3 tmp4

% Finally include effect of surface 3 on the others
%--------------------------------------------------
% As the gama1 intermediate matrix is built as the sum of 3 matrices, I can
% spare some memory by building them one at a time, and summing directly
weight = vol.cond(3)/((vol.cond(1)+vol.cond(2))*2*pi);
Ci3    = bem_Cij_lin(vol.bnd(1).pnt,vol.bnd(3).pnt,vol.bnd(3).tri, weight,defl(3));
gama1  = - tmp5*Ci3; % gama1 = - tmp5*C13;

weight = vol.cond(3)/((vol.cond(2)+vol.cond(3))*2*pi);
Ci3    = bem_Cij_lin(vol.bnd(2).pnt,vol.bnd(3).pnt,vol.bnd(3).tri, weight,defl(3));
gama1  = gama1 - tmp6*Ci3; % gama1 = - tmp5*C13 - tmp6*C23;

weight = 1/(2*pi);
Ci3    = bem_Cii_lin(vol.bnd(3).tri,vol.bnd(3).pnt, weight,defl(3),vol.bnd(3).pnt4);
gama1  = gama1 - Ci3; % gama1 = - tmp5*C13 - tmp6*C23 - C33st;
clear Ci3

% Build system matrix
%--------------------
i_gama1 = inv(gama1);
vol.mat = [i_gama1*tmp5 i_gama1*tmp6 i_gama1];

% remember the type
vol.type = 'bemcp';


function K = kmgraph(kern,d1,d2,ind1,ind2,kerparam),
% Marginalized Graph Kernel by Koji Tsuda
% The actual implementation uses the \delta version of the kernel (see publication)
% and a tuned C implementation by Alex Zien.
% Despite the paper we are using a normed K(X1,X2)/sqrt(K(X1,X1)*K(X2,X2))
% version.
% To use this kernel ensure that the function graphkerneldelta.cpp
% is compiled in the 'functions' folder.
% Furthermore note that d1.X must contain !cells! of the form :
% Let x,y,z be a graph, then
% struct(x)
%     adjacency: [NxN double]
%     vertices: [Nx1 double]
% and d=data({x,y,z}') is a valid data object
%
% gb 03-Mar-2004

X1=get_x(d1,ind1);
X2=get_x(d2,ind2);

K=zeros(length(X2),length(X1));
eps=1e-5;

lambda = kerparam;

if( size(ind1)==size(ind2))
    if( ind1==ind2)
        for i=ind1
            for j=ind2
                Ks(j,i)=graphkerneldelta(X1{i}.adjacency,X2{j}.adjacency,X1{i}.vertices,X2{j}.vertices,lambda,eps);

            end
        end

        for i=ind1
            for j=ind2
                K(j,i)=Ks(j,i)/sqrt(Ks(j,j)*Ks(i,i));
            end
        end
    end
else

    for i=ind1
        for j=ind2
            k12=graphkerneldelta(X1{i}.adjacency,X2{j}.adjacency,X1{i}.vertices,X2{j}.vertices,lambda,eps);
            k11=graphkerneldelta(X1{i}.adjacency,X1{i}.adjacency,X1{i}.vertices,X1{i}.vertices,lambda,eps);
            k22=graphkerneldelta(X2{j}.adjacency,X2{j}.adjacency,X2{j}.vertices,X2{j}.vertices,lambda,eps);
            K(j,i)=k12/sqrt(1e-5+k11*k22);
        end
    end

end


function acq = acqimiqr_vbmc(Xs,vp,gp,optimState,fmu,fs2,fbar,vtot)
%VBMC_ACQIMIQR Integrated median interquantile range acquisition function.

u = 0.6745; % norminv(0.75)

if isempty(Xs)
    % Return acquisition function info struct
    acq.importance_sampling = true;
    acq.importance_sampling_vp = false;
    acq.log_flag = true;
    return;
elseif ischar(Xs)
    switch lower(Xs)
        case 'islogf1'
        % Importance sampling log base proposal (shared part)
            acq = fmu;
        case 'islogf2'
        % Importance sampling log base proposal (added part)
        % (Full log base proposal is fixed + added)
            fs = sqrt(fs2);
            acq = u*fs + log1p(-exp(-2*u*fs));
        case 'islogf'
        % Importance sampling log base proposal distribution
            fs = sqrt(fs2);
            acq = fmu + u*fs + log1p(-exp(-2*u*fs));
    end
    return;
end

% Different importance sampling inputs for different GP hyperparameters?
multipleinputs_flag = size(optimState.ActiveImportanceSampling.Xa,3) > 1;

% Xs is in *transformed* coordinates

[Nx,D] = size(Xs);
Ns = size(fmu,2);
Na = size(optimState.ActiveImportanceSampling.Xa,1);

% Estimate observation noise at test points from nearest neighbor
[~,pos] = min(sq_dist(bsxfun(@rdivide,Xs,optimState.gplengthscale),gp.X_rescaled),[],2);
sn2 = gp.sn2new(pos);
% sn2 = min(sn2,1e4);
ys2 = fs2 + sn2;    % Predictive variance at test points

if multipleinputs_flag
    Xa = zeros(Na,D);
else
    Xa = optimState.ActiveImportanceSampling.Xa;
end
acq = zeros(Nx,Ns);

%% Compute integrated acquisition function via importance sampling

for s = 1:Ns
    hyp = gp.post(s).hyp;
    L = gp.post(s).L;
    Lchol = gp.post(s).Lchol;
    sn2_eff = 1/gp.post(s).sW(1)^2;

    if multipleinputs_flag
        Xa(:,:) = optimState.ActiveImportanceSampling.Xa(:,:,s);
    end

    % Compute cross-kernel matrix Ks_mat
    if gp.covfun(1) == 1    % Hard-coded SE-ard for speed
        ell = exp(hyp(1:D))';
        sf2 = exp(2*hyp(D+1));
        Ks_mat = sq_dist(gp.X*diag(1./ell),Xs*diag(1./ell));
        Ks_mat = sf2 * exp(-Ks_mat/2);

        Ka_mat = sq_dist(Xa*diag(1./ell),Xs*diag(1./ell));
        Ka_mat = sf2 * exp(-Ka_mat/2);

        %Kax_mat = sq_dist(Xa*diag(1./ell),gp.X*diag(1./ell));
        %Kax_mat = sf2 * exp(-Kax_mat/2);
        Kax_mat(:,:) = optimState.ActiveImportanceSampling.Kax_mat(:,:,s);
    else
        error('Other covariance functions not supported yet.');
    end

    if Lchol
        C = Ka_mat' - Ks_mat'*(L\(L'\Kax_mat'))/sn2_eff;
    else
        C = Ka_mat' + Ks_mat'*(L*Kax_mat');
    end

    tau2 = bsxfun(@rdivide,C.^2,ys2(:,s));
    s_pred = sqrt(max(bsxfun(@minus,optimState.ActiveImportanceSampling.fs2a(:,s)',tau2),0));

    lnw = optimState.ActiveImportanceSampling.lnw(s,:);

    zz = bsxfun(@plus,lnw,u*s_pred + log1p(-exp(-2*u*s_pred)));
    lnmax = max(zz,[],2);
    acq(:,s) = log(sum(exp(bsxfun(@minus,zz,lnmax)),2)) + lnmax;
end

if Ns > 1
    M = max(acq,[],2);
    acq = M + log(sum(exp(bsxfun(@minus,acq,M)),2)/Ns);
end

end

%SQ_DIST Compute matrix of all pairwise squared distances between two sets
% of vectors, stored in the columns of the two matrices, a (of size n-by-D)
% and b (of size m-by-D).
function C = sq_dist(a,b)

n = size(a,1);
m = size(b,1);
mu = (m/(n+m))*mean(b,1) + (n/(n+m))*mean(a,1);
a = bsxfun(@minus,a,mu); b = bsxfun(@minus,b,mu);
C = bsxfun(@plus,sum(a.*a,2),bsxfun(@minus,sum(b.*b,2)',2*a*b'));
C = max(C,0);

end


classdef chebfun3
%CHEBFUN3   CHEBFUN3 class for representing functions on [a,b]x[c,d]x[e,g].
%   Class for approximating functions defined on finite cuboids. The
%   functions should be smooth.
%
%   CHEBFUN3(F) constructs a CHEBFUN3 object representing the function F on
%   [-1, 1] x [-1, 1] x [-1, 1].
%
%   The input F may be any of the following:
%   i)   A function handle like @(x,y,z) x.*y + cos(x).*z
%   ii)  A string like 'cos(x) + sin(x.*y.*z)'
%   iii) A discrete tensor corresponding to values of a function at points
%   generated by ndgrid.
%
%   F should be vectorized in the sense that it may be evaluated at a
%   tensor of points and returns a tensor in the output.
%
%   CHEBFUN3(F, 'eps', ep) specifies chebfun3eps to be ep.
%
%   CHEBFUN3(F, [A B C D E G]) specifies a cuboid [A B] x [C D] x [E G]
%   where the function F is defined. A, B, C, D, E and G must all be finite.
%
%   CHEBFUN3(F, [m n p]) returns a representation of a trivariate
%   polynomial of length (m, n, p), i.e., with degree m-1 in x, degree
%   n-1 in y and degree p-1 in z. The polynomial is compressed in low
%   trilinear rank form and the trilinear rank (rx, ry, rz) is still
%   determined adaptively.
%
%   CHEBFUN3(F, 'rank', [rx ry rz]) returns a CHEBFUN3 that is an
%   approximation to F and has the trilinear rank (rx, ry, rz).
%
%   CHEBFUN3(F, 'trig') or CHEBFUN3(F, 'periodic') constructs a trig-type
%   CHEBFUN3 object if the input function handle F is triply periodic on
%   [-1, 1] x [-1, 1] x [-1, 1]. In this case, factor quasimatrices of the
%   low rank format will be represented by Fourier instead of Chebyshev
%   technology.
%
%   If F is a discrete tensor, F = (f_{ijk}), the numbers f_{ijk} are used
%   as function values at tensor product Chebyshev points of the 2nd kind
%   generated by ndgrid.
%
%   CHEBFUN3(F, 'equi') can be used if F is a discrete tensor of values at
%   equispaced points in 3D.
%
%   CHEBFUN3(F, 'coeffs') returns a CHEBFUN3 in which the coefficients of
%   the Chebyshev expansion are the entries of the discrete tensor F.
%
%  Examples:
%
%   f = chebfun3(@(x,y,z) exp(-(x.^2 + y.^2 + z.^2))); isosurface(f)
%
%   f = chebfun3('sin(10*pi*x).^2 + sin(10*x.*y.*z)'); sum3(f)
%
%   cheb.gallery3
%
%   f1 = chebfun3('tanh(sqrt(3)*x)');   rank(f1), g1 = grad(f1); g1(0,0,0)
%   f2 = chebfun3('tanh(x+y+z)'); rank(f2), g2 = grad(f2); g2(0,0,0)
%
%   Chebfun3 is based on:
%
%   [1] B. Hashemi and L. N. Trefethen, Chebfun in three dimensions, SIAM
%   J. Sci. Comput., 39 (2017), C341-C363.
%
%   [2] S. Dolgov, D. Kressner, and C. Stroessner, Functional Tucker
%   approximation using Chebyshev interpolation, SIAM J. Sci. Comput.,
%   43 (2021), A2190-A2210.
%
%   By default since March 2023, chebfun3 objects are constructed as described
%   in [2].  The original constructor described in [1] can be invoked with
%   CHEBFUN3(F, 'classic').
%
% See also CHEBFUN3V and CHEBFUN3T.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% CLASS PROPERTIES:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
properties
    % COLS: Mode-1 fibers, i.e., columns which are functions of x used in
    % Tucker representation.
    cols

    % ROWS: Mode-2 fibers, i.e. rows which are functions of y used in
    % Tucker representation.
    rows

    % TUBES: Mode-3 fibers, i.e. tubes which are functions of z used in
    % Tucker representation.
    tubes

    % CORE: discrete core tensor in Tucker representation
    core

    % DOMAIN: box of CHEBFUN3, default is [-1, 1] x [-1, 1] x [-1, 1].
    domain
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% CLASS CONSTRUCTOR:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
methods
    function f = chebfun3(varargin)
        % The main CHEBFUN3 constructor!

        % Return an empty CHEBFUN3:
        if ( (nargin == 0) || isempty(varargin{1}) )
            return
        end

        % Call the constructor, all the work is done here:
        f = constructor(f, varargin{:});
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% CLASS METHODS:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
methods (Access = public, Static = true)
    % Outer product of discrete tensors.
    varargout = outerProd(varargin);

    % Tensor x matrix.
    varargout = txm(varargin);

    % Unfold a discrete tensor to create a matrix.
    varargout = unfold(varargin);

    % Reshape a discrete matrix to get a tensor.
    varargout = fold(varargin);

    % Convert 3D values to 3D coefficients.
    coeffs3D = vals2coeffs(vals3D);

    % Convert 3D coefficients to 3D values.
    vals3D = coeffs2vals(coeffs3D);

    % Faster subscripts from linear index.
    varargout = myind2sub(varargin);

    % HOSVD of discrete tensors.
    varargout = discreteHOSVD(varargin);

end

methods (Access = public)
    % Retrieve and modify preferences for this class.
    varargout = subsref(f, index);

    % Get properties of a CHEBFUN3 object.
    out = get(f, idx);

    % Permute a CHEBFUN3.
    out = permute(f, varargin);

    % Evaluate a CHEBFUN3.
    out = feval(f, varargin);

    % Evaluate at vectors to get a tensor of values.
    out = fevalt(f, varargin);

    % Evaluate on a tensor product grid.
    varargout = chebpolyval3(varargin);

    % Sample a CHEBFUN3 on a tensor product grid
    varargout = sample(varargin);

    % Tucker rank of a CHEBFUN3 (i.e., size of the core in each direction).
    varargout = rank(f);

    % Length of a CHEBFUN3 (i.e., the number of Chebyshev or Fourier points
    % at each direction).
    varargout = length(f);

    % Size of a CHEBFUN3.
    varargout = size(f, varargin);

    % Minimum of a CHEBFUN3 along one dimension.
    varargout = min(varargin);

    % Maximum of a CHEBFUN3 along one dimension.
    varargout = max(varargin);

    % Minimum of a CHEBFUN3 along two dimensions.
    varargout = min2(varargin);

    % Maximum of a CHEBFUN3 along two dimensions.
    varargout = max2(varargin);

    % Global minimum of a CHEBFUN3.
    varargout = min3(f);

    % Global maximum of a CHEBFUN3.
    varargout = max3(f);

    % Global minimum and maximum of a CHEBFUN3.
    varargout = minandmax3(f);

    % Norm of a CHEBFUN3.
    varargout = norm(f, p);

    % Display a CHEBFUN3.
    varargout = disp(f, varargin);

    % Display a CHEBFUN3.
    varargout = display(varargin);

    % Simplify a CHEBFUN3.
    out = simplify(varargin);

    % Vertical scale of a CHEBFUN3.
    out = vscale(f);

    % Vertical concatenation of CHEBFUN3 objects.
    out = vertcat(varargin);

    % Just one common root of 3 CHEBFUN3 objects.
    varargout = root(f, g, h);

    % Number of degrees of freedom needed to represent a CHEBFUN3.
    out = ndf(f);

    % Get the low-rank representation (Tucker expansion) of a CHEBFUN3.
    varargout = tucker(f);

    % Definite integral of a CHEBFUN3 over the domain in one
    % direction. Output is a Chebfun2 object.
    out = sum(varargin);

    % Definite integral of a CHEBFUN3 over the domain in two directions.
    % Output is a Chebfun object.
    out = sum2(varargin);

    % Definite integral of a CHEBFUN3 over its domain.
    out = sum3(f);

    % Restrict the domain of a CHEBFUN3. Output might be a scalar,
    % a Chebfun, a Chebfun2 or a Chebfun3 object.
    out = restrict(f, varargin);

    % Volume of the domain of a CHEBFUN3.
    out = domainvolume(f);

    % Line integral of a CHEBFUN3 over a 3D parametric curve.
    out = integral(f, varargin);

    % Surface integral of a CHEBFUN3 over a parametric surface represented
    % as a CHEBFUN2V.
    out = integral2(f, varargin);

    % Integral of a CHEBFUN3 over a restricted cuboid.
    out = integral3(f, varargin);

    % Average or mean value of a CHEBFUN3 in one direction.
    out = mean(f, varargin);

    % Average or mean value of a CHEBFUN3 in two directions.
    out = mean2(f, varargin);

    % Average or mean value of a CHEBFUN3.
    out = mean3(f);

    % Standard deviation of a CHEBFUN3 along one variable.
    out = std(f, varargin);

    % Standard deviation of a CHEBFUN3 along two variables.
    out = std2(f, varargin);

    % Standard deviation of a CHEBFUN3.
    out = std3(f);

    % Squeeze a CHEBFUN3 into a CHEBFUN2 or a CHEBFUN.
    out = squeeze(f);

    % Create a scatter plot of the core tensor of a CHEBFUN3.
    varargout = coreplot(f, varargin);

    % Plot a CHEBFUN3.
    out = plot(f, varargin);

    % Plot slices of a CHEBFUN3.
    out = slice(f, varargin);

    % Scan plot of a CHEBFUN3.
    out = scan(f, varargin);

    % Isosurface plot of a CHEBFUN3.
    out = isosurface(f, varargin);

    % SURF for a CHEBFUN3 over its domain.
    varargout = surf(f, varargin);

    % plotcoeffs of a CHEBFUN3.
    varargout = plotcoeffs(f, varargin);

    % Tensor of coefficients of a CHEBFUN3.
    varargout = chebcoeffs3(f);

    % Tensor of coefficients of a CHEBFUN3 of specified size.
    varargout = coeffs3(f, varargin);
end

methods (Access = private, Static = true)
    % Classic constructor for CHEBFUN3 objects
    f = chebfun3classic(f, op, pref, dom, vectorize, fiberDim)

    % Constructor for CHEBFUN3 objects of tensor inputs.
    f = chebfun3double(f, op, dom, pref, isEqui)

    % Default constructor for CHEBFUN3 objects.
    f = chebfun3f(f, op, pref, dom, vectorize);
end

methods
    % Unary plus for a CHEBFUN3.
    out = uplus(f);

    % Plus for CHEBFUN3 objects.
    out = plus(f, g, tol);

    % Unary minus for a CHEBFUN3.
    out = uminus(f);

    % Subtraction of two CHEBFUN3 objects.
    out = minus(f, g);

    % Pointwise multiplication for CHEBFUN3 objects.
    out = times(f, g);

    % Pointwise multiplication for CHEBFUN3 objects.
    out = mtimes(f, g);

    % Pointwise power of a CHEBFUN3.
    out = power(varargin);

    % Pointwise right divide of CHEBFUN3 objects.
    out = rdivide(f, g);

    % Right divide for CHEBFUN3 objects.
    out = mrdivide(f, g);

    % Pointwise CHEBFUN3 left array divide.
    out = ldivide(f, g);

    % Left divide for CHEBFUN3 objects.
    out = mldivide(f, g);

    % Absolute value of a CHEBFUN3.
    out = abs(f);

    % Real part of a CHEBFUN3.
    out = real(f);

    % Imaginary part of a CHEBFUN3.
    out = imag(f);

    % Complex conjugate of a CHEBFUN3.
    out = conj(f);

    % Create f + i g for two CHEBFUN3 objects.
    out = complex(f, g);

    % Sine of a CHEBFUN3.
    out = sin(f);

    % Cosine of a CHEBFUN3.
    out = cos(f);

    % Tangent of a CHEBFUN3.
    out = tan(f);

    % Tangent of a CHEBFUN3 (in degrees).
    out = tand(f);

    % Hyperbolic tangent of a CHEBFUN3.
    out = tanh(f);

    % Exponential of a CHEBFUN3.
    out = exp(f);

    % Hyperbolic sine of a CHEBFUN3.
    out = sinh(f);

    % Hyperbolic cosine of a CHEBFUN3.
    out = cosh(f);

    % Compose command for CHEBFUN3 objects.
    out = compose(f, varargin);

    % Square root of a CHEBFUN3.
    out = sqrt(f, varargin);

    % Natural logarithm of a CHEBFUN3.
    out = log(f, varargin);

    % HOSVD of a CHEBFUN3.
    varargout = hosvd(f, varargin);

    % Test whether a CHEBFUN3 object is empty.
    out = isempty(f);

    % Test if a CHEBFUN3 is identically zero over its domain.
    varargout = iszero(f);

    % Real-valued CHEBFUN3 test.
    out = isreal(f);

    % Equality test for CHEBFUN3 objects.
    out = isequal(f, g);

    % Partial derivative of a CHEBFUN3 object.
    out = diff(f, varargin);

    % Partial derivative of a CHEBFUN3 object in the first variable.
    out = diffx(f, varargin);

    % Partial derivative of a CHEBFUN3 object in the second variable.
    out = diffy(f, varargin);

    % Partial derivative of a CHEBFUN3 object in the third variable.
    out = diffz(f, varargin);

    % Gradient of a CHEBFUN3.
    varargout = grad(f);

    % Gradient of a CHEBFUN3.
    varargout = gradient(f);

    % Normal vector of a CHEBFUN3.
    varargout = normal(f, varargin);

    % Determinant of Jacobian of three CHEBFUN3 objects.
    f = jacobian(f, g, h);

    % Laplacian of a CHEBFUN3.
    out = lap(f);

    % Laplacian of a CHEBFUN3.
    out = laplacian(f);

    % Biharmonic operator of a CHEBFUN3.
    out = biharm(f);

    % Biharmonic operator of a CHEBFUN3.
    out = biharmonic(f);

    % Indefinite integral of a CHEBFUN3 in one variable.
    out = cumsum(f, varargin);

    % Indefinite integral of a CHEBFUN3 in two variables.
    out = cumsum2(f, varargin);

    % Indefinite integral of a CHEBFUN3 in all variables.
    out = cumsum3(f);
end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% HIDDEN METHODS:
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods ( Hidden = true, Static = false )

        % Test if two CHEBFUN3 objects have the same domain.
        out = domainCheck(f, g);

    end
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Sample program for blind source separation using independent low-rank   %
% matrix analysis (ILRMA)                                                 %
%                                                                         %
% Coded by D. Kitamura (d-kitamura@ieee.org)                              %
%                                                                         %
% Copyright 2020 Daichi Kitamura                                          %
%                                                                         %
% These programs are distributed only for academic research at            %
% universities and research institutions.                                 %
% It is not allowed to use or modify these programs for commercial or     %
% industrial purpose without our permission.                              %
% When you use or modify these programs and write research articles,      %
% cite the following references:                                          %
%                                                                         %
% # Original paper (The algorithm was called "Rank-1 MNMF" in this paper) %
% D. Kitamura, N. Ono, H. Sawada, H. Kameoka, H. Saruwatari, "Determined  %
% blind source separation unifying independent vector analysis and        %
% nonnegative matrix factorization," IEEE/ACM Trans. ASLP, vol. 24,       %
% no. 9, pp. 1626-1641, September 2016.                                   %
%                                                                         %
% # Book chapter (The algorithm was renamed as "ILRMA")                   %
% D. Kitamura, N. Ono, H. Sawada, H. Kameoka, H. Saruwatari, "Determined  %
% blind source separation with independent low-rank matrix analysis,"     %
% Audio Source Separation. Signals and Communication Technology.,         %
% S. Makino, Ed. Springer, Cham, pp. 125-155, March 2018.                 %
%                                                                         %
% See also:                                                               %
% http://d-kitamura.net                                                   %
% http://d-kitamura.net/demo-ILRMA_en.html                                %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear;
close all;

% Set parameters
seed = 1; % pseudo random seed
refMic = 1; % reference microphone for back projection
resampFreq = 16000; % resampling frequency [Hz]
nSrc = 2; % number of sources
fftSize = 4096; % window length in STFT [points]
shiftSize = 2048; % shift length in STFT [points]
windowType = "hamming"; % window function used in STFT
nBases = 10; % number of bases (for ilrmaType=1, nBases is # of bases for "each" source. for ilrmaType=2, nBases is # of bases for "all" sources)
nIter = 100; % number of iterations (define by checking convergence behavior with drawConv=true)
ilrmaType = 1; % 1 or 2 (1: ILRMA w/o partitioning function, 2: ILRMA with partitioning function)
dofParam = 1; % degree-of-freedom parameter of Student's t distribution (positive value, for t-ILRMA, 1: Cauchy)
sigDom = 2; % domain of signal for low-rank source model (positive value, for t-ILRMA, 2: power, 1: amplitude)
applyNormalize = 1; % 0 or 1 or 2 (0: do not apply normalization in each iteration, 1: apply average-power-based normalization in each iteration to improve numerical stability (the monotonic decrease of the cost function may be lost), 2: apply back projection in each iteration)
applyWhitening = false; % true or false (true: apply whitening to the observed multichannel spectrograms)
drawConv = true; % true or false (true: plot cost function values in each iteration and show convergence behavior, false: faster and do not plot cost function values)

% Fix random seed
RandStream.setGlobalStream(RandStream('mt19937ar','Seed',seed))

% Input data and resample
[srcSig(:,:,1), sampFreq] = audioread('./input/drums.wav'); % signal x channel x source (source image)
[srcSig(:,:,2), sampFreq] = audioread('./input/piano.wav'); % signal x channel x source (source image)
srcSigResample(:,:,1) = resample(srcSig(:,:,1), resampFreq, sampFreq, 100); % resampling for reducing computational cost
srcSigResample(:,:,2) = resample(srcSig(:,:,2), resampFreq, sampFreq, 100); % resampling for reducing computational cost

% Mix source images of each channel to produce observed mixture signal
mixSig(:,1) = srcSigResample(:,1,1) + srcSigResample(:,1,2);
mixSig(:,2) = srcSigResample(:,2,1) + srcSigResample(:,2,2);
if abs(max(max(mixSig))) > 1 % check clipping
    error('Cliping detected while mixing.\n');
end

% Blind source separation based on ILRMA
[estSig, cost] = ILRMA(mixSig, nSrc, resampFreq, nBases, fftSize, shiftSize, windowType, nIter, ilrmaType, refMic, applyNormalize, applyWhitening, drawConv);

% Blind source separation based on t-ILRMA
% [estSig, cost] = tILRMA(mixSig, nSrc, resampFreq, nBases, dofParam, sigDom, fftSize, shiftSize, windowType, nIter, refMic, applyNormalize, applyWhitening, drawConv);

% Blind source separation based on consistent ILRMA
% [estSig, cost] = consistentILRMA(mixSig, nSrc, resampFreq, nBases, fftSize, shiftSize, windowType, nIter, refMic, applyWhitening, drawConv);

% Blind source separation based on ILRMA-ISS
% [estSig, cost] = ILRMAISS(mixSig, nSrc, resampFreq, nBases, fftSize, shiftSize, windowType, nIter, ilrmaType, refMic, applyNormalize, applyWhitening, drawConv);

% Output separated signals
outputDir = sprintf('./output');
if ~isdir( outputDir )
    mkdir( outputDir );
end
audiowrite(sprintf('%s/observedMixture.wav', outputDir), mixSig, resampFreq); % observed signal
audiowrite(sprintf('%s/originalSource1.wav', outputDir), srcSigResample(:,refMic,1), resampFreq); % source signal 1
audiowrite(sprintf('%s/originalSource2.wav', outputDir), srcSigResample(:,refMic,2), resampFreq); % source signal 2
audiowrite(sprintf('%s/estimatedSignal1.wav', outputDir), estSig(:,1), resampFreq); % estimated signal 1
audiowrite(sprintf('%s/estimatedSignal2.wav', outputDir), estSig(:,2), resampFreq); % estimated signal 2

fprintf('The files are saved in "./output".\n');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% EOF %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function RE = estimate_registration_error(fixedCoordinates,movingCoordinates,displacementField,spacing)
% ESTIMATE_REGISTRATION_ERROR Estimates the target registration error
%
% RE = estimate_registration_error(...
%   fixedCoordinates,movingCoordinates,displacementField,spacing)
%
% INPUT ARGUMENTS
% fixedCoordinates      - Coordinates in fixed image
% movingCoordinates     - Coordinates in moving image
% displacementField     - Displacement field, pointing from fixed to moving
% spacing               - Pixel spacing
%
% OPTIONAL INPUT ARGUMENTS
% N/A
%
% OUTPUT ARGUMENTS
% RE                    - Registration error

% Copyright (c) 2012 Daniel Forsberg
% danne.forsberg@outlook.com
%
% 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/>.

dims = length(displacementField);
deformedFixedCoordinates = fixedCoordinates;
switch dims
    case 1
        xDisp = interp3(displacementField{1},fixedCoordinates(:,1));
        deformedFixedCoordinates(:,1) = fixedCoordinates(:,1) + xDisp;
        RE = deformedFixedCoordinates - movingCoordinates;
        RE = sqrt((RE(:,1)*spacing(1)).^2);
    case 2
        xDisp = interp3(displacementField{1},fixedCoordinates(:,1),fixedCoordinates(:,2));
        yDisp = interp3(displacementField{2},fixedCoordinates(:,1),fixedCoordinates(:,2));
        deformedFixedCoordinates(:,1) = fixedCoordinates(:,1) + xDisp;
        deformedFixedCoordinates(:,2) = fixedCoordinates(:,2) + yDisp;
        RE = deformedFixedCoordinates - movingCoordinates;
        RE = sqrt((RE(:,1).^spacing(1)).^2 + (RE(:,2)*spacing(2)).^2);
    case 3
        xDisp = interp3(displacementField{1},fixedCoordinates(:,1),fixedCoordinates(:,2),fixedCoordinates(:,3));
        yDisp = interp3(displacementField{2},fixedCoordinates(:,1),fixedCoordinates(:,2),fixedCoordinates(:,3));
        zDisp = interp3(displacementField{3},fixedCoordinates(:,1),fixedCoordinates(:,2),fixedCoordinates(:,3));
        deformedFixedCoordinates(:,1) = fixedCoordinates(:,1) + xDisp;
        deformedFixedCoordinates(:,2) = fixedCoordinates(:,2) + yDisp;
        deformedFixedCoordinates(:,3) = fixedCoordinates(:,3) + zDisp;
        RE = deformedFixedCoordinates - movingCoordinates;
        RE = sqrt((RE(:,1)*spacing(1)).^2 + (RE(:,2)*spacing(2)).^2 + (RE(:,3)*spacing(3)).^2);
    otherwise
        error('Not implemented for dimensions higher than three.')
end


function g = dexpKernGradient(kern, x1, varargin)

% DEXPKERNGRADIENT Gradient of the double exponential kernel's parameters.
%
% FORMAT
% DESC computes the gradient of functions with respect to the double
% exponential kernel's parameters. As well as the kernel structure
% and the input positions, the user provides a matrix PARTIAL which gives
% the partial derivatives of the function with respect to the relevant
% elements of the kernel matrix.
% ARG kern : the kernel structure for which the gradients are being
% computed.
% ARG x1 : the input locations for which the gradients are being
% computed in the form of a design matrix.
% ARG partial : matrix of partial derivatives of the function of
% interest with respect to the kernel matrix. The argument takes
% the form of a square matrix of dimension  numData, where numData is
% the number of rows in x1.
% RETURN g : gradients of the function of interest with respect to
% the kernel parameters. The ordering of the vector should match
% that provided by the function kernExtractParam.
%
% FORMAT
% DESC computes the derivatives as above, but input locations are
% now provided in two matrices associated with rows and columns of
% the kernel matrix.
% ARG kern : the kernel structure for which the gradients are being
% computed.
% ARG x1 : the input locations associated with the rows of the
% kernel matrix in the form of a design matrix.
% ARG x2 : the input locations associated with the columns of the
% kernel matrix in the form of a design matrix.
% ARG partial : matrix of partial derivatives of the function of
% interest with respect to the kernel matrix. The matrix should
% have the same number of rows as x1 and the same number of columns
% as x2 has rows.
% RETURN g : gradients of the function of interest with respect to
% the kernel parameters.
%
% SEEALSO dexpKernParamInit, kernGradient, dexpKernDiagGradient, kernGradX
%
% COPYRIGHT : David Luengo, 2009

% KERN

if nargin < 4
    x2 = x1;
else
    x2 = varargin{1};
end

[K, sK, n1] = dexpKernCompute(kern, x1, x2);

g(1) = kern.variance * sum(sum(varargin{end} .* sK ...
    .* (1 - n1*kern.decay)));
g(2) = kern.decay * sum(sum(varargin{end} .* sK));

%/~
if any(isnan(g))
  warning('g is NaN')
end
%~/


function plotExcitationIRF(varargin)
% Plots the excitation IRF for each hydro structure's bodies in
% the heave, surge and pitch degrees of freedom.
%
% Usage:
% ``plotExcitationIRF(hydro, hydro2, hydro3, ...)``
%
% Parameters
% ----------
%     varargin : struct(s)
%         The hydroData structure(s) created by the other BEMIO functions.
%         One or more may be input.
%

if isempty(varargin)
    error(['plotExcitationIRF: No arguments passed. Include one or more hydro ' ...
        'structures when calling: plotExcitationIRF(hydro1, hydro2, ...)']);
end

B=1;  % Wave heading index
figHandle = figure('Position',[950,100,975,521]);
titleString = ['Normalized Excitation Impulse Response Functions:   ',...
    '$$\bar{K}_i(t) = {\frac{1}{2\pi}}\int_{-\infty}^{\infty}{\frac{X_i(\omega,\theta)e^{i{\omega}t}}{{\rho}g}}d\omega$$'];
subtitleStrings = {'Surge','Heave','Pitch'};
xString = {'$$t (s)$$','$$t (s)$$','$$t (s)$$'};
yString = {['$$\bar{K}_1(t,\theta$$',' = ',num2str(varargin{1}.theta(B)),'$$^{\circ}$$)'],...
    ['$$\bar{K}_3(t,\theta$$',' = ',num2str(varargin{1}.theta(B)),'$$^{\circ}$$)'],...
    ['$$\bar{K}_5(t,\theta$$',' = ',num2str(varargin{1}.theta(B)),'$$^{\circ}$$)']};

notes = {'Notes:',...
    ['$$\bullet$$ The IRF should tend towards zero within the specified timeframe. ',...
    'If it does not, attempt to correct this by adjusting the $$\omega$$ and ',...
    '$$t$$ range and/or step size used in the IRF calculation.'],...
    ['$$\bullet$$ Only the IRFs for the first wave heading, surge, heave, and ',...
    'pitch DOFs are plotted here. If another wave heading or DOF is significant ',...
    'to the system, that IRF should also be plotted and verified before proceeding.']};

numHydro = length(varargin);
for ii = 1:numHydro
    numBod = varargin{ii}.Nb;
    tmp1 = strcat('X',num2str(ii));
    X.(tmp1) = varargin{ii}.ex_t;
    tmp2 = strcat('Y',num2str(ii));
    a = 0;
    for i = 1:numBod
        m = varargin{ii}.dof(i);
        Y.(tmp2)(1,i,:) = squeeze(varargin{ii}.ex_K(a+1,B,:));
        Y.(tmp2)(2,i,:) = squeeze(varargin{ii}.ex_K(a+3,B,:));
        Y.(tmp2)(3,i,:) = squeeze(varargin{ii}.ex_K(a+5,B,:));
        legendStrings{i,ii} = [varargin{ii}.body{i}];
        a = a + m;
    end
end

formatPlot(figHandle,titleString,subtitleStrings,xString,yString,X,Y,legendStrings,notes)
saveas(figHandle,'Excitation_IRFs.png');

end


function h = plus(f, g)
%+   Plus for SEPARABLEAPPROX objects.
%
% F + G adds F and G. F and G can be scalars or SEPARABLEAPPROX objects.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

if ( ~isa(f, 'separableApprox') ) % ??? + SEPARABLEAPPROX

    h = plus(g, f);

elseif ( isempty(g) ) % SEPARABLEAPPROX + []

    h = f;

elseif ( isempty(f) ) % [] + SEPARABLEAPPROX

    h = g;

elseif ( isa( g, 'double' ) )           % SEPARABLEAPPROX + DOUBLE

    g = compose( 0*f,@plus, g);   % promote double to object class.
    h = plus(f, g);

elseif ( ~isa(g, 'separableApprox') )          % SEPARABLEAPPROX + ???

    error( 'CHEBFUN:SEPARABLEAPPROX:plus:unknown', ...
        ['Undefined function ''plus'' for input arguments of type %s ' ...
        'and %s.'], class(f), class(g));

else                                     % SEPARABLEAPPROX + SEPARABLEAPPROX

    % Domain Check:
    if ( ~domainCheck(f, g) )
        error('CHEBFUN:SEPARABLEAPPROX:plus:domain', 'Inconsistent domains.');
    end

    % Type check:
    if ( ~strcmp( class(f),class(g) ) )
        error( 'CHEBFUN:SEPARABLEAPPROX:plus:unknown', ...
            ['Undefined function ''plus'' for input arguments of type %s ' ...
            'and %s. Try converting to the same type.'], class(f), class(g));
    end

    % Check for zero SEPARABLEAPPROX objects:
    if ( iszero(f) )
        h = g;
    elseif ( iszero(g) )
        h = f;
    else
        % Add together two nonzero SEPARABLEAPPROX objects:
        h = compression_plus(f, g);

    end

end

end

function h = compression_plus(f, g)
% Add SEPARABLEAPPROX objects together by a compression algorithm.

% The algorithm is as follows:
% If A = XY^T and B = WZ^T, then A + B = [X W]*[Y Z]^T,
% [Qleft, Rleft] = qr([X W])
% [Qright, Rright] = qr([Y Z])
% A + B = Qleft * (Rleft * Rright') * Qright'
% [U, S, V] = svd( Rleft * Rright' )
% A + B = (Qleft * U) * S * (V' * Qright')     -> new low rank representation

% Hack: Ensure g has the smaller pivot values.
if ( norm(f.pivotValues, -inf) < norm(g.pivotValues, -inf) )
    % [TODO]: Understand why this works!
    h = compression_plus(g, f);
    return
end

fScl = diag(1./f.pivotValues);
gScl = diag(1./g.pivotValues);
cols = [f.cols, g.cols];
rows = [f.rows, g.rows];

[Qcols, Rcols] = qr(cols);
[Qrows, Rrows] = qr(rows);

Z = zeros(length(fScl), length(gScl));
D = [ fScl, Z ; Z.', gScl ];
[U, S, V] = svd(Rcols * D * Rrows.');
% Take diagonal from SIGMA:
s = diag(S);

% Compress the format if possible.
% [TODO]: What should EPS be in the tolerance check below?
vf = vscale(f);
vg = vscale(g);

vscl = 2*max(vf, vg);
% Remove singular values that fall below eps*vscale:
idx = find( s > 10*eps * vscl, 1, 'last');

if ( isempty(idx) )
    % Return 0 separableApprox
    h = 0*f;
else
    U = U(:,1:idx);
    V = V(:,1:idx);
    s = s(1:idx);
    h = f;
    h.cols = Qcols * U;
    h.rows = Qrows * conj(V);
    % [TODO]: PivotValues have very little meaning after this compression step.
    % For now we assign the singular values as the pivot values.
    h.pivotValues = 1./s;
end

end


function p = minkowski_sum(a, b)

if size(a, 2) == 1 || size(b, 2) == 1
  p = bsxfun(@plus, a, b);
else
  p = zeros(2, size(a, 2) * size(b, 2));
  idx = 0;
  for j = 1:size(a, 2)
    p(:,idx+(1:size(b,2))) = bsxfun(@plus, a(:,j), b);
    idx = idx + size(b,2);
  end
  k = convhull(p(1,:), p(2,:), 'simplify', true);
  p = p(:,k(1:end-1));
end


function [xEst,PEst]=batchLSNonlinMeasLinDyn(xInit,z,h,F,R,kD,HJacob,numIter)
%%BATCHLSNONLINMEASLINDYN Perform batch least squares state estimation
%                      refinement under a nonlinear measurement model and a
%                      linear dynamic model without process noise. The
%                      iterative nonlinear least squares algorithm is used.
%
%INPUTS: xInit The initial estimate of the target state at the time of the
%              kDth measurement that is to be refined through iteration.
%            z The zDim X N matrix of measurements for the whole batch. It
%              is assumed that the measurements have the same
%              dimensionality over the batch.
%            h A NX1 cell array of function handles for the measurement
%              function that transform the state into the measurement
%              domain at each step. If the same measurement function is
%              used for all steps in the batch, then h can just be the
%              single function handle used.
%            F An xDim X xDim X (N-1) hypermatrix of matrices. The state at
%              discrete-time k+1 is modeled as F(:,:,k) times the state at
%              time k with no process noise. Alternatively, if all of the
%              state transition matrices are the same, one can just pass a
%              single xDim X xDim matrix. Note that all of the F matrices
%              must be invertible if kD is not equal to one.
%            R The zDim X zDim X N hypermatrix of measurement covariance
%              matrices. Alternatively, if all of the measurement
%              covariance matrices are the same, one can just pass a
%              single zDim X zDim matrix.
%           kD The discrete time-step at which the smoothed state estimate
%              is desired, where z(:,1) is at discrete time-step 1 (not 0).
%       HJacob A NX1 cell array of function handles for the measurement
%              Jacobian matrix that each takes the target state as a
%              parameter. If a single measurement Jacobian matrix is used
%              for all steps of the batch, then HJacob can just be the
%              single function handle used. If an empty matrix is passed or
%              HJacob is not provided, then HJacob will be found using
%              numerical differentiation via the numDiff function with
%              default parameters.
%      numIter The numer of iterations to perform in the nonlinear least
%              squares algorithm. If this parameter is omitted, the
%              default is 10.
%
%OUTPUTS: xEst The refined batch state estimate at step kD.
%         PEst A covariance matrix estimate that goes with the state
%              estimate.
%
%The algorithm is an implementation of the nonlinear iterative least
%squares algorithm of Chapter 3.4 of [1]. The covariance provided is the
%covariance associated with the algorithm. The measurement matrices for the
%propagated state as well as the transition matrices for the propagated
%state are taken as derived in Section 3.3.2 of [2].
%
%The algorithm is likely to diverge if xEst is particularly bad. A more
%robust albeit slower estimation algorithm is
%batchLSNonlinMeasLinDynProcNoise, which also supports the inclusion of
%process noise.
%
%REFERENCES:
%[1] Y. Bar-Shalom, X. R. Li, and T. Kirubarajan, Estimation with
%    Applications to Tracking and Navigation. New York: John Wiley and
%    Sons, Inc, 2001.
%[2] A. B. Poore, B. J. Slocumb, B. J. Suchomel, F. H. Obermeyer, S. M.
%    Herman, and S. M. Gadaleta, "Batch maximum likelihood (ML) and maximum
%    a posteriori (MAP) estimation with process noise for tracking
%    applications," in Proceedings of SPIE: Signal and Data Processing of
%    Small Targets, vol. 5204, San Diego, CA, 3 Aug. 2003, pp. 188-199.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<8)
    numIter=10;
end
if(nargin<7)
    HJacob=[];
end

xEst=xInit;

zDim=size(z,1);
xDim=size(xEst,1);
numSteps=size(z,2);

if(isa(h,'function_handle'))
    h=repmat({h},[numSteps,1]);
end

if(isa(HJacob,'function_handle'))
    HJacob=repmat({HJacob},[numSteps,1]);
end

if(isempty(HJacob))
    HJacob=cell(numSteps,1);
    for curStep=1:numSteps
        HJacob{curStep}=@(x)numDiff(x,h{curStep},zDim);
    end
end

if(size(R,3)==1)
    R=repmat(R,[1,1,numSteps]);
end

J=zeros(zDim*numSteps,xDim);
zPredStacked=zeros(zDim*numSteps,1);

RStackedInv=inv(blkDiagRep(R));

if(size(F,3)==1)
    F=repmat(F,[1,1,numSteps-1]);
end

transMats=getTransMats(F,kD);

for curIter=1:numIter
    minIdx=1;
    for curStep=1:numSteps
        maxIdx=minIdx+zDim-1;

        xCur=transMats(:,:,curStep)*xEst;
        J(minIdx:maxIdx,:)=HJacob{curStep}(xCur)*transMats(:,:,curStep);

        zPredStacked(minIdx:maxIdx)=h{curStep}(xCur);
        minIdx=maxIdx+1;
    end

    xEst=xEst+lsqminnorm(J'*RStackedInv*J,J'*RStackedInv*(z(:)-zPredStacked));
end

%If a covariance matrix estimate is desired.
if(nargout==2)
    minIdx=1;
    for curStep=1:numSteps
        maxIdx=minIdx+zDim-1;

        xCur=transMats(:,:,curStep)*xEst;
        J(minIdx:maxIdx,:)=HJacob{curStep}(xCur)*transMats(:,:,curStep);

        minIdx=maxIdx+1;
    end

    PEst=pinv(J'*RStackedInv*J);
end

end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% function [yc,His] = GaussNewton(fctn,yc,varargin)
%
% Gauss-Newton scheme with variable line search for minimizing J = fctn(yc)
%
% Input:
%   fctn        function handle
%   yc          starting guess
%   varargin    optional parameter, see below
%
% Output:
%   yc          numerical optimizer (current iterate)
%   his         iteration history
%
% Note: the linear systems are solved using solveLinearSystem.m
%==============================================================================

function [yc,His] = GaussNewton(fctn,yc,varargin)

if nargin==0
    help(mfilename)
    E9_Hands_NPIR
    yc = 'endOfMinimalExample';
    return;
end;

% parameter initialization -----------------------------------------------
maxIter      = 10;              % maximum number of iterations
tolJ         = 1e-3;            % for stopping, objective function
tolY         = 1e-2;            %   - " -     , current value
tolG         = 1e-2;            %   - " -     , norm of gradient
lineSearch   = @Armijo;         % linesearch scheme
LSmaxIter    = 10;              % maximum number of line search iterations
LSreduction  = 1e-4;            % minimal reduction in line search
vecNorm      = @norm;           % norm to be used for dJ and dy
solver       = [];              % linear solver
yStop        = [];              % used for stopping in multi-level framework
Jstop        = [];              %
Plots        = @(iter,para) []; % for plots;

for k=1:2:length(varargin),     % overwrites default parameter
  eval([varargin{k},'=varargin{',int2str(k+1),'};']);
end;

if ~isa(Plots,'function_handle') && (Plots == 0 || strcmp(Plots,'off')),
  Plots        = @(iter,para) []; % for plots;
end;

if isempty(yStop), yStop  = yc; end; % yStop used for stopping only
% -- end parameter setup   ----------------------------------------------

% some output
if isstring(solver) || ischar(solver)
    solverName = solver;
elseif isa(solver,'function_handle')
    solverName = func2str(solver);
else
    error(1);
end
FAIRmessage(sprintf('JM 2011/01/02 : %s / %s / %s',...
  mfilename,func2str(lineSearch),solverName))
fprintf('[ maxIter=%s / tolJ=%s / tolY=%s / tolG=%s / numel(yc)=%d]\n',...
  num2str(maxIter),num2str(tolJ),num2str(tolY),num2str(tolG),length(yc));

% -- initialize  ---------------------------------------------------------
STOP = zeros(5,1);

if isempty(Jstop),
  % evaluate objective function for stopping values and plots
  [Jstop,para] = fctn(yStop); Jstop = abs(Jstop) + (Jstop == 0);
  Plots('stop',para);
end;

% evaluate objective function for starting values and plots
[Jc,para,dJ,H] = fctn(yc);
Plots('start',para);
iter = 0; yOld = 0*yc; Jold = Jc; y0 = yc;

hisStr    = {'iter','J','Jold-J','|\nabla J|','|dy|','LS'};
his        = zeros(maxIter+2,6);
his(1,1:3) = [-1,Jstop,Jstop-Jc];
his(2,:)   = [0,Jc,Jstop-Jc,vecNorm(dJ),vecNorm(yc-yStop),0];

% some output
fprintf('%4s %-12s %-12s %-12s %-12s %4s\n%s\n',...
  hisStr{:},char(ones(1,64)*'-'));
dispHis = @(var) ...
  fprintf('%4d %-12.4e %-12.3e %-12.3e %-12.3e %4d\n',var);
dispHis(his(1,:));
dispHis(his(2,:));
% -- end initialization   ------------------------------------------------

%-- start the iteration --------------------------------------------------
while 1,
  % check stopping rules
  STOP(1) = (iter>0) && abs(Jold-Jc)   <= tolJ*(1+abs(Jstop));
  STOP(2) = (iter>0) && (norm(yc-yOld) <= tolY*(1+norm(yStop)));
  STOP(3) = norm(dJ)                   <= tolG*(1+abs(Jstop));
  STOP(4) = norm(dJ)                   <= 1e6*eps;
  STOP(5) = (iter >= maxIter);
  if all(STOP(1:3)) || any(STOP(4:5)), break;  end;

  iter = iter + 1;

  % solve the Gauss-Newton System
  [dy,solver] = solveLinearSystem(-dJ',H,solver);

  % check descent direction
  % note: descent is not granted if using an iterative solver
  descent =   dJ * dy;
  if descent > 0,
    warning('no descent direction, switch to -dy!')
    dy      = -dy;
  end;

  % perform Armijo line-search
  [t,yt,LSiter] = lineSearch(fctn,yc,dy,Jc,dJ,...
      'para',para,'LSmaxIter',LSmaxIter,'LSreduction',LSreduction);
  if (t == 0),
      break;
  end; % break if line-search fails

  % save old values and update
  yOld = yc; Jold = Jc; yc = yt;
  [Jc,para,dJ,H] = fctn(yc); % evalute objective function

  % some output
  his(iter+2,:) = [iter,Jc,Jold-Jc,vecNorm(dJ),vecNorm(yc-yOld),LSiter];
  dispHis(his(iter+2,:));
  para.normdY = vecNorm(yc - yOld);
  Plots(iter,para);
% pause
end;%while; % end of iteration loop
%-------------------------------------------------------------------------
Plots(iter,para);

% clean up
His.str = hisStr;
His.his = his(1:iter+2,:);
fprintf('STOPPING:\n');
fprintf('%d[ %-10s=%16.8e <= %-25s=%16.8e]\n',STOP(1),...
  '(Jold-Jc)',(Jold-Jc),'tolJ*(1+|Jstop|)',tolJ*(1+abs(Jstop)));
fprintf('%d[ %-10s=%16.8e <= %-25s=%16.8e]\n',STOP(2),...
  '|yc-yOld|',norm(yc-yOld),'tolY*(1+norm(yc)) ',tolY*(1+norm(yStop)));
fprintf('%d[ %-10s=%16.8e <= %-25s=%16.8e]\n',STOP(3),...
  '|dJ|',norm(dJ),'tolG*(1+abs(Jstop))',tolG*(1+abs(Jstop)));
fprintf('%d[ %-10s=%16.8e <= %-25s=%16.8e]\n',STOP(4),...
  'norm(dJ)',norm(dJ),'eps',1e3*eps);
fprintf('%d[ %-10s=  %-14d >= %-25s=  %-14d]\n',STOP(5),...
  'iter',iter,'maxIter',maxIter);

FAIRmessage([mfilename,' : done !'],'=');

%==============================================================================


function value = r4_aie ( x )

%*****************************************************************************80
%
%% R4_AIE evaluates the exponential scaled Airy function Ai of an R4 argument.
%
%  Discussion:
%
%    If X <= 0
%      R4_AIE ( X ) = R4_AI ( X )
%    else
%      R4_AIE ( X ) = R4_AI ( X ) * exp ( 2/3 X^(3/2) )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the Airy function Ai of X.
%
  persistent aifcs
  persistent aigcs
  persistent aipcs
  persistent naif
  persistent naig
  persistent naip
  persistent x32sml
  persistent x3sml
  persistent xbig

  if ( isempty ( naif ) )
    aifcs = [ ...
     -0.03797135849666999750E+00, ...
      0.05919188853726363857E+00, ...
      0.00098629280577279975E+00, ...
      0.00000684884381907656E+00, ...
      0.00000002594202596219E+00, ...
      0.00000000006176612774E+00, ...
      0.00000000000010092454E+00, ...
      0.00000000000000012014E+00, ...
      0.00000000000000000010E+00 ]';
    aigcs = [ ...
      0.01815236558116127E+00, ...
      0.02157256316601076E+00, ...
      0.00025678356987483E+00, ...
      0.00000142652141197E+00, ...
      0.00000000457211492E+00, ...
      0.00000000000952517E+00, ...
      0.00000000000001392E+00, ...
      0.00000000000000001E+00 ]';
    aipcs = [ ...
     -0.0187519297793868E+00, ...
     -0.0091443848250055E+00, ...
      0.0009010457337825E+00, ...
     -0.0001394184127221E+00, ...
      0.0000273815815785E+00, ...
     -0.0000062750421119E+00, ...
      0.0000016064844184E+00, ...
     -0.0000004476392158E+00, ...
      0.0000001334635874E+00, ...
     -0.0000000420735334E+00, ...
      0.0000000139021990E+00, ...
     -0.0000000047831848E+00, ...
      0.0000000017047897E+00, ...
     -0.0000000006268389E+00, ...
      0.0000000002369824E+00, ...
     -0.0000000000918641E+00, ...
      0.0000000000364278E+00, ...
     -0.0000000000147475E+00, ...
      0.0000000000060851E+00, ...
     -0.0000000000025552E+00, ...
      0.0000000000010906E+00, ...
     -0.0000000000004725E+00, ...
      0.0000000000002076E+00, ...
     -0.0000000000000924E+00, ...
      0.0000000000000417E+00, ...
     -0.0000000000000190E+00, ...
      0.0000000000000087E+00, ...
     -0.0000000000000040E+00, ...
      0.0000000000000019E+00, ...
     -0.0000000000000009E+00, ...
      0.0000000000000004E+00, ...
     -0.0000000000000002E+00, ...
      0.0000000000000001E+00, ...
     -0.0000000000000000E+00 ]';
    eta = 0.1 * r4_mach ( 3 );
    naif = r4_inits ( aifcs, 9, eta );
    naig = r4_inits ( aigcs, 8, eta );
    naip = r4_inits ( aipcs, 34, eta );
    x3sml = eta^0.3333;
    x32sml = 1.3104 * x3sml * x3sml;
    xbig = r4_mach ( 2 )^0.6666;
  end

  if ( x < - 1.0 )
    [ xm, theta ] = r4_aimp ( x );
    value = xm * cos ( theta );
  elseif ( abs ( x ) <= x32sml )
    z = 0.0;
    value = 0.375 + ( r4_csevl ( z, aifcs, naif ) ...
      - x * ( 0.25 + r4_csevl ( z, aigcs, naig ) ) );
  elseif ( abs ( x ) <= x3sml )
    z = 0.0;
    value = 0.375 + ( r4_csevl ( z, aifcs, naif ) ...
      - x * ( 0.25 + r4_csevl ( z, aigcs, naig ) ) );
    value = value * exp ( 2.0 * x * sqrt ( x ) / 3.0 );
  elseif ( x <= 1.0 )
    z = x * x * x;
    value = 0.375 + ( r4_csevl ( z, aifcs, naif ) ...
      - x * ( 0.25 + r4_csevl ( z, aigcs, naig ) ) );
    value = value * exp ( 2.0 * x * sqrt ( x ) / 3.0 );
  elseif ( x < xbig )
    sqrtx = sqrt ( x );
    z = 2.0 / ( x * sqrtx ) - 1.0;
    value = ( 0.28125 + r4_csevl ( z, aipcs, naip ) ) / sqrt ( sqrtx );
  else
    sqrtx = sqrt ( x );
    z = - 1.0;
    value = ( 0.28125 + r4_csevl ( z, aipcs, naip ) ) / sqrt ( sqrtx );
  end

  return
end


function [coherogram,phase,t,f] = bz_MTCoherogram(lfp1,lfp2,varargin)

%MTCoherogram - Compute LFP coherogram by multi-taper estimation.
%
%  USAGE
%
%    [coherogram,phase,t,f] = MTCoherogram(lfp1,lfp2,<options>)
%
%    lfp1,lfp2      wide-band LFPs (one channel each).
%    <options>      optional list of property-value pairs (see table below)
%
%    =========================================================================
%     Properties    Values
%    -------------------------------------------------------------------------
%     'frequency'   sampling rate (in Hz) (default = from timestamps if
%                   available, otherwise 1250Hz)
%     'range'       frequency range (in Hz) (default = all)
%     'window'      duration (in s) of the time window (default = 5)
%     'overlap'     overlap between successive windows (default = window/2)
%     'step'        step between successive windows (default = window/2)
%     'tapers'      relative resolution and order of the tapers [NW K]
%                   (default = [3 5])
%     'pad'         FFT padding (see help for cohgramc) (default = 0)
%     'show'        plot results (default = 'off')
%     'cutoffs'     cutoff values for color plot (default = [0 1])
%    =========================================================================
%
%  NOTES
%
%    The LFP can be provided either as a time stamped matrix (list of time-voltage
%    pairs), or as a voltage vector - in which case the frequency must be specified.
%
%    The time displacement between successive short time coherences can be supplied
%    either as a 'step' (explicit time difference) or as an 'overlap' (between
%    successive time windows).
%
%  OUTPUT
%
%    coherogram     coherogram magnitude
%    phase          coherogram phase
%    t              time bins
%    f              frequency bins
%
%  DEPENDENCIES
%
%    This function requires the chronux toolbox.
%
%  SEE
%
%    See also MTCoherence, MTSpectrum, MTSpectrogram, PlotColorMap.

% Copyright (C) 2010-2014 by Michaël Zugaro
%
% 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.

% Make sure chronux is installed and functional
CheckChronux('cohgramc');

% Defaults
f = 1250;
frequency = [];
window = 5;
range = [];
overlap = [];
step = [];
show = 'off';
tapers = [3 5];
pad = 0;
cutoffs = [0 1];

% Check number of parameters
if nargin < 2 | mod(length(varargin),2) ~= 0,
  error('Incorrect number of parameters (type ''help MTCoherogram'' for details).');
end

% Check parameter sizes
if size(lfp1,2) ~= 1 && size(lfp1,2) ~= 2,
	error('Parameter ''lfp1'' is not a vector or a Nx2 matrix (type ''help MTCoherogram'' for details).');
end
if size(lfp2,2) ~= 1 && size(lfp2,2) ~= 2,
	error('Parameter ''lfp2'' is not a vector or a Nx2 matrix (type ''help MTCoherogram'' for details).');
end

% Parse parameter list
for i = 1:2:length(varargin),
	if ~ischar(varargin{i}),
		error(['Parameter ' num2str(i+2) ' is not a property (type ''help MTCoherogram'' for details).']);
	end
	switch(lower(varargin{i})),
		case 'frequency',
			frequency = varargin{i+1};
			if ~isdscalar(frequency,'>0'),
				error('Incorrect value for property ''frequency'' (type ''help MTCoherogram'' for details).');
			end
		case 'range',
			range = varargin{i+1};
			if ~isdvector(range,'#2','<','>=0'),
				error('Incorrect value for property ''range'' (type ''help MTCoherogram'' for details).');
			end
		case 'window',
			window = varargin{i+1};
			if ~isdscalar(window,'>0'),
				error('Incorrect value for property ''window'' (type ''help MTCoherogram'' for details).');
			end
		case 'overlap',
			overlap = varargin{i+1};
			if ~isdscalar(overlap,'>0'),
				error('Incorrect value for property ''overlap'' (type ''help MTCoherogram'' for details).');
			end
		case 'step',
			step = varargin{i+1};
			if ~isdscalar(step,'>0'),
				error('Incorrect value for property ''step'' (type ''help MTCoherogram'' for details).');
			end
		case 'tapers',
			tapers = varargin{i+1};
			if ~isivector(tapers,'#2','>0'),
				error('Incorrect value for property ''tapers'' (type ''help MTCoherogram'' for details).');
			end
		case 'pad',
			pad = varargin{i+1};
			if ~isiscalar(pad,'>-1'),
				error('Incorrect value for property ''pad'' (type ''help MTCoherogram'' for details).');
			end
		case 'show',
			show = varargin{i+1};
			if ~isstring_FMAT(show,'on','off'),
				error('Incorrect value for property ''show'' (type ''help MTCoherogram'' for details).');
			end
		case 'cutoffs',
			cutoffs = varargin{i+1};
			if ~isdvector(cutoffs,'#2','>=0','<'),
				error('Incorrect value for property ''cutoffs'' (type ''help MTCoherogram'' for details).');
			end
		otherwise,
			error(['Unknown property ''' num2str(varargin{i}) ''' (type ''help MTCoherogram'' for details).']);
	end
end

% Determine LFP frequency
if isempty(frequency),
	if size(lfp1,2) == 2,
		frequency = 1/median(diff(lfp1(:,1)));
	else
		frequency = f;
	end
end

% Determine step/overlap
if isempty(step),
	if isempty(overlap),
		overlap = window/2;
	end
else
	if isempty(overlap),
		overlap = window-step;
	elseif overlap ~= window-step,
		error('Incompatible ''step'' and ''overlap'' parameters (type ''help MTCoherogram'' for details).');
	end
end

% Compute and plot coherogram
parameters.Fs = frequency;
if ~isempty(range), parameters.fpass = range; end
parameters.tapers = tapers;
parameters.pad = pad;
[coherogram,phase,~,~,~,t,f] = cohgramc(lfp1,lfp2,[window window-overlap],parameters);
% t = t'+lfp1(1,1);
f = f';
coherogram = coherogram';
% coherogram = permute(coherogram,[2 1 3]);  % Previous code by Gabrielle Girardeau, keep it around just in case
phase = phase';
if strcmp(lower(show),'on'),
  figure;hold on;
  subplot(2,1,1);
  PlotColorMap(coherogram,'x',t,'y',f,'cutoffs',cutoffs,'newfig','off');
  xlabel('Time (s)');
  ylabel('Frequency (Hz)');
  title('Coherogram Amplitude');
  subplot(2,1,2);
  PlotColorMap(phase,'x',t,'y',f,'cutoffs',[-pi pi],'newfig','off');
  xlabel('Time (s)');
  ylabel('Frequency (Hz)');
  title('Coherogram Phase');
end


function  Raw = simObservation(SimRob, SimSen, SimLmk, Opt)

% SIMOBSERVATION Observe simulated landmarks.
%   RAW = SIMOBSERVATION(SIMROB,SIMSEN,SIMLMK) returns the raw data
%   captured for the sensor.
%       SIMROB: simulated robot structure
%       SIMSEN: simulated sensor strucure
%       SIMLMK: simulated landmarks strucure
%
%   See also SIMMOTION PROJEUCPNTINTOPINHOLEONROB

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

Raw.type = 'simu';

switch SimSen.type

    case {'pinHole'}      % camera pinHole

        % Project virtual world's points
        [Raw.data.points.coord, s] = projEucPntIntoPinHoleOnRob(...
            SimRob.frame, ...
            SimSen.frame, ...
            SimSen.par.k, ...
            SimSen.par.d, ...
            SimLmk.points.coord);
        Raw.data.points.app  = SimLmk.points.id;

        % Add sensor noise
        Raw.data.points.coord = Raw.data.points.coord + ...
            SimSen.par.pixErr*randn(size(Raw.data.points.coord));

        % Remove non visible
        vis = isVisible(Raw.data.points.coord,s,SimSen.par.imSize);
        Raw.data.points.coord(:, ~vis)  = [];
        Raw.data.points.app(~vis) = [];

        % Project virtual world's segments
        [Raw.data.segments.coord, s] = projSegLinIntoPinHoleOnRob(...
            SimRob.frame, ...
            SimSen.frame, ...
            SimSen.par.k, ...
            SimLmk.segments.coord);
        Raw.data.segments.app  = SimLmk.segments.id;

        % Add sensor noise
        Raw.data.segments.coord = Raw.data.segments.coord + ...
            SimSen.par.cov*randn(size(Raw.data.points.coord));

        % Remove non visible
        [Raw.data.segments.coord,vis] = visibleSegment( ...
            Raw.data.segments.coord,...
            s,...
            SimSen.par.imSize,...
            0,...                      % N pixels margin
            Opt.obs.lines.minLength);  % min segment length
        Raw.data.segments.coord(:, ~vis)  = [];
        Raw.data.segments.app(~vis) = [];

    case {'pinHoleDepth'}      % camera pinHole

        % Project virtual world's points
        Raw.data.points.coord = projEucPntIntoPhdOnRob(...
            SimRob.frame, ...
            SimSen.frame, ...
            SimSen.par.k, ...
            SimSen.par.d, ...
            SimLmk.points.coord);
        Raw.data.points.app  = SimLmk.points.id;

        % Add sensor noise
        Raw.data.points.coord = Raw.data.points.coord + ...
            SimSen.par.cov*randn(size(Raw.data.points.coord));

        % Remove non visible
        vis = isVisible(Raw.data.points.coord(1:2,:),Raw.data.points.coord(3,:),SimSen.par.imSize);
        Raw.data.points.coord(:, ~vis)  = [];
        Raw.data.points.app(~vis) = [];

        % TODO: Lines not implemented for sensor type 'pihHoleDepth'

        %         % Project virtual world's segments
        Raw.data.segments.coord = [];
        %         [Raw.data.segments.coord, s] = projSegLinIntoPinHoleOnRob(...
        %             SimRob.frame, ...
        %             SimSen.frame, ...
        %             SimSen.par.k, ...
        %             SimLmk.segments.coord);
        %         Raw.data.segments.app  = SimLmk.segments.id;
        Raw.data.segments.app = [];
        %
        %         % Add sensor noise
        %         Raw.data.segments.coord = Raw.data.segments.coord + ...
        %             SimSen.par.pixErr*randn(size(Raw.data.segments.coord));
        %
        %         % Remove non visible
        %         [Raw.data.segments.coord,vis] = visibleSegment( ...
        %             Raw.data.segments.coord,...
        %             s,...
        %             SimSen.par.imSize,...
        %             0,...                      % N pixels margin
        %             Opt.obs.lines.minLength);  % min segment length
        %         Raw.data.segments.coord(:, ~vis)  = [];
        %         Raw.data.segments.app(~vis) = [];

    case {'omniCam'}      % Omnidirectional camera

        % Project virtual world's points
        [Raw.data.points.coord, s] = projEucPntIntoOmniCamOnRob(...
            SimRob.frame, ...
            SimSen.frame, ...
            SimSen.par.k, ...
            SimSen.par.d, ...
            SimLmk.points.coord);
        Raw.data.points.app  = SimLmk.points.id;

        % Add sensor noise
        Raw.data.points.coord = Raw.data.points.coord + ...
            SimSen.par.pixErr*randn(size(Raw.data.points.coord));

        % Remove non visible
        vis = isVisible(Raw.data.points.coord,s,SimSen.par.imSize);
        Raw.data.points.coord(:, ~vis)  = [];
        Raw.data.points.app(~vis) = [];

    otherwise
        % Print an error and exit
        error('??? Unknown sensor type ''%s''.',SimSen.type);

end % end of the "switch" on sensor type

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function [J,Msp,Nsp]=mtfftpt(data,tapers,nfft,t,f,findx)
% Multi-taper fourier transform for point process given as times
%
% Usage:
% [J,Msp,Nsp]=mtfftpt (data,tapers,nfft,t,f,findx) - all arguments required
% Input:
%       data        (struct array of times with dimension channels/trials;
%                   also takes in 1d array of spike times as a column vector)
%       tapers      (precalculated tapers from dpss)
%       nfft        (length of padded data)
%       t           (time points at which tapers are calculated)
%       f           (frequencies of evaluation)
%       findx       (index corresponding to frequencies f)
% Output:
%       J (fft in form frequency index x taper index x channels/trials)
%       Msp (number of spikes per sample in each channel)
%       Nsp (number of spikes in each channel)
if nargin < 6; error('Need all input arguments'); end;
if isstruct(data); C=length(data); else C=1; end% number of channels
K=size(tapers,2); % number of tapers
nfreq=length(f); % number of frequencies
if nfreq~=length(findx); error('frequency information (last two arguments) inconsistent'); end;
H=fft(tapers,nfft,1);  % fft of tapers
H=H(findx,:); % restrict fft of tapers to required frequencies
w=2*pi*f; % angular frequencies at which ft is to be evaluated
Nsp=zeros(1,C); Msp=zeros(1,C);
for ch=1:C;
  if isstruct(data);
     fnames=fieldnames(data);
     eval(['dtmp=data(ch).' fnames{1} ';'])
     indx=find(dtmp>=min(t)&dtmp<=max(t));
     if ~isempty(indx); dtmp=dtmp(indx);
     end;
  else
     dtmp=data;
     indx=find(dtmp>=min(t)&dtmp<=max(t));
     if ~isempty(indx); dtmp=dtmp(indx);
     end;
  end;
  Nsp(ch)=length(dtmp);
  Msp(ch)=Nsp(ch)/length(t);
  if Msp(ch)~=0;
      data_proj=interp1(t',tapers,dtmp);
      exponential=exp(-i*w'*(dtmp-t(1))');
      J(:,:,ch)=exponential*data_proj-H*Msp(ch);
  else
      J(1:nfreq,1:K,ch)=0;
  end;
end;


function r = mrdivide(a,b)
%MRDIVIDE     Implements  a / b  for gradient
%

% written  10/16/98     S.M. Rump
% modified 04/04/04     S.M. Rump  set round to nearest for safety
%                                    complete redesign
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
% modified 08/26/12     S.M. Rump  global variables removed
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  if prod(size(b))~=1
    error('Gradient division only for scalar denominator')
  end

  N = getappdata(0,'INTLAB_GRADIENT_NUMVAR');

  if ~isa(a,'gradient')          % non-gradient / gradient scalar
    r.x = a / b.x;
    n = prod(size(a));
    if n==1                     % non-gradient scalar / gradient scalar
      D = -a / sqr(b.x);
      r.dx = b.dx * D;
    else                        % non-gradient array / gradient scalar
      D = 1 / sqr(b.x);
      if issparse(b.dx)
        ax = sparse(-a(:));
      else
        ax = -a(:);
      end
      r.dx = ax * ( b.dx * D );
    end
  elseif ~isa(b,'gradient')      % gradient / non-gradient scalar
    r.x = a.x / b;
    r.dx = a.dx / b;
  else                          % gradient / gradient scalar
    r.x = a.x / b.x;
    D = 1 / sqr(b.x);
    n = prod(size(a.x));
    if n==1                     % gradient scalar / gradient scalar
      Num = a.dx * b.x - a.x * b.dx;
      r.dx = Num * D;
    else                        % gradient array / gradient scalar
      if issparse(a.dx)
        ax = sparse(a.x(:));
      else
        ax = a.x(:);
      end
      Num = a.dx * b.x - ax * b.dx;
      r.dx = Num * D;
    end
  end

  r = class(r,'gradient');

  if rndold
    setround(rndold)
  end


function [dtf, dtfvar, n] = ft_connectivity_dtf(inputdata, varargin)

% FT_CONNECTIVITY_DTF computes the directed transfer function.
%
% Use as
%   [d, v, n] = ft_connectivity_dtf(inputdata, ...)
%
% The input should be a spectral transfer matrix organized as
%   Nrpt x Nchan x Nchan x Nfreq (x Ntime)
% where Nrpt can be 1.
%
% The output represents
%   d = partial directed coherence matrix Nchan x Nchan x Nfreq (x Ntime).
%       If multiple observations in the input, the average is returned.
%   v = variance of d across observations.
%   n = number of observations.
%
% Typically, nrpt should be 1 where the spectral transfer matrix is computed across
% observations. When nrpt>1 and hasjack=true, the input is assumed to contain the
% leave-one-out estimates of the spectral transfer matrix, thus a more reliable
% estimate of the relevant quantities.
%
% Additional optional input arguments come as key-value pairs:
%   'hasjack'   = boolean, specifying whether the input contains leave-one-outs,
%                 required for correct variance estimate (default = false)
%   'crsspctrm' = matrix containing the cross-spectral density. If this
%                 matrix is defined, the function returns the ddtf, which
%                 requires an estimation of partial coherence from this matrix.
%   'invfun'    = 'inv' (default) or 'pinv', the function used to invert the
%                 crsspctrm matrix to obtain the partial coherence. Pinv is
%                 useful if the data are poorly-conditioned.
%   'feedback'  = 'none', 'text', 'textbar', 'dial', 'etf', 'gui' type of feedback showing progress of computation, see FT_PROGRESS
%
% See also CONNECTIVITY, FT_CONNECTIVITYANALYSIS

% Copyright (C) 2009-2017, Jan-Mathijs Schoffelen
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

hasjack   = ft_getopt(varargin, 'hasjack', 0);
powindx   = ft_getopt(varargin, 'powindx');
feedback  = ft_getopt(varargin, 'feedback', 'none');
crsspctrm = ft_getopt(varargin, 'crsspctrm');
invfun    = ft_getopt(varargin, 'invfun', 'inv');

switch invfun
  case {'inv' 'pinv'}
    invfun = str2func(invfun);
  otherwise
    ft_error('unknown specification of inverse function for the transfer matrix');
end

if ~isempty(powindx)
  % this error message is rather uninformative, but is kept for now for
  % backward compatibility reasons (i.e. it might exist when called from
  % ft_connectivityanalysis
  ft_error('linearly indexed data for DTF computation is at the moment not supported');
end

siz    = [size(inputdata) 1];
n      = siz(1);
outsum = zeros(siz(2:end));
outssq = zeros(siz(2:end));

% check the crsspctrm; if it is present, compute the partial coherence
if ~isempty(crsspctrm)
  assert(isequal(size(crsspctrm),size(inputdata)), 'the input data should be of the same size as the crsspctrm');
  fprintf('computing dDTF in the presence of a crsspctrm\n');

  % the crsspctrm allows for the partial coherence to be computed
  pdim   = prod(siz(4:end));
  tmpcrs = reshape(crsspctrm, [siz(1:3) pdim]);
  ft_progress('init', feedback, 'computing partial coherence...');
  for k = 1:n
    ft_progress(k/n, 'computing partial coherence for replicate %d from %d\n', k, n);
    tmp = reshape(tmpcrs(k,:,:,:), [siz(2:3) pdim]);
    for m = 1:pdim
      tmp(:,:,m) = invfun(tmp(:,:,m));
      tmp(:,:,m) = abs(tmp(:,:,m))./sqrt(abs(diag(tmp(:,:,m))*diag(tmp(:,:,m))'));
    end
    tmpcrs(k,:,:,:) = tmp;
  end
  ft_progress('close');
  crsspctrm = reshape(tmpcrs, siz);
end

% data should be represented as chan_chan_therest
for j = 1:n
  tmph   = reshape(inputdata(j,:,:,:,:), siz(2:end));
  if isempty(crsspctrm)
    % plain DTF
    den    = sum(abs(tmph).^2,2);
    tmpdtf = abs(tmph)./sqrt(repmat(den, [1 siz(2) 1 1 1]));
  else
    % dDTF
    tmpc   = reshape(crsspctrm(j,:,:,:,:), siz(2:end));
    den    = sum(sum(abs(tmph).^2,3),2);
    tmpdtf = abs(tmph)./sqrt(repmat(den, [1 siz(2) siz(4) 1 1 1]));
    tmpdtf = tmpdtf.*tmpc;
  end
  outsum = outsum + tmpdtf;
  outssq = outssq + tmpdtf.^2;
end
dtf = outsum./n;

if n>1 % FIXME this is strictly only true for jackknife, otherwise other bias is needed
  if hasjack
    bias = (n - 1).^2;
  else
    bias = 1;
  end
  dtfvar = bias.*(outssq - (outsum.^2)/n)./(n-1);
else
  dtfvar = [];
end

% this is to achieve the same convention for all connectivity metrics:
% row -> column
for k = 1:prod(siz(4:end))
  dtf(:,:,k)    = transpose(dtf(:,:,k));
  if ~isempty(dtfvar)
    dtfvar(:,:,k) = transpose(dtfvar(:,:,k));
  end
end


function Az = ComputeAz(meta, percent, pixel_coords)
% COMPUTAZ Compute azimuth angle (angle from north) across spatial frequency
%
% Should work generically for many types of SAR complex data, not just
% spotlight collects.
%
% INPUTS:
%   meta         - required: SICD metadata structure
%   percent      - optional: Array of values from 0 to 100.  Indicates the
%                  fraction across the spatial frequency in azimuth for
%                  which to compute azimuth angle (not including zeropad).
%                  Default is scene center point center of aperture.
%   pixel_coords - optional: [column_index, row_index] (az,rng) index to
%                  point of interest.  Default is scene center point (SCP).
%
% OUTPUTS:
%   Az - computed azimuth angle for each PERCENT value
%
% Author: Wade Schwartzkopf, NGA/IDT
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

%% Handle default values for input parameters
% Default for percent is just SCPCOA azimuth
% If percent is not passed, we might not need TimeCOAPoly, so we check this first.
if ~exist('percent','var')
    % SCPCOA.AzimAng is an actual field in latest SICD spec
    if isfield(meta,'SCPCOA') && isfield(meta.SCPCOA,'AzimAng')
        Az = meta.SCPCOA.AzimAng;
        return;
    end
    percent = 50; % If SCPCOA.AzimAng not available, we will compute it.
end
if ~isfield(meta.Grid,'TimeCOAPoly') % TimeCOAPoly field is required
    % For spotlight, we can take some shortcuts with missing metadata
    if isfield(meta,'CollectionInfo') &&...
            isfield(meta.CollectionInfo,'RadarMode')&&...
            isfield(meta.CollectionInfo.RadarMode,'ModeType')&&...
            strcmpi(meta.CollectionInfo.RadarMode.ModeType,'SPOTLIGHT')
        if isfield(meta,'SCPCOA') && isfield(meta.SCPCOA,'SCPTime') &&...
                isfield(meta,'Position') && isfield(meta.Position,'ARPPoly')
            meta.Grid.TimeCOAPoly = meta.SCPCOA.SCPTime;
        elseif isfield(meta,'SCPCOA') && isfield(meta.SCPCOA,'ARPPos') &&...
                isfield(meta.SCPCOA,'ARPVel')
            meta.Grid.TimeCOAPoly = 0;
            meta.Position.ARPPoly.X = [meta.SCPCOA.ARPPos.X; meta.SCPCOA.ARPVel.X];
            meta.Position.ARPPoly.Y = [meta.SCPCOA.ARPPos.Y; meta.SCPCOA.ARPVel.Y];
            meta.Position.ARPPoly.Z = [meta.SCPCOA.ARPPos.Z; meta.SCPCOA.ARPVel.Z];
        end
    end
    if ~isfield(meta.Grid,'TimeCOAPoly')
        error('COMPUTEAZ:TIMECOAPOLY_MISSING','Unable to compute SICD Grid.TimeCOAPoly field from complex data.');
    end
end
% Point of interest (POI) only required for spatially variant COA
if exist('pixel_coords','var') && ~isempty(pixel_coords)
    % Convention for point_slant_to_ground() pixel coordinates is reverse
    % of what is passed to this function (column/row), so we have to swap
    % values.
    poi_ecf = geodetic_to_ecf(point_slant_to_ground([pixel_coords(2); pixel_coords(1)], meta)).';
    % Calculate center of aperture (COA) for given point
    time_coa = sicd_polyval2d(meta.Grid.TimeCOAPoly,pixel_coords(1),pixel_coords(2),meta);
else % Default to SCP
    poi_ecf = [meta.GeoData.SCP.ECF.X, meta.GeoData.SCP.ECF.Y, meta.GeoData.SCP.ECF.Z];
    time_coa = meta.Grid.TimeCOAPoly(1); % SCP COA time is this by definition
    if any(meta.Grid.TimeCOAPoly(2:end)~=0) % Spatially variant COA
        warning('COMPUTEAZ:PIXEL_COORDS_REQUIRED','For non-spotlight data, point of interest must be specified for accurate azimuth angles.');
    end
end

%% Calculate geometry info for center of aperture
pos_coefs=[meta.Position.ARPPoly.X(end:-1:1)...
    meta.Position.ARPPoly.Y(end:-1:1)...
    meta.Position.ARPPoly.Z(end:-1:1)]; % Position polynomial
ARP=[polyval(pos_coefs(:,1),time_coa)...
    polyval(pos_coefs(:,2),time_coa)...
    polyval(pos_coefs(:,3),time_coa)]; % Aperture position at COA
% Velocity polynomial is derivative of position polynomial
vel_coefs=pos_coefs(1:end-1,:).*repmat(((size(pos_coefs,1)-1):-1:1)',[1 3]);
ARV=[polyval(vel_coefs(:,1), time_coa)...
    polyval(vel_coefs(:,2), time_coa)...
    polyval(vel_coefs(:,3), time_coa)]; % Aperture velocity at COA
LOS = poi_ecf - ARP; % Line of site vector at COA
R = norm(LOS); % Range from sensor to POI at COA
V = norm(ARV); % Speed at COA
DCA =  acos((ARV/V)*(LOS.'/R)); % Doppler Cone Angle to POI at COA

%% Compute effective aperture positions for each percentage
Theta = meta.Grid.Col.ImpRespBW/meta.Grid.Row.KCtr; % Angular aperture spanned by this complex image
EffectiveDuration = (Theta*R/V)/sin(DCA); % Theta is expressed as a ratio, rather than an angle
% For the purposes of this computation, we assume a linear flight path
% centered around the COA ARP and ARV, and that the center of the deskewed
% spatial frequency in azimuth is the azimuth angle at COA.
t = EffectiveDuration * ((-1/2) + (percent(:) / 100));
pos = [ARP(1) + (t * ARV(1)), ...
    ARP(2) + (t * ARV(2)), ...
    ARP(3) + (t * ARV(3))];
% We could also compute position given the position given the full position
% polynomial.  However, given that the PERCENT parameter is actually a
% fraction of the spatial frequency space and not time, it is not clear
% that this will be any more accurate.
% t = time_coa + (EffectiveDuration * ((-1/2) + (percent(:) / 100)));
% pos=[polyval(pos_coefs(:,1),t)...
%     polyval(pos_coefs(:,2),t)...
%     polyval(pos_coefs(:,3),t)];

%% Calculate actual azimuth angles
gpn = wgs_84_norm(poi_ecf); % Ground plane normal, based on wgs_84 ellipsoid
range = pos-repmat(poi_ecf,[size(t,1),1]); % Range vectors for each time
north_ground = [ 0 0 1 ]-([ 0 0 1 ]*gpn)*gpn.'; % Project north onto ground plane
range_ground=range-(range*gpn)*gpn.'; % Project range vector onto ground plane
Az = atan2( cross( range_ground, repmat(north_ground,[size(t,1),1]), 2 ) * gpn, ...
    range_ground * north_ground.')*180/pi; % Angle between the two projected vectors
Az(Az < 0) = Az(Az < 0) + 360; % Assure in [0,360], not [-180,180]
Az = reshape(Az,size(percent)); % Reform to shape of original input

end

% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////


function [simple_rules,skipped] = simplifyGrRules(grRules,expanded)
%simplifyGrRules  Simplify and condense the logic of model grRules.
%
% simplifyGrRules uses Matlab's symbolic toolbox to mathematically simplify
% gene rules. Each rule is converted into a symbolic boolean expression,
% and these expressions are simplified yielding rules that are as reduced/
% simplified as possible, while still maintaining functional equivalency to
% the original grRule logic.
%
% The function will also remove unnecessary parentheses, trailing ANDs/ORs,
% and duplicate genes in model grRules. The function requires that the gene
% names/IDs do not contain spaces, parentheses, "&", "|", or "#".
% Furthermore, grRules must separate gene names/IDs from ANDs and ORs by at
% least one space. For example,
%
%       OK: "GENE1 and GENE2", or "GENE1 & GENE2"
%   NOT OK: "GENE1andGENE2", or "GENE1&GENE2"
%
%
% *!WARNING!* Simplifying grRules with this function may eliminate some
%             gene-reaction associations.
%             For example: 'G1 or (G1 and G2)' simplifies to 'G1'
%
%
% USAGE:
%
%   [simple_rules,skipped] = simplifyGrRules(grRules,expanded);
%
%
% INPUT:
%
%   grRules        The grRules field from a genome-scale metabolic model.
%                  Note that grRules can use written ("and" "or") or
%                  symbolic ("&" "|") boolean operators.
%
%   expanded       (Optional, default FALSE). If TRUE, the grRule will be
%                  returned in its expanded form, where groups of ANDs
%                  (enzyme complexes) are separated by ORs, rather than
%                  using a shorter nested format.
%
%                  For example:
%
%                   input: G1 and G2 and (G3 or G4) and (G4 or G3 or G3) and G1
%
%              simplified: G1 and G2 and (G3 or G4)
%
%       simplify + expand: (G1 and G2 and G3) or (G1 and G2 and G4)
%
%                  NOTE: Expanding rules can make them very long, and in
%                        some cases will exceed Matlab's limit on the
%                        length of a symbolic expression, requiring them to
%                        be skipped.
%
%
% OUTPUT:
%
%   simple_rules   Updated/simplified grRules.
%
%   skipped        Logical vector indicating which grRules were skipped
%                  because they were too long to be converted into a
%                  symbolic equation. For these cases, the grRule will be
%                  copied into simple_rules without any simplification.
%

if nargin < 2
    expanded = false;
end

% perform a preliminary "clean" of the gene rules
grRules = cleanGrRules(grRules);

% check if the grRules use written or symbolic boolean operators
if any(contains(grRules,{'&','|'}))
    Btype = 'symbol';  % record rule type, to revert back at the end
else
    % convert "and" to "&" and "or" to "|"
    grRules = regexprep(grRules, ' or ', ' | ');
    grRules = regexprep(grRules, ' and ', ' & ');
    Btype = 'text';  % record rule type, to revert back at the end
end

% get list of all gene IDs present in grRules
rxnGenes = cellfun(@(r) unique(regexp(r,'[^&|\(\) ]+','match')),grRules,'UniformOutput',false);
nonEmpty = ~cellfun(@isempty,rxnGenes);
allGenes = unique([rxnGenes{nonEmpty}]');

% Replace all gene IDs with G1, G2, G3, etc. to avoid potential problems
% with certain gene ID formats (e.g., Entrez IDs are numbers, which will
% cause problems).
tempIDs = strcat('G',arrayfun(@num2str,(1:length(allGenes))','UniformOutput',false));  % construct new ID vector
convertGene = @(g) tempIDs{ismember(allGenes,g)};  % define conversion function
grRules = regexprep(grRules, '[^&|\(\) ]+', '${convertGene($0)}');  % find and convert all gene IDs to new temporary type

% identify all rules containing both ANDs and ORs
AndOrInd = find(contains(grRules,'&') & contains(grRules,'|'));

% initialize output specifying which rules were skipped due to complexity
skipped = false(size(grRules));

% iterate through complex rules (those containing both ANDs and ORs)
for i = 1:length(AndOrInd)

    r = grRules{AndOrInd(i)};

    % attempt to convert string to symbolic equation
    try
        reqn = str2sym(r);
    catch
        fprintf('grRule #%u was too long (%u characters), and therefore skipped.\n',AndOrInd(i),numel(r));
        skipped(AndOrInd(i)) = true;
        continue
    end

    % simplify eqn, allowing for a max of 10 steps
    reqn = simplify(reqn,10);

    % attempt to expand equation if requested
    reqn_noexpand = reqn;
    if ( expanded )
        try
            reqn = expand(reqn);
        catch
            fprintf('Failed to expand grRule #%u due to excess length. Rule will remain in simplified form.\n',AndOrInd(i));
        end
    end

    % Add parentheses around groups of genes. This may add some
    % unnecessary parentheses, but they will be cleaned up later.
    if is_or(reqn) || is_and(reqn)
        % the input and output of this function are of the type "char"
        try
            % sometimes an expanded equation will fail here
            r = add_parentheses(char(reqn));
        catch
            fprintf('Failed to expand grRule #%u due to excess length. Rule will remain in simplified form.\n',AndOrInd(i));
            reqn = reqn_noexpand;
            r = add_parentheses(char(reqn));
        end
        r = regexprep(r,'^\(|\)$','');  % remove the extra set of parentheses that always enclose the expression
    end

    % update rule in grRule vector
    grRules{AndOrInd(i)} = r;

end

% restore original gene IDs
restoreGene = @(g) allGenes{ismember(tempIDs,g)};  % define conversion function
grRules = regexprep(grRules, '[^&|\(\) ]+', '${restoreGene($0)}');  % find and restore all gene IDs to original ID

% change boolean operators back to original type
if strcmpi(Btype,'text')
    grRules = regexprep(grRules, ' \| ', ' or ');
    grRules = regexprep(grRules, ' & ', ' and ');
end

% assign output
simple_rules = grRules;

end

%% Add parentheses around groups of equation terms
function eqn_str_new = add_parentheses(eqn_str)
% This is a recursive function, which groups terms in an equation with
% parentheses in order to clearly indicate the order of operations. This is
% necessary to avoid ambiguity when the equations are converted back into
% gene rules.
%
% NOTE: both eqn_str_new and eqn_str are strings (char).

% obtain symbolic form of equation
eqn = str2sym(eqn_str);

% separate equation into terms/pieces ("children")
eqn_pieces = arrayfun(@char,children(eqn),'UniformOutput',false);

% find pieces that are further separable, and recursively separate until
% all pieces are single genes
ind = find(contains(eqn_pieces,{'|','&'}));
for i = 1:length(ind)
    eqn_pieces{ind(i)} = add_parentheses(eqn_pieces{ind(i)});
end

% re-assemble equation pieces, adding | or &, depending on the input eqn
if is_or(eqn)
    eqn_str_new = char(strcat('(',join(eqn_pieces,' | '),')'));
elseif is_and(eqn)
    eqn_str_new = char(strcat('(',join(eqn_pieces,' & '),')'));
else
    % if the input eqn was simply one gene, just return the input
    eqn_str_new = eqn_str;
end

end

%% Functions to test whether an equation is a collection of ORs or ANDs
function res = is_or(eqn)
% check if outermost operations contain ORs
res = length(regexp(char(eqn),'\|')) > length(regexp(char(children(eqn)),'\|'));
end

function res = is_and(eqn)
% check if outermost operations contain ANDs
res = length(regexp(char(eqn),'&')) > length(regexp(char(children(eqn)),'&'));
end