function [reg_min,G,reg_param,minG] = gcv(U,s,b,method)
%GCV Plot the GCV function and find its minimum.
%
% [reg_min,G,reg_param] = gcv(U,s,b,method)
% [reg_min,G,reg_param] = gcv(U,sm,b,method)  ,  sm = [sigma,mu]
%
% Plots the GCV-function
%          || A*x - b ||^2
%    G = -------------------
%        (trace(I - A*A_I)^2
% as a function of the regularization parameter reg_param.
% Here, A_I is a matrix which produces the regularized solution.
%
% The following methods are allowed:
%    method = 'Tikh' : Tikhonov regularization   (solid line )
%    method = 'tsvd' : truncated SVD or GSVD     (o markers  )
%    method = 'dsvd' : damped SVD or GSVD        (dotted line)
% If method is not specified, 'Tikh' is default.
%
% If any output arguments are specified, then the minimum of G is
% identified and the corresponding reg. parameter reg_min is returned.

% Per Christian Hansen, IMM, Dec. 16, 2003.

% Reference: G. Wahba, "Spline Models for Observational Data",
% SIAM, 1990.

% Set defaults.
if (nargin==3), method='Tikh'; end  % Default method.
npoints = 200;                      % Number of points on the curve.
smin_ratio = 16*eps;                % Smallest regularization parameter.

% Initialization.
[m,n] = size(U); [p,ps] = size(s);
beta = U'*b; beta2 = norm(b)^2 - norm(beta)^2;
if (ps==2)
  s = s(p:-1:1,1)./s(p:-1:1,2); beta = beta(p:-1:1);
end
if (nargout > 0), find_min = 1; else find_min = 0; end

if (strncmp(method,'Tikh',4) | strncmp(method,'tikh',4))

  % Vector of regularization parameters.
  reg_param = zeros(npoints,1); G = reg_param; s2 = s.^2;
  reg_param(npoints) = max([s(p),s(1)*smin_ratio]);
  ratio = (s(1)/reg_param(npoints))^(1/(npoints-1));
  for i=npoints-1:-1:1, reg_param(i) = ratio*reg_param(i+1); end

  % Intrinsic residual.
  delta0 = 0;
  if (m > n && beta2 > 0), delta0 = beta2; end

  % Vector of GCV-function values.
  for i=1:npoints
    G(i) = gcvfun(reg_param(i),s2,beta(1:p),delta0,m-n);
  end

  if nargout == 0
      % Plot GCV function.
      loglog(reg_param,G,'-'), xlabel('\lambda'), ylabel('G(\lambda)')
      title('GCV function')
  end

  % Find minimum, if requested.
  if (find_min)
    [minG,minGi] = min(G); % Initial guess.
    reg_min = fminbnd('gcvfun',...
      reg_param(min(minGi+1,npoints)),reg_param(max(minGi-1,1)),...
      optimset('Display','off'),s2,beta(1:p),delta0,m-n); % Minimizer.
    minG = gcvfun(reg_min,s2,beta(1:p),delta0,m-n); % Minimum of GCV function.
    if nargout == 0
        ax = axis;
        HoldState = ishold; hold on;
        loglog(reg_min,minG,'*r',[reg_min,reg_min],[minG/1000,minG],':r')
        title(['GCV function, minimum at \lambda = ',num2str(reg_min)])
        axis(ax)
        if (~HoldState), hold off; end
    end
  end

elseif (strncmp(method,'tsvd',4) | strncmp(method,'tgsv',4))

  % Vector of GCV-function values.
  rho2(p-1) = abs(beta(p))^2;
  if (m > n & beta2 > 0), rho2(p-1) = rho2(p-1) + beta2; end
  for k=p-2:-1:1, rho2(k) = rho2(k+1) + abs(beta(k+1))^2; end
  G = zeros(p-1,1);
  for k=1:p-1
    G(k) = rho2(k)/(m - k + (n - p))^2;
  end
  reg_param = (1:p-1)';

  % Plot GCV function.
  semilogy(reg_param,G,'o'), xlabel('k'), ylabel('G(k)')
  title('GCV function')

  % Find minimum, if requested.
  if (find_min)
    [minG,reg_min] = min(G);
    ax = axis;
    HoldState = ishold; hold on;
    semilogy(reg_min,minG,'*r',[reg_min,reg_min],[minG/1000,minG],':r')
    title(['GCV function, minimum at k = ',num2str(reg_min)])
    axis(ax);
    if (~HoldState), hold off; end
  end

elseif (strncmp(method,'dsvd',4) | strncmp(method,'dgsv',4))

  % Vector of regularization parameters.
  reg_param = zeros(npoints,1); G = reg_param;
  reg_param(npoints) = max([s(p),s(1)*smin_ratio]);
  ratio = (s(1)/reg_param(npoints))^(1/(npoints-1));
  for i=npoints-1:-1:1, reg_param(i) = ratio*reg_param(i+1); end

  % Intrinsic residual.
  delta0 = 0;
  if (m > n & beta2 > 0), delta0 = beta2; end

  % Vector of GCV-function values.
  for i=1:npoints
    G(i) = gcvfun(reg_param(i),s,beta(1:p),delta0,m-n,1);
  end

  % Plot GCV function.
  loglog(reg_param,G,':'), xlabel('\lambda'), ylabel('G(\lambda)')
  title('GCV function')

  % Find minimum, if requested.
  if (find_min)
    [minG,minGi] = min(G); % Initial guess.
    reg_min = fminbnd('gcvfun',...
      reg_param(min(minGi+1,npoints)),reg_param(max(minGi-1,1)),...
      optimset('Display','off'),s,beta(1:p),delta0,m-n,1); % Minimizer.
    minG = gcvfun(reg_min,s,beta(1:p),delta0,m-n,1); % Minimum of GCV function.
    ax = axis;
    HoldState = ishold; hold on;
    loglog(reg_min,minG,'*r',[reg_min,reg_min],[minG/1000,minG],':r')
    title(['GCV function, minimum at \lambda = ',num2str(reg_min)])
    axis(ax)
    if (~HoldState), hold off; end
  end

elseif (strncmp(method,'mtsv',4) | strncmp(method,'ttls',4))

  error('The MTSVD and TTLS methods are not supported')

else
  error('Illegal method')
end


function linpack_s_test03 ( )

%*****************************************************************************80
%
%% TEST03 tests SCHUD.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 June 2009
%
%  Author:
%
%    John Burkardt
%
  p = 20;
  ldr = p;
  nz = 1;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST03\n' );
  fprintf ( 1, '  For single precision, general storage,\n' );
  fprintf ( 1, '  SCHUD updates a Cholesky decomposition.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  In this example, we use SCHUD to solve a\n' );
  fprintf ( 1, '  least squares problem R * b = z.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The number of equations is P = %d\n', p );
%
%  Initialize.
%
  r(1:p,1:p) = 0.0;
  z(1:p,1:nz) = 0.0;
  for i = 1 : p
    x(i) = i;
  end
%
%  Use SCHUD to form R, Z and RHO by adding X and Y a row at a time.
%  X is a row of the least squares matrix and Y the right hand side.
%
  seed = 123456789;

  for i = 1 : p
    [ row, seed ] = r4mat_uniform_01 ( 1, p, seed );
    y(1) = row(1:p) * x(1:p)';
    rho(1) = 0.0;
    [ r, z, rho, c, s ] = schud ( r, ldr, p, row, z, p, nz, y, rho );
  end
%
%  Generate the least squares solution, b = inverse ( R ) * Z.
%
  for j = 1 : nz

    b(1:p,1) = z(1:p,j);
    job = 01;

    [ b, info ] = strsl ( r, ldr, p, b, job );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Solution vector # %d\n', j );
    fprintf ( 1, '  (Should be (1,2,3...,n))\n' );
    fprintf ( 1, '\n' );

    for i = 1 : p
      if ( i <= 5 | p-5 < i )
        fprintf ( 1, '  %6d  %14f\n', i, b(i,1) );
      end
      if ( i == 5 )
        fprintf ( 1, '  ......  ..............\n' );
      end
    end

  end

  return
end


function [ b, info ] = cgtsl ( n, c, d, e, b )

%*****************************************************************************80
%
%% CGTSL solves a complex general tridiagonal system.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 April 2007
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, complex C(N); the subdiagonal of the matrix in entries C(2:N).
%
%    Input, complex D(N); the diagonal of the tridiagonal matrix.
%
%    Input, complex E(N), the superdiagonal of the matrix in entries E(1:N-1).
%
%    Input, complex B(N), the right hand side.
%
%    Output, complex B(N).  the solution.
%
%    Output, integer INFO.
%    0, normal value.
%    K, if the K-th element of the diagonal becomes exactly zero.  The
%    subroutine returns when this is detected.
%
  info = 0;
  c(1) = d(1);

  if ( 1 <= n-1 )

    d(1) = e(1);
    e(1) = 0.0;
    e(n) = 0.0;

    for k = 1 : n-1

      if ( cabs1 ( c(k) ) <= cabs1 ( c(k+1) ) )

        t      = b(k);
        b(k)   = b(k+1);
        b(k+1) = t;

        t      = c(k);
        c(k)   = c(k+1);
        c(k+1) = t;

        t      = d(k);
        d(k)   = d(k+1);
        d(k+1) = t;

        t      = e(k);
        e(k)   = e(k+1);
        e(k+1) = t;

      end

      if ( cabs1 ( c(k) ) == 0.0 )
        info = k;
        return
      end

      t = -c(k+1) / c(k);
      c(k+1) = d(k+1) + t * d(k);
      d(k+1) = e(k+1) + t * e(k);
      e(k+1) = 0.0;
      b(k+1) = b(k+1) + t * b(k);

    end

  end

  if ( cabs1 ( c(n) ) == 0.0 )
    info = n;
    return
  end
%
%  Back solve.
%
  b(n) = b(n) / c(n);

  if ( 1 < n )

    b(n-1) = ( b(n-1) - d(n-1) * b(n) ) / c(n-1);

    for k = n-2 : -1 : 1
      b(k) = ( b(k) - d(k) * b(k+1) - e(k) * b(k+2) ) / c(k);
    end

  end

  return
end


function [dat] = ft_preproc_derivative(dat, order)

% FT_PREPROC_DERIVATIVE computes the temporal Nth order derivative of the
% data
%
% Use as
%   [dat] = ft_preproc_derivative(dat, order)
% where
%   dat        data matrix (Nchans X Ntime)
%   order      number representing the Nth derivative (default = 1)
%
% See also PREPROC

% Copyright (C) 2008, Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    FieldTrip is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

% set the defaults if options are not specified
if nargin<2 || isempty(order)
  order = 1;
end

% preprocessing fails on channels that contain NaN
if any(isnan(dat(:)))
  ft_warning('FieldTrip:dataContainsNaN', 'data contains NaN values');
end

% compute the derivative
for i=1:order
  dat = gradient(dat);
end


 function [xs, ni] = tml_sps(x, Gt, yi, bi, ri, niter, pixmax, curv)
%function [xs, ni] = tml_sps(x, Gt, yi, bi, ri, niter, pixmax, curv)
%	One iteration of the ML-SPS algorithm for transmission Poisson problem
%	(separable paraboloidal surrogates)
%	model: Y_i ~ Poisson(b_i exp(-[G x]_i) + r_i)
%	Input
%		x	[np,1]	initial guess
%		Gt		transpose of system matrix
%		yi		transmission sinogram
%		bi		blank scan factors
%		ri		background (randoms, scatter, crosstalk, etc)
%		bi,ri:		optional (can use empty matrices)
%		yi,bi,ri	must have identical dimensions
%		pixmax		upper constraint for pixel values
%			can be scalar (e.g. 'inf') or an array the size of x
%		curv		'oc' for erdogan's optimal curvatures
%				'pc' for erdogan's fast precomputed curvatures,
%					which usually gives faster convergence,
%					but can be nonmonotone
%				'nr' newton curvatures, can be nonmonotone
%	Output
%		x [np,niter]	updated image vectors each iteration
%
%	fix: the really slick way to do this would be to use
%	the fast precomputed denominator and just backtrack to the monotone
%	version on those rare occasions when it goes downhill
%
%	Copyright 2000-3-01	Jeff Fessler	The University of Michigan

if nargin < 3, ir_usage, end

[nb, na] = size(yi);

if (nargin < 4 || isempty(bi))
	bi = ones(size(yi));
end
if (nargin < 5 || isempty(ri))
	ri = zeros(size(yi));
end
if (nargin < 6 || isempty(niter))
	niter = 2;
end
if (nargin < 7 || isempty(pixmax))
	pixmax = inf;
end
if (nargin < 8 || isempty(curv))
	curv = 'oc';
end

trl_check(yi, bi, ri);

	gi = sum(Gt)';	% g_i = sum_j g_ij

	if strcmp(curv, 'pc')
		ni = trl_curvature_pre(yi, bi, ri);	% precomputed
		denom = Gt * (gi .* ni(:));
	elseif strcmp(curv, 'nr')
		warning 'newton curvatures can be non-monotone'
	elseif ~strcmp(curv, 'oc')
		error 'curv not implemented'
	end

xs = zeros(length(x), niter);
x = max(x,0);
x = min(x,pixmax);
xs(:,1) = x;

%
%	loop over iterations
%
for ii=2:niter
	li = reshape(Gt' * x, size(yi));	% l=G*x "line integrals"
	bel = bi .* exp(-li);
	yb = bel + ri;			% predicted measurement means

	dothi = (1 - yi ./ yb) .* bel;

	if strcmp(curv, 'oc')
		%	optimal curvatures (for ensured monotone increase)
		ni = trl_curvature(yi, bi, ri, li, 'oc');
		denom = Gt * (gi .* ni(:));
	elseif strcmp(curv, 'nr')
		ni = (1 - ri.*yi./yb.^2) .* bel;
		denom = Gt * (gi .* ni(:));
	end

	x = x + (Gt * dothi(:)) ./ denom;	% there's the update!
	x = max(x,0);				% enforce nonnegativity
	x = min(x,pixmax);			% enforce upper bound constraint

	xs(:,ii) = x;
end


%% Copyright (C) 2003 Willem J. Atsma
%% Copyright (C) 2014-2016, 2019 Colin B. Macdonald
%%
%% This program is free software; you can redistribute it and/or
%% modify it under the terms of the GNU General Public
%% License as published by the Free Software Foundation;
%% either version 3, or (at your option) any later version.
%%
%% This software is distributed in the hope that it will be useful,
%% but WITHOUT ANY WARRANTY; without even the implied
%% warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
%% PURPOSE.  See the GNU General Public License for more
%% details.
%%
%% You should have received a copy of the GNU General Public
%% License along with this software; see the file COPYING.  If not,
%% see <http://www.gnu.org/licenses/>.

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @deftypemethod  @@sym {@var{doublec} =} sym2poly (@var{p})
%% @deftypemethodx @@sym {@var{c} =} sym2poly (@var{p}, @var{x})
%% Return vector of coefficients of a symbolic polynomial.
%%
%% In the two-input form, the second argument @var{x} specifies the free
%% variable; in this case this function returns a row vector @var{c} of
%% symbolic expressions. The coefficients correspond to decreasing exponent
%% of the free variable.  Example:
%% @example
%% @group
%% syms x y
%% sym2poly(2*x^2 + 3*x - pi, x)
%%    @result{} (sym) [2  3  -π]  (1×3 matrix)
%% sym2poly(x^2 + y*x, x)
%%    @result{} (sym) [1  y  0]  (1×3 matrix)
%% @end group
%% @end example
%%
%% @strong{Warning}: Using the single-argument form, the coefficient vector
%% @var{c} is a plain numeric vector (double).  This is for compatibility
%% with the Matlab Symbolic Math Toolbox.
%% We suggest making this clear in your code by explicitly casting to @code{double},
%% as in:
%% @example
%% @group
%% syms x
%% @c doctest: +SKIP_IF(compare_versions (OCTAVE_VERSION(), '6.0.0', '<'))
%% double(sym2poly(pi*x^3 + 3*x/2 + exp(sym(1))))
%%    @result{}     3.1416      0   1.5000   2.7183
%% @end group
%% @end example
%% You may prefer specifying @var{X} or using @code{coeffs}:
%% @example
%% @group
%% coeffs(pi*x^3 + 3*x/2 + exp(sym(1)), 'all')
%%    @result{} (sym) [π  0  3/2  ℯ]  (1×4 matrix)
%% @end group
%% @end example
%%
%% If @var{p} is not a polynomial the result has no warranty.  SymPy can
%% certainly deal with more general concepts of polynomial but we do not
%% yet expose all of that here.
%%
%% @seealso{poly2sym, @@sym/coeffs, polyval, roots}
%% @end deftypemethod

%% Created: 18 April 2003
%% Changed: 25 April 2003
%%    Removed the use of differentiate to get to coefficients - round-off
%%     errors cause problems. Now using newly created sumterms().
%% Changed: 6 May 2003
%%    Removed the attempt to use ldegree(), degree() and coeff() - results
%%     with these are inconsistent.
%% Changed: 16 April 2014
%%    Used the comment header and tests in OctSymPy, but rewrote
%%    the body (by Colin Macdonald).

function c = sym2poly(p, x)

  if ~(isscalar(p))
    error ('sym2poly: works for scalar input only');
  end

  if (nargin == 1)
    ss = findsymbols(p);
    if (length (ss) >= 2)
      error ('sym2poly: input has more than one symbol: not clear what you want me to do')
    elseif (length (ss) == 1)
      x = ss{1};
    else
      x = sym('x');
    end
    convert_to_double = true;
  elseif (nargin == 2)
    convert_to_double = false;
  else
    print_usage ();
  end

  cmd = { 'f = _ins[0]'
          'x = _ins[1]'
          'p = Poly.from_expr(f,x)'
          'return p.all_coeffs(),' };

  c2 = pycall_sympy__ (cmd, sym(p), sym(x));
  if (isempty(c2))
    error ('sym2poly: empty python output, can this happen?  A bug.')
  end

  % FIXME: should be able to convert c2 to array faster than array
  % expansion!  Particularly in the case where we just convert to
  % double anyway!
  c = sym([]);
  for j = 1:numel(c2)
    % Bug #17
    %c(j) = c2{j};
    idx.type = '()'; idx.subs = {j};
    c = subsasgn(c, idx, c2{j});
  end

  if (convert_to_double)
    c = double(c);
  end

end

%!shared x,y,a,b,c
%! syms x y a b c
%!assert (isequal (sym2poly (x^2 + 3*x - 4), [1 3 -4]))
%!assert (isequal (sym2poly (x^6 - x^3), [1 0 0 -1 0 0 0]))
%!assert (isequal (sym2poly (x^2 + 3*x - 4, x), [1 3 -4]))
%!assert (norm (sym2poly (pi*x^2 + exp(sym(1))) - [pi 0 exp(1)]) < 10*eps)
%% types
%!assert (isa (sym2poly (x^2 + 3*x - 4), 'double'))
%!assert (isa (sym2poly (x^2 + 3*x - 4, x), 'sym'))
%% tests with other vars
%!assert (isequal (sym2poly (x^2+y*x, x), [sym(1) y sym(0)]))
%!assert (isequal (sym2poly (x^2+y*x, y), [x x^2]))
%% inverse relationship
%!assert (isequal (sym2poly (poly2sym ([a b c], x), x), [a b c]))
%!assert (isequal (poly2sym (sym2poly(a*x^2 + c, x), x), a*x^2 + c))
%!assert (isequal (sym2poly (poly2sym ([1 2 3])), [1 2 3]))

%!error <more than one symbol>
%! % too many symbols for single-input
%! p = a*x^2 + 2;
%! c = sym2poly (p);

%!assert (isequal (sym2poly (sym(5)), sym(5)))


function [satp] = satposin(t,eph)
%  SATPOSIN   Calculation of X,Y,Z coordinates in an INERTIAL
%	      reference frame at time t for given ephemeris eph

% Written by Kai Borre
% November 15, 1996

   GM = 3.986008e14;		% earth's universal gravitational parameter
				% m^3/s^2

   %  Units are either seconds, meters, or radians
   %  Assigning the local variables to eph
   svprn   =   eph(1);
   af2	   =   eph(2);
   M0	   =   eph(3);
   roota   =   eph(4);
   deltan  =   eph(5);
   ecc	   =   eph(6);
   omega   =   eph(7);
   cuc	   =   eph(8);
   cus	   =   eph(9);
   crc	   =  eph(10);
   crs	   =  eph(11);
   i0	   =  eph(12);
   idot    =  eph(13);
   cic	   =  eph(14);
   cis	   =  eph(15);
   Omega0  =  eph(16);
   Omegadot=  eph(17);
   toe	   =  eph(18);
   af0	   =  eph(19);
   af1	   =  eph(20);
   t0c	   =  eph(21);

   % Procedure for coordinate calculation
   A = roota*roota;
   t = check_t(t-t0c);
   n0 = sqrt(GM/A^3);
   n = n0+deltan;
   M = M0+n*t;
   M = rem(M,2*pi);
   E = M;
   E_old = E+.001;
   while abs(E-E_old) >= 1.e-12
      E_old = E;
      E = M+ecc*sin(E);
   end
   v = atan2(sqrt(1-ecc^2)*sin(E), cos(E)-ecc);
   phi = v+omega;
   phi = rem(phi,2*pi);
   u = phi		      + cuc*cos(2*phi)+cus*sin(2*phi);
   r = A*(1-ecc*cos(E))       + crc*cos(2*phi)+crs*sin(2*phi);
   i = i0+idot*t	      + cic*cos(2*phi)+cis*sin(2*phi);
   Omega = Omega0+Omegadot*t;
   x1 = cos(u)*r;
   y1 = sin(u)*r;
   satp(1,1) = x1*cos(Omega)-y1*cos(i)*sin(Omega);
   satp(2,1) = x1*sin(Omega)+y1*cos(i)*cos(Omega);
   satp(3,1) = y1*sin(i);

%%%%%%%%% end satposin.m %%%%%%%%%


function [logbf] = spm_mlm_posthoc (mlm,c,a)
% Post-hoc model comparison of multivariate linear models
% FORMAT [logbf] = spm_mlm_posthoc (mlm,c,a)
%
% mlm          MLM data structure - see spm_mlm_bayes.m
%              This contains eg. the [p x d] posterior mean regression
%              coefficient matrix mlm.wmean.
%
% c            [k x p*d] contrast matrix defining k-dimensional subspace
% a            hypothesized value (zeros(k,1) by default)
%
% The contrast matrix and hypothesized value define the reduced model.
% The contrast is applied to the vectorised parameters w = vec(mlm.wmean)
%
% The Bayes Factor in favour of the alternative hypothesis over the null
% is computed using a Savage-Dickey ratio (the probability of the
% hypothesized value under the prior versus its probability under the
% posterior)
%
% bf = p(c*w=a|mlm)/p(c*w=a|Y,mlm)
%
% logbf        Log Bayes Factor
%___________________________________________________________________________
% Copyright (C) 2011 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_mlm_posthoc.m 4651 2012-02-09 16:03:39Z will $

wpost=mlm.wmean(:);

post_mean=c*wpost;
k=size(post_mean,1);
prior_mean=zeros(k,1);

post_cov=c*mlm.wcov*c';

% Get prior covariance matrix
prec_prior=size(mlm.wcov,1);
for g=1:mlm.prior.groups,
    prec_prior=prec_prior+mlm.prior.group(g).mean_alpha*mlm.prior.group(g).index;
end
w_prior_cov=diag(1./prec_prior);
prior_cov=c*w_prior_cov*c';

if nargin < 3 | isempty (a)
    a=zeros(k,1);
end

% Enforce symmetry - to avoid numerical issues
prior_cov=(prior_cov+prior_cov')/2;
post_cov=(post_cov+post_cov')/2;

% For now just use spm_mvNpdf. This can be made more efficient !
prior_prob=spm_mvNpdf(a,prior_mean,prior_cov);
post_prob=spm_mvNpdf(a,post_mean,post_cov);

logbf=log(prior_prob)-log(post_prob);


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

function phi = cf_merton(u,lnS,T,r,d,sigma,a,b,lambda)
% characteristic function for the Merton model
    phi = exp(cf_black(u,lnS,T,r,d,sigma) ...
        + cf_lognormjump(u,a,b,lambda,T));
end


function [connectivity, pValues, freq] = bst_granger_spectral(X, Y, Fs, order, inputs)
% BST_GRANGER_SPECTRAL  Granger causality at each frequency between any two
%                       signals.
%
% Inputs:
%   X                 - first set of signals, one signal per row
%                       [X: A x N or A x N x T matrix]
%   Y                 - second set of signals, one signal per row
%                       [Y: B x N or B x N x T matrix]
%                       (default: Y = X)
%   Fs                - sampling rate (we assume uniform sampling rate)
%                       [FS: scalar, FS > freq(end)*2]
%   order             - maximum order of autogressive model
%                       [p: integer > 1, default 10]
%   inputs            - structure of parameters:
%   |-freq            - frequencies of interest if desired
%   |-freqResolution  - maximum freq resolution in Hz, to limit NFFT
%   |                   [DF: double, default [] (i.e. no limit)]
%   |-nTrials         - # of trials in concantenated signal
%   |-flagFPE         - if true, optimize order for autoregression
%   |                   if false (default), use same order in autoregression
%   |-standardize     - if true (default), remove mean from each signal
%   |                   if false, assume signal has already been detrended
%
% Outputs:
%   connectivity      - A x B matrix of spectral Granger causalities from
%                       source to sink. For each signal pair (a,b) we calculate
%
%                                        S_{sink} (f)
%                  ------------------------------------------------------------
%                  S_{sink}(f) - |H_{sink, source} (f)|^2 sigma_{source | sink}
%
%                       with S_{sink}(f) as the power spectral density of a @ f
%                            H_{sink, source}(f) as the transfer function @ f
%                            sigma_{source | sink} as the conditional variance
%                             of the residual at b given the residual at a,
%                             calculated using the residual covariance.
%                       By default, GC(a,a) = 0 if Y is empty.
%                       [C: MX x MY x NF matrix]
%   pValues           - parametric p-value for corresponding spectral Granger
%                       causality in mean estimate
%                       [P: MX x MY x NF matrix]
%   freq              - frequencies corresponding to the previous two metrics
%                       [F: NF x 1 vector]
%
% See also BST_GRANGER, BST_COHERENCE_MVAR.
%
% Call:
%   connectivity = bst_granger_spectral(X, Y, 200, 10, inputs); % general call
%   connectivity = bst_granger_spectral(X, [], 200, 30, inputs); % every pair
%   connectivity = bst_granger_spectral(X, [], 200, 30, inputs); % more variance
% Parameter examples:
%   inputs.freq           = 0:0.1:100; % specify desired frequencies
%   inputs.freqResolution = 0.1; % have a high-point FFT
%   inputs.nTrials        = 9; % use trial-average covariances in AR estimation
%   inputs.flagFPE        = true; % use AR model with best information criteria

% Note: for those following Chicharro2012, I have used the equivalence
%                       sigma_{xx}^(xy) |H_{xx}^(xy) (w)|^2
%                                       =
%                S_{xx}(w) - sigma_{yy}^(xy) |H_{xy}^(xy) (w)|^2
% which follows Geweke1982 instead. As Chicharro notes, it may be better to use his
% formulation because it separates out instantaneous causality I think.

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@
%
% Authors: Sergul Aydore & Syed Ashrafulla, 2012

% reformat to a 2D matrix if necessary, and pull out # of trials
if ndims(X) == 3
  inputs.nTrials = size(X,3);
  X = reshape(X, size(X, 1), []);
elseif ~isfield(inputs, 'nTrials')
  inputs.nTrials = 1;
end

% lengths of things
nSamples = size(X, 2);
nTimes = nSamples / inputs.nTrials;

% standardization: zero mean & unit variance, remove linear & quadratic trends as well
if isfield(inputs, 'standardize') && inputs.standardize
  detrender = [ ...
    ones(1, nTimes); ... % constant trend in data
    linspace(-1, 1, nTimes); ... % linear trend in data
    3/2 * linspace(-1, 1, nTimes).^2 - 1/2 ... % quadratic trend in data
  ];

  % detrend X
  for iTrial = 1:inputs.nTrials
    X(:, (iTrial-1)*nTimes + (1:nTimes)) = X(:, (iTrial-1)*nTimes + (1:nTimes)) - ( X(:, (iTrial-1)*nTimes + (1:nTimes)) / detrender ) * detrender;
    X(:, (iTrial-1)*nTimes + (1:nTimes)) = diag( sqrt(sum(X(:, (iTrial-1)*nTimes + (1:nTimes)).^2, 2)) ) \ X(:, (iTrial-1)*nTimes + (1:nTimes));
  end

  % detrend Y only if it is not empty
  if ~isempty(Y)
    Y = reshape(Y, size(Y, 1), []); % reshape to 2D matrix first
    for iTrial = 1:inputs.nTrials
      Y(:, (iTrial-1)*nTimes + (1:nTimes)) = Y(:, (iTrial-1)*nTimes + (1:nTimes)) - ( Y(:, (iTrial-1)*nTimes + (1:nTimes)) / detrender ) * detrender;
      Y(:, (iTrial-1)*nTimes + (1:nTimes)) = diag( sqrt(sum(Y(:, (iTrial-1)*nTimes + (1:nTimes)).^2, 2)) ) \ Y(:, (iTrial-1)*nTimes + (1:nTimes));
    end
  end
end

% number of FFT bins required
if ~isfield(inputs, 'freq') || isempty(inputs.freq) % frequencies of interest are not defined
  if isfield(inputs, 'freqResolution') && ~isempty(inputs.freqResolution) && (size(X,2) > round(Fs / inputs.freqResolution))
    nFFT = 2^nextpow2( round(Fs / inputs.freqResolution) );
  else % use a default frequency resolution of 1Hz to mirror the standard frequency resolution in bst_coherence_welch.m
    nFFT = 2^nextpow2( round(Fs / 1) );
  end
elseif numel(inputs.freq) == 1
  nFFT = inputs.freq;
else
  nFFT = 2^nextpow2(max( length(inputs.freq)-1, (Fs/2) / min(diff( sort(inputs.freq(:)) )) )) * 2;
end

% default: Order 10 used in BrainStorm
if isempty(order)
  order = 10;
end

% default: Single-trial
if ~isfield(inputs, 'nTrials') || isempty(inputs.nTrials)
  inputs.nTrials = 1;
end

% default: do not optimize model order
if ~isfield(inputs, 'flagFPE') || isempty(inputs.flagFPE)
  inputs.flagFPE = false;
end

%% Differentiate between auto-causality and cross-causality for speed

if isempty(Y) % auto-causality between signals in X, so we can halve the number of models to estimate

  % preallocate the spectral causality matrix
  connectivity = zeros(size(X, 1), size(X, 1), nFFT/2);

  % iterate over all pairs of sinks & sources
  for iX = 1:size(X, 1)
    for iY = iX+1 : size(X, 1) % to avoid auto-causality

        % two-variate model for given source
        [transfers, noiseCovariance, order] = bst_mvar([X(iX, :); X(iY, :)], order, inputs.nTrials, inputs.flagFPE);

        % spectra and power of forward system
        [spectra, freq, forward] = bst_granger_spectral_spectrum(transfers, noiseCovariance, nFFT, Fs);

        % Geweke-Granger spectral causality from source to sink
        unrestricted = abs(spectra(1, 1, :)); restriction = forward(1, 2, :); % S_{sink}(f) and |H_{sink, source} (f)|^2, w/ abs to get rid of 1e-16 imag part
        residualVariance = noiseCovariance(2,2) - noiseCovariance(2, 1) / noiseCovariance(1, 1) * noiseCovariance(1, 2); % partial variance of source
        restricted = abs(unrestricted) - abs(restriction).^2 * residualVariance; % S_{sink} (f) - |H_{sink, source} (f)|^2 sigma_{source | sink}
        connectivity(iX, iY, abs(restricted) > 1e-60) = unrestricted(abs(restricted) > 1e-60) ./ restricted(abs(restricted) > 1e-60) - 1;% Geweke-Granger
        % sigma_{source | sink} = partial covariance which is the formula above (sigma_{source} - rho_{source, sink} / sigma_{sink} * rho_{sink, source})

        % Geweke-Granger spectral causality from sink back to source (to halve the # of MVAR fittings)
        unrestricted = abs(spectra(2, 2, :)); restriction = forward(2, 1, :); % S_{source}(f) and |H_{source, sink} (f)|^2, w/ abs to get rid of 1e-16 imag part
        residualVariance = noiseCovariance(1,1) - noiseCovariance(1, 2) / noiseCovariance(2, 2) * noiseCovariance(2, 1); % partial variance of sink
        restricted = abs(unrestricted) - abs(restriction).^2 * residualVariance; % S_{source} (f) - |H_{source, sink} (f)|^2 sigma_{sink | source}
        connectivity(iY, iX, abs(restricted) > 1e-60) = unrestricted(abs(restricted) > 1e-60) ./ restricted(abs(restricted) > 1e-60) - 1; % Geweke-Granger
        % sigma_{sink | source} = partial covariance which is the formula above (sigma_{sink} - rho_{sink, source} / sigma_{source} * rho_{source, sink})

    end

    % diagonal will equal the maximum of all inflows and outflows for iX, specific to each frequency
    connectivity(iX, iX, :) = max( max(connectivity(iX, :, :), [], 2), max(connectivity(:, iX, :), [], 1) );

  end

else % we have to use all pairs of signals

  % preallocate the spectral causality matrix
  connectivity = zeros(size(X, 1), size(Y, 1), nFFT/2);
  duplicates = zeros(0, 2);

  % iterate over all pairs of sinks & sources
  for iX = 1:size(X, 1)
    for iY = 1:size(Y, 1)

      if max(abs(X(iX, :) - Y(iY, :))) > eps % by default, if X(sink) = Y(source), the causality is 0 everywhere

        % 2-variate model for given source
        [transfers, noiseCovariance, order] = bst_mvar([X(iX, :); Y(iY, :)], order, inputs.nTrials, inputs.flagFPE);

        % spectra and power of forward system
        [spectra, freq, forward] = bst_granger_spectral_spectrum(transfers, noiseCovariance, nFFT, Fs);
        spectra = abs(spectra(1, 1, :)); % limit to the autospectrum of the sink S_{sink} (f) and take absolute value to rid the 1e-16 imaginary part
        forward = forward(1, 2, :); % limit to the cross-transfer from source to sink H_{sink, source} (f)

        % partial covariance of residual
        residualVariance = noiseCovariance(2,2) - noiseCovariance(2,1) / noiseCovariance(1, 1) * noiseCovariance(1, 2);
        % sigma_{source | sink} = partial covariance which is the formula above (sigma_{source} - rho_{source, sink} / sigma_{sink} * rho_{sink, source})

        % Geweke-Granger spectral causality from source to sink
        restricted = spectra - abs(forward).^2 * residualVariance; % S_{sink} (f) - |H_{sink, source} (f)|^2 sigma_{source | sink}
        connectivity(iX, iY, abs(restricted) > 1e-60) = spectra(abs(restricted) > 1e-60) ./ restricted(abs(restricted) > 1e-60) - 1; % Geweke-Granger

      else % save duplicates to modify later

        duplicates(end+1, :) = [iX iY]; %#ok<AGROW>

      end

    end
  end

  % for duplicate indices, set the causality value to the maximum of all inflows for iX and outflows for iY
  for iDuplicate = 1:size(duplicates, 1)
    connectivity(duplicates(iDuplicate, 1), duplicates(iDuplicate, 2), :) = max( ...
      max(connectivity(duplicates(iDuplicate, 1), :, :), [], 2), ...
      max(connectivity(:, duplicates(iDuplicate, 2), :), [], 1) ...
      );
  end

end

%% Interpolate to desired frequencies & perform statistics if desired
if isfield(inputs, 'freq') && ~isempty(inputs.freq) && numel(inputs.freq) > 1
  connectivity = permute(interp1(freq, permute(connectivity, [3 1 2]), inputs.freq), [2 3 1]);
  % pValues = permute(interp1(freq, permute(pValues, [3 1 2]), inputs.freq), [2 3 1]);
  freq = inputs.freq;
end

pValues = NaN; % no parametric p-values for now

end

%% ======================================================== estimation for multivariate autoregression ========================================================
function [spectra, freq, forward] = bst_granger_spectral_spectrum(transfers, noiseCovariance, nFFT, Fs)
% BST_MVAR_SPECTRUM     Calculate the parametric spectra of a bivariate system
%                       with given MVAR coefficients & an estimate of the
%                       covariance matrix of the innovation (i.e. noise)
%                       process.
%
% Inputs:
%   transfers         - transfer matrices in AR process
%                       [A: 2 x 2P matrix, P = order]
%   noiseCovariance   - variance of residuals
%                       [C: 2 x 2 matrix]
%   nFFT              - number of FFT bins to calculate spectra
%                       [NF: positive number, usually power of 2]
%   Fs                - sampling frequency of data
%                       [FS: double, FS > freq(end)*2]
%
% Outputs:
%   spectra           - cross-spectrum between each pair of variables
%                       [S: 2 x 2 x NF/2 matrix]
%   freq              - frequencies used based on # of FFT bins
%                       [F: NF/2 x 1 matrix]
%   forward           - forward transform in frequency from source to sink
%                       [H: 2 x 2 x NF/2 matrix]
%   --> all outputs have length NF/2, ignoring frequenices past Fs/2
%
% Call:
%   spectra = bst_mvar_spectrum(transfers, C, 512, 200); % basic
%   spectra = bst_mvar_spectrum(transfers, C, 2048, 200); % add FFT interp
%   [spectra, freq] = bst_mvar_spectrum(transfers, C, 512, 200); % grab freqs
%   [~, ~, forward] = bst_mvar_spectrum(transfers, C, 64, 200); % causality

% Notes:
% The DTFT we want is
% H = I - sum_p C_p e^{-j 2 pi f/Fs p} = sum_p D_p e^{-j 2 pi f/Fs (p-1)} where D_1 = 1 and D_p = -C_{p-1} for p > 1
% MatLab's FFT provides
% G = sum_n x_n e^{-j 2 pi (k-1)/N (n-1)}
% so the matching is p = n and (k-1)/N = f/Fs, after vectorizing H.

% frequencies to estimate cross-spectra
freq = Fs/2*linspace(0, 1, nFFT/2 + 1);
freq(end) = [];

%% Inverse of transfer function in frequency
% Inverse transfer means the transfer function from the sources to the innovations. We calculate this at each frequency.
% The inverse transfer from source a to innovation b is
%                1 - \sum_{n=1}^N a_{ab} [n] e^{-j2pi * f * n}

inverse = fft(reshape([eye(2) -transfers], 4, [])', nFFT); % reshape so we can do a vector FFT quickly
inverse = inverse(1:nFFT/2, :); % restrict to the first symmetric half of the spectrum

%% Forward transfer in autoregressive model

% pieces of 2x2 inverse
forward = zeros(2, 2, nFFT/2); % an important caveat is that I did not reshape inverse earlier;
forward(1,1,:) = inverse(:,4); forward(1,2,:) = -inverse(:,3); % as a result, we have a column-wise index mapping: 4 = (2,2) and 3 = (1,2)
forward(2,1,:) = -inverse(:,2); forward(2,2,:) = inverse(:,1); % in addition, 2 = (2,1) and 1 = (1,1). then these elements fit the 2x2 matrix inverse
detInverse = inverse(:,1).*inverse(:,4) - inverse(:,3).*inverse(:,2); % the same thing happens here, using the indexing to avoid a reshape() call

% normalization by determinant to get inverse
forward = bst_bsxfun(@rdivide, forward, reshape(detInverse, [1 1 length(freq)])); % complete the inversion by dividing by frequency-dependent determinant

%% Cross-spectrum from forward model
% The forward transfer is the inverse of the inverse transfer at each frequency f. Denoted H(f), the power spectral density is then HH' at each frequency.

% the loop that we won't use
% for idxFreq = 1:nFFT
%   spectra(:, :, idxFreq) = H(:,:,idxFreq) * noiseCovariance * H(:,:,idxFreq)';
% end

% formula for the matrix multiplication
spectra(1,1,:) = ... % 1,1 element is H_11 C_11 H_11^* + H_12 C_12 H_11^* + H_11 C_12 H_12^* + H_12 C_22 H_12^* (and I combine the middle two into 2 Re{.})
  noiseCovariance(1,1) * forward(1,1,:) .* conj(forward(1,1,:)) ...
  + noiseCovariance(1,2) * real(forward(1,2,:) .* conj(forward(1,1,:))) * 2 ...
  + noiseCovariance(2,2) * forward(1,2,:) .* conj(forward(1,2,:));
spectra(1,2,:) = ... % 1,2 element is H_11 C_11 H_21^* + H_12 C_12 H_21^* + H_11 C_12 H_22^* + H_12 C_22 H_22^*
  noiseCovariance(1,1) * forward(2,1,:) .* conj(forward(1,1,:)) ...
  + noiseCovariance(1,2) * forward(2,2,:) .* conj(forward(1,1,:)) ...
  + noiseCovariance(1,2) * forward(2,1,:) .* conj(forward(1,2,:)) ...
  + noiseCovariance(2,2) * forward(1,2,:) .* conj(forward(1,2,:));
spectra(2,1,:) = conj(spectra(1,2,:)); % for speed, force the 2,1 element to be the conjugate of the 1,2 element so we have conjugate symmetry
spectra(2,2,:) = ... % 2,2 element is H_21 C_11 H_21^* + H_22 C_12 H_21^* + H_21 C_12 H_22^* + H_22 C_22 H_22^* (and I combine the middle two into 2 Re{.})
  noiseCovariance(1,1) * forward(2,1,:) .* conj(forward(2,1,:)) ...
  + noiseCovariance(1,2) * real(forward(2,2,:) .* conj(forward(2,1,:))) * 2 ...
  + noiseCovariance(2,2) * forward(2,2,:) .* conj(forward(2,2,:));

%% Normalize for sampling rate, and truncate if necessary

% normalization
forward = forward / sqrt(Fs);
spectra = spectra / Fs;

%% Confidence intervals
% taken from Kay, pp 194-195
% alpha = 1 - ci;
% original = -sqrt(2) * erfcinv( 2 * ( 1 - alpha/2) );
% lower = abs(spectra) * (1 - sqrt(2 * order / nTimes) * original);
% upper = abs(spectra) * (1 + sqrt(2 * order / nTimes) * original);

end %% <== FUNCTION END


function [bcx,bcy] = specific_flow(xbd,ybd)
%regcavity_flow   Reference problem 5.3 default inflow condition
%   [bcx,bcy] = specific_flow(xbd,ybd);
%   input
%          xbd          x coordinate vector
%          ybd          y coordinate vector
%
%   specifies regularized cavity flow boundary condition
%   IFISS function: DJS; 6 March 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
bcx=0*xbd; bcy=0*xbd;
k=find(ybd==1 & xbd>-1 & xbd<1); bcx(k)=(1-xbd(k).*xbd(k)).*(1+xbd(k).*xbd(k));
return


% THE DIAGNOSTIC TOOLS (in the diag-folder):
%
% Covergence diagnostics
%   PSRF     - Potential Scale Reduction Factor
%   CPSRF    - Cumulative Potential Scale Reduction Factor
%   MPSRF    - Multivariate Potential Scale Reduction Factor
%   CMPSRF   - Cumulative Multivariate Potential Scale Reduction Factor
%   IPSRF    - Interval-based Potential Scale Reduction Factor
%   CIPSRF   - Cumulative Interval-based Potential Scale Reduction Factor
%   KSSTAT   - Kolmogorov-Smirnov goodness-of-fit hypothesis test
%   HAIR     - Brooks' hairiness convergence diagnostic
%   CUSUM    - Yu-Mykland convergence diagnostic for MCMC
%   SCORE    - Calculate score-function convergence diagnostic
%   GBINIT   - Initial iterations for Gibbs iteration diagnostic
%   GBITER   - Estimate number of additional Gibbs iterations
%
% Time series analysis
%   ACORR      - Estimate autocorrelation function of time series
%   ACORRTIME  - Estimate autocorrelation evolution of time series (simple)
%   GEYER_ICSE - Compute autocorrelation time tau using Geyer's
%                initial convex sequence estimator
%                (requires Optimization toolbox)
%   GEYER_IMSE - Compute autocorrelation time tau using Geyer's
%                initial monotone sequence estimator
%
% Kernel density estimation etc.:
%   KERNEL1  - 1D Kernel density estimation of data
%   KERNELS  - Kernel density estimation of independent components of data
%   KERNELP  - 1D Kernel density estimation, with automatic kernel width
%   NDHIST   - Normalized histogram of N-dimensional data
%   HPDI     - Estimates the Bayesian HPD intervals
%
% Manipulation of MCMC chains
%   THIN     - Delete burn-in and thin MCMC-chains
%   JOIN     - Join similar structures of arrays to one structure of arrays
%   BATCH    - Batch MCMC sample chain and evaluate mean/median of batches
%
% Misc:
%   CUSTATS   - Calculate cumulative statistics of data
%   BBPRCTILE - Bayesian bootstrap percentile
%   GRADCHEK  - Checks a user-defined gradient function using finite
%               differences.
%   DERIVATIVECHECK - Compare user-supplied derivatives to
%                     finite-differencing derivatives.
%


function determ = wilk04_determinant ( )

%*****************************************************************************80
%
%% WILK04_DETERMINANT returns the determinant of the WILK04 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real DETERM, the determinant.
%
  determ = 0.9143E-04 * 0.7156E-04 * 0.9504E-04 * 0.7123E-04;

  return
end


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
% ##2
%==============================================================================
%
% - initialize
%     data:            Hands,           see setup2DhandData, level = 5;
%     visualization:   viewImage2D,     see viewImage.m
%     interpolation:   splineInter,   see inter.m
%     distance:        SSD,             see distance.m
%     regularizer:     mfElastic,       see regularizer.m
% - initialize FAIR plots
% - run optimization
%     NPIR:            Non-Parametric Image Registration, Trust-Region
% ===============================================================================

clear; close all; help(mfilename)

setup2DhandData
level = 5; omega = ML{level}.omega; m = ML{level}.m;
imgModel('reset','imgModel','splineInter','regularizer','moments','theta',0.01);
distance('reset','distance','SSD');
regularizer('reset','regularizer','mfElastic','alpha',1000,'mu',1,'lambda',0);

[T,R] = imgModel('coefficients',ML{level}.T,ML{level}.R,omega);
xc    = getStaggeredGrid(omega,m); % starting guess and reference for regularization
Rc    = imgModel(R,omega,center(xc,m));

% - initialize FAIR plots
FAIRplots('set','mode','TR-mf','fig',1);
FAIRplots('init',struct('Tc',T,'Rc',R,'omega',omega,'m',m));

% - run Non-Parametric Image Registration (Gauss-Newton)
fctn = @(yc) NPIRobjFctn(T,Rc,omega,m,xc,yc);  fctn([]);
yc = TrustRegion(fctn,xc,'Plots',@FAIRplots);
%==============================================================================


function [D,S,C] = signed_distance_direction(P,V,F)
  % SIGNED_DISTANCE_DIRECTION Compute a direction which decreases signed
  % distance with respect to an input mesh (V,F) at a set of points P.
  %
  % [D,S,C] = signed_distance_direction(P,V,F)
  %
  % Inputs:
  %   P  #P by dim list of query points
  %   V  #V by dim list of mesh vertex positions
  %   F  #F by dim+1 list of mesh indices into V
  % Outputs:
  %   D  #P by dim list of normalized directions
  %   S  #P signed distances
  %   C  #P by dim list of closest points
  %

  dim = size(F,2);
  switch dim
  case 2
    % Facets are really edges
    E = F;
    % O(n*m) too slow...
    [T,sqrD] = project_to_lines(P,V(E(:,1),:),V(E(:,2),:),'Segments',true);
    % snap to line segment
    [~,J] = min(sqrD,[],2);
    T = T(sub2ind(size(T),1:size(P,1),J'))';
    C = V(E(J,1),:) + bsxfun(@times,T,(V(E(J,2),:)-V(E(J,1),:)));
    s = -2*(winding_number(V,E,P))+1;
    vec = C-P;
    % signed distance direction
    D = bsxfun(@times,s,normalizerow(vec));
    % signed distance
    S = s.*normrow(D);
  case 3
    [S,I,C,N] = signed_distance(P,V,F,'SignedDistanceType','pseudonormal');
    D = normalizerow(C-P);
    D = bsxfun(@times,sign(S),D);
    min_dist = 1e-5;
    too_close = abs(S) < min_dist;
    % Normals always point outside regardless of the eval point.
    D(too_close,:) = -N(too_close,:);
  end

  %% Find closest points
  %[sqrD,I,C] = point_mesh_squared_distance(P,V,F);
  %% Compute barycentric coordinates on closest faces
  %B = barycentric_coordinates(C,V(F(I,1),:),V(F(I,2),:),V(F(I,3),:));
  %% Direction to closest point
  %D = normalizerow(C-P);
  %% Determine which are too close to trust
  %min_sqr_dist = 1e-10;
  %too_close = sqrD < min_sqr_dist;
  %% Determine if closest point is on vertex, edge, or face AND too clost to
  %% trust direction.
  %epsilon = 1e-15;
  %on_face = (sum(B<=epsilon,2)==0) & too_close;
  %on_edge = (sum(B<=epsilon,2)==1) & too_close;
  %on_vertex = (sum(B<=epsilon,2)==2) & too_close;
  %% Determine which vertex or edge of that face
  %[~,which_vertex] = find(B(on_vertex,:)>epsilon);
  %[~,which_edge] = find(B(on_edge,:)<=epsilon);
  %% Compute normals for faces, vertices and edges
  %% Q: Are area normals OK?
  %% H: [Bærentzen 2002/2005] suggests angle weighting.
  %N_face = normalizerow(normals(V,F));
  %N_vertex = per_vertex_normals(V,F);
  %% map to corners
  %N_vertex = N_vertex(F,:);
  %[N_edge,E,EMAP] = per_edge_normals(V,F);
  %% map to directed edges
  %N_edge = N_edge(EMAP,:);
  %% This is an expensive way to find out if inside/outside a closed mesh
  %w = winding_number(V,F,P);
  %% Flip sign for interior points
  %S = (1-2*w);
  %D = bsxfun(@times,S,D);
  %% Use inverse normals for those that are too close
  %if any(on_vertex)
  %  D(on_vertex,:) = ...
  %    -N_vertex(sub2ind(size(F),I(on_vertex),which_vertex),:);
  %end
  %if any(on_edge)
  %  D(on_edge,:) = -N_edge(sub2ind(size(F),I(on_edge),which_edge),:);
  %end
  %if any(on_face)
  %  D(on_face,:) = -N_face(I(on_face),:);
  %end
end


function [fProbability_] = callback_LogLikelihoodA1A2Value(fAValues, caCatalogs, mControl, fBValue)
% function [fProbability] = callback_LogLikelihoodAValue(fAValue, caCatalogs, mControl)
% -------------------------------------------------------------------------------------
% Helper callback-function for calc_MaxLikelihoodA.m
%   Computes the negative log-likelihood sum of a given a- and fixed b-value for a
%   set of given catalogs
%
% Input parameters:
%   fAValue        a-value
%   caCatalogs      Cell array containing the set of catalogs
%   mControl        Controlmatrix containing informations about the single catalogs
%                   mControl(n,:) contains information about caCatalogs{n}
%                   Column 1: Starting time of catalog
%                   Column 2: Magnitude of completeness
%                   Column 3: Starting magnitude bin
%                   Column 4: Magnitude bin stepsize (must be 0.1)
%   fBValue         Fixed b-value
%
% Output parameters:
%   fProbability    Negative log-likelihood of the given a- and b-value for the set of given catalogs
%
% Danijel Schorlemmer
% July 5, 2002

% Init variable
global fProbability;
vProbabilities = [];

[nRow_, nColumn_] = size(mControl);
fTotalLength_ = mControl(nRow_,6)-mControl(1,1);

% Loop over all catalogs
for nCnt_ = 1:length(caCatalogs)
  % Extract catalog from cell array
  mCatalog_ = caCatalogs{nCnt_};
  % Determine maximum magnitude of catalog
  fMaxMag_ = max(mCatalog_(:,6));
  % Set up vector of available magnitude bins
  vCnt_ = (mControl(nCnt_,2):mControl(nCnt_,4):(fMaxMag_+mControl(nCnt_,4)))'; % Add one more magnitude bin for later use of diff()
  % Compute lengths of periods and ajust thea-value
  fTimeLength_ = mControl(nCnt_,6) - mControl(nCnt_,1);
  fTimeRatio_ = fTimeLength_/fTotalLength_;
  % Compute the cumulative FMD
  if mControl(nCnt_,5) == 1;  % this is activity rate 1
      fA_ = fAValues(1) + log10(fTimeRatio_);
      vNumber_ = 10.^(fA_ - (fBValue * vCnt_));
  elseif  mControl(nCnt_,5) == 2;  % this is activity rate 1
      fA_ = fAValues(2) + log10(fTimeRatio_);
      vNumber_ = 10.^(fA_ - (fBValue * vCnt_));
  end
  % Determine the number of events in each magnitude bin
  mPredictionFMD_ = -diff(vNumber_);
  % Create the FMD for the period of observation
  vObservedFMD_ = histogram(mCatalog_(:,6), mControl(nCnt_,2):mControl(nCnt_,4):fMaxMag_);
  % Calculate the likelihoods for both of the models
  vProb_ = calc_log10poisspdf(vObservedFMD_', mPredictionFMD_);
  % Return the values (multiply by -1 to return the lowest value for the highest probability
  vProbabilities = [vProbabilities; sum(vProb_)];
end
% Sum the probabilities for all given catalogs
fProbability_ = (-1) * sum(vProbabilities);

fProbability = fProbability_;


function [PDR, deltaHD, deltaSEN, deltaPRO, deltaCOL, CBR] = CV2XMode4(beta,lambda,Pt,S,B);

% CV2XMode4 is the main script of the implementation of the analytical
% models of the communication performance of C-V2X or LTE-V Mode 4
% described in the following paper:
%
%    Manuel Gonzalez-Martín, Miguel Sepulcre, Rafael Molina-Masegosa, Javier Gozalvez,
%    "Analytical Models of the Performance of C-V2X Mode 4 Vehicular Communications",
%    IEEE Transactions on Vehicular Technology, Vol. 68, Issue 2, Feb. 2019. DOI: 10.1109/TVT.2018.2888704
%    Final version available at: https://ieeexplore.ieee.org/document/8581518
%    Post-print version available at: https://arxiv.org/abs/1807.06508
%
% The paper presents analytical models for the average PDR (Packet Delivery Ratio) as a
% function of the distance between transmitter and receiver, and for the four different
% types of transmission errors that can be encountered in C-V2X or LTE-V Mode 4. The models
% have been validated for a wide range of transmission parameters and traffic densities by
% comparing the results obtained with the analytical models to those obtained with a C-V2X
% or LTE-V Mode 4 simulator implemented by the authors over the Veins simulation platform.
%
% CV2XMode4.m is the main script you have to run to get the PDR curve as a function of the
% distance for a given set of parameters, and the probability of each of the four
% transmission errors.
%
% The resulting figures are compared with simulations when the same configuration
% is available in the ./simulations folder.
%
% The resulting figures are stored in the ./fig folder.
%
% Input parameters:
%    beta: traffic density in veh/m. Values tested: 0.1, 0.2 and 0.3.
%    lambda: packet transmission frequency in Hz. Values tested: 10 and 20.
%    Pt: transmission power in dBm. Values tested: 20 and 23.
%    S: number of sub-channels. Values tested: 2 and 4.
%    B: packet size in bytes. Values tested: 190.
%
% Output metrics:
%    PDR: Packet Delivery Ratio for different Tx-Rx distances
%    deltaHD: probability of packet loss due to half-duplex transmissions for different Tx-Rx distances
%    deltaSEN: probability of packet loss due to a received signal power below the sensing power threshold for different Tx-Rx distances
%    deltaPRO: probability of packet loss due to propagation effects for different Tx-Rx distances
%    deltaCOL: probability of packet loss due to packet collisions for different Tx-Rx distances
%    CBR: Channel Busy Ratio between 0 and 1
%
% Overall code structure:
%     CV2XMode4.m
%         |---->   CV2XMode4_common.m   ----> get_PL_SH.m, get_SINRdistribution.m, get_BLER.m
%         |---->   CV2XMode4_Step2.m    ----> get_PL_SH.m, get_SINRdistribution.m, get_BLER.m
%         |---->   CV2XMode4_Step3.m    ----> get_PL_SH.m, get_SINRdistribution.m, get_BLER.m
%
% The equations that are identified with a number between brackets in this script are the ones
% that also appear in the paper so that they can be easily identified.

    disp('=========================================================')
    disp('Input parameters:')
    fprintf('  beta   = %f veh/m \n', beta)
    fprintf('  lambda = %d Hz \n', lambda)
    fprintf('  Pt     = %d dBm \n', Pt)
    fprintf('  S      = %d subchannels \n', S)
    fprintf('  B      = %d bytes \n', B)

    distance = [0:25:500];  % Tx-Rx distances to evaluate (m)

    Psen = -90.5;               % Sensing threshold (dBm)

    step_dB = 0.1;              % Discrete steps to compute the PDF of the SNR and SINR (dB)

    % Calculate the number of RBs that are needed to transmit each message
    % and the coding used based on the number of sub-channels and packet size:
    switch B
        case 190
            switch S
                case 4
                    coding = 1;   % Used to identify the BLER vs SINR curve to be used (190 Bytes, QPSK r=0.7, Vr = 280 km/h)
                    RBs = 10;     % Number of RBs needed to transmit the DATA field of each message
                case 2
                    coding = 2;   % Used to identify the BLER vs SINR curve to be used (190 Bytes, QPSK r=0.5, Vr = 280 km/h)
                    RBs = 12;     % Number of RBs needed to transmit the DATA field of each message
            end
    end

    noise = -95 - 10*log10(50/RBs);     % Noise corresponding to the DATA field of each message. Assumes a noise figure of 9dB and 10MHz channel (background noise of -95dBm). The total number of RBs in 10MHz is 50.

    % Calculate errors associated to HD, SEN and PRO:
    [ deltaHD_pre , deltaSEN_pre , deltaPRO_pre ] = CV2XMode4_common( lambda , Pt , distance, Psen , step_dB , noise , coding );

    % Calculate probability of collision considering only Step 2 and CBR:
    [ deltaCOL2_pre , CBR ] = CV2XMode4_Step2( beta , lambda , Pt , S , distance , Psen , step_dB , noise , coding , deltaPRO_pre );

    % Calculate weighting factor alpha using equation (22):
    if CBR < 0.2
        alpha = 0;
    elseif CBR <= 0.7
        alpha = 2*CBR - 0.4;
    else
        alpha = 1;
    end

    % Calculate probability of collision considering only Step 3:
    if alpha < 1
         [ deltaCOL3_pre ] = CV2XMode4_Step3( beta , lambda , Pt , S , distance , Psen , step_dB , noise , coding , deltaPRO_pre );
    else
        deltaCOL3_pre = 0;
    end

    % Calculate final probabilities for each type of error:
    deltaHD   = deltaHD_pre;                               % Equation (6.1)
    deltaSEN  = deltaSEN_pre .* (1 - deltaHD);             % Equation (6.2)
    deltaPRO  = deltaPRO_pre .* (1 - deltaHD_pre) .* (1 - deltaSEN_pre);  % Equation (6.3)
    deltaCOL2 = deltaCOL2_pre .* (1 - deltaHD_pre) .* (1 - deltaSEN_pre) .* (1 - deltaPRO_pre); % Equation (6.4)
    deltaCOL3 = deltaCOL3_pre .* (1 - deltaHD_pre) .* (1 - deltaSEN_pre) .* (1 - deltaPRO_pre); % Equation (6.5)
    deltaCOL = alpha*deltaCOL2 + (1-alpha)*deltaCOL3; % Equation (21)

    % Calculate PDR:
    PDR = 1 - deltaHD - deltaSEN - deltaPRO - deltaCOL; % Equation (6)

    % Presentation of the obtained results:

    % Load simulation results (if available):
    simulation_path = [pwd '\simulations\' num2str(S) 'subchannels\'  num2str(Pt) 'dBm'];
    sim_file = [ simulation_path '\FALLOS_LOS_D2D_' num2str(beta*1000) 'vehpkm_16Alg_' num2str(1/lambda) 's_' num2str(8*B) 'bit.fig' ];
    fig_name = ['CV2XMode4_beta' num2str(beta) '-lambda' num2str(lambda) '-Pt' num2str(Pt) '-S' num2str(S) '-B' num2str(B)];

    if exist(sim_file,'file')==2
        open(sim_file)
        lh = findall(gca, 'type', 'line');
        X = get(lh,'xdata');
        Y = get(lh,'ydata');
        deltaHD_sim = Y{5};
        deltaSEN_sim = Y{1};
        deltaPRO_sim = Y{4};
        deltaCOL_sim = Y{3};
        close

        figure; hold on; grid on; box on
        plot(distance , deltaHD_sim/100,'b-','LineWidth',2)
        plot(distance , deltaSEN_sim/100,'m-','LineWidth',2)
        plot(distance , deltaPRO_sim/100,'r-','LineWidth',2)
        plot(distance , deltaCOL_sim/100,'k-','LineWidth',2)

    else
        figure; hold on; grid on; box on
    end

    % Plot errors:
    ylabel('Error probability')
    xlabel('Distance [m]')
    plot(distance , deltaHD,'b--','LineWidth',2)
    plot(distance , deltaSEN,'m--','LineWidth',2)
    plot(distance , deltaPRO,'r--','LineWidth',2)
    plot(distance , deltaCOL,'k--','LineWidth',2)
    ylim([0 1])

    if exist(sim_file,'file')==2
        legend('\delta_{HD} Simulation','\delta_{SEN} Simulation','\delta_{PRO} Simulation','\delta_{COL} Simulation','\delta_{HD} Analytical','\delta_{SEN} Analytical','\delta_{PRO} Analytical','\delta_{COL} Analytical','Location','northwest')
    else
        legend('\delta_{HD} Analytical','\delta_{SEN} Analytical','\delta_{PRO} Analytical','\delta_{COL} Analytical','Location','northwest')
    end
    hgsave(['fig/' fig_name '_errors.fig'])

    % Plot PDR:
    figure; hold on; grid on; box on
    if exist(sim_file,'file')==2
        PDR_sim = (100 - deltaHD_sim - deltaSEN_sim - deltaPRO_sim - deltaCOL_sim)/100;
        plot(distance,PDR_sim,'b-','LineWidth',2)
        plot(distance,PDR,'b--','LineWidth',2)
        legend('PDR Simulation','PDR Analytical')
    else
        plot(distance,PDR,'--')
        legend('PDR Analytical')
    end
    ylim([0 1])
    ylabel('PDR')
    xlabel('Distance [m]')
    hgsave(['fig/' fig_name '_PDR.fig'])

    fprintf('Channel load: CBR = %.2f, alpha = %f \n',CBR, alpha)

    if exist(sim_file,'file')==2
        MAD_HD  = mean( abs(deltaHD*100 - deltaHD_sim) );    % Equation (35)
        MAD_SEN = mean( abs(deltaSEN*100 - deltaSEN_sim) );  % Equation (35)
        MAD_PRO = mean( abs(deltaPRO*100 - deltaPRO_sim) );  % Equation (35)
        MAD_COL = mean( abs(deltaCOL*100 - deltaCOL_sim) );  % Equation (35)
        MAD     = mean( abs(PDR - PDR_sim) * 100 );          % Equation (35)

        disp('Mean Absolute Deviation results : ')
        fprintf('PDR \tHD  \tSEN \tPRO \tCOL \n')
        fprintf('%.2f\t%.2f\t%.2f\t%.2f\t%.2f \n', MAD, MAD_HD, MAD_SEN, MAD_PRO, MAD_COL)
    end

    disp('=========================================================')

 return


function [qp,QP_ep] = epose2qpose(ep)

% EPOSE2QPOSE  Euler-specified to quaternion-specified pose conversion.
%
%   QP = EPOSE2QPOSE(EP) returns a full 7-pose QP=[X;Q] from a full 6-pose
%   QE=[X;E], where X is 3D opsition and Q and E are 3D orientations.
%
%   [QP,Jep] = EPOSE2QPOSE(...) returns also the Jacobian matrix
%
%   See also QPOSE2EPOSE, EULERANGLES, QUATERNION, FRAME.

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

if any(size(ep) ~= [6,1])
    warning('Input Euler-pose should be a column 6-vector')
end

qp            = zeros(7,1);  % empty quaternion pose
qp(1:3)       = ep(1:3);     % position copy
P             = eye(3);      % Jacobian of copy function
[qp(4:7),Q_e] = e2q(ep(4:6));% orientation and Jacobian

QP_ep         = [P zeros(3);zeros(4,3) Q_e]; % Full Jacobian

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

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   SLAMTB is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

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

%   # END GPL LICENSE


function [lab,q] = psdeig(x,K)
% [lab,q] = psdeig(x,K)
%
% PSDEIG  Computes spectral coefficients of x w.r.t. K
%   Arguments "q" is optional - without it's considerably faster.
%   FLOPS indication: 1.3 nk^3 versus 9.0 nk^3 for nk=500,
%                     1.5 nk^3        9.8 nk^3 for nk=50.
%
% **********  INTERNAL FUNCTION OF SEDUMI **********
%
% See also sedumi

% This file is part of SeDuMi 1.1 by Imre Polik and Oleksandr Romanko
% Copyright (C) 2005 McMaster University, Hamilton, CANADA  (since 1.1)
%
% Copyright (C) 2001 Jos F. Sturm (up to 1.05R5)
%   Dept. Econometrics & O.R., Tilburg University, the Netherlands.
%   Supported by the Netherlands Organization for Scientific Research (NWO).
%
% Affiliation SeDuMi 1.03 and 1.04Beta (2000):
%   Dept. Quantitative Economics, Maastricht University, the Netherlands.
%
% Affiliations up to SeDuMi 1.02 (AUG1998):
%   CRL, McMaster University, Canada.
%   Supported by the Netherlands Organization for Scientific Research (NWO).
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc.,  51 Franklin Street, Fifth Floor, Boston, MA
% 02110-1301, USA

Ks = K.s;
if isempty(Ks),
    lab = [];
    return
end
Kq  = Ks .* Ks;
nr  = K.rsdpN;
nc  = length(Ks);
N   = sum(Kq) + sum(Kq(nr+1:end));
xi  = length(x) - N;
ei  = 0;
lab = zeros(sum(Ks),1);
needv = nargout > 1;
if needv,
    q = zeros(N,1);
    vi = 0;
end
for i = 1 : nc,
    ki = Ks(i);
    qi = Kq(i);
    XX = x(xi+1:xi+qi);
    xi = xi+qi;
    if i > nr,
        XX = XX + 1i*x(xi+1:xi+qi);
        xi = xi+qi;
    end
    XX = reshape(XX,ki,ki);
    XX = XX + XX';
    try
        if needv,
            [QQ,DD] = eig(XX);
            DD = diag(DD);
        else
            DD = eig(XX);
        end
    catch
        % If eig() fails to converge, fall back onto svd(). This costs
        % more, so we don't want to use it every time.
        [QQ,DD,VV] = svd(XX);
        DD = diag(DD).*sign(real(sum(conj(QQ).*VV)'));
    end
    lab(ei+1:ei+ki) = 0.5*DD;
    ei = ei + ki;
    if needv,
        q(vi+1:vi+qi) = real(QQ);
        vi = vi + qi;
        if i > nr,
            q(vi+1:vi+qi) = imag(QQ);
            vi = vi + qi;
        end
    end
end


function channels = EPW(N, beta)
m = log2(N);
channels = zeros(N, 1);
for i = 0 : N - 1
    bin_seq_str = dec2bin(i, m);
    bin_seq = zeros(m, 1);
    for j = 1 : m
        if bin_seq_str(j) == '1'
            bin_seq(j) = 1;
        end
    end
    bin_seq = bin_seq(m : -1 : 1);
    sum = 0;
    for j = 1 : m
        if m >= 9
            sum = sum + bin_seq(j) * (beta^(j - 1) + 0.221 * 0.9889^(j - 1) - bin_seq(9) * 0.0371 * 0.5759^(j - 1) - bin_seq(8) * 0.047 * 0.4433^(j - 1));
        else
            if m == 8
                sum = sum + bin_seq(j) * (beta^(j - 1) + 0.221 * 0.9889^(j - 1) - bin_seq(8) * 0.047 * 0.4433^(j - 1));
            else
                sum = sum + bin_seq(j) * (beta^(j - 1) + 0.221 * 0.9889^(j - 1));
            end
        end
    end
    channels(i + 1) = sum;
end
end


function [dnum_bin, counts_per_bin, sum_per_bin, smallest_per_bin, biggest_per_bin, median_per_bin, std_per_bin, median_time_interval] = bin_irregular(dnum, data, binsize, snum, enum, stepsize)
% BIN_IRREGULAR bin an irregularly-sampled timeseries (like earthquake origin times).
%
%    Usage:
%      [dnum_bin, counts_per_bin, sum_per_bin, smallest_per_bin, biggest_per_bin, median_per_bin, std_per_bin, median_time_interval] = bin_irregular(dnum, data, binsize, snum, enum, [stepsize])
%
%    INPUTS:
%      dnum            - irregular spaced date vector in datenum format
%      data            - data values corresponding to dnum samples
%      binsize         - binsize (in days) to use for output series
%      snum            - start datenum (first centre used for output data)
%      enum            - end datenum (last centre used for output data)
%      stepsize        - (optional) Normally bins do not overlap. But if stepsize is set to a value smaller than binsize, bins will overlap.
%
%    OUTPUTS:
%      dnum_bin        	- regular space date vector (centres of bins)
%      counts_per_bin  	- number of values per bin
%      sum_per_bin     	- sum of all values in each bin
%      smallest_per_bin 	- smallest data value in each bin
%      biggest_per_bin 	- biggest data value in each bin
%      median_per_bin  	- median value in each bin
%      std_per_bin     	- standard deviation of all values in each bin
%      median_time_interval - median time interval between values in each bin
%
%    See also:

% AUTHOR: Glenn Thompson
% $Date$
% $Revision$
l1=length(dnum);
l2=length(data);
dnum_bin = [];
counts_per_bin = [];
sum_per_bin = [];
smallest_per_bin = [];
biggest_per_bin = [];
median_per_bin = [];
std_per_bin = [];
median_time_interval = [];

if (l1==l2)
    dnum_bin = snum+binsize/2 : stepsize : enum-binsize/2; % centres of the bins
    for c=1:length(dnum_bin)
	binstart = dnum_bin(c) - binsize/2; % start of this bin
	binend = dnum_bin(c) + binsize/2;   % end of this bin
	i = find(dnum >= binstart & dnum < binend);
	if length(i)>0
		d = data(i);
		thisdnum = dnum(i);
		counts_per_bin(c) = length(d);
		sum_per_bin(c)=nansum(d);
		median_per_bin(c)=nanmedian(d);
		std_per_bin(c) = std(d);
        	smallest_per_bin(c)=min(d);
        	biggest_per_bin(c)=max(d);
		median_time_interval(c)=median(thisdnum(2:end)-thisdnum(1:end-1));
	else
		counts_per_bin(c) = 0;
		sum_per_bin(c) = 0;
		std_per_bin(c) = 0;
		median_per_bin(c) = NaN;
		smallest_per_bin(c)=NaN;
		biggest_per_bin(c)=NaN;
	end
    end
else
    disp('Could not bin - vector lengths dont match');
end


function img = discreteCurve(varargin)
%DISCRETECURVE Discretize a planar curve
%
%   IMG = discreteCurve(DIM, CURVE, WIDTH)
%   DIM is the size of image, with the format [x0 dx x1;y0 dy y1]
%   CURVE is a series of points describing the curve
%   WIDTH is the max distance between pixel centers and points of the
%   curve.
%
%   IMG = discreteCurve(LX, LY, ...);
%   Specifes the pixels coordinates with the two row vectors LX and LY.
%
%   Example
%   % creates a ring
%   circle = circleAsPolygon([25 25 15], 120);
%   img = discreteCurve([1 1 50;1 1 50], circle, 3);
%   imshow(img);
%
%   See Also
%   imShapes, discretePolyline, discretePolygon

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2007-03-19
% Copyright 2006 INRA - CEPIA Nantes - MIAJ (Jouy-en-Josas).

%   HISTORY
%   19/06/2007: update doc
%   04/03/2009: use meshgrid
%   29/05/2009: use more possibilities for specifying grid

% compute coordinate of image voxels
[lx, ly, varargin] = parseGridArgs(varargin{:});
[x, y]   = meshgrid(lx, ly);

% get polyline vertex coordinates
curve = varargin{1};
varargin(1) = [];

% determines width of the polyline
width = 2;
if ~isempty(varargin)
    width = varargin{1};
end

try
    % try with vectorized version (greedy !!!)
    dist = reshape(minDistancePoints([x(:) y(:)], curve), size(x));
catch
    % if not enough memory, use loop instead
    dist = zeros(size(x));
    for i = 1:length(lx)
        dist(:, i) = minDistancePoints([x(:,i) y(:,i)], curve);
    end
end

% create image : simple threshold over 2 dimensions
img = abs(dist) < width;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Find the start time offset based on the cyclic prefix
%
% This method is immune to the issues that plague the ZC sequences (frequency offset causes a time shift in the
% correlation results)
%
% It's best to provide this function an upsampled copy of the burst to help fix any fractional time offset that might be
% present
%
% @param samples Complex IQ samples that make up the full burst
% @param sample_rate Sample rate (in Hz) of the provided samples
% @return start_offset Sample index that the burst starts at (first sample of the first cyclic prefix)
function [start_offset] = find_sto_cp(samples, sample_rate)
    [long_cp_len, short_cp_len] = get_cyclic_prefix_lengths(sample_rate);
    fft_size = get_fft_size(sample_rate);
    cyclic_prefix_length_schedule = [...
        long_cp_len, ...
        short_cp_len, ...
        short_cp_len, ...
        short_cp_len, ...
        short_cp_len, ...
        short_cp_len, ...
        short_cp_len, ...
        short_cp_len, ...
        long_cp_len];
    num_ofdm_symbols = length(cyclic_prefix_length_schedule);

    full_burst_len = sum(cyclic_prefix_length_schedule) + (fft_size * num_ofdm_symbols);
    num_tests = length(samples) - full_burst_len;
    scores_cp_sto = zeros(1, num_tests);

    for idx=1:num_tests
        offset = idx;
        scores = zeros(1, num_ofdm_symbols);

        % Extract and correlate the samples that each cyclic prefix is expected
        % to be at
        for cp_idx=1:num_ofdm_symbols
            cp_len = cyclic_prefix_length_schedule(cp_idx);

            % Extract the full OFDM symbol including cyclic prefix
            window = samples(offset:offset + fft_size + cp_len - 1);

            % Extract the cyclic prefix and the final samples of the symbol
            left = window(1:cp_len);
            right = window(end - cp_len + 1:end);

            % Correlate the two windows
            scores(cp_idx) = abs(xcorr(left, right, 0));

            % Move the sample pointer forward by the full symbol size
            offset = offset + cp_len + fft_size;
        end

        % In the real DroneID the first OFDM symbol needs to be ignored since
        % it isn't always present.  So, just average the correlation scores of
        % all but the first element
        scores_cp_sto(idx) = sum(scores(2:end)) / (length(scores) - 1);

    end

    % Find the index of the highest score
    [~, start_offset] = max(scores_cp_sto);
end


function varargout = transformPoint3d(pts, transfo, varargin)
%TRANSFORMPOINT3D Transform a point with a 3D affine transform.
%
%   PT2 = transformPoint3d(PT1, TRANS);
%   PT2 = transformPoint3d(X1, Y1, Z1, TRANS);
%   where PT1 has the form [xp yp zp], and TRANS is a 3-by-3, 3-by-4, or
%   4-by-4 matrix, returns the point transformed according to the affine
%   transform specified by TRANS.
%
%   The function accepts transforms given using the following formats:
%   [a b c]   ,   [a b c j] , or [a b c j]
%   [d e f]       [d e f k]      [d e f k]
%   [g h i]       [g h i l]      [g h i l]
%                                [0 0 0 1]
%
%   PT2 = transformPoint3d(PT1, TRANS)
%   also work when PT1 is a N-by-3-by-M-by-P-by-ETC array of double. In
%   this case, PT2 has the same size as PT1.
%
%   PT2 = transformPoint3d(X1, Y1, Z1, TRANS);
%   also work when X1, Y1 and Z1 are 3 arrays with the same size. In this
%   case, PT2 will be a 1-by-3 cell containing {X Y Z} outputs of size(X1).
%
%   [X2, Y2, Z2] = transformPoint3d(...);
%   returns the result in 3 different arrays the same size as the input.
%   This form can be useful when used with functions like meshgrid or warp.
%
%   MESH2 = transformPoint3d(MESH, TRANS)
%   transforms the field 'vertices' of the struct MESH and returns the same
%   struct with the transformed vertices.
%   (It is recommended to use the function 'transformMesh', within the
%   "meshes3d" module).
%
%   See also
%     points3d, transforms3d, transformMesh, createTranslation3d
%     createRotationOx, createRotationOy, createRotationOz, createScaling
%

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2005-02-10
% Copyright 2005-2022 INRA - TPV URPOI - BIA IMASTE

%% Parse input arguments

% Check special case: if first argument is a struct with a field named
% 'vertices', then the output will be the same struct, but with the
% transformed vertices.
if nargin == 2 && isstruct(pts) && isfield(pts, 'vertices')
    mesh = pts;
    mesh.vertices = transformPoint3d(mesh.vertices, transfo);
    varargout = {mesh};
    return;
end

% Parse x, y, and z coordinates of input points from input arguments
if nargin == 2
    % Point coordinates are given in a single N-by-3-by-M-by-etc argument.
    % Preallocate x, y, and z to size N-by-1-by-M-by-etc, then fill them in
    dim = size(pts);
    dim(2) = 1;
    [x, y, z] = deal(zeros(dim, class(pts)));
    x(:) = pts(:,1,:);
    y(:) = pts(:,2,:);
    z(:) = pts(:,3,:);

elseif nargin == 4
    % Point coordinates are given in 3 different arrays
    x = pts;
    y = transfo;
    z = varargin{1};
    transfo = varargin{2};
    dim = size(x);

else
    error('MatGeom:geom3d:WrongInputArgumentNumber', ...
        'Requires number of input arguments to be either 2 or 4');
end

%% Process transformation matrix

% extract the linear and the translation parts of the matrix
linear = transfo(1:3, 1:3)';
trans = [0 0 0];
if size(transfo, 2) > 3
    trans = transfo(1:3, 4)';
end

%% Main processing

% convert coordinates
try
    % vectorial processing, if there is enough memory.
    % same as:
    % res = (transfo * [x(:) y(:) z(:) ones(NP, 1)]')';
    res = bsxfun(@plus, [x(:) y(:) z(:)] * linear, trans);

    % Back-fill x,y,z with new result (saves calling costly reshape())
    x(:) = res(:,1);
    y(:) = res(:,2);
    z(:) = res(:,3);

catch ME
    disp(ME.message)
    % process each point one by one, writing in existing array
    NP = numel(x);
    for i = 1:NP
        res = [x(i) y(i) z(i)] * linear + trans;
        x(i) = res(1);
        y(i) = res(2);
        z(i) = res(3);
    end
end

% process output arguments
if nargout <= 1
    % results are stored in a unique array
    if length(dim) > 2 && dim(2) > 1
        warning('geom3d:shapeMismatch',...
            'Shape mismatch: Non-vector xyz input should have multiple x,y,z output arguments. Cell {x,y,z} returned instead.')
        varargout{1} = {x,y,z};
    else
        varargout{1} = [x y z];
    end

elseif nargout == 3
    % results are returned in three array with same size.
    varargout = {x, y, z};
end


function s = addrice(s)
%ADDRICE Add the Rician distribution.

%   Copyright 1993-2004 The MathWorks, Inc.
%   $Revision: 1.1.6.10 $  $Date: 2004/02/01 22:10:34 $

j = length(s) + 1;
s(j).name = 'Rician';
s(j).code = 'rician';
s(j).pnames = {'s' 'sigma'};
s(j).pdescription = {'noncentrality' 'scale'};
s(j).prequired = [false false];
s(j).fitfunc = @ricefit;
s(j).likefunc = @ricelike;
s(j).cdffunc = @ricecdf;
s(j).pdffunc = @ricepdf;
s(j).invfunc = @riceinv;
s(j).statfunc = @ricestat;
s(j).loginvfunc = [];
s(j).logcdffunc = [];
s(j).hasconfbounds = false;
s(j).censoring = true;
s(j).paramvec = true;
s(j).support = [0 Inf];
s(j).closedbound = [false false];
s(j).iscontinuous = true;
s(j).islocscale = false;
s(j).uselogpp = false;

% ==== Rician distribution functions ====

% these distribution functions do not yet handle arrays of parameters

function y = ricepdf(x,s,sigma)
%RICEPDF Rician probability density function (pdf).
s(s < 0) = NaN;
sigma(sigma <= 0) = NaN;

x(x<0) = 0;
sigsq = sigma.^2;
expon = (x.^2 + s.^2)./(2.*sigsq);
y = (x./sigsq) .* exp(-expon) .* besseli(0, x.*s./sigsq);
y(expon > (log(realmax(class(x)))-1)) = 0; % fix up 0*Inf

function p = ricecdf(x,s,sigma)
%RICECDF Rician cumulative distribution function (cdf).
s(s < 0) = NaN;
sigma(sigma <= 0) = NaN;

x(x<0) = 0;
p = ncx2cdf((x./sigma).^2, 2, (s./sigma).^2);

function x = riceinv(p,s,sigma)
%RICEINV Inverse of the Rician cumulative distribution function (cdf).
s(s < 0) = NaN;
sigma(sigma <= 0) = NaN;

x = sigma .* sqrt(ncx2inv(p, 2, (s./sigma).^2));

function r = ricernd(s,sigma,varargin)
%RICERND Random arrays from the Rician distribution.
s(s < 0) = NaN;
sigma(sigma <= 0) = NaN;

[err, sizeOut] = statsizechk(2,s,sigma,varargin{:});
if err > 0
    error('stats:ricernd:InconsistentSizes','Size information is inconsistent.');
end

r = sigma .* sqrt(ncx2rnd(2, (s./sigma).^2, sizeOut));

function [m,v] = ricestat(s,sigma)
%RICESTAT Mean and variance for the Rician distribution.
s(s < 0) = NaN;
sigma(sigma <= 0) = NaN;

t = .5 .* (s./sigma).^2;
m = sigma.*sqrt(.5.*pi).*exp(-.5.*t) .* ((1+t).*besseli(0,.5.*t) + t.*besseli(1,.5.*t));
v = 2.*sigma.^2 + s.^2 - m.^2;

function [nlogL,acov] = ricelike(params,data,cens,freq)
%RICELIKE Negative log-likelihood for the Rician distribution.
if nargin < 4 || isempty(freq), freq = ones(size(data)); end
if nargin < 3 || isempty(cens), cens = zeros(size(data)); end

nlogL = rice_nloglf(params, data, cens, freq);
if nargout > 1
    acov = mlecov(params, data, 'nloglf',@rice_nloglf, 'cens',cens, 'freq',freq);
end

% ==== Rician fitting functions ====

function [phat,pci] = ricefit(x,alpha,cens,freq,opts)
%NAKAFIT Parameter estimates and confidence intervals for Rician data.

if nargin < 2 || isempty(alpha), alpha = .05; end
if nargin < 3 || isempty(cens), cens = zeros(size(x)); end
if nargin < 4 || isempty(freq), freq = ones(size(x)); end
if nargin < 5, opts = []; end

if any(x <= 0)
    error('stats:ricefit:BadData','The data in X must be positive');
end

% Moment estimators of the uncensored data as starting point
% E[x.^2] = s.^2 + 2.*sigma.^2
% E[x.^4] = s.^4 + 8.*s.^2.*sigma.^2 + 8.*sigma.^4
xsqunc = x(cens == 0).^2;
meanxsq = mean(xsqunc); meanx4th = mean(xsqunc);
if meanxsq.^2 < meanx4th && meanx4th < 2.*meanxsq.^2
    s4th = 2.*meanxsq.^2 - meanx4th;
    ssq = sqrt(s4th);
    sigsq = .5.*(meanxsq - ssq);
    start = [sqrt(ssq) sqrt(sigsq)];
else
    start = cast([1 1],class(x));
end

% The default options include turning fminsearch's display off.  This
% function gives its own warning/error messages, and the caller can turn
% display on to get the text output from fminsearch if desired.
options = statset(statset('ricefit'), opts);
tolBnd = options.TolBnd;
options = optimset(options);

% Maximize the log-likelihood with respect to s and sigma.
[phat,nll,err,output] = ...
    fminsearch(@rice_nloglf, start, options, x, cens, freq, tolBnd);
if (err == 0)
    % fminsearch may print its own output text; in any case give something
    % more statistical here, controllable via warning IDs.
    if output.funcCount >= options.MaxFunEvals
        wmsg = 'Maximum likelihood estimation did not converge.  Function evaluation limit exceeded.';
    else
        wmsg = 'Maximum likelihood estimation did not converge.  Iteration limit exceeded.';
    end
    warning('stats:ricefit:IterOrEvalLimit',wmsg);
elseif (err < 0)
    error('stats:ricefit:NoSolution',...
          'Unable to reach a maximum likelihood solution.');
end

% Compute CIs using a normal approximation for phat.
if nargout > 1
    acov = mlecov(phat, x, 'nloglf',@rice_nloglf, 'cens',cens, 'freq',freq);
    probs = [alpha/2; 1-alpha/2];
    se = sqrt(diag(acov))';
    pci = norminv([probs probs], [phat; phat], [se; se]);
end

function nll = rice_nloglf(parms, x, cens, freq, tolBnd)
%RICE_NLOGLF Objective function for Rician maximum likelihood.
s = parms(1);
sigma = parms(2);
sigsq = sigma.^2;

% Restrict sigma to the open interval (0, Inf).
if nargin > 4
    if s < tolBnd || sigma < tolBnd
        nll = Inf;
        return
    end
end

bess0 = besseli(0, x.*s./sigsq);
rsq = (x.^2 + s.^2)./(2.*sigsq);
L = -rsq + log(bess0) + log(x./sigsq);
ncen = sum(freq.*cens);
if ncen > 0
    cen = (cens == 1);
    xcen = x(cen);
    L(cen) = log(marcumq(s./sigma,xcen./sigma));
end
nll = -sum(freq .* L);

% Don't have derivatives of the Marcum's Q, so can't compute an analytic
% gradient with censoring.
%
% if nargout > 1
%     dlogbess0 = besseli(1, x.*s./sigsq) ./ bess0;
%     dL1 = (-s + dlogbess0.*x) ./ sigsq;
%     dL2 = (rsq - 1 - dlogbess0.*x.*s./sigsq) ./ sigma;
%     if ncen > 0
% %         dL1(cen) = ;
% %         dL2(cen) = ;
%     end
%     ngrad = -[sum(freq .* dL1) sum(freq .* dL2)];
% end

function Q = marcumq(a,b)
% Q = MARCUMQ(A,B) returns Marcum's "Q" function.

if isa(a,'single') || isa(b,'single')
   Q = repmat(single(NaN), size(a));
else
   Q = repmat(NaN, size(a));
end
Q(a~=Inf & b==0) = 1;
Q(a~=Inf & b==Inf) = 0;
Q(a==Inf & b~=Inf) = 1;
z = (isnan(Q) & a==0 & b~=Inf);
if (any(z))
   Q(z) = exp((-b(z).^2)./2);
end

z = isnan(Q) & ~isnan(a) & ~isnan(b);
if (any(z(:)))
%    aa = (a(z).^2)./2;
   aa = (a.^2)./2;
   bb = (b(z).^2)./2;

   d = exp(-aa);
   h = d;
   f = bb.*exp(-bb);
   k = 1;
   delta = f .* h;
   sum = delta;
   j = (delta > sum.*eps(class(delta)));
   while any(j)
      d = aa.*d./k;
      h = h + d;
      f = bb.*f./(k+1);
      delta = f .* h;
      sum(j) = sum(j) + delta(j);
      j = (delta > sum.*eps(class(delta)));
      k = k + 1;
   end
   Q(z) = 1 - sum;
end


function g = rbfbkp(net, x, z, n2, deltas)
%RBFBKP	Backpropagate gradient of error function for RBF network.
%
%	Description
%	G = RBFBKP(NET, X, Z, N2, DELTAS) takes a network data structure NET
%	together with a matrix X of input vectors, a matrix  Z of hidden unit
%	activations, a matrix N2 of the squared distances between centres and
%	inputs, and a matrix DELTAS of the  gradient of the error function
%	with respect to the values of the output units (i.e. the summed
%	inputs to the output units, before the activation function is
%	applied). The return value is the gradient G of the error function
%	with respect to the network weights. Each row of X corresponds to one
%	input vector.
%
%	This function is provided so that the common backpropagation
%	algorithm can be used by RBF network models to compute gradients for
%	the output values (in RBFDERIV) as well as standard error functions.
%
%	See also
%	RBF, RBFGRAD, RBFDERIV
%

%	Copyright (c) Ian T Nabney (1996-2001)

% Evaluate second-layer gradients.
gw2 = z'*deltas;
gb2 = sum(deltas);

% Evaluate hidden unit gradients
delhid = deltas*net.w2';

gc = zeros(net.nhidden, net.nin);
ndata = size(x, 1);
t1 = ones(ndata, 1);
t2 = ones(1, net.nin);
% Switch on activation function type
switch net.actfn

case 'gaussian' % Gaussian
   delhid = (delhid.*z);
   % A loop seems essential, so do it with the shortest index vector
   if (net.nin < net.nhidden)
      for i = 1:net.nin
         gc(:,i) = (sum(((x(:,i)*ones(1, net.nhidden)) - ...
            (ones(ndata, 1)*(net.c(:,i)'))).*delhid, 1)./net.wi)';
      end
   else
      for i = 1:net.nhidden
         gc(i,:) = sum((x - (t1*(net.c(i,:)))./net.wi(i)).*(delhid(:,i)*t2), 1);
      end
   end
   gwi = sum((n2.*delhid)./(2.*(ones(ndata, 1)*(net.wi.^2))), 1);

case 'tps'	% Thin plate spline activation function
   delhid = delhid.*(1+log(n2+(n2==0)));
   for i = 1:net.nhidden
      gc(i,:) = sum(2.*((t1*(net.c(i,:)) - x)).*(delhid(:,i)*t2), 1);
   end
   % widths are not adjustable in this model
   gwi = [];
case 'r4logr' % r^4 log r activation function
   delhid = delhid.*(n2.*(1+2.*log(n2+(n2==0))));
   for i = 1:net.nhidden
      gc(i,:) = sum(2.*((t1*(net.c(i,:)) - x)).*(delhid(:,i)*t2), 1);
   end
   % widths are not adjustable in this model
   gwi = [];
otherwise
   error('Unknown activation function in rbfgrad')
end

g = [gc(:)', gwi, gw2(:)', gb2];


function mv=v_roteucode(m)
%V_ROTEUCODE decodes a string specifying a rotation axis sequence
%     M(n)     a string of n characters from the set determining the order of rotation axes
%              as listed below. Note that the control characters 'rdoOaA' may occur anywhere in the string:
%                'x','y','z'    rotate around the given axis by the corresponding angle
%                               given in e()
%                '1','2','3'    90 degree rotation around x,y or z axis; doesn't use a value from e()
%                '4','5','6'    180 degree rotation around x,y or z axis; doesn't use a value from e()
%                '7','8','9'    270 degree rotation around x,y or z axis; doesn't use a value from e()
%                'r','d'        all angles are given in radians or degrees  [radians]
%             'o','O','a','A'   selects whether to rotate the object or the coordinate axes and
%                               whether the rotation axes remain fixed in space for consecutive
%                               rotations (extrinsic) or else move with each rotation (intrinsic).
%                                  'o' = object-extrinsic [default]
%                                  'O' = object-intrinsic
%                                  'a' = axes-extrinsic
%                                  'A' = axes-intrinsic
% Outputs:
%
%     mv(7,k)    where k-1 is the number of non-control characters in the input string m
%                    mv(1,j) = Code for the j'th rotation: 1,2,3 for x,y,z rotation and 4 to 12 for the fixed rotations listed above.
%                              All entries are in the range [1,12] except for mv(1,k)=0.
%                    mv(2,j) = index into euler angle array for x,y,z rotations. mv(2,k) gives total number of euler angles needed
%                    mv(3,j) = rotation class before rotation j. mv(3,k) is the final rotation class and equals 52 for arbitrary rotations.
%                    mv(4,j) = index into a vectorized matrix of the entry that becomes non-zero after rotation j
%                    mv(5,j) = index into a vectorized matrix of the other changing element in the same column
%                    mv(6,j) = +-1 = sign of the sine term affecting entry mv(4,j). For mv(1,j) in [1,3], mv(6,j)=0 if the rotation is unnecessary
%                    mv(7,j) = +-1 = sign of entry mv(5,j) before rotation j if known
%                Special entries:
%                    mv(7,k) = -1 to invert the rotation (i.e. transpose the matrix) or +1 otherwise
%                    mv(4,k) = scale factor for euler angles: +-1 or +-pi/180
%
%
% The string M specifies the seqeunce of axes about which the rotations are performed. There are 12
% possible 3-character sequences that avoid consecutive repetitions. These are 'Euler angles' if
% there is a repeated axis or 'Tait-Bryan angles' if not. Common choices are:
% (1) 'zxz' the most common Euler angle set
% (2) 'xyz' corresponds to 'roll, pitch, yaw' for an aeroplane heading in the x direction with y to
%     the right and z down. The intrinsic equivalent is 'Ozyx' corresponding to 'yaw, pitch, roll'.
% (3) 'z1z1z' involves 5 rotations, in which all the non-fixed rotations are around the z axis.
%

%      Copyright (C) Mike Brookes 2007-2020
%      Version: $Id: v_roteucode.m 11260 2020-07-18 20:07:58Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

persistent mes trmap zel mch mvch  jch nch
if isempty(mes)     % setup fixed arrays and initialize cache of mode strings
    nch=5;          % size of cache
    mch=cell(nch,1); % cache of input character strings
    mvch=cell(nch,1);  % cache of output mv codes
    flefch=zeros(nch,2);  % cache of output flef codes
    jch=(1:nch); % cache usage order jch(1) is the most recent, jch(nch) the oldest
    for i=1:nch
        mch{i}='';
        mvch{i}=[0;0;1;1;0;0;1];
    end
    mes=[1:3 10:12 7:9 4:6]; % sign reversal look-up table
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % The trmap and zel arrays contain information about each of 52 different  %
    % patterns of -1,0,+1 that may exist in a rotation matrix as follows:      %
    %                                                                          %
    %   1-3 : identity matrix rows in order: 123, 231, 312                     %
    %   4-6 : negated identity matrix rows in order: 132, 213, 321             %
    %   7-12: As 1-6 but with rows 2,3 negated                                 %
    %  13-18: As 1-6 but with rows 1,3 negated                                 %
    %  19-24: As 1-6 but with rows 1,2 negated                                 %
    %  25-33: +1 in position (i-24) and 0's in remainder of this row and col   %
    %  34-42: -1 in position (i-24) and 0's in remainder of this row and col   %
    %  43-51: 0 in position (i-42)                                             %
    %  52: no special symmetry                                                 %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % trmap(i,j) gives the pattern that i is transformed into by rotation j    %
    % where j=1:3 corresponds to x,y,z and j=4:12 corresponds to the 9         %
    % multiples of 90 degree rotations listed in the main comments.            %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    trmap=[ 25 29 33 16 24 11  7 13 19 22 12 17;
        28 32 27 17 22 12  8 14 20 23 10 18;
        31 26 30 18 23 10  9 15 21 24 11 16;
        34 41 39 13 20  9 10 16 22 19  8 15;
        37 35 42 14 21  7 11 17 23 20  9 13;
        40 38 36 15 19  8 12 18 24 21  7 14;
        25 38 42 22  6 23  1 19 13 16 18  5;
        28 41 36 23  4 24  2 20 14 17 16  6;
        31 35 39 24  5 22  3 21 15 18 17  4;
        34 32 30 19  2 21  4 22 16 13 14  3;
        37 26 33 20  3 19  5 23 17 14 15  1;
        40 29 27 21  1 20  6 24 18 15 13  2;
        34 29 42 10 12  5 19  1  7  4 24 23;
        37 32 36 11 10  6 20  2  8  5 22 24;
        40 26 39 12 11  4 21  3  9  6 23 22;
        25 41 30  7  8  3 22  4 10  1 20 21;
        28 35 33  8  9  1 23  5 11  2 21 19;
        31 38 27  9  7  2 24  6 12  3 19 20;
        34 38 33  4 18 17 13  7  1 10  6 11;
        37 41 27  5 16 18 14  8  2 11  4 12;
        40 35 30  6 17 16 15  9  3 12  5 10;
        25 32 39  1 14 15 16 10  4  7  2  9;
        28 26 42  2 15 13 17 11  5  8  3  7;
        31 29 36  3 13 14 18 12  6  9  1  8;
        25 44 45 25 36 26 25 34 34 25 27 35;
        43 26 45 27 26 34 35 26 35 36 26 25;
        43 44 27 35 25 27 36 36 27 26 34 27;
        28 47 48 28 39 29 28 37 37 28 30 38;
        46 29 48 30 29 37 38 29 38 39 29 28;
        46 47 30 38 28 30 39 39 30 29 37 30;
        31 50 51 31 42 32 31 40 40 31 33 41;
        49 32 51 33 32 40 41 32 41 42 32 31;
        49 50 33 41 31 33 42 42 33 32 40 33;
        34 44 45 34 27 35 34 25 25 34 36 26;
        43 35 45 36 35 25 26 35 26 27 35 34;
        43 44 36 26 34 36 27 27 36 35 25 36;
        37 47 48 37 30 38 37 28 28 37 39 29;
        46 38 48 39 38 28 29 38 29 30 38 37;
        46 47 39 29 37 39 30 30 39 38 28 39;
        40 50 51 40 33 41 40 31 31 40 42 32;
        49 41 51 42 41 31 32 41 32 33 41 40;
        49 50 42 32 40 42 33 33 42 41 31 42;
        43 52 52 43 45 44 43 43 43 43 45 44;
        52 44 52 45 44 43 44 44 44 45 44 43;
        52 52 45 44 43 45 45 45 45 44 43 45;
        46 52 52 46 48 47 46 46 46 46 48 47;
        52 47 52 48 47 46 47 47 47 48 47 46;
        52 52 48 47 46 48 48 48 48 47 46 48;
        49 52 52 49 51 50 49 49 49 49 51 50;
        52 50 52 51 50 49 50 50 50 51 50 49;
        52 52 51 50 49 51 51 51 51 50 49 51;
        52 52 52 52 52 52 52 52 52 52 52 52];
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Each Euler angle is chosen so that the inverse rotation forces a specific element  %
    % of the rotation matrix to zero. zel(k,j,i) gives information about which element   %
    % ceases to be zero when a rotation around axis j is applied to pattern i.           %
    %    k=1 gives the index into a vectorized matrix of the entry that becomes non-zero %
    %    k=2 gives the index of the other element in the same column that changes        %
    %    k=3 gives the sign of the sine term affecting the first of these entries        %
    %    k=4 gives the sign of the initial value of the second of these entries if known %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    zel=reshape([  6  5  1  1  3  1 -1  1  2  1  1  1;
        2  3 -1  1  1  3  1  1  5  4  1  1;
        3  2  1  1  4  6  1  1  1  2 -1  1;
        5  6 -1 -1  3  1 -1 -1  2  1  1 -1;
        3  2  1 -1  6  4 -1 -1  1  2 -1 -1;
        2  3 -1 -1  1  3  1 -1  4  5 -1 -1;
        6  5  1 -1  3  1 -1  1  2  1  1  1;
        2  3 -1 -1  1  3  1 -1  5  4  1  1;
        3  2  1 -1  4  6  1 -1  1  2 -1 -1;
        5  6 -1  1  3  1 -1 -1  2  1  1 -1;
        3  2  1  1  6  4 -1 -1  1  2 -1  1;
        2  3 -1  1  1  3  1  1  4  5 -1  1;
        6  5  1  1  3  1 -1 -1  2  1  1 -1;
        2  3 -1 -1  1  3  1 -1  5  4  1 -1;
        3  2  1  1  4  6  1 -1  1  2 -1  1;
        5  6 -1  1  3  1 -1  1  2  1  1  1;
        3  2  1 -1  6  4 -1  1  1  2 -1 -1;
        2  3 -1  1  1  3  1  1  4  5 -1 -1;
        6  5  1 -1  3  1 -1 -1  2  1  1 -1;
        2  3 -1  1  1  3  1  1  5  4  1 -1;
        3  2  1 -1  4  6  1  1  1  2 -1 -1;
        5  6 -1 -1  3  1 -1  1  2  1  1  1;
        3  2  1  1  6  4 -1  1  1  2 -1  1;
        2  3 -1 -1  1  3  1 -1  4  5 -1  1;
        0  0  0  0  3  1 -1  1  2  1  1  1;
        3  2  1  1  0  0  0  0  1  2 -1  1;
        2  3 -1  1  1  3  1  1  0  0  0  0;
        0  0  0  0  6  4 -1  1  5  4  1  1;
        6  5  1  1  0  0  0  0  4  5 -1  1;
        5  6 -1  1  4  6  1  1  0  0  0  0;
        0  0  0  0  9  7 -1  1  8  7  1  1;
        9  8  1  1  0  0  0  0  7  8 -1  1;
        8  9 -1  1  7  9  1  1  0  0  0  0;
        0  0  0  0  3  1 -1 -1  2  1  1 -1;
        3  2  1 -1  0  0  0  0  1  2 -1 -1;
        2  3 -1 -1  1  3  1 -1  0  0  0  0;
        0  0  0  0  6  4 -1 -1  5  4  1 -1;
        6  5  1 -1  0  0  0  0  4  5 -1 -1;
        5  6 -1 -1  4  6  1 -1  0  0  0  0;
        0  0  0  0  9  7 -1 -1  8  7  1 -1;
        9  8  1 -1  0  0  0  0  7  8 -1 -1;
        8  9 -1 -1  7  9  1 -1  0  0  0  0;
        0  0  0  0  1  3  1  1  1  2 -1  1;
        2  3 -1  1  0  0  0  0  2  1  1  1;
        3  2  1  1  3  1 -1  1  0  0  0  0;
        0  0  0  0  4  6  1  1  4  5 -1  1;
        5  6 -1  1  0  0  0  0  5  4  1  1;
        6  5  1  1  6  4 -1  1  0  0  0  0;
        0  0  0  0  7  9  1  1  7  8 -1  1;
        8  9 -1  1  0  0  0  0  8  7  1  1;
        9  8  1  1  9  7 -1  1  0  0  0  0;
        0  0  0  0  0  0  0  0  0  0  0  0]',4,3,52);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Convert the m string
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if ~ischar(m) % lecacy call with integer m argument
    m=char(m+'w'); % convert to characters
end
ich=find(strcmp(m,mch),1);      % check if already in the cache
if isempty(ich)                 % not yet in the cache
    mm=m-'w';                   % convert to integers with x -> 1
    mi=mm>=-31 & mm<=-29;       % find characters XYZ
    mm(mi)=mm(mi)+32;           % convert XYZ to xyz (for compatibility)
    mi=mm>=-70 & mm<=-62;       % find digits 1:9
    mm(mi)=mm(mi)+74;           % convert to 4:12
    mi=mm<=0;                   % select control characters
    mc=mm(mi);                  % controls
    mm=mm(~mi);                 % rotations
    ef=1;                       % angle scale factor
    es=1;                       % angle sign
    fl=1;                       % default to no rotation matrix tranposing
    for i=1:length(mc)
        switch mc(i)
            case -5             % 'r' = radians
            case -19            % 'd' = degrees
                ef=pi/180;      % scale factor to convert to radians
            case -37            % 'R' = negated radians
                ef=-1;
            case -51            % 'D' = negated degrees
                ef=-pi/180;      % scale factor to convert to radians
            case -8             % 'o' = object-extrinsic
            case -40            % 'O' = object-intrinsic
                fl=-1;
                es=-1;
            case -22            % 'a' = axes-extrinsic
                fl=-1;
            case -54            % 'A' = axes-intrinsic
                es=-1;
            otherwise
                error('Invalid character: %s',mc(i)+'w')
        end
    end
    ef=ef*es;               % change sign of scale factor if necessary
    if es<0
        mm=mes(mm);         % sign-reverse: interchage 4,5,6 with 10,11,12
    end
    nm=length(mm);
    mv=zeros(7,nm+1);
    mv(1,:)=[mm 0];
    mv(2,:)=cumsum([mm<=3 0]);      % index into euler angle array
    mv(3,1)=1; % initial pattern is the identity matrix
    for i=1:nm                % loop for each rotation
        mmi=mm(i); % rotation code
        mv(3,i+1)=trmap(mv(3,i),mmi); % pattern ID after rotation
        if mmi<4
            mv(4:7,i)=zel(:,mmi,mv(3,i)); % information about which matrix elements change from zero
        end
    end
    mv(end)=fl;
    mv(end-3)=ef;
    % now save in the cache
    ich=jch(nch);       % find oldest cache entry
    mch{ich}=m;                 % save input string
    mvch{ich}=mv;               % save parameters
    jch=[ich jch(1:nch-1)];     % age all the other cache entries
else                            % already in the cache
    kch=find(jch==ich,1);       % find existing ich entry
    jch(1:kch)=[ich jch(1:kch-1)];
    mv=mvch{ich};               % retrieve from cache
end