function [S, Serr, per, tau, exctn, lambda] = armode(A, C, th)
%ARMODE	Eigendecomposition of AR model.
%
%  [S,Serr,per,tau,exctn]=ARMODE(A,C,th) computes the
%  eigendecomposition of an AR(p) model that has been fitted using
%  ARFIT. The input arguments of ARMODE are output of ARFIT.
%
%  The columns of the output matrix S contain the estimated eigenmodes
%  of the AR model. The output matrix Serr contains margins of error
%  for the components of the estimated eigenmodes S, such that
%  (S +/- Serr) are approximate 95% confidence intervals for the
%  individual components of the eigenmodes.
%
%  The two-row matrices per and tau contain in their first rows the
%  estimated oscillation period per(1,k) and the estimated damping
%  time tau(1,k) of the eigenmode S(:,k). In their second rows, the
%  matrices per and tau contain margins of error for the periods and
%  damping times, such that
%     ( per(1,k) +/- per(2,k) )   and   ( tau(1,k) +/- tau(2,k) )
%  are approximate 95% confidence intervals for the period and damping
%  time of eigenmode S(:,k).
%
%  For a purely relaxatory eigenmode, the period is infinite (Inf).
%  For an oscillatory eigenmode, the periods are finite.
%
%  The excitation of an eigenmode measures its dynamical importance
%  and is returned as a fraction exctn that is normalized such that
%  the sum of the excitations of all eigenmodes equals one.
%
%  See also ARFIT, ARCONF.

%  Modified 13-Oct-00
%  Author: Tapio Schneider
%	   tapio@gps.caltech.edu

  ccoeff   = .95;                       % confidence coefficient
  m 	   = size(C,1);			% dimension of state space
  p 	   = size(A,2) / m; 		% order of model
  if p <= 0
    error('Order must be greater 0.');
  end

  % Assemble coefficient matrix of equivalent AR(1) model
  A1 	   = [A; eye((p-1)*m) zeros((p-1)*m,m)];

  % Eigenvalues and eigenvectors of coefficient matrix of equivalent
  % AR(1) model
  [BigS,d] = eig(A1);  			% columns of BigS are eigenvectors
  lambda   = diag(d);    		% vector containing eigenvalues
  lambda   = lambda(:); 	        % force lambda to be column vector

  % Warning if the estimated model is unstable
  if any(abs(lambda) > 1)
    warning(sprintf(['The estimated AR model is unstable.\n',...
		     '\t Some excitations may be negative.']))
  end

  % Fix phase of eigenvectors such that the real part and the
  % imaginary part of each vector are orthogonal
  BigS     = adjph(BigS);

  % Return only last m components of each eigenvector
  S 	   = BigS((p-1)*m+1:p*m, :);

  % Compute inverse of BigS for later use
  BigS_inv = inv(BigS);

  % Recover the matrix Uinv that appears in the asymptotic covariance
  % matrix of the least squares estimator (Uinv is output of AR)
  if (size(th,2) == m*p+1)
    % The intercept vector has been fitted by AR; in computing
    % confidence intervals for the eigenmodes, this vector is
    % irrelevant. The first row and first column in Uinv,
    % corresponding to elements of the intercept vector, are not
    % needed.
    Uinv   = th(3:size(th,1), 2:size(th,2));

  elseif (size(th,2) == m*p)
    %  No intercept vector has been fitted
    Uinv   = th(2:size(th,1), :);
  else
    error('Input arguments of ARMODE must be output of ARFIT.')
  end
  % Number of degrees of freedom
  dof 	 = th(1,1);
  % Quantile of t distribution for given confidence coefficient and dof
  t      = tquant(dof, .5+ccoeff/2);

  % Asymptotic covariance matrix of estimator of coefficient matrix A
  Sigma_A  = kron(Uinv, C);

  % Noise covariance matrix of system of relaxators and oscillators
  CovDcpld = BigS_inv(:, 1:m) * C * BigS_inv(:, 1:m)';

  % For each eigenmode j: compute the period per, the damping time
  % tau, and the excitation exctn; also get the margins of error for
  % per and tau
  for j=1:m*p				% eigenmode number
    a		= real(lambda(j)); 	% real part of eigenvalue j
    b 		= imag(lambda(j)); 	% imaginary part of eigenvalue j
    abs_lambda_sq= abs(lambda(j))^2;  	% squared absolute value of eigenvalue j
    tau(1,j) 	= -2/log(abs_lambda_sq);% damping time of eigenmode j

    % Excitation of eigenmode j
    exctn(j) 	= real(CovDcpld(j,j) / (1-abs_lambda_sq));

    % Assemble derivative of eigenvalue with respect to parameters in
    % the coefficient matrix A
    dot_lam 	= zeros(m^2*p, 1);
    for k=1:m
      dot_lam(k:m:k+(m*p-1)*m) = BigS_inv(j,k) .* BigS(1:m*p,j);
    end
    dot_a 	= real(dot_lam); 	% derivative of real part of lambda(j)
    dot_b 	= imag(dot_lam); 	% derivative of imag part of lambda(j)

    % Derivative of the damping time tau w.r.t. parameters in A
    phi 	= tau(1,j)^2 / abs_lambda_sq * (a*dot_a + b*dot_b);
    % Margin of error for damping time tau
    tau(2,j) 	= t * sqrt(phi'*Sigma_A*phi);

    % Period of eigenmode j and margin of error for period. (The
    % if-statements avoid warning messages that may otherwise result
    % from a division by zero)
    if (b == 0 & a >= 0)      % purely real, nonnegative eigenvalue
      per(1,j)	= Inf;
      per(2,j)  = 0;
    elseif (b == 0 & a < 0)   % purely real, negative eigenvalue
      per(1,j)	= 2;
      per(2,j)  = 0;
    else                      % complex eigenvalue
      per(1,j)	= 2*pi/abs(atan2(b,a));

      % Derivative of period with respect to parameters in A
      phi 	= per(1,j)^2 / (2*pi*abs_lambda_sq)*(b*dot_a-a*dot_b);
      % Margin of error for period
      per(2,j) 	= t * sqrt(phi'*Sigma_A*phi);
    end
  end

  % Give the excitation as `relative importance' that sums to one
  exctn 	= exctn/sum(exctn);

  % Compute confidence intervals for eigenmodes
  % -------------------------------------------
  % Shorthands for matrix products
  XX 	        = real(BigS)'*real(BigS);
  YY 	        = imag(BigS)'*imag(BigS);
  XY 	        = real(BigS)'*imag(BigS);

  % Need confidence intervals only for last m rows of BigS
  row1 	        = (p-1)*m+1; % first row for which confidence interval is needed

  mp = m*p;                           	% dimension of equivalent AR(1) model
  for k=1:mp        			% loop over columns of S
    for l=row1:mp  			% loop over rows of S

      % evaluate gradient of S_{lk}
      for ii=1:m
	for jj=1:mp
	  % compute derivative with respect to A(ii,jj)
	  zsum = 0;
	  zkkr = 0; 			% real part of Z_{kk}
	  zkki = 0; 			% imaginary part of Z_{kk}
	  for j=1:mp
	    if (j ~= k) 		% sum up elements that appear in Z_{kk}
	      zjk  = BigS_inv(j,ii)*BigS(jj,k)/(lambda(k)-lambda(j));
	      zjkr = real(zjk);
	      zjki = imag(zjk);
	      zkkr = zkkr+zjki*(XY(k,j)-XY(j,k))-zjkr*(XX(k,j)+YY(k,j));
	      zkki = zkki+zjki*(YY(k,j)-XX(k,j))-zjkr*(XY(k,j)+XY(j,k));
	      zsum = zsum+BigS(l,j)*zjk;
	    end
	  end
	  % now add Z_{kk}
	  zkki = zkki / (XX(k,k)-YY(k,k));
	  grad_S((jj-1)*m+ii) = zsum+BigS(l,k)*(zkkr+i*zkki);
	end
      end
      Serr(l,k) = t * ( sqrt( real(grad_S)*Sigma_A*real(grad_S)') ...
			 + i*sqrt( imag(grad_S)*Sigma_A*imag(grad_S)') );
    end
  end

  % Only return last m*p rows of Serr
  Serr = Serr(row1:m*p, :);


function a = r8cbb_add ( n1, n2, ml, mu, a, i, j, value )

%*****************************************************************************80
%
%% R8CBB_ADD adds a value to an entry of a R8CBB matrix.
%
%  Discussion:
%
%    The R8CBB storage format is for a compressed border banded matrix.
%    Such a matrix has the logical form:
%
%      A1 | A2
%      ---+---
%      A3 | A4
%
%    with A1 a (usually large) N1 by N1 banded matrix, while A2, A3 and A4
%    are dense rectangular matrices of orders N1 by N2, N2 by N1, and N2 by N2,
%    respectively.
%
%    The R8CBB format is the same as the DBB format, except that the banded
%    matrix A1 is stored in compressed band form rather than standard
%    banded form.  In other words, we do not include the extra room
%    set aside for fill in during pivoting.
%
%    A should be defined as a vector.  The user must then store
%    the entries of the four blocks of the matrix into the vector A.
%    Each block is stored by columns.
%
%    A1, the banded portion of the matrix, is stored in
%    the first (ML+MU+1)*N1 entries of A, using the obvious variant
%    of the LINPACK general band format.
%
%    The following formulas should be used to determine how to store
%    the entry corresponding to row I and column J in the original matrix:
%
%    Entries of A1:
%
%      1 <= I <= N1, 1 <= J <= N1, (J-I) <= MU and (I-J) <= ML.
%
%      Store the I, J entry into location
%      (I-J+MU+1)+(J-1)*(ML+MU+1).
%
%    Entries of A2:
%
%      1 <= I <= N1, N1+1 <= J <= N1+N2.
%
%      Store the I, J entry into location
%      (ML+MU+1)*N1+(J-N1-1)*N1+I.
%
%    Entries of A3:
%
%      N1+1 <= I <= N1+N2, 1 <= J <= n1.
%
%      Store the I, J entry into location
%      (ML+MU+1)*N1+N1*N2+(J-1)*N2+(I-N1).
%
%    Entries of A4:
%
%      N1+1 <= I <= N1+N2, N1+1 <= J <= N1+N2
%
%      Store the I, J entry into location
%      (ML+MU+1)*N1+N1*N2+(J-1)*N2+(I-N1).
%      (same formula used for A3).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N1, N2, the order of the banded and dense blocks.
%    N1 and N2 must be nonnegative, and at least one must be positive.
%
%    Input, integer ML, MU, the lower and upper bandwidths.
%    ML and MU must be nonnegative, and no greater than N1-1.
%
%    Input, real A((ML+MU+1)*N1 + 2*N1*N2 + N2*N2), the R8CBB matrix.
%
%    Input, integer I, J, the indices of the entry to be incremented.
%
%    Input, real VALUE, the value to be added to the (I,J) entry.
%
%    Output, real A((ML+MU+1)*N1 + 2*N1*N2 + N2*N2), the modified R8CBB matrix.
%
  if ( value == 0.0 )
    return;
  end
%
%   Check for I or J out of bounds.
%
  if ( i <= 0 .or. n1+n2 < i )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8CBB_ADD - Fatal error!\n' );
    fprintf ( 1, '  Illegal input value of row index I = %d/n', i );
    error ( 'R8CBB_ADD - Fatal error!' );
  end

  if ( j <= 0 | n1+n2 < j )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8CBB_ADD - Fatal error!\n' );
    fprintf ( 1, '  Illegal input value of column index J = %d/n',j );
    error ( 'R8CBB_ADD - Fatal error!' );
  end
%
%  The A1 block of the matrix.
%
%  Check for out of band problems.
%
  if ( i <= n1 & j <= n1 )
    if ( mu < (j-i) | ml < (i-j) )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'R8CBB_ADD - Warning!\n' );
      fprintf ( 1, '  Unable to add to entry (%d,%d).\n', i, j );
      return
    else
      ij = (i-j+mu+1)+(j-1)*(ml+mu+1);
    end
%
%  The A2 block of the matrix:
%
  elseif ( i <= n1 & n1 < j )
    ij = (ml+mu+1)*n1+(j-n1-1)*n1 + i;
%
%  The A3 and A4 blocks of the matrix.
%
  elseif ( n1 < i )
    ij = (ml+mu+1)*n1+n2*n1+(j-1)*n2 + (i-n1);
  end

  a(ij) = a(ij) + value;

  return
end


function [X,P] = femUnk(fe)
%+========================================================================+
%|                                                                        |
%|              OPENFEM - LIBRARY FOR FINITE ELEMENT METHOD               |
%|           openFem is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal & Francois Alouges (c) 2017-2018.          |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%|             francois.alouges@polytechnique.edu                         |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : femUnk.m                                      |
%|    #    |   VERSION    : 0.40                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal & François Alouges            |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 14.03.2018                                    |
%| ( === ) |   SYNOPSIS   : Unknowns and reduction matrix for constrained |
%|  `---'  |                finite elements                               |
%+========================================================================+

% Initialization with dofs
X  = fe.dof;
Nx = size(X,1);
P  = speye(Nx);

% Dirichlet
if ~isempty(fe.dir)
    % Finite element for dirichlet mesh
    fed = fem(fe.dir,fe.typ);

    % Dof indices
    I = find(~ismember(X,fed.dof,'rows'));

    % Reduction matrix
    M = speye(Nx);
    M = M(:,I);

    % Update
    P  = P * M;
    X  = X(I,:);
    Nx = length(I);
end

% Junction
if ~isempty(fe.jct)
    % Number of junction
    Njct = length(fe.jct)/2;

    % Initialization
    feJct = cell(1,Njct);
    Ijct  = cell(1,Njct);

    % Finite element for junction meshes
    for i = 1:Njct
        if isa(fe.jct{2*i-1},'msh') && isnumeric(fe.jct{2*i})
            feJct{i} = fem(fe.jct{2*i-1},fe.typ);
        else
            error('femUnknown.m : unavailable case');
        end
    end

    % Valid dof indices for junction meshes
    for i = 1:Njct
        Xi         = feJct{i}.dof;
        Ijct{i}    = zeros(size(Xi,1),1);
        [~,I,J]    = intersect(Xi,X,'rows','stable');
        Ijct{i}(I) = J;
    end
    Ijct = cell2mat(Ijct);

    % Extract dirichlet condition and multiple indices
    bool = (Ijct(:,Njct) == 0);
    for i = 1:Njct-1
        bool = bool + (Ijct(:,Njct) == Ijct(:,i));
    end
    Ijct = Ijct(~bool,:);

    % Final dof after extraction of the last indices
    I = setdiff((1:Nx)',Ijct(:,end));

    % Linear relation matrix
    M = speye(Nx);
    for  i = 1:Njct
        for j = setdiff(1:Njct,i)
            M = M + sparse(Ijct(:,i),Ijct(:,j),-fe.jct{2*j}/fe.jct{2*i},Nx,Nx);
        end
    end

    % Reduction
    M = M(:,I);

    % Update
    P  = P * M;
    X  = X(I,:);
%     Nx = length(I);
end
end


function M = elliptopefactory(n, k)
% Manifold of n-by-n psd matrices of rank k with unit diagonal elements.
%
% function M = elliptopefactory(n, k)
%
% A point X on the manifold is parameterized as YY^T where Y is a matrix of
% size nxk. As such, X is symmetric, positive semidefinite. We restrict to
% full-rank Y's, such that X has rank exactly k. The point X is numerically
% represented by Y (this is more efficient than working with X, which may
% be big). Tangent vectors are represented as matrices of the same size as
% Y, call them Ydot, so that Xdot = Y Ydot' + Ydot Y and diag(Xdot) == 0.
% The metric is the canonical Euclidean metric on Y.
%
% The diagonal constraints on X (X(i, i) == 1 for all i) translate to
% unit-norm constraints on the rows of Y: norm(Y(i, :)) == 1 for all i.
% The set of such Y's forms the oblique manifold. But because for any
% orthogonal Q of size k it holds that (YQ)(YQ)' = YY', we "group" all
% matrices of the form YQ in an equivalence class. The set of equivalence
% classes is a Riemannian quotient manifold, implemented here.
%
% Note that this geometry formally breaks down at rank-deficient Y's.
% This does not appear to be a major issue in practice when optimization
% algorithms converge to rank-deficient Y's, but convergence theorems no
% longer hold. As an alternative, you may use the oblique manifold (it has
% larger dimension, but does not break down at rank drop.)
%
% The geometry is taken from the 2010 paper:
% M. Journee, P.-A. Absil, F. Bach and R. Sepulchre,
% "Low-Rank Optimization on the Cone of Positive Semidefinite Matrices".
% Paper link: http://www.di.ens.fr/~fbach/journee2010_sdp.pdf
%
%
% Please cite the Manopt paper as well as the research paper:
%     @Article{journee2010low,
%       Title   = {Low-rank optimization on the cone of positive semidefinite matrices},
%       Author  = {Journ{\'e}e, M. and Bach, F. and Absil, P.-A. and Sepulchre, R.},
%       Journal = {SIAM Journal on Optimization},
%       Year    = {2010},
%       Number  = {5},
%       Pages   = {2327--2351},
%       Volume  = {20},
%       Doi     = {10.1137/080731359}
%     }
%
%
% See also: obliquefactory symfixedrankYYfactory spectrahedronfactory

% This file is part of Manopt: www.manopt.org.
% Original author: Bamdev Mishra, July 12, 2013.
% Contributors:
% Change log:
%   July 18, 2013 (NB):
%       Fixed projection operator for rank-deficient Y'Y.
%
%   Aug.  8, 2013 (NB):
%       No longer using nested functions, to aim at Octave compatibility.
%       Sign error in right hand side of the call to minres corrected.
%
%   June 24, 2014 (NB):
%       Used code snippets from obliquefactory to speed up projection,
%       retraction, egrad2rgrad and rand: the code now uses bsxfun for this.
%
%   Apr. 3, 2015 (NB):
%       Replaced trace(A'*B) by A(:)'*B(:) : equivalent but faster.
%
%   Apr. 17, 2018 (NB):
%       Removed dependency on lyap entirely.
%
%   Sep.  6, 2018 (NB):
%       Removed M.exp() as it was not implemented.

% TODO: modify normalize_rows and project_rows to work without transposes.
% TODO: enhance ehess2rhess to also use bsxfun.

    if k < 2
        warning('manopt:elliptopefactory:lowk', ...
                'k should be an integer >= 2. At k = 1, the set is discrete.');
    end

    M.name = @() sprintf('YY'' quotient manifold of %dx%d psd matrices of rank %d with diagonal elements being 1', n, k);

    M.dim = @() n*(k-1) - k*(k-1)/2; % Extra -1 is because of the diagonal constraint that

    % Euclidean metric on the total space
    M.inner = @(Y, eta, zeta) eta(:)'*zeta(:);

    M.norm = @(Y, eta) sqrt(M.inner(Y, eta, eta));

    M.dist = @(Y, Z) error('elliptopefactory.dist not implemented yet.');

    M.typicaldist = @() 10*k;

    M.proj = @projection;

    M.tangent = M.proj;
    M.tangent2ambient = @(Y, eta) eta;

    M.retr = @retraction;

    M.egrad2rgrad = @egrad2rgrad;

    M.ehess2rhess = @ehess2rhess;

    % Notice that the hash of two equivalent points will be different...
    M.hash = @(Y) ['z' hashmd5(Y(:))];

    M.rand = @() random(n, k);

    M.randvec = @randomvec;

    M.lincomb = @matrixlincomb;

    M.zerovec = @(Y) zeros(n, k);

    M.transp = @(Y1, Y2, d) projection(Y2, d);

    M.vec = @(Y, u_mat) u_mat(:);
    M.mat = @(Y, u_vec) reshape(u_vec, [n, k]);
    M.vecmatareisometries = @() true;

end

% Given a matrix X, returns the same matrix but with each column scaled so
% that they have unit 2-norm.
% See obliquefactory.
function X = normalize_rows(X)
    X = X';
    norms = sqrt(sum(X.^2, 1));
    X = bsxfun(@times, X, 1./norms);
    X = X';
end

% Orthogonal projection of each row of H to the tangent space at the
% corresponding row of X, seen as a point on a sphere.
% See obliquefactory.
function PXH = project_rows(X, H)
    X = X';
    H = H';
    % Compute the inner product between each vector H(:, i) with its root
    % point X(:, i), that is, X(:, i).' * H(:, i). Returns a row vector.
    inners = sum(X.*H, 1);
    % Subtract from H the components of the H(:, i)'s that are parallel to
    % the root points X(:, i).
    PXH = H - bsxfun(@times, X, inners);
    PXH = PXH';
end

% Projection onto the tangent space, i.e., on the tangent space of
% ||Y(i, :)|| = 1
function etaproj = projection(Y, eta)
    eta = project_rows(Y, eta);

    % Projection onto the horizontal space
    YtY = Y'*Y;
    SS = YtY;
    AS = Y'*eta - eta'*Y;
    Omega = lyapunov_symmetric(SS, AS);

    % It does not seem necessary to enforce skew-symmetry numerically.
    % Omega = (Omega-Omega')/2;

    etaproj = eta - Y*Omega;
end

% Retraction
function Ynew = retraction(Y, eta, t)
    if nargin < 3
        t = 1.0;
    end
    Ynew = Y + t*eta;
    Ynew = normalize_rows(Ynew);
end

% Euclidean gradient to Riemannian gradient conversion.
% We only need the ambient space projection: the remainder of the
% projection function is not necessary because the Euclidean gradient must
% already be orthogonal to the vertical space.
function rgrad = egrad2rgrad(Y, egrad)
    rgrad = project_rows(Y, egrad);
end

% Euclidean Hessian to Riemannian Hessian conversion.
% TODO: speed this function up using bsxfun.
function Hess = ehess2rhess(Y, egrad, ehess, eta)
    k = size(Y, 2);

    % Directional derivative of the Riemannian gradient
    scaling_grad = sum((egrad.*Y), 2); % column vector of size n
    scaling_grad_repeat = scaling_grad*ones(1, k);

    Hess = ehess - scaling_grad_repeat.*eta;

    scaling_hess = sum((eta.*egrad) + (Y.*ehess), 2);
    scaling_hess_repeat = scaling_hess*ones(1, k);
    % directional derivative of scaling_grad_repeat
    Hess = Hess - scaling_hess_repeat.*Y;

    % Project on the horizontal space
    Hess = projection(Y, Hess);
end

% Random point generation on the manifold
function Y = random(n, k)
    Y = randn(n, k);
    Y = normalize_rows(Y);
end

% Random vector generation at Y
function eta = randomvec(Y)
    eta = randn(size(Y));
    eta = projection(Y, eta);
    nrm = norm(eta, 'fro');
    eta = eta / nrm;
end


function varargout = linop_test( op, cmode, maxits )

%LINOP_TEST Performs an adjoint test on a linear operator.
%    LINOP_TEST( OP ) attempts to verify that a linear operator OP obeys
%    the inner product test: <A*x,y> = <x,A'*y> for all x, y. OP must be a
%    TFOCS linear operator with hard-coded size information; that is,
%    OP([],0) must return valid size info.
%
%    When called with a single argument, LINOP_TEST creates real test
%    vectors for X and Y. To test complex operators, use the two-argument
%    version LINOP_TEST( OP, cmode ), where:
%        cmode = 'R2R': real input, real output
%        cmode = 'R2C': real input, complex output
%        cmode = 'R2CC': real input, conjugate-symmetric complex output
%        cmode = 'C2R': complex input, real output
%        cmode = 'CC2R': conjugate-symmetric complex input, real output
%        cmode = 'C2C': complex input, complex output
%
%    The conjugate-symmetric options follow the symmetry conventions of Matlab's FFT.
%
%    LINOP_TEST( OP, CMODE, MAXITS ) performs MAXITS iterations of the
%    test loop. MAXITS=25 is the default.
%
%   myNorm = LINOP_TEST(...) returns an estimate of the myNorm of
%       the linear operator.

error(nargchk(1,3,nargin));
if isnumeric( op ),
    if nargin < 2 || isempty( cmode )
        disp('warning: for matrix inputs, this assumes the cmode is ''C2C'' unless you specify otherwise');
        cmode = 'C2C';
    end
    op = linop_matrix( op, cmode );
end
y_conjSymmetric     = false;
x_conjSymmetric     = false;
if nargin < 2 || isempty( cmode ),
    x_real = true;
    y_real = true;
else
    switch upper( cmode ),
        case 'R2R', x_real = true; y_real = true;
        case 'R2C', x_real = true; y_real = false;
        case 'R2CC', x_real = true; y_real = false;  y_conjSymmetric    = true;
        case 'C2R', x_real = false; y_real = true;
        case 'CC2R', x_real = false; y_real = true;  x_conjSymmetric    = true;
        case 'C2C', x_real = false; y_real = false;
        case 'CC2CC', x_real = false; y_real = false;
            x_conjSymmetric    = true;
            y_conjSymmetric    = true; % this is probably never going to happen though
        otherwise, error( 'Invalid cmode: %s', cmode );
    end
end
if nargin < 3 || isempty(maxits),
    maxits = 25;
end
sz = op([],0);
if ~iscell(sz)
    sz = { [sz(2),1], [sz(1),1] };
elseif isempty(sz{1})
    disp('warning: could not detect the size; this often happens when using a scalar input, which represents scaling');
    if isempty( sz{2} )
        disp('  Proceeding under the assumption that the domain is 1D');
        sz{1} = [1,1];
        sz{2} = [1,1];
    else
        disp('  Proceeding under the assumption that the domain equals the range');
        sz{1}   = sz{2};
    end
elseif isempty(sz{2})
    disp('warning: could not detect the size; this often happens when using a scalar input, which represents scaling');
    disp('  Proceeding under the assumption that the domain equals the range');
    sz{2}   = sz{1};
end
nf = 0;
na = 0;
errs = zeros(1,maxits+1);
nxe = 0; nye = 0;
for k = 1 : maxits,

    %
    % The adjoint test
    %

    if x_real,
        x = randn(sz{1});
    else
        x = randn(sz{1})+1j*randn(sz{1});
        if x_conjSymmetric
            x = make_conj_symmetrix( x );
        end
    end

    if y_real,
        y = randn(sz{2});
    else
        y = randn(sz{2})+1j*randn(sz{2});
        if y_conjSymmetric
            y = make_conj_symmetrix( y );
        end
    end

    nx = myNorm(x);
    Ax = op(x,1);
    nf = max( nf, myNorm(Ax)/nx );
    Ax_y = tfocs_dot( Ax, y );

    ny = myNorm(y);
    Ay = op(y,2);
    na = max( na, myNorm(Ay) / ny );
    Ay_x = tfocs_dot( x, Ay );

    errs(k) = abs(Ax_y-Ay_x)/(nx*ny);

    %
    % The myNorm iteration
    %

    if nxe == 0,
        if x_real,
            xx = randn(sz{1});
        else
            xx = randn(sz{1}) + 1j*randn(sz{1});
            if x_conjSymmetric
                xx = make_conj_symmetrix( xx );
            end
        end
        nxe = myNorm(xx);
    end
    yy = op(xx/nxe,1);
    nye = max(realmin,myNorm(yy));
    xx = op(yy/nye,2);
    nxe = myNorm(xx);

end

%
% Use the estimated singular vectors for a final adjoint est
%

if nxe > 0,
    Ax_y = tfocs_dot( op(xx,1), yy );
    Ay_x = tfocs_dot( op(yy,2), xx );
    errs(end) = abs(Ax_y-Ay_x) / (nxe*nye);
end

%
% Display the output
%

nmax = max(nye,nxe);
myNorm_err = abs(nye-nxe) / nmax;
peak_err = max(errs) / nmax;
mean_err = mean(errs) / nmax;
rc = { 'complex', 'real', 'complex symmetric' };
fprintf( 'TFOCS linear operator test:\n' );
fprintf( '   Input size:  [' ); fprintf( ' %d', sz{1} ); fprintf( ' ], %s\n', rc{x_real+1+2*x_conjSymmetric} );
fprintf( '   Output size: [' ); fprintf( ' %d', sz{2} ); fprintf( ' ], %s\n', rc{y_real+1+2*y_conjSymmetric} );
fprintf( 'After %d iterations:\n', maxits  );
fprintf( '    myNorm estimates (forward/adjoint/error): %g/%g/%g\n', nye, nxe, myNorm_err );
fprintf( '       Gains: forward %g, adjoint %g\n', nf, na );
fprintf( '    Inner product error:\n' );
fprintf( '       Mean (absolute/relative): %g/%g\n', mean(errs), mean_err );
fprintf( '       Peak (absolute/relative): %g/%g\n', max(errs), peak_err );
fprintf( '       (inner product errors should 1e-10 or smaller)\n');

good = true;
if myNorm_err/max( nye, nxe ) > 1e-4
    fprintf('  Detected mismatch in forward/adjoint norm estimates. This is potentially a bad sign. Check your implementation\n');
    good = false;
end
if mean_err > 1e-8
    fprintf('  The mean error (relative) is high. This is a bad sign. Check your implementation\n');
    good = false;
end
if good
    fprintf('  Allowing for some roundoff error, there are no obvious errors. This is good.\n');
end

if nargout > 0
    varargout{1} = mean([nye,nxe]);
end

% Improvement as suggested by Graham Coleman
% Allows for 3D arrays.
% This also changes default behavior of 2D arrays
% to now use the Frobenius norm instead of spectral norm.
% This is wise, since it's a much quicker computation.
function y = myNorm(x)
y = norm( x(:) );

function y = make_conj_symmetrix( y )
ny = size( y, 1 );
y(1,:)  = real(y(1,:));         % DC component is 0
if round(ny/2) == ny/2  % even
    y(ny/2+1,:)     = real(y(ny/2+1,:));    % Nyquist component is 0
    y( ny:-1:(ny/2+2) )     = conj( y(2:ny/2) );
else                    % odd
    y( ny:-1:((ny+1)/2+1) ) = conj( y(2:((ny+1)/2)) );
end

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


%==============================================================================
% This code is part of the Matlab-based toolbox
% LagLDDDM - A Lagrangian Gauss--Newton--Krylov Solver for Mass- and
%                        Intensity-Preserving Diffeomorphic Image Registration
%
% For details and license info see
% - https://github.com/C4IR/FAIR.m/tree/master/add-ons/LagLDDMM
%
% function [yc,dy,para] = getTrafoFromVelocityRK4(vc,yc,varargin)
%
% compute transformation yc by integrating velocity field in time using
% a fourth-order Runge-Kutta method. Here, vc is assumed to be stationary.
% For instationary velocities, use getTrafoFromInstatinaryVelocityRK4.m
%
% For more details see Sec. 3 of the paper:
%
% @article{MangRuthotto2017,
%   Title = {A {L}agrangian {G}auss--{N}ewton--{K}rylov solver for mass- and intensity-preserving diffeomorphic image registration},
%   Year = {2017},
%   Journal = {SIAM Journal on Scientific Computing},
%   Author = {A. Mang, L. Ruthotto},
% }
%
% Input:
%
%  vc   - discrete velocity field (nodal, cell-centered, or staggered)
%  yc   - particle positions
%
% Additional REQUIRED Input (provided through varargin)
%
%  omega        - spatial domain (required!)
%  m            - number of cells in each direction (required!)
%
% Optional Input (provided through varargin)
%
%  doDerivative - compute derivative w.r.t. velocity
%  tspan        - time interval (default: [0 1])
%  N            - number of time discretization points (nodal) (default: 5)
%  storeInter   - store intermediate transformation (e.g., for visualization)
%
% Output:
%
%  yc           - end point of characteristics
%  dy           - derivative w.r.t. vc
%  para         - info such as CFL
%
% =========================================================================
function [yc,dy,para] = getTrafoFromVelocityRK4(vc,yc,varargin)

if nargin==0
    runMinimalExample
    return;
end
doDerivative = (nargout>1);
omega = [];
m     = [];
tspan = [0 1];
N     = 5;
storeInter = false;
for k=1:2:length(varargin)     % overwrites default parameter
    eval([varargin{k},'=varargin{',int2str(k+1),'};']);
end;
if isempty(omega) || isempty(m)
    error('%s - omega and m must be provided through varargin or trafo(''set'',''omega'',omega,''m'',m')
end
dt   = (tspan(2)-tspan(1))/(N-1); % note dt is negatave when going backwards in time
dy = [];
if doDerivative
    dy = sparse(numel(yc),numel(vc));
end
% compute the CFL number (not actually required for stability in Lagrangian
% methods, but an indicator of how many voxels the particles may move in
% one time step.
h    = (omega(2:2:end)-omega(1:2:end))./m;
% vt   =  sqrt(sum(reshape(vc.^2,[],dim),2));
% CFL  = max(vt)*dt/min(h);
CFL = 0.0;
para = struct('CFL',CFL,'dt',dt,'N',N,'omega',omega,'m',m,'h',h);
if storeInter
    para.YC = zeros(numel(yc),N);
    para.YC(:,1) = yc;
end
for k=1:N-1
    [vi,dvidy] = linearInterGrid(vc,omega,m,yc,'doDerivative',doDerivative);
    if doDerivative
        Ty = getLinearInterGridMatrix(omega,m,yc);
        dyi = Ty + dvidy*dy;
        dytemp = dyi;
    end
    ytemp = vi;
    yi    = yc + .5*dt*vi;

    [vi,dvidy] = linearInterGrid(vc,omega,m,yi,'doDerivative',doDerivative);
    if doDerivative
        Ty = getLinearInterGridMatrix(omega,m,yi);
        dyi = Ty + dvidy*(dy+.5*dt*dyi);
        dytemp = dytemp + 2*dyi;
    end
    ytemp = ytemp + 2*vi;
    yi    = yc + .5*dt*vi;

    [vi,dvidy] = linearInterGrid(vc,omega,m,yi,'doDerivative',doDerivative);
    if doDerivative
        Ty = getLinearInterGridMatrix(omega,m,yi);
        dyi = Ty + dvidy*(dy+.5*dt*dyi);
        dytemp = dytemp + 2*dyi;
    end
    ytemp = ytemp + 2*vi;
    yi    = yc + dt*vi;
    [vi,dvidy] = linearInterGrid(vc,omega,m,yi,'doDerivative',doDerivative);
    if doDerivative
        Ty = getLinearInterGridMatrix(omega,m,yi);
        dyi = Ty + dvidy*(dy+dt*dyi);
        dytemp = dytemp + dyi;
        dy  = dy + (dt/6)*dytemp;
    end
    ytemp = ytemp + vi;
    yc    = yc + (dt/6)*ytemp;
    if storeInter, para.YC(:,k+1) = yc; end
end

function runMinimalExample

omegaV = [-1 1 -1 1];
omega = .1*omegaV;
m     = [24 24];
tspan     = [2 3];
N     = 2;
yc    = getNodalGrid(omega,m)+.0002;

% test cell-centered grid
regularizer('set','regularizer','mbCurvature','alpha',1)
xc      = reshape(getCellCenteredGrid(omegaV,m),[],2);
vc      = [.3*sign(xc(:,1)).*xc(:,1).^2 sin(pi*xc(:,2))];
fctn    = @(vc) getTrafoFromVelocityRK4(vc,yc,'omega',omegaV,'m',m,'T',tspan,'N',N);
checkDerivative(fctn,vc(:))

% test nodal grid
regularizer('set','regularizer','mbElasticNodal','alpha',1)
xc      = reshape(getNodalGrid(omegaV,m),[],2);
vc      = [.3*sign(xc(:,1)).*xc(:,1).^2 sin(pi*xc(:,2))];
fctn    = @(vc) getTrafoFromVelocityRK4(vc,yc,'omega',omegaV,'m',m,'T',tspan,'N',N);
checkDerivative(fctn,vc(:))

% test staggered grid
regularizer('set','regularizer','mbElastic','alpha',1)
vc  = grid2grid(vc(:),m,'nodal','staggered');
fctn    = @(vc) getTrafoFromVelocityRK4(vc,yc,'omega',omegaV,'m',m,'T',tspan,'N',N);
checkDerivative(fctn,vc(:))

omegaV = [-1 1 -1 1 -1 1];
omega = .1*omegaV;
m     = [16 16 8];
tspan = [4 3];
N     = 10;
yc    = getNodalGrid(omega,m)+.0002;

regularizer('set','regularizer','mbCurvature','alpha',1)
xc      = reshape(getCellCenteredGrid(omegaV,m),[],3);
vc      = [.3*sign(xc(:,1)).*xc(:,1).^2 sin(pi*xc(:,2)) xc(:,3)];
fctn    = @(vc) getTrafoFromVelocityRK4(vc,yc,'omega',omegaV,'m',m,'T',tspan,'N',N);
% [yc,dy] = fctn(vc(:));
checkDerivative(fctn,0*vc(:))

% test nodal grid
regularizer('set','regularizer','mbElasticNodal','alpha',1)
xc      = reshape(getNodalGrid(omegaV,m),[],3);
vc      = [.3*sign(xc(:,1)).*xc(:,1).^2 sin(pi*xc(:,2)) xc(:,3)];
fctn    = @(vc) getTrafoFromVelocityRK4(vc,yc,'omega',omegaV,'m',m,'T',tspan,'N',N);
checkDerivative(fctn,vc(:))

% test staggered grid
regularizer('set','regularizer','mbElastic','alpha',1)
vc  = grid2grid(vc(:),m,'nodal','staggered');
fctn    = @(vc) getTrafoFromVelocityRK4(vc,yc,'omega',omegaV,'m',m,'T',tspan,'N',N);
checkDerivative(fctn,vc(:))


%This Matlab script can be used to reproduce Figure 7.13 in the monograph:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.0 (Last edited: 2017-11-04)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

%Empty workspace and close figures
close all;
clear;

%% Define parameters
lambda = 1; %Wavelength (normalized)
M_H = 8; %Number of antenna per horizontal row
M_V = 4; %Number of rows
d_H = 0.5*lambda; %Horizontal antenna spacing
d_V = 0.5*lambda; %Vertical antenna spacing

%Define the antenna geometry
M = M_H*M_V; %Total number of antennas
U = zeros(3,M); %Matrix containing the position of the antennas

i = @(m) mod(m-1,M_H); %Horizontal index
j = @(m) floor((m-1)/M_H); %Vertical index

for m = 1:M
    U(:,m) = [0; i(m)*d_H; j(m)*d_V]; %Position of the mth element
end

%% Compute the spatial signature for various directions
varphi = linspace(-pi/2,pi/2,1000);
theta= [0,-pi/3];
P = zeros(length(varphi),length(theta));
v = ones(M,1);

for i = 1:length(varphi)

    for j = 1:length(theta)

        P(i,j) = abs(v'*functionSpatialSignature3DLoS(U,varphi(i),theta(j),lambda))/M;

    end

end

P = P/max(max(P));
P = 10*log10(P);

%% Plot the simulation results
figure;
hold on; box on;
plot(varphi/pi,P(:,1)','-k','LineWidth',1)
plot(varphi/pi,P(:,2)','r--','LineWidth',1)

ylim([-30,0])
ax = gca;
set(ax, 'XTick', [-0.5, -0.25, 0, 0.25, 0.5]);
xlabel('Azimuth angle $\varphi$ in multiples of $\pi$','Interpreter','latex');
ylabel('Normalized array response [dB]');
legend({'$\theta = 0$', '$\theta = -\pi/3$   '},'Interpreter','latex');


function [xUpdate,PUpdate,innov,Pzz,W]=QMCKalUpdateWithPred(z,R,zPred,PzPred,otherInfo)
%%QMCKALUPDATEWITHPRED Given the output of the measurement prediction step
%           from QMCKalMeasPred and a measurement, complete the measurement
%           update step of the quasi-Monte Carlo Kalman filter with
%           additive measurement noise. Separating the measurement
%           prediction step from the rest of the update step can make the
%           creation of multiple measurement association hypotheses from a
%           single target prediction more efficient. The full measurement
%           update function is QMCKalUpdate.
%
%INPUTS: z The zDimX1 vector measurement.
%        R The zDim X zDim measurement covariance matrix in the native
%          coordinate system of the measurement.
%    zPred The zDimXnumComp measurement predictions from the filter.
%   PzPred The zDimXzDimXnumComp covariance matrices associated with zPred.
% otherInfo The intermediate results returned in the otherInfo output of
%          the QMCKalMeasPred function.
%
%OUTPUTS: xUpdate The xDimXnumComp updated state vectors.
%         PUpdate The updated xDimXxDimXnumComp state covariance matrices
%                 associated with xUpdate.
%      innov, Pzz The zDimXnumComp innovations and the zDimXzDim innovation
%                 covariance matrices are returned in case one wishes to
%                 analyze the consistency of the estimator or use those
%                 values in gating or likelihood evaluation.
%               W The xDimXzDimXnumComp gains used in the update.
%
%See the comments to the function QMCKalMeasPred for an example of usage of
%this function. See the comments to QMCKalUpdate for more information on
%the algorithm.
%
%June 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

innovTrans=otherInfo.innovTrans;
stateTrans=otherInfo.stateTrans;
xPred=otherInfo.xPred;
PPred=otherInfo.PPred;
Pxz=otherInfo.Pxz;

xDim=size(xPred,1);
numComp=size(xPred,2);
zDim=size(z,1);

xUpdate=zeros(xDim,numComp);
PUpdate=zeros(xDim,xDim,numComp);
innov=zeros(zDim,numComp);
Pzz=zeros(zDim,zDim,numComp);
W=zeros(xDim,zDim,numComp);

for k=1:numComp
    Pzz(:,:,k)=PzPred(:,:,k)+R;

    %The innovation, transformed as necessary to keep values in a desired
    %range.
    innov(:,k)=innovTrans(z,zPred(:,k));

    %The filter gain
    W(:,:,k)=Pxz(:,:,k)/Pzz(:,:,k);

    %Updated state estimate
    xUpdate(:,k)=stateTrans(xPred(:,k)+W(:,:,k)*innov(:,k));

    %Updated state covariance matrix
    PUpdate(:,:,k)=PPred(:,:,k)-W(:,:,k)*Pzz(:,:,k)*W(:,:,k)';
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.


function [I,B1,B2,B3] = in_element(V,F,P,varargin)
  % IN_ELEMENT test for each p in P whether it lies inside each f in F defined
  % over V.
  %
  % I = in_element(V,F,P)
  % [I,B1,B2,B3] = in_element(V,F,P,'ParameterName',ParameterValue,...)
  %
  % Inputs:
  %   V  #V by dim list of vertex positions
  %   F  #F by dim+1 list of element indices
  %   P  #P by dim list of query positions
  %   Optional:
  %     'Method' followed by one of the following {'knn'}:
  %       'brute-force' no acceleration O(#P * #F)
  %       'edge-walk' walk along edges ~O(#P * sqrt(#F)) Starting with a random
  %         barycenter step along the edges that intersect with the ray toward
  %         the query point. If a boundary is reached then search along all
  %         boundary edges and jump to farthest hit. **dim=2 only**
  %       'knn' use knnsearch to find closest element barycenters
  %       'spatial-hash' spatial hash on regular grid ~O(#P * sqrt(#F)) **dim=2
  %         only**
  %     'First'  only keep first match {false}
  %     'Quiet' suppress warnings {false}
  %     'Epsilon' epsilon used for determining inclusion {eps}
  % Outputs:
  %   I  #P by #F matrix of bools
  %   B1  #P by #F list of barycentric coordinates
  %   B2  #P by #F list of barycentric coordinates
  %   B3  #P by #F list of barycentric coordinates
  %
  % Example:
  %   P = bsxfun(@plus,min(V),bsxfun(@times,rand(100,2),max(V)-min(V)));
  %   [I,B1,B2,B3] = in_element(V,F,P);
  %   % Only keep first
  %   [mI,J] = max(I,[],2);
  %   I = sparse(1:size(I,1),J,mI,size(I,1),size(I,2));
  %   % Mask barycentric coordinates
  %   B1 = B1.*I;
  %   B2 = B2.*I;
  %   B3 = B3.*I;
  %   Q = B1*V(F(:,1),:) + B2*V(F(:,2),:) + B3*V(F(:,3),:);
  %   Q = Q(any(I,2),:);
  %   tsurf(F,V);
  %   hold on;
  %   plot(Q(:,1),Q(:,2),'or','LineWidth',6);
  %   plot(P(:,1),P(:,2),'ob','LineWidth',2);
  %   hold off;
  %

  function [I] = in_element_brute_force(V,F,P)
    dim = size(V,2);
    assert(dim+1 == size(F,2));

    % number of elements
    m = size(F,1);
    % number of query points
    np = size(P,1);

    switch dim
    case 3
      T = F;
      % tet face ares
      vol = abs(volume(V,T));
      allF = [ ...
        T(:,2) T(:,4) T(:,3); ...
        T(:,1) T(:,3) T(:,4); ...
        T(:,1) T(:,4) T(:,2); ...
        T(:,1) T(:,2) T(:,3); ...
        ];
      % List of tets for each face f of each tet t for each point p
      TP = cat(2, ...
        repmat(allF,[1 1 np]), ...
        permute(repmat(size(V,1)+(1:np),m*4,1),[1 3 2]));
      TP = reshape(permute(TP,[1 3 2]),m*4*np,dim+1);
      Pvol = abs(volume([V;P],TP));
      % Pvol(t,f,p) --> volume of tet t, face f with point p
      Pvol = reshape(Pvol,[size(T,1) dim+1 np]);
      % sumvol(p,t) --> sum of volumes of tets made with faces of t and p
      sumvol = permute(sum(Pvol,2),[3 1 2]);
      I = sparse(abs(bsxfun(@minus,sumvol,vol')) < sqrt(epsilon));
    case 2
      % triangle side lengths
      l = [ ...
        sqrt(sum((V(F(:,2),:)-V(F(:,3),:)).^2,2)) ...
        sqrt(sum((V(F(:,3),:)-V(F(:,1),:)).^2,2)) ...
        sqrt(sum((V(F(:,1),:)-V(F(:,2),:)).^2,2)) ...
        ];

      B = zeros([np m dim+1]);
      for ii = 1:(dim+1)
        jj = mod(ii+1,dim+1)+1;
        kk = mod(ii,dim+1)+1;
        ljj = pdist2(P,V(F(:,jj),:));
        lkk = pdist2(P,V(F(:,kk),:));

        % semiperimeters
        s = bsxfun(@plus,l(:,ii)',ljj + lkk)*0.5;
        % Heron's formula for area
        B(:,:,ii) = 2*sqrt(s.*(bsxfun(@minus,s,l(:,ii)').*(s-ljj).*(s-lkk)));
      end
      % sum of barycentric coordinates
      sumA = sum(B,3);
      % area of element
      dblA = doublearea(V,F);
      %% check whether sum is more than true are
      %I = ~bsxfun(@gt,sumA,dblA');
      I = sparse((bsxfun(@minus,sumA,dblA')) < sqrt(epsilon));
    case 1
      % To-do: this is actually being computed in a dense way
      I = sparse( ...
        ((P <= V(F(:,1))') & (P >= V(F(:,2))')) | ...
        ((P <= V(F(:,2))') & (P >= V(F(:,1))')));
    end
    %B1 = sparse(B(:,:,1));
    %B2 = sparse(B(:,:,2));
    %B3 = sparse(B(:,:,3));
  end

  function I = in_element_hash_helper(V,F,P)
    assert(size(F,2) == 3, ...
      'F must contain triangles for Method=''spatial-hash''');
    num_bins = ceil(sqrt(size(F,1)));
    bin_x = ceil(sqrt(num_bins));
    bin_y = ceil(num_bins/bin_x);
    num_bins = bin_x*bin_y;
    % spatial hash
    function VH = hash(V,MN,MX,bin_x,bin_y)
      [~,X] = histc(V(:,1),linspace(MN(1),MX(1),bin_x));
      [~,Y] = histc(V(:,2),linspace(MN(2),MX(2),bin_y));
      VH = sub2ind([bin_x bin_y],X,Y);
    end
    %% http://stackoverflow.com/a/5929567/148668
    %primes = [ 40960001, 59969537 45212177];
    %hash = @(X) mod( ...
    %  bitxor( int32(V(:,1)*primes(1)), int32(V(:,2)*primes(2))), ...
    %  num_bins);
    MN = min([V;P]);
    MX = max([V;P]);
    VH = hash(V,MN,MX,bin_x,bin_y);
    PH = hash(P,MN,MX,bin_x,bin_y);
    % This is wrong for triangles that span more hash cells than their vertices:
    % any time a triangle lands on the corner....
    FH = sparse(repmat(1:size(F,1),1,size(F,2))',VH(F(:)),1,size(F,1),num_bins)~=0;
    [~,FHI,FHJ] = find(FH);
    [FHJX,FHJY] = ind2sub([bin_x,bin_y],FHJ);
    % This assumes that #P >> #F
    I = sparse(size(P,1),size(F,1));
    for h = 1:num_bins
      %Vh = V(VH==h,:);
      IFh = FH(:,h);
      if any(IFh)
        IPh = PH==h;
        Ph = P(IPh,:);
        if any(IPh)
          Fh = F(IFh,:);
          Ih = in_element_brute_force(V,Fh,Ph);
          I(IPh,IFh) = Ih;
        end
      end
    end
  end

  % default values
  method = [];
  first = false;
  quiet = false;
  epsilon = eps;
  % Map of parameter names to variable names
  params_to_variables = containers.Map( {'Method','First','Quiet','Epsilon'}, ...
    {'method','first','quiet','epsilon'});
  v = 1;
  while v <= numel(varargin)
    param_name = varargin{v};
    if isKey(params_to_variables,param_name)
      assert(v+1<=numel(varargin));
      v = v+1;
      % Trick: use feval on anonymous function to use assignin to this
      % workspace
      feval(@()assignin('caller',params_to_variables(param_name),varargin{v}));
    else
      error('Unsupported parameter: %s',varargin{v});
    end
    v=v+1;
  end
  if isempty(method)
    switch size(V,2)
    case 1
      method = 'brute-force';
    otherwise
      method = 'knn';
    end
  end

  switch method
  case 'brute-force'
    I = in_element_brute_force(V,F,P);
  case 'spatial-hash'
    % Try 45?? spatial grid, too (reduce corner cases)
    switch size(V,2)
    case 2
      R = [cos(pi/4) -sin(pi/4);sin(pi/4) cos(pi/4)];
    case 3
      R = [cos(pi/4) -sin(pi/4) 0 ;sin(pi/4) cos(pi/4) 0; 0 0 1];
    end
    I = in_element_hash_helper(V,F,P) | in_element_hash_helper(V*R,F,P*R);

    % Find any obviously incorrect values: not inside but winding number says
    % inside. Could still missing something if mesh overlaps itself.
    NI = ~any(I,2);
    switch size(F,2)
    case 3
      O = outline(F);
    case 4
      O = boundary_faces(F);
    end
    WI = abs(winding_number(V,O,P(NI,:))/(2*pi))>0.5;
    WI = sparse(find(NI),1,WI,size(P,1),1)~=0;
    % redo any that currently are not in any but winding number says are inside
    RI = NI & WI;
    I(RI,:) = in_element_brute_force(V,F,P(RI,:));

  case 'edge-walk'

    assert(size(F,2) == 3,'F must contain triangles for Method=''edge-walk''');
    % List of all "half"-edges: 3*#F by 2
    allE = [F(:,[2 3]); F(:,[3 1]); F(:,[1 2])];
    % Sort each row
    sortallE = sort(allE,2);
    % IC(i) tells us where to find sortallE(i,:) in uE:
    % so that sortallE(i,:) = uE(IC(i),:)
    [uE,~,IC] = unique(sortallE,'rows');
    % uE2F(e,f) = i means face f's ith edge is unique edge e
    uE2F = sparse(IC(:),repmat(1:size(F,1),1,3)',reshape(repmat(1:3,size(F,1),1),[],1));
    % uE2F(e,f) = 1 means face f is adjacent to unique edge e
    uE2F1 = sparse(IC(:),repmat(1:size(F,1),1,3)',1);
    % Face-face Adjacency matrix
    A = uE2F1'*uE2F;
    % A(f,g) = i means face f's ith edge is shared with g
    A = A-diag(diag(A));

    I = sparse(size(P,1),size(F,1));
    B1 = sparse(size(I,1),size(I,2));
    B2 = sparse(size(I,1),size(I,2));
    B3 = sparse(size(I,1),size(I,2));

    [~,is_b] = on_boundary(F);
    EF = repmat(1:size(F,1),1,3)';
    EFI = reshape(repmat(1:3,size(F,1),1),[],1);
    O = allE(is_b(:),:);
    OF = EF(is_b(:));
    OFI = EFI(is_b(:));

    % initial closest vertex
    BC = barycenter(V,F);
    % Centroid
    f_init = snap_points(mean(V),BC);
    %% Random initial guess
    %f_init = ceil(rand(1,1)*size(F,1));
    for p = 1:size(P,1)
      % current closest face, barycenter
      f = f_init;
      q = BC(f_init,:);
      % incoming edge
      e_in = [];
      while true
        % current point
        % edges to test
        E = ... %mod(bsxfun(@plus,setdiff([1;2;3],e_in),-1+(1:2)),3)+1;
          [2 3;3 1;1 2];
        out = ...
          lineSegmentIntersect([q P(p,:)],[V(F(f,E(:,1)),:) V(F(f,E(:,2)),:)]);
        out.intAdjacencyMatrix(e_in) = false;
        e_out = find(out.intAdjacencyMatrix);
        if isempty(e_out)
          % no hits so we're in the element
          I(p,f) = 1;
          B = barycentric_coordinates( ...
            P(p,:),V(F(f,1),:),V(F(f,2),:),V(F(f,3),:));
          B1(p,f) = B(1); B2(p,f) = B(2); B3(p,f) = B(3);
        else
          f_prev = f;
          if numel(e_out) > 1
            % Take farthests
            [sd,si] = sort(out.intNormalizedDistance1To2(e_out),'descend');
            e_out = e_out(si(1));
            % TODO: recurse on all in f
          end
          f = find(A(:,f_prev)==e_out);
          % Todo if
          if isempty(f)
            % no neighbors so we're shooting outside.
            out = ...
              lineSegmentIntersect([q P(p,:)],[V(O(:,1),:) V(O(:,2),:)]);
            hits = find(out.intAdjacencyMatrix);
            [sd,si] = sort(out.intNormalizedDistance1To2(hits),'descend');
            f = OF(hits(si(1)));
            e_in = OFI(hits(si(1)));
            if f==f_prev
              error('Point is outside');
            end
          else
            if numel(f) > 1
              f = f(1);
              if ~quiet
                warning('Ignoring non-manifold edge: might miss multiply inside.');
              end
              % TODO: recurse on all in f
            end
            e_in = A(f_prev,f);
          end
        end
        %tsurf(F,V);
        %hold on;
        %tsurf(F(f,:),V,'CData',-1);
        %tsurf(F(f_prev,:),V,'CData',1);
        %plot_edges([q;P(p,:)],[1 2],'r');
        %plot_edges(V,[F(f_prev,E(:,1));F(f_prev,E(:,2))]','b');
        %plot(P(p,1),P(p,2),'*');
        %plot_edges(V,[F(f_prev,E(e_out,1));F(f_prev,E(e_out,2))]', ...
        %  'y','LineWidth',2);
        %hold off;
        %drawnow;
        %input('');
      end
    end

    return
  case 'knn'
    % assumes samples are all inside exactly one element
    BC = barycenter(V,F);
    I = sparse(size(P,1),size(F,1));
    B1 = sparse(size(P,1),size(F,1));
    B2 = sparse(size(P,1),size(F,1));
    B3 = sparse(size(P,1),size(F,1));
    B4 = sparse(size(P,1),size(F,1));
    % indices of points we haven't found yet
    IP = 1:size(P,1);

    prev_k = 0;
    k = 2;
    while true
      K = knnsearch(BC,P(IP,:),'K',k);
      K = K(:,prev_k+1:end);
      for ki = 1:size(K,2)
        switch size(F,2)
        case 3
          B = abs(barycentric_coordinates( ...
            P(IP,:), ...
            V(F(K(:,ki),1),:), ...
            V(F(K(:,ki),2),:), ...
            V(F(K(:,ki),3),:)));
        case 4
          B = abs(barycentric_coordinates( ...
            P(IP,:), ...
            V(F(K(:,ki),1),:), ...
            V(F(K(:,ki),2),:), ...
            V(F(K(:,ki),3),:), ...
            V(F(K(:,ki),4),:)));
        end
        found = abs(sum(B,2)-1)<sqrt(epsilon);
        I(sub2ind(size(I),IP,K(:,ki)')) = found;
        % DOESN'T REALLY PAY OFF TO COMPUTE THESE HERE
        %B1(sub2ind(size(I),IP,K(:,ki)')) = found.*B(:,1);
        %B2(sub2ind(size(I),IP,K(:,ki)')) = found.*B(:,2);
        %B3(sub2ind(size(I),IP,K(:,ki)')) = found.*B(:,3);
        %if size(F,2) == 4
        %  B4(sub2ind(size(I),IP,K(:,ki)')) = found.*B(:,4);
        %end
        % Peel off found
        IP = IP(~found);
        if isempty(IP)
          break;
        end
        K = K(~found,:);
      end

      %for p = 1:numel(IP)
      %  Kp = K(p,:);
      %  U = V(F(Kp,:),:);
      %  FKp = reshape((1:size(U,1))',size(U,1)/size(F,2),size(F,2));
      %  I(IP(p),Kp) = in_element_brute_force(U,FKp,P(IP(p),:));
      %end
      %IP = find(~any(I,2));

      if isempty(IP)
        break;
      end
      prev_k = k;
      k = min(prev_k*2,size(BC,1));
      if k == size(BC,1)
        if ~quiet
          warning('Some points not found');
        end
        break;
      end
    end

    %return;

  end

  if first
    % Only keep first
    [mI,J] = max(I,[],2);
    I = sparse(1:size(I,1),J,mI,size(I,1),size(I,2));
  end

  % Compute barycentric coordinates
  if nargout > 1
    B1 = sparse(size(I,1),size(I,2));
    B2 = sparse(size(I,1),size(I,2));
    B3 = sparse(size(I,1),size(I,2));
    B4 = sparse(size(I,1),size(I,2));
    work_I = I;
    while true
      % Peel off a layer
      [mIf,Jf] = max(work_I,[],2);
      If = find(mIf);
      Jf = Jf(If);
      mIf = mIf(If);
      if isempty(If)
        break
      end
      work_I = work_I - sparse(If,Jf,mIf,size(work_I,1),size(work_I,2));;
      Pf = P(If,:);
      Ff = F(Jf,:);
      switch size(Ff,2)
      case 4
        Bf = barycentric_coordinates(Pf, ...
          V(Ff(:,1),:),V(Ff(:,2),:),V(Ff(:,3),:),V(Ff(:,4),:));
      case 3
        Bf = barycentric_coordinates(Pf,V(Ff(:,1),:),V(Ff(:,2),:),V(Ff(:,3),:));
      case 2
        Bf = barycentric_coordinates(Pf,V(Ff(:,1),:),V(Ff(:,2),:));
      end
      B1(sub2ind(size(B1),If,Jf)) = Bf(:,1);
      B2(sub2ind(size(B2),If,Jf)) = Bf(:,2);
      if size(Bf,2) >= 3
        B3(sub2ind(size(B3),If,Jf)) = Bf(:,3);
      end
      if size(Bf,2) >= 4
        B4(sub2ind(size(B3),If,Jf)) = Bf(:,4);
      end
    end
  end

end


function value = i4_mach ( i )

%*****************************************************************************80
%
%% I4_MACH returns integer machine constants.
%
%  Discussion:
%
%    Input/output unit numbers.
%
%      I1MACH(1) = the standard input unit.
%      I1MACH(2) = the standard output unit.
%      I1MACH(3) = the standard punch unit.
%      I1MACH(4) = the standard error message unit.
%
%    Words.
%
%      I1MACH(5) = the number of bits per integer storage unit.
%      I1MACH(6) = the number of characters per integer storage unit.
%
%    Integers.
%
%    Assume integers are represented in the S digit base A form:
%
%      Sign * (X(S-1)*A**(S-1) + ... + X(1)*A + X(0))
%
%    where 0 <= X(1:S-1) < A.
%
%      I1MACH(7) = A, the base.
%      I1MACH(8) = S, the number of base A digits.
%      I1MACH(9) = A**S-1, the largest integer.
%
%    Floating point numbers
%
%    Assume floating point numbers are represented in the T digit
%    base B form:
%
%      Sign * (B**E) * ((X(1)/B) + ... + (X(T)/B**T) )
%
%    where 0 <= X(I) < B for I=1 to T, 0 < X(1) and EMIN <= E <= EMAX.
%
%      I1MACH(10) = B, the base.
%
%    Single precision
%
%      I1MACH(11) = T, the number of base B digits.
%      I1MACH(12) = EMIN, the smallest exponent E.
%      I1MACH(13) = EMAX, the largest exponent E.
%
%    Double precision
%
%      I1MACH(14) = T, the number of base B digits.
%      I1MACH(15) = EMIN, the smallest exponent E.
%      I1MACH(16) = EMAX, the largest exponent E.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 April 2007
%
%  Author:
%
%    Original FORTRAN77 version by Phyllis Fox, Andrew Hall, Norman Schryer
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Phyllis Fox, Andrew Hall, Norman Schryer,
%    Algorithm 528,
%    Framework for a Portable Library,
%    ACM Transactions on Mathematical Software,
%    Volume 4, Number 2, June 1978, page 176-188.
%
%  Parameters:
%
%    Input, integer I, chooses the parameter to be returned.
%    1 <= I <= 16.
%
%    Output, integer VALUE, the value of the chosen parameter.
%
  if ( i < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4_MACH - Fatal error!\n' );
    fprintf ( 1, '  The input argument I is out of bounds.\n' );
    fprintf ( 1, '  Legal values satisfy 1 <= I <= 16.\n' );
    fprintf ( 1, '  I = %d\n', i );
    value = 0;
    error ( 'I4_MACH - Fatal error!' );
  elseif ( i == 1 )
    value = 5;
  elseif ( i == 2 )
    value = 6;
  elseif ( i == 3 )
    value = 7;
  elseif ( i == 4 )
    value = 6;
  elseif ( i == 5 )
    value = 32;
  elseif ( i == 6 )
    value = 4;
  elseif ( i == 7 )
    value = 2;
  elseif ( i == 8 )
    value = 31;
  elseif ( i == 9 )
    value = 2147483647;
  elseif ( i == 10 )
    value = 2;
  elseif ( i == 11 )
    value = 24;
  elseif ( i == 12 )
    value = -125;
  elseif ( i == 13 )
    value = 128;
  elseif ( i == 14 )
    value = 53;
  elseif ( i == 15 )
    value = -1021;
  elseif ( i == 16 )
    value = 1024;
  elseif ( 16 < i )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4_MACH - Fatal error!\n' );
    fprintf ( 1, '  The input argument I is out of bounds.\n' );
    fprintf ( 1, '  Legal values satisfy 1 <= I <= 16.\n' );
    fprintf ( 1, '  I = %d\n', i );
    value = 0;
    error ( 'I4_MACH - Fatal error!' );
  end

  return
end


%MTRAJ Multi-axis trajectory between two points
%
% [Q,QD,QDD] = MTRAJ(TFUNC, Q0, QF, M) is a multi-axis trajectory (MxN) varying
% from configuration Q0 (1xN) to QF (1xN) according to the scalar trajectory function
% TFUNC in M steps. Joint velocity and acceleration can be optionally returned as
% QD (MxN) and QDD (MxN) respectively.  The trajectory outputs have one row per
% time step, and one column per axis.
%
% The shape of the trajectory is given by the scalar trajectory function
% TFUNC which is applied to each axis:
%      [S,SD,SDD] = TFUNC(S0, SF, M);
% and possible values of TFUNC include @lspb for a trapezoidal trajectory, or
% @tpoly for a polynomial trajectory.
%
% [Q,QD,QDD] = MTRAJ(TFUNC, Q0, QF, T) as above but T (Mx1) is a time
% vector which dictates the number of points on the trajectory.
%
% Notes::
% - If no output arguments are specified Q, QD, and QDD are plotted.
% - When TFUNC is @tpoly the result is functionally equivalent to JTRAJ except
%   that no initial velocities can be specified. JTRAJ is computationally a little
%   more efficient.
%
% See also JTRAJ, MSTRAJ, LSPB, TPOLY.

% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

function [S,Sd,Sdd] = mtraj(tfunc, q0, qf, M)

    if ~isa(tfunc, 'function_handle')
        error('first argument must be a function handle');
    end

    M0 = M;
    if ~isscalar(M)
        M = length(M);
    end
    if numcols(q0) ~= numcols(qf)
        error('must be same number of columns in q0 and qf')
    end

    s = zeros(M, numcols(q0));
    sd = zeros(M, numcols(q0));
    sdd = zeros(M, numcols(q0));

    for i=1:numcols(q0)
        % for each axis
        [s(:,i),sd(:,i),sdd(:,i)] = tfunc(q0(i), qf(i), M);
    end

% - If no output arguments are specified S, SD, and SDD are plotted
%   against time.

    switch nargout
        case 0
            clf

            if isscalar(M0)
                t = [1:M0]';
            else
                t = M0;
            end
            subplot(311)
            plot(t, s); grid; ylabel('s');

            subplot(312)
            plot(t, sd); grid; ylabel('sd');

            subplot(313)
            plot(t, sdd); grid; ylabel('sdd');
            if ~isscalar(M0)
                xlabel('time')
            else
                for c=get(gcf, 'Children');
                    set(c, 'XLim', [1 M0]);
                end
            end
            shg
        case 1
            S = s;
        case 2
            S = s;
            Sd = sd;
        case 3
            S = s;
            Sd = sd;
            Sdd = sdd;
    end


function value = meixner ( n, beta, c, x )

%*****************************************************************************80
%
%% MEIXNER evaluates Meixner polynomials at a point.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 February 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Walter Gautschi,
%    Orthogonal Polynomials: Computation and Approximation,
%    Oxford, 2004,
%    ISBN: 0-19-850672-4,
%    LC: QA404.5 G3555.
%
%  Parameters:
%
%    Input, integer N, the maximum order of the polynomial.
%    N must be at least 0.
%
%    Input, real BETA, the Beta parameter.  0 < BETA.
%
%    Input, real C, the C parameter.  0 < C < 1.
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE(N+1), the value of the polynomials at X.
%
  if ( beta <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'MEIXNER - Fatal error!\n' );
    fprintf ( 1, '  Parameter BETA must be positive.\n' );
    error ( 'MEIXNER - Fatal error!\n');
  end

  if ( c <= 0.0 || 1.0 <= c )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'MEIXNER - Fatal error!\n' );
    fprintf ( 1, '  Parameter C must be strictly between 0 and 1.\n' );
    error ( 'MEIXNER - Fatal error!\n');
  end

  if ( n < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'MEIXNER - Fatal error!\n' );
    fprintf ( 1, '  Parameter N must be nonnegative.\n' );
    error ( 'MEIXNER - Fatal error!\n');
  end

  OFFSET = 1;

  value(0+OFFSET) = 1.0;

  if ( n == 0 )
    return
  end

  value(1+OFFSET) = ( c - 1 ) * x / beta / c + 1.0;

  if ( n == 1 )
    return
  end

  for i = 1 : n - 1
    value(i+1+OFFSET) = ( ...
      ( ( c - 1.0 ) * x + ( 1.0 + c ) * i + beta * c ) * value(i+OFFSET) ...
      - i * value(i-1+OFFSET) ...
      ) / ( i + beta );
  end

  return
end


function [dfdx, fx] = VBA_numericDiff(fName, idxArg2Diff, varargin)
% // VBA toolbox //////////////////////////////////////////////////////////
%
% [dfdx, fx] = numericDiff(fname, idxArg2Diff, fArg1, fArg2, ...)
% numerical evaluation of derivatives
%
% This function evaluates numerically the derivatives of the function
% 'fname' with respect to its 'idxArg2Diff' input argument at the point
% defined by the input arguments [fArg1, fArg2, ..., fArgN]
%
% IN:
%   - fname: handle or name of the function to be differentiated.
%     ie either its name or a function handle. The latter can be very useful
%     if the function is itself a subfunction of a function!
%   - idxArg2Diff: the index of the argument to differentiate the function
%     with (1 <= idxArg2Diff <= numel(varargin))
%   - fArg1, fArg2, ..., fArgN: list of arguments which is required to call
%     the function 'fname', and which defines the ordinate at which the
%     derivative will be numerically evaluated
%
% OUT:
%   - dfdx: an m x p matrix containing the numerical differentiation
%     of the function evaluated at {fArg1, ..., fArgN}, where p is the
%     number of elements of the function's first output and m is the number
%     of elements of the idxArg2Diff input argument. If the function output
%     or the differentiated argument are in matrix form, they will be
%     vectorized first.
%   - f: the function evaluation at x
%
% NB: Mixed partials can be obtained by recursive call of this routine,
% e.g. consider:
%
% dfdx_idx_j = numericDiff(@numericDiff,i+2,fname,j,arg1,arg2,...,argn)
% dfdx_idx_j = reshape(dfdx_idx_j,mi,mj,p)
%
% The first line evaluates the numerical derivative of the function
% numericDiff(fname,j,arg1,arg2,...,argn) wrt to its (i+2)th entry, which
% is arg_i.
% The second line reshapes the output such that the first dimension is the
% dimension of argi (mi), the second is of the dimension of argj (mj), and
% the last dimension is the one of the function output itself (p).
%
% /////////////////////////////////////////////////////////////////////////

% perturbation scale
epsilon = 1e-4;

% shortcut
fArgs = varargin;

% evaluate function at the specified argument
% =========================================================================
try
    fx = VBA_vec(fName(fArgs{:}));
catch
    message = sprintf(...
        '*** VBA_numericDiff: Can not call %s with the provided arguments (nArgs = %d).', ...
        func2str(fName), numel(fArgs));
    error(message);
end

% evaluate the perturbation df of the function fname in the neighbourhood
% of the specified argument (ie at x + dx) and calculate the derivative wrt x
% =========================================================================

% pre-allocate the variables
m = numel (fArgs{idxArg2Diff});
p = numel (fx);
dfdx = zeros (m, p);

% loop over the dimension of idxArg2Diff-th argument
dx = epsilon * fArgs{idxArg2Diff};
dx (abs(dx) <= eps) = epsilon;

% compute effect of pertubations
for i = 1 : m
    xpdx = fArgs;
    xpdx{idxArg2Diff}(i) = xpdx{idxArg2Diff}(i) + dx(i);
    dfdx(i,:) = (VBA_vec (fName (xpdx{:})) - fx)' / dx(i);
end


function [results]=VBA_Shapley(posterior,out,varargin)
% [results]=VBA_Shapley(posterior,out,[options])
%
% Compute the Shapley values of the model's factors (inputs or paramters).
% These scores measure the relative influence of these factor on the variaince
% explained by the model.
% -------------------------------------------------------------------------
% IN:
%  - posterior, out: model structures
%  - varargin:       list of option/values pairs (see below)
% OUT:
%  - results: structure of results with the following fields, depending on
%  the chose coalitions
%  .parameters/inputs:
%         Shapley values (first order) of paramters/inputs. This is a n x p
%         matrix where n is the number of factors of interest and p the
%         number of observations in the model.
%  - interaction: Interaction score, ie relative change in Shapley values for a
%         perturbation in respective inputs. It is an array with the same
%         size as the inputs whose elements are similar to sv.
% -------------------------------------------------------------------------
% options:
%   coalitions:         ('parameters') | 'inputs' | 'interactions'
%      > factor of interest of the analysis. For 'interaction', compute the
%      Shapley value of parameters and their relative change for selective
%      input perturbations.
%   inputPerturbation   -> ('zero') | 'average' | 'average_nonzero'
%      > type of perturbation to apply on inputs: set to zero, average
%      accross the experiment, or set non zero inputs to their average
%   paramType           -> ('phi')  | 'theta'
%      > parameters of interest.
%   paramIdx            -> (all)  | paramIdx
%      > restrict parameters of interest to the index list paramIdx
%   inputIdx            -> (all)  | inputIdx
%      > restrict inputs of interest to the index list inputIdx
%   obsIdx              -> (all)  | ObsIdx
%      > restrict observations of interest to the index list obsIdx
% -------------------------------------------------------------------------

%% Complete option structure
% -------------------------------------------------------------------------

if numel(varargin) == 1 && isstruct(varargin{1})
    options = varargin{1};
else
    options.coalitions = {'parameters','inputs','interactions'};
    options.inputPerturbation = {'zero','average','average_nonzero'};
    options.paramType = {'phi','theta'};
    options.paramIdx = 1:out.dim.n_phi ;
    options.inputIdx = 1:out.dim.u ;
    if size(out.y,2) == 1 % catch vertical data as unique observation
        options.obsIdx = 0;
    else
        options.obsIdx = 1:out.dim.p ;
    end
    parser = inputParser;
    parser.parse (varargin{:});
    options = parser.Results;
    %options = parseargs(options,varargin{:});
end

%% Prepare perturbation scheme
% -------------------------------------------------------------------------

% dimensions
nu = numel(options.inputIdx);
nw = numel(options.paramIdx);
if options.obsIdx==0 % catch vertical data as unique observation
    nResps=1;
else
    nResps = numel(options.obsIdx);
end

% factorial perturbations on coalitions of interest
switch options.coalitions
    case 'interactions'
        % factorial perturbation of parmeeters with normal inputs,
        kw = full(VBA_spm_perm_mtx(nw));
        k = [kw ones(2^nw,nu)] ;
        % factorial perturbation of parameters with each input respectively
        % pertrubed
        ku = ones(nu)-eye(nu);
        for i=1:nu
            k = vertcat(k, [kw repmat(ku(i,:),size(kw,1),1)]);
        end
        % plus factorial perturbation of inputs alone
        % (ie 2^nw + nu x 2^nw + 2^nu coalitions)
        k = vertcat(k,[ones(2^nu,nw) full(VBA_spm_perm_mtx(nu))]);
    case 'parameters'
        % factorial perturbation of paramters, normal inputs
        k = full(VBA_spm_perm_mtx(nw));
        k = [k ones(size(k,1),nu)];
    case 'inputs'
        % factorial perturbation of inputs, normal paramters
        k = full(VBA_spm_perm_mtx(nu));
        k = [ones(size(k,1),nw) k];
end
nk = size(k,1);

%% Compute explained variances
% -------------------------------------------------------------------------

% loop over coalitions
ve = nan(nk,nResps);
parfor t = 1:size(k,1)
    kt = k(t,:);
    w_perm = kt(1:nw);
    u_perm = kt(nw+(1:nu));
    ve(t,:) = explainedVar(posterior,out,options,u_perm,w_perm) ;
end

% normalize
ve1 = ve(1,:);
ve0 = ve(end,:);
for i=1:nResps
    ve(:,i) = (ve(:,i) - ve0(i) )/(ve1(i)-ve0(i)) ;
end

%% Compute Shapley values
% -------------------------------------------------------------------------

% restrict coalitions to effects of interest
switch options.coalitions
    case 'interactions'
        spl = [2^nw*ones(nu+1,1); 2^nu];
        k = mat2cell(k,spl,nw+nu);
        for i=1:nu+1
            k{i} = k{i}(:,1:nw);
        end
        k{nu+2} = k{nu+2}(:,nw+(1:nu));
        ve = mat2cell(ve,spl,nResps);
    case 'parameters'
        k = {k(:,1:nw)};
        ve = {ve};
    case 'inputs'
        k = {k(:,nw+(1:nu))};
        ve = {ve};
end

% compute first order scores
for ii = 1:numel(k)
    n = size(k{ii},2);
    nn = factorial(n);
%     v{ii} = nan(n,nResps);
    for m=1:n % loop over players
        % Shapley coeficients
        i = k{ii}(:,m);
        z = sum(k{ii},2);
        coef = (2*i-1).*factorial(z-i).*factorial(n-z-(1-i))/nn;
        % compute shapley values per se
        v{ii}(m,:) =  coef'*ve{ii};
    end
end

% compute interactions if necessary
sv = v{1};
if strcmp(options.coalitions,'interactions')
    for iu=1:nu
        svi{iu} = (v{1}-v{iu+1})./v{1};
    end
    % shapley value of inputs
    svu = v{nu+2};
else
    svi={};
end

%% Store results
% -------------------------------------------------------------------------
switch options.coalitions
    case 'parameters'
        results.parameters = sv;
    case 'inputs'
        results.inputs = sv;
    case 'interactions'
        results.parameters = sv;
        results.inputs = svu;
        results.interactions = svi;
end

end
%% Subfunctions
% =========================================================================

% -------------------------------------------------------------------------
% Compute explained variance of the model given by posterior and out, with
% the inputs and paramters pertrubed according to u_swicth and w_switch
% respectively (switch = 1 -> normal, switch=0 -> perturbation)
% -------------------------------------------------------------------------
function v=explainedVar(posterior,out,options,u_switch,w_switch)

    % prevent unecessary bells and whistles
    out.options.verbose = 0;
    out.options.DisplayWin = 0;
    out.options.inF{1}.fast = true;

    % prepare degraded model
    % .....................................................................

    % == pertub inputs
    % index of inputs to perturb
    inputIdx = options.inputIdx(u_switch==0);
    % apply perturbation
    switch options.inputPerturbation
        case 'zero'
            out.u(inputIdx,:) = 0;
        case 'average'
            out.u(inputIdx,:) = mean(out.u(inputIdx,:),2);
        case 'average_nonzero'
            for iu=inputIdx
                idxNZ = find(out.u(iu,:)~=0);
                out.u(iu,idxNZ) = mean(out.u(iu,idxNZ));
            end
    end

    % == perturb parameters
    paramIdx = options.paramIdx(w_switch==0);
    switch options.paramType
        case 'phi'
            posterior.muPhi(paramIdx) = 0*posterior.muPhi(paramIdx);
        case 'theta'
            posterior.muTheta(paramIdx) = 0*posterior.muTheta(paramIdx);
    end

    % predict data
    % .....................................................................
    [yp,~,~,~,er] = VBA_simulate (...
        out.options.dim.n_t,...
        out.options.f_fname,...
        out.options.g_fname,...
        posterior.muTheta,...
        posterior.muPhi,...
        out.u,...
        Inf,...
        Inf,...
        out.options,...
        posterior.muX0);
    g = yp-er;
    y = out.y;

    % if vertical data, transpose everything
    if options.obsIdx == 0
        g = g';
        y = y';
        options.obsIdx = 1;
        out.options.isYout = out.options.isYout';
    end

    % compute explained variance
    % .....................................................................
    v = nan(1,numel(options.obsIdx));
    for i=1:numel(options.obsIdx) % for each observation of interest
        obsIdx = options.obsIdx(i);
        in_idx = find(out.options.isYout(obsIdx,:) == 0);
        v(i) = 1-((var(y(obsIdx,in_idx)-g(obsIdx,in_idx))/var(y(obsIdx,in_idx))))  ;
    end

end


function [gout,a,fc,L,info] = waveletfilters(Ls, scales, varargin)
%WAVELETFILTERS Generates wavelet filters
%   Usage: [gout,a,fc,L,info] = waveletfilters(Ls,scales)
%          [gout,a,fc,L,info] = waveletfilters(Ls,'bins', fs, fmin, fmax, bins)
%          [gout,a,fc,L,info] = waveletfilters(Ls,'linear', fs, fmin, fmax, channels)
%
%   Input parameters:
%         Ls     : System length
%         scales : Vector of wavelet scales
%   Output parameters:
%         gout  : Cell arrary of wavelet filters
%         a     : Downsampling rate for each channel.
%         fc    : Center frequency of each channel.
%         L     : Next admissible length suitable for the generated filters.
%         info  : Struct with additional outputs
%
%   `waveletfilters(Ls,scales)` constructs a system of wavelet filters covering
%   scales in the range *scales* for system length *Ls*. A scale of 1 corresponds
%   to a wavelet filter with peak positioned at the frequency 0.1 relative
%   to the Nyquist rate.
%
%   `[g,a,fc]=waveletfilters(Ls, 'bins', fs,fmin,fmax,bins)` constructs a set of
%   wavelets *g* which cover the required frequency range
%   `fmin`-`fmax` with `bins` filters per octave starting at `fmin`. All
%   filters have (approximately) equal $Q=f_c/f_b$. The
%   frequency interval below fmin not covered by these is captured
%   by additional lowpass filter(s) . The signal length *Ls*
%   is mandatory, since we need to avoid too narrow frequency windows
%
%   `[g,a,fc]=waveletfilters(Ls, 'linear', fs, fmin, fmax, channels)` constructs
%    a set of wavelets *g* which cover the required frequency range
%   `fmin`-`fmax` with `channels` equidistantly spaced filters starting at `fmin`.
%
%   Wavelet types
%   --------------------
%
%   The following wavelets can be passed as a flag:
%
%   'cauchy'     Cauchy wavelet (default parameters [alpha beta gamma] = [300 0 3])
%
%   'morse'      Generalized morse wavelet (default parameters [alpha beta gamma] = [300 0 3])
%
%   'morlet'     Morlet wavelet (default parameters sigma = [4])
%
%   'fbsp'       Frequency B-spline wavelet (default parameters [order fb] = [4 2])
%
%   'analyticsp' Analytic spline wavelet (default parameters [order fb] = [4 2])
%
%   'cplxsp'     Complex spline wavelet (default parameters [order fb] = [4 2])
%
%   A scale of 1 corresponds
%   to a wavelet filter with peak positioned at the frequency 0.1 relative
%   to the Nyquist rate. By default, this function does not allow center frequencies
%   exceeding the Nyquist rate, except with the optional parameter 'analytic',
%   see below. This implies that scales below 0.1 (or 0.05 for the 'analytic' scheme)
%   are not supported.
%   For more details on the construction of the wavelets and the available
%   wavelet types, please see |freqwavelet|.
%
%   By default, wavelet filters are peak normalized before being adjusted
%   to the proposed downsampling factor. The peak normalization can be
%   overridden by forwarding any norm flag accepted by |setnorm|.
%
%   Downsampling factors
%   --------------------
%
%   The integer downsampling rates of the channels must all divide the
%   signal length, |filterbank| will only work for input signal lengths
%   being multiples of the least common multiple of the downsampling rates.
%   See the help of |filterbanklength|.
%   The fractional downsampling rates restrict the filterbank to a single
%   length *L=Ls*.
%
%   `[gout,a]=waveletfilters(...,'regsampling')` constructs a non-uniform
%   filterbank with integer subsampling factors. This is the default.
%
%   `[gout,a]=waveletfilters(...,'uniform')` constructs a uniform filterbank
%   where the integer downsampling rate is the same for all the channels. This
%   results in the most redundant representation which produces nice plots.
%
%   `[gout,a]=waveletfilters(...,'fractional')` constructs a filterbank with
%   fractional downsampling rates *a*.
%   This results in the least redundant system.
%
%   `[gout,a]=waveletfilters(...,'fractionaluniform')` constructs a filterbank
%   with fractional downsampling rates *a*, which are uniform for all filters
%   except the "filling" low-pass filter which can have different
%   fractional downsampling rates. This is useful when uniform subsampling
%   and low redundancy at the same time are desirable.
%
%   Lowpass filters
%   --------------------
%
%   `[gout,a]=waveletfilters(...,'single')` uses a single lowpass filter
%   for covering the range from zero frequency to the center frequency of the
%   largest scale specified. This is the default.
%
%   `[gout,a]=waveletfilters(...,'repeat')` constructs frequency-shifted
%   copies of the largest scale wavelet to cover the range from zero frequency
%   to the center frequency of the largest scale specified.
%
%   `[gout,a]=waveletfilters(...,'none')` foregoes the construction of a
%   lowpass filter. This option cannot be expected to yield an invertible
%   filterbank.
%
%   Additional parameters
%   ---------------------
%
%   `waveletfilter` accepts the following optional parameters:
%
%     'redmul',redmul    Redundancy multiplier. Increasing the value of this
%                        will make the system more redundant by lowering the
%                        channel downsampling rates. It is only used if the
%                        filterbank is a non-uniform filterbank. Default
%                        value is *1*. If the value is less than one, the
%                        system may no longer be painless.
%
%     'redtar',redtar    Target redundancy. The downsampling factors will be
%                        adjusted to achieve a redundancy as close as possible
%                        to 'redtar'.
%
%     'trunc_at',trunc_at     Applies hard thresholding of the wavelet filters
%                             at the specified threshold value to reduce their
%                             support size.
%                             The default value is *trunc_at=10e-5*. When no
%                             truncation is desired, *trunc_at=0* should be chosen.
%
%     'delay',delay      A scalar, numeric vector of function handle that
%                        specifies delays for the wavelet filters. A
%                        numeric vector must have at least as many entries
%                        as there are filters in the filterbank. A function
%                        handle must accept two inputs *(k-1,a(k))*, where
%                        *k* is the channel index and *a* are the
%                        downsampling rates. If a function handle is given
%                        and 'redtar' is specified, delays are computed
%                        based on the final value of *a*.
%
%     'real'             Allows positive scales with center frequencies up
%                        to Nyquist. This is the default.
%
%     'complex'          Allows positive scales with center frequencies up
%                        to Nyquist, which are also mirrored to cover
%                        negative scales.
%
%     'analytic'         Allows positive scales with center frequencies up
%                        to twice the Nyquist frequency. This setting is
%                        suitable for the analysis of analytic signals.
%
%     'startfreq'        Allows to manually set a starting frequency for
%                        the wavelet range. Can not be lower than fmin.
%
%   Examples:
%   ---------
%
%   In the first example, we analyze a glockenspiel signal with a
%   regularly sampled wavelet filterbank using a frequency B-spline
%   wavelet of order 4 and with parameter fb=3 and visualize the result:::
%
%     [f,fs]=gspi;  % Get the test signal
%     Ls = length(f);
%     scales = linspace(10,0.1,100);
%     [g,a,fc,L]=waveletfilters(Ls,scales, {'fbsp', 4, 3}, 'repeat');
%     c=filterbank(f,g,a);
%     plotfilterbank(c,a,fc,fs,90);
%
%   In the second example, we construct a wavelet filterbank with a
%   lowpass channels based on a Cauchy wavelet and verify it.
%   The plot shows the frequency responses of
%   filters used for analysis (top) and synthesis (bottom). :::
%
%     [f,fs]=greasy;  % Get the test signal
%     Ls = length(f);
%     M0 = 511; %Desired number of channels (without 0 Hz-lowpass channel)
%     max_freqDiv10 = 10;  % 10 corresponds to the nyquist frequency
%     freq_step = max_freqDiv10/M0;
%     rate = 44100;
%     start_index = 1;
%     min_freqHz = rate/10*freq_step
%     min_scale_freq = min_freqHz*start_index
%     min_freqDiv10 = freq_step*start_index; %1/25; % By default, the reference scale for freqwavelet has center frequency 0.1
%     scales = 1./linspace(min_freqDiv10,max_freqDiv10,M0);
%     alpha = 1-2/(1+sqrt(5)); % 1-1/(goldenratio) delay sequence
%     delays = @(n,a) a*(mod(n*alpha+.5,1)-.5);
%     CauchyAlpha = 600;
%     [g, a,fc,L,info] = waveletfilters(Ls,scales,{'cauchy',CauchyAlpha},'uniform','single','energy', 'delay',delays, 'redtar', 8);
%
%     c=filterbank(f,{'realdual',g},a);
%     r=2*real(ifilterbank(c,g,a));
%     if length(r) > length(f)
%         norm(r(1:length(f))-f)
%     else
%         norm(r-f(1:length(r)))
%      end
%     % Plot frequency responses of individual filters
%     gd=filterbankrealdual(g,a,L);
%     figure(1);
%     subplot(2,1,1);
%     filterbankfreqz(gd,a,L,fs,'plot','linabs','posfreq');
%
%     subplot(2,1,2);
%     filterbankfreqz(g,a,L,fs,'plot','linabs','posfreq');
%
%   See also: freqwavelet, filterbank, setnorm

% AUTHORS: Nicki Holighaus, Zdenek Prusa, Guenther Koliander, Clara Hollomey

complainif_notenoughargs(nargin,2,upper(mfilename));
complainif_notposint(Ls,'Ls',upper(mfilename));

%parse input arguments:
if ~isnumeric(scales)
    fs = varargin{1};
    fmin = varargin{2};
    fmax = varargin{3};
    channels = varargin{4};
    switch scales
        case 'linear'
            definput.flags.inputmode = {'linear', 'logarithmic', 'bins', 'scales'};
        %case 'logarithmic'
        %    definput.flags.inputmode = {'logarithmic', 'bins', 'scales', 'linear'};
        case 'bins'
            definput.flags.inputmode = {'bins', 'scales', 'linear', 'logarithmic'};
        otherwise
            error('%s: second argument must either be a scales vector or define the f-mapping.',upper(mfilename))
    end
    %this is a slightly more efficient way to remove the first 4 args from
    %varargin
    varargin = circshift(varargin,-4);
    varargin = varargin(1:end-4);
else
    definput.flags.inputmode = {'scales', 'linear', 'logarithmic', 'bins'};
end

definput.import={'setnorm'};
definput.importdefaults={'null'};
definput.flags.real = {'real','complex','analytic'};
definput.flags.lowpass  = {'single','repeat','none'};
definput.flags.sampling = {'regsampling','uniform',...
                           'fractional','fractionaluniform'};
definput.flags.wavelettype = getfield(arg_freqwavelet(),'flags','wavelettype');
definput.keyvals.redmul=1;
definput.keyvals.redtar=[];
definput.keyvals.delay = 0;
definput.keyvals.trunc_at  = 10^(-5);
definput.keyvals.fs = 2;
definput.keyvals.startfreq = [];%only relevant if 'repeat'

[varargin,winCell] = arghelper_filterswinparser(definput.flags.wavelettype,varargin);
[flags,kv]=ltfatarghelper({},definput,varargin);

if isempty(winCell), winCell = {flags.wavelettype}; end

if ~isa(kv.delay,'function_handle') && ~isnumeric(kv.delay)
    error('%s: delay must be a function handle or numeric.',upper(mfilename));
end

if ~isscalar(kv.redmul) || kv.redmul <= 0
    error('%s: redmul must be a positive scalar.',upper(mfilename));
end

if ~isempty(kv.redtar)
    if ~isscalar(kv.redtar) || kv.redtar <= 0
        error('%s: redtar must be a positive scalar.',upper(mfilename));
    end
end

%parse the input format: map fmin and fmax to scales according to the input
%parameter specification
if ~flags.do_scales
    nf = fs/2;
    if flags.do_linear
        min_freq = fmin/nf *10;%map to freqwavelets nyquist f
        max_freq = fmax/nf * 10;
        scales = 1./linspace(min_freq,max_freq,channels);
  % elseif flags.do_logarithmic
%
%        fc = 2.^linspace(log2(fmin), log2(fmax), channels);
%        fc = fc/nf * 10;
%        scales = 1./fc;

    elseif flags.do_bins

        if isscalar(channels)
            % Number of octaves
            b = ceil(log2(fmax/fmin))+1;
            bins = channels*ones(b,1);
        else
            bins = channels;
        end

        fc = zeros(sum(bins),1);

        ll = 0;
        for kk = 1:length(bins)
            fc(ll+(1:bins(kk))) = ...
                fmin*2.^(((kk-1)*bins(kk):(kk*bins(kk)-1)).'/bins(kk));
            ll = ll+bins(kk);
        end

        % Get rid of filters with frequency centers >=fmax and nf
        % This will leave the first bigger than fmax it it is lower than nf
        temp = find(fc>=fmax ,1);
        if fc(temp) >= nf
            fc = fc(1:temp-1);
        else
            fc = fc(1:temp);
        end

        channels = length(fc);
        min_freq = fmin/nf *10;%map to freqwavelets nyquist f
        max_freq = fmax/nf * 10;
        scales = 1./linspace(min_freq,max_freq,channels);
    end
    scales_sorted = sort(scales,'descend');
    if ~isempty(kv.startfreq)%set the start frequency
        startfreq = kv.startfreq/nf * 10;
        scales_start = find(1./scales_sorted > startfreq,1,'first');%find first scale whose equiv. f is larger than fmin
        scales = scales(scales_start:end);
    end
end

if ~isnumeric(scales) || any(scales < 0.1)
   error('%s: scales must be positive and numeric.',upper(mfilename));
end

if size(scales,2)>1
    if size(scales,1)==1
        % scales was a row vector.
        scales=scales(:);
    else
        error('%s: scales must be a vector.',upper(mfilename));
    end
end

%% Generate mother wavelet to determine parameters from
[~,info] = freqwavelet(winCell,Ls,1,'asfreqfilter','efsuppthr',kv.trunc_at,'basefc',0.1);
basea = info.aprecise;

%% Determine total number of filters and natural subsampling factor for lowpass
%[aprecise, M, lowpass_number, lowpass_at_zero] = c_det_lowpass(Ls, scales, basea, flags, kv);
if numel(scales) < 4 && flags.do_single
    error('%s: Lowpass generation requires at least 4 scales.',upper(mfilename));
elseif numel(scales) < 2 && flags.do_repeat
    error('%s: Lowpass generation requires at least two scales.',upper(mfilename));
end

% Get number of scales and sort them
M = numel(scales);
scales_sorted = sort(scales,'descend');
%% Determine total number of filters and natural subsampling factor for lowpass
if flags.do_repeat
% Maybe adjust this to not guarantee some distance between first filter and zero frequency.
    lowpass_number = scales_sorted(2)/(scales_sorted(1)-scales_sorted(2));
        if abs(lowpass_number - round(lowpass_number)) < eps*10^3
            % determine if lowpass is centered around 0 Hz
            lowpass_number = round(lowpass_number);
            lowpass_at_zero = 1;
        else
            lowpass_at_zero = 0;
        end
        lowpass_number = floor(lowpass_number);
        if lowpass_number == 0
            lowpass_number = 1;
        end
        M = M + lowpass_number;
        aprecise = (basea.*scales_sorted(1))*ones(lowpass_number,1);

elseif flags.do_single
    lowpass_number = 1;
    lowpass_at_zero = 1;
    M = M+1;
    %this is an estimated value
    aprecise = (0.2./scales_sorted(4))*Ls; % This depends on how single lowpass is called (l.195ff). Maybe automate. Adapt if necessary!!!
else
    lowpass_number = 0;
    lowpass_at_zero = 0;
    aprecise = [];
end

%% Get subsampling factors
aprecise = [aprecise;basea.*scales];

if any(aprecise<1)
    error(['%s: Bandwidth of at least one of the filters is bigger than fs. '],upper(mfilename));
end

aprecise=aprecise/kv.redmul;
if any(aprecise<1)
    error('%s: The maximum redundancy mult. for this setting is %5.2f',...
         upper(mfilename), min(basea./scales));
end

%% Compute the downsampling rate
if flags.do_regsampling
    a = ones(M,1);

    [lower_scale,~] = max(scales);
    [upper_scale,~] = min(scales);
    lower_scale = floor(log2(1/lower_scale));
    upper_scale = floor(log2(1/upper_scale));

    % Find minimum a in each octave and floor23 it
    % to shrink "a" to the next composite number
    ct=1;
    for kk = lower_scale:upper_scale
        tempidx = find( floor(log2(1./scales)) == kk );
        [~,tempminidx] = min(1/scales(tempidx));
        idx = tempidx(tempminidx);

        % Deal the integer subsampling factors
        a(tempidx) = floor23(aprecise(idx));
        ct=ct+1;
    end

    % Determine the minimal transform length lcm(a)
    L = filterbanklength(Ls,a);

    % Heuristic trying to reduce lcm(a)
    while L>2*Ls && ~(all(a==a(1)))
        maxa = max(a);
        a(a==maxa) = 0;
        a(a==0) = max(a);
        L = filterbanklength(Ls,a);
    end

elseif flags.do_fractional
    L = Ls;
    N=ceil(Ls./aprecise);
    a=[repmat(Ls,M,1),N];
elseif flags.do_fractionaluniform
    L = Ls;
    if lowpass_at_zero
        aprecise(2:end)= min(aprecise(2:end));
    else
        aprecise= repmat(min(aprecise),numel(aprecise),1);
    end
    N=ceil(Ls./aprecise);
    a=[repmat(Ls,M,1),N];
elseif flags.do_uniform
    a=floor(min(aprecise));
    L=filterbanklength(Ls,a);
    a = repmat(a,M,1);
end
% Get an expanded "a" / Convert "a" to LTFAT 2-column fractional format
afull=comp_filterbank_a(a,M,struct());

%if flags.do_uniform
%    a = a(:,1);
%end
%==========================================================================
%% Adjust the downsampling rates in order to achieve 'redtar'

if ~isempty(kv.redtar)
   if size(afull,2) == 2
        a = afull(:,1)./afull(:,2);
   else
       a = afull;
   end

    if ~flags.do_real
        org_red = sum(1./a);
    elseif lowpass_at_zero
        org_red = 1./a(1) + sum(2./a(2:end));
    else
        org_red = sum(2./a);
    end

    a = floor(a*org_red/kv.redtar);
    a(a==0) = 1;

    if ~flags.do_uniform
        N_new=ceil(L./a);
        if flags.do_complex
            N_new = [N_new;N_new(end:-1:2)];
        end
        a=[repmat(L,numel(N_new),1),N_new];
    else
        L = filterbanklength(L,a);
        a=[a,ones(length(a), 1)];
    end
else
    a = afull;
end

%% Compute the scaling of the filters and the numeric delay vector
% Filters are scaled such that the energy of the subband coefficients
% remains approximately constant independent of the decimation factor
if isa(kv.delay,'function_handle')
    delayvec = zeros(M,1);
    for kk = 1:M
        delayvec(kk) = kv.delay(kk-1,a(kk,1)./a(kk,2));
    end
elseif numel(kv.delay) == 1
    delayvec = repmat(kv.delay,M,1);
elseif ~isempty(kv.delay) && size(kv.delay,2) > 1
    delayvec = kv.delay(:);
else
    error('%s: delay must be scaler or have enough elements to cover all channels.',upper(mfilename));
end
scal=sqrt(a(:,1)./a(:,2));

if flags.do_complex

    if lowpass_at_zero
       a=[a;flipud(a(2:end,:))];
       scal=[scal;flipud(scal(2:end))];
       delayvec=[delayvec;flipud(delayvec(2:end))];
    else
        a=[a;flipud(a)];
        scal=[scal;flipud(scal)];
        delayvec=[delayvec;flipud(delayvec)];
    end

    [gout_positive,info_positive] = freqwavelet(winCell,L,scales,...
        'asfreqfilter','efsuppthr',kv.trunc_at,'basefc',0.1,...
        'scal',scal(lowpass_number+1:M),'delay', delayvec(lowpass_number+1:M),flags.norm);
    [gout_negative,info_negative] = freqwavelet(winCell,L,-flipud(scales),...
        'asfreqfilter','efsuppthr',kv.trunc_at,'basefc',0.1,...
        'negative','scal',scal(M+1:M+numel(scales)),'delay', delayvec(M+1:M+numel(scales)), flags.norm);
    gout = [gout_positive,gout_negative];
    fields = fieldnames(info_positive);
    info = struct();
    for kk = 1:length(fields)
            info.(fields{kk}) = [info_positive.(fields{kk}),info_negative.(fields{kk})];
    end
elseif flags.do_analytic
    [gout,info] = freqwavelet(winCell,L,scales,...
        'asfreqfilter','efsuppthr',kv.trunc_at,'basefc',0.1,...
        'analytic','scal',scal(lowpass_number+1:M),'delay', delayvec(lowpass_number+1:M),flags.norm);
else
    if lowpass_at_zero
        % Scale the lowpass filters
        scal(1)=scal(1)/sqrt(2);
    end

    [gout,info] = freqwavelet(winCell,L,scales,'asfreqfilter','efsuppthr',...
        kv.trunc_at,'basefc',0.1,'scal',scal(lowpass_number+1:M),'delay', delayvec(lowpass_number+1:M),flags.norm);
end

%% Generate lowpass filters if desired
[gout, info] = comp_fblowpassfilters(winCell, gout, a, L, info, scales, scal, delayvec(1:lowpass_number), lowpass_at_zero, kv, flags);

info.lowpassstart = lowpass_number + 1;%startindex of actual wavelets (tentative)
% Assign fc and adjust for sampling rate
if flags.do_scales
    fc = (kv.fs/2).*info.fc;
else
    fc = nf.*info.fc;
end

if flags.do_uniform || flags.do_regsampling
    a = a(:,1);
end