function s = logsumexp(x, dim)
% Returns log(sum(exp(x),dim)) while avoiding numerical underflow.
% Default is dim = 1 (columns).
% Written by Mo Chen (mochen@ie.cuhk.edu.hk). March 2009.

% Downloaded from:
% http://www.mathworks.com/matlabcentral/fileexchange/28899-logsumexp/content/logsumexp.m
%
% Copyright (c) 2010, Michael Chen
% All rights reserved.

% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are
% met:

%     * Redistributions of source code must retain the above copyright
%       notice, this list of conditions and the following disclaimer.
%     * Redistributions in binary form must reproduce the above copyright
%       notice, this list of conditions and the following disclaimer in
%       the documentation and/or other materials provided with the distribution

% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
% POSSIBILITY OF SUCH DAMAGE.

if nargin == 1,
    % Determine which dimension sum will use
    dim = find(size(x)~=1,1);
    if isempty(dim), dim = 1; end
end

% subtract the largest in each column
y = max(x,[],dim);
x = bsxfun(@minus,x,y);
s = y + log(sum(exp(x),dim));
i = find(~isfinite(y));
if ~isempty(i)
    s(i) = y(i);
end


function bti_test06 ( )

%*****************************************************************************80
%
%% BTI_TEST06 tests BURGERS_TIME_INVISCID.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 April 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BTI_TEST06\n' );
  fprintf ( 1, '  Test BURGERS_TIME_INVISCID\n' );

  method = 6;
  nx = 81;
  nt = 200;
  t_max = 2.0;
  bc = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Method: 6, Godonov.\n' );
  fprintf ( 1, '  Initial condition: expansion\n' );
  fprintf ( 1, '  Number of space nodes = %d\n', nx );
  fprintf ( 1, '  Number of time steps = %d\n', nt );
  fprintf ( 1, '  Final time T_MAX = %g\n', t_max );
  fprintf ( 1, '  Boundary condition = %d\n', bc );

  U = burgers_time_inviscid ( method, @ic_expansion, nx, nt, t_max, bc );

  x = linspace ( -1.0, +1.0, nx );

  figure ( 6 )

  plot ( x, U(1:50:(nt+1),:), 'Linewidth', 3 )
  grid on
  xlabel ( '<-- X -->' )
  ylabel ( '<-- U(X,T) -->' )
  title ( 'Burgers, inviscid, initial expansion, Godonov' )

  filename = 'bti_test06.png';
  print ( '-dpng', filename )
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Saved plot as "%s"\n', filename );

  return
end


function cnoise_test02 ( m, n, var, r, alpha )

%*****************************************************************************80
%
%% CNOISE_TEST02 tests F_ALPHA_TGAUSSIAN.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 April 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the number of realizations to create.
%
%    Input, integer N, the size of the noise vector.
%
%    Input, real VAR, the variance of the Gaussian distribution.
%
%    Input, real R, defines the truncated range of the distribution.
%
%    Input, real ALPHA, the value of ALPHA.
%
  scale = exp ( 0.5 * ( 1.0 - alpha ) * log ( n ) );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CNOISE_TEST02:\n' );
  fprintf ( 1, '  Test F_ALPHA_TGAUSSIAN.\n' );
  fprintf ( 1, '  Noise is generated from a truncated Gaussian distribution\n' );
  fprintf ( 1, '  with a specified variance and range.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of realizations M = %d\n', m );
  fprintf ( 1, '  Dimension of noise vector N = %d\n', n );
  fprintf ( 1, '  Noise exponent ALPHA = %f\n', alpha );
  fprintf ( 1, '  Variance of Gaussian distribution VAR = %f\n', var );
  fprintf ( 1, '  Truncated range = [ %f, %f ]\n', -r, r );
  fprintf ( 1, '  Scale factor SCALE = %f\n', scale );

  res = zeros ( m, 1 );

  tic
  for i = 1 : m
    x = f_alpha_tgaussian ( n, var, r, alpha );
    x = scale * x;
    res(i) = mean ( x );
  end
  result_time = toc;

  result_mean = mean ( res );
  result_std = std ( res );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Mean value of noise = %f\n', result_mean );
  fprintf ( 1, '  STD of noise        = %f\n', result_std );
  fprintf ( 1, '  Computation time    = %f seconds\n', result_time );

  return
end


function A = randsym(n,cnd)
%RANDSYM      Random symmetric matrix
%
%   res = randsym(n,cnd)
%
% cnd, if specified, is the approximate condition number of the generated matrix
%

% written  11/05/12     S.M. Rump
% modified 12/06/12     S.M. Rump  lower case
%

  if nargin==1
    A = randn(n);
  else
    s = exp( [ 0 rand(1,n-2) 1 ] * log(cnd) ) / cnd;
    A = randorth(n);
    A = A' * diag(s.*sign(randn(1,n))) * A;
  end

  % make sure A is symmetric
  A = tril(A)+tril(A,-1)';


function [K] = spm_sptop(sigma,q,c)
% Sparse Toeplitz convolution matrix given convolution kernel
% FORMAT [K] = spm_sptop(sigma,q,c)
%
% sigma - of Gaussian kernel K (or kernel itself)
% q     - order of matrix
% c     - kernel index at t = 0 {default c = length(sigma)/2)
% K     - q x q sparse convolution matrix
%_______________________________________________________________________
%
% Returns a q x q sparse convolution matrix.  If sigma is a scalar then
% a symmetrical Gaussian convolution matrix is returned with kernel width
% = sigma.  If sigma is a vector than sigma constitutes the kernel.  To
% obtain an assymmetrical convolution matrix (i.e. implement a phase shift
% set c = 1.
%
% Boundary handling: The row-wise sum of K is set to unity (kernel truncation)
%
%_______________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_sptop.m 1143 2008-02-07 19:33:33Z spm $

% if sigma = 0, return identity matrix; if q = 1, return 1.
%-----------------------------------------------------------------------
if ~any(sigma); K = speye(q); return; end
if q == 1;      K = 1;        return; end

% otherwise get kernel function
%-----------------------------------------------------------------------
if length(sigma) == 1
    E  = ceil(3*sigma);
    x  = [-E:E];
    k  = exp(-x.^2/(2*sigma^2));
else
    if nargin < 3
        c = length(sigma)/2;
    end
    E  = length(sigma);
    x  = [1:E] - fix(c);
    k  = sigma;
end

% and create convolution matrix
%-----------------------------------------------------------------------
K  = k(:)*ones(1,q);
j  = ones(length(k),1)*[1:q];
i  = x(:)*ones(1,q) + j;

% setting the row-wise sum to unity
%-----------------------------------------------------------------------
Q  = find((i >= 1) & (i <= q));
K  = sparse(i(Q),j(Q),K(Q));
Q  = sum(K');
K  = inv(diag(Q + (~Q)))*K;


function mbasis = basis_matrix_overhauser_nul ( alpha )

%*****************************************************************************80
%
%% BASIS_MATRIX_OVERHAUSER_NUL sets up the nonuniform left Overhauser spline basis matrix.
%
%  Discussion:
%
%    This basis matrix assumes that the data points P1, P2, and
%    P3 are not uniformly spaced in T, and that P1 corresponds to T = 0,
%    and P2 to T = 1. (???)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real ALPHA.
%    ALPHA = || P2 - P1 || / ( || P3 - P2 || + || P2 - P1 || )
%
%    Output, real MBASIS(3,3), the basis matrix.
%
  mbasis(1,1) =   1.0 / alpha;
  mbasis(1,2) = - 1.0 / ( alpha * ( 1.0 - alpha ) );
  mbasis(1,3) =   1.0 / ( 1.0 - alpha );

  mbasis(2,1) = - ( 1.0 + alpha ) / alpha;
  mbasis(2,2) =   1.0 / ( alpha * ( 1.0 - alpha ) );
  mbasis(2,3) = - alpha / ( 1.0 - alpha );

  mbasis(3,1) =   1.0;
  mbasis(3,2) =   0.0;
  mbasis(3,3) =   0.0;

  return
end


function [ Y, gR, gPhi, gTheta ] = AFMT3( X, sigma, mAFMT, nAFMT, pAFMT, extrapVal, interpMethod, algorithm )
%Computes discrete approximation to analytic fourier-mellin transformation.
%Parameters:
%           x                   ...matrix with input data
%           sigma               ...strictly positive number, ensuring
%                                  existence of transformation
%           mAFMT               ...size of mellin-transformation (check)
%
%           nAFMT               ...size of fourier-transformation (check)

%algorithm selects: F-AFMT      ...fast AFMT
%                   D-AFMT      ...discrete AFMT
%
% (c) Lukas Mauch, Thomas Kuestner
% ---------------------------------------------------------------------

switch algorithm
    %% forward-transformation (3d):
    case 'F-AFMT'
        [yLogPol, gR, gPhi, gTheta] = convToLogPol(X, mAFMT, nAFMT, pAFMT, interpMethod);
        scaleFactor = exp(2*pi*(gR./max(max(max(gR)))) * sigma); %maximal radius normalized to 2*pi
        Y = fftnshift(scaleFactor .* yLogPol, 1:3);

    case 'D-AFMT'

    %% backward-transformation:
    case 'iF-AFMT'
        [sizeY, sizeX, sizeZ]  = size(X);

        %Compute center positions oM and oN:
        oY = fix(mAFMT/2)+1;
        oX = fix(nAFMT/2)+1;
        oZ = fix(pAFMT/2)+1;

        %Cartesian sample points:
        [gridCartX, gridCartY, gridCartZ] = meshgrid( -oX+1:nAFMT-oX, -oY+1:mAFMT-oY, -oZ+1:pAFMT-oZ );

        %Log-polar plane sample points:
        R  = sqrt(max(abs(oY-1), abs(mAFMT-oY))^2 + max(abs(oX-1), abs(nAFMT-oX))^2 + ...
             max(abs(oZ-1), abs(pAFMT-oZ))^2);
        [gridR, gridPhi, gridTheta]   = meshgrid( log(R)/sizeX:log(R)/sizeX:log(R), ...
                                       linspace(0,2*pi, sizeY), ...
                                       linspace(-pi/2, pi/2, sizeZ));

        scaleFactor = exp(-2*pi*sigma*(gridR./max(max(max(gridR)))));
        yLogPol = scaleFactor .* ifftnshift(X, 1:3);

        Y = convToCart(yLogPol, gridR, gridPhi, gridTheta, gridCartX, gridCartY, gridCartZ, interpMethod);

    case 'iD-AFMT'

    otherwise
    error('Unknown type of algorithm!')

end

    %% helper functions:
    function [xLogPol, gridR, gridPhi, gridTheta] = convToLogPol( xCart, sizeR, sizePhi, sizeTheta, interpMethod )
        [m, n, p]  = size(xCart);

        %Compute center positions oM and oN:
        oM = fix(m/2)+1;
        oN = fix(n/2)+1;
        oP = fix(p/2)+1;

        %Cartesian sample points:
        [gridX, gridY, gridZ] = meshgrid( -oN+1:n-oN, fliplr(-oM+1:m-oM), -oP+1:p-oP );

        %Log-polar plane sample points:
        maxR = sqrt(max(abs(oN-1), abs(n-oN))^2 + max(abs(oM-1), abs(m-oM))^2 + max(abs(oP-1), abs(p-oP))^2);
        [gridR, gridPhi, gridTheta]   = meshgrid( log(maxR)/sizeR:log(maxR)/sizeR:log(maxR), ...
                                      linspace(0,2*pi,sizePhi), ...
                                      linspace(-pi/2, pi/2, sizeTheta));

        %Perform conversion to log-polar coordinates, using linear interpolation:
        convGridX   = exp(gridR) .* cos(gridPhi) .* cos(gridTheta);
        convGridY   = exp(gridR) .* sin(gridPhi) .* cos(gridTheta);
        convGridZ   = exp(gridR) .* sin(gridTheta);
        xLogPol     = interp3(gridX, gridY, gridZ, xCart, ...
                        convGridX, convGridY, convGridZ, interpMethod, extrapVal);
    end

    function xCart = convToCart( xLogPol, gridR, gridPhi, gridTheta, gridX, gridY, gridZ, interpMethod )
        %Perform conversion to cartesian coordinates:
%         convGridX   = reshape(exp(gridR) .* cos(gridPhi) .* cos(gridTheta),[], 1);
%         convGridY   = reshape(exp(gridR) .* sin(gridPhi) .* cos(gridTheta),[], 1);
%         convGridZ   = reshape(exp(gridR) .* sin(gridTheta),[], 1);
%         F = TriScatteredInterp(convGridX, convGridY, convGridZ, reshape(xLogPol, [], 1), interpMethod);
%         xCart       = F(gridX, gridY, gridZ);

        %interpolation in r-phi plane (worse but fast):
        convGridR       = sqrt(abs(gridX).^2 + abs(gridY).^2 + abs(gridZ).^2);
        convGridPhi     = atan(gridY./gridX);
        convGridTheta   = asin(gridZ./convGridR);
        %unwrap:
        qA = logical(logical((gridX >= 0)) .* logical((gridY<=0)));
        qB = logical(logical((gridX < 0)) .* logical((gridY<=0)));
        qC = logical(logical((gridX < 0)) .* logical((gridY>0)));
        qD = logical(logical((gridX >= 0)) .* logical((gridY>0)));
        convGridPhi(qA) = abs(convGridPhi(qA));
        convGridPhi(qB) = pi-convGridPhi(qB);
        convGridPhi(qC) = pi+abs(convGridPhi(qC));
        convGridPhi(qD) = 2*pi-convGridPhi(qD);
        convGridPhi(isnan(convGridPhi)) = 0;
        convGridTheta(isnan(convGridTheta)) = 0;

        xCart       = interp3(exp(gridR), gridPhi, gridTheta, xLogPol, ...
                        convGridR, convGridPhi, convGridTheta, interpMethod, extrapVal);

    end

end


function [L, U, P, Q] = gecp(A)
%GECP calculate Gauss elimination with complete pivoting
%
% (G)aussian (E)limination (C)omplete (P)ivoting
% Input : A nxn matrix
% Output
% L = Lower triangular matrix with ones as diagonals
% U = Upper triangular matrix
% P and Q permutations matrices so that P*A*Q = L*U
%
% See also LU
%
% written by : Cheilakos Nick
[n, n] = size(A);
p = 1:n;
q = 1:n;
for k = 1:n-1
    [maxc, rowindices] = max( abs(A(k:n, k:n)) );
    [maxm, colindex] = max(maxc);
    row = rowindices(colindex)+k-1; col = colindex+k-1;
    A( [k, row], : ) = A( [row, k], : );
    A( :, [k, col] ) = A( :, [col, k] );
    p( [k, row] ) = p( [row, k] ); q( [k, col] ) = q( [col, k] );
    if A(k,k) == 0
      break
    end
    A(k+1:n,k) = A(k+1:n,k)/A(k,k);
    i = k+1:n;
    A(i,i) = A(i,i) - A(i,k) * A(k,i);
end
L = tril(A,-1) + eye(n);
U = triu(A);
   P = eye(n);
   P = P(p,:);
   Q = eye(n);
   Q = Q(:,q);


function line = fitLine3d(points)
%FITLINE3D Fit a 3D line to a set of points.
%
%   LINE = fitLine3d(PTS)
%
%   Example
%     pts = randn(300, 3);
%     pts = transformPoint3d(pts, createScaling3d([6 4 2]));
%     pts = transformPoint3d(pts, createRotationOx(pi/6));
%     pts = transformPoint3d(pts, createRotationOy(pi/4));
%     pts = transformPoint3d(pts, createRotationOz(pi/3));
%     pts = transformPoint3d(pts, createTranslation3d([5 4 3]));
%     elli = equivalentEllipsoid(pts);
%     figure; drawPoint3d(pts); axis equal;
%     hold on; drawEllipsoid(elli, ...
%         'drawEllipses', true, 'EllipseColor', 'b', 'EllipseWidth', 3);
%     line = fitLine3d(pts);
%     drawLine3d(line, 'color', 'm', 'LineWidth', 4);
%
%   See also
%     lines3d, equivalentEllipsoid, fitPlane
%

% ------
% Author: David Legland
% E-mail: david.legland@grignon.inra.fr
% Created: 2012-11-11, using Matlab 7.9.0.529 (R2009b)
% Copyright 2012-2022 INRA - Cepia Software Platform

% number of points
n = size(points, 1);

% compute centroid
center = mean(points);

% compute the covariance matrix
covPts = cov(points)/n;

% perform a principal component analysis with 2 variables,
% to extract inertia axes
[U, S] = svd(covPts);

% sort axes from greater to lower
[dummy, ind] = sort(diag(S), 'descend'); %#ok<ASGLU>

% format U to ensure first axis points to positive x direction
U = U(ind, :);
if U(1,1) < 0
    U = -U;
    % keep matrix determinant positive
    U(:,3) = -U(:,3);
end

line = [center U(:,1)'];


function [gx, gy, gz] = gaussPtetra(X1, X2, X3, X4, ng)

%% USAGE: generate Gaussian nodes on tetra mesh
%
% INPUTS:
% p --- nt-by-3 vector
% t --- nt-by-4 vector
% ng --- number of Gaussian nodes in an triangle
%            possible values = 1, 4, 5, 11, 15.
%
% OUTPUTS:
% gx --- nt-by-ng vector: stores x coordinates of Gaussian nodes
% gy --- nt-by-ng vector: stores y coordinates of Gaussian nodes
% gz --- nt-by-ng vector: stores z coordinates of Gaussian nodes
%
% Last Modified: 07/02/2020 by Xu Zhang
%%

G = zeros(size(X1,1),3*ng); i = 1;

if ng == 1 % accurate up to polynomial degree p = 1

    w1 = 1/4; % point at centroid
    G(:,[i,i+ng,i+2*ng]) = w1*(X1+X2+X3+X4);

elseif ng == 4 % accurate up to polynomial degree p = 2

    w1 = 0.585410196624969; w2 = 0.138196601125011; % median line
    G(:,[i,i+ng,i+2*ng]) = w1*X1 + w2*(X2+X3+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*X2 + w2*(X1+X3+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*X3 + w2*(X1+X2+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*X4 + w2*(X1+X2+X3);

elseif ng == 5 % accurate up to polynomial degree p = 3

    w1 = 1/4; % point at centroid
    G(:,[i,i+ng,i+2*ng]) = w1*(X1+X2+X3+X4); i = i+1;

    w1 = 1/2; w2 = 1/6;% median line
    G(:,[i,i+ng,i+2*ng]) = w1*X1 + w2*(X2+X3+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*X2 + w2*(X1+X3+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*X3 + w2*(X1+X2+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*X4 + w2*(X1+X2+X3);

elseif ng == 11 % accurate up to polynomial degree p = 4

    w1 = 1/4; % point at centroid
    G(:,[i,i+ng,i+2*ng]) = w1*(X1+X2+X3+X4); i = i+1;

    w1 = 0.785714285714286; w2 = 0.071428571428571; % median line
    G(:,[i,i+ng,i+2*ng]) = w1*X1 + w2*(X2+X3+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*X2 + w2*(X1+X3+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*X3 + w2*(X1+X2+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*X4 + w2*(X1+X2+X3); i = i+1;

    w1 = 0.399403576166799; w2 = 0.100596423833201; % line opposite sides
    G(:,[i,i+ng,i+2*ng]) = w1*(X1+X2) + w2*(X3+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*(X1+X3) + w2*(X2+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*(X1+X4) + w2*(X2+X3); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*(X2+X3) + w2*(X1+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*(X2+X4) + w2*(X1+X3); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*(X3+X4) + w2*(X1+X2);

elseif ng == 15 % accurate up to polynomial degree p = 5

    w1 = 1/4; % point at centroid
    G(:,[i,i+ng,i+2*ng]) = w1*(X1+X2+X3+X4); i = i+1;

    w1 = 0; w2 = 1/3; % median line
    G(:,[i,i+ng,i+2*ng]) = w1*X1 + w2*(X2+X3+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*X2 + w2*(X1+X3+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*X3 + w2*(X1+X2+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*X4 + w2*(X1+X2+X3); i = i+1;

    w1 = 0.727272727272727; w2 = 0.090909090909091; % median line
    G(:,[i,i+ng,i+2*ng]) = w1*X1 + w2*(X2+X3+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*X2 + w2*(X1+X3+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*X3 + w2*(X1+X2+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*X4 + w2*(X1+X2+X3); i = i+1;

    w1 = 0.066550153573664; w2 = 0.433449846426336; % line opposite sides
    G(:,[i,i+ng,i+2*ng]) = w1*(X1+X2) + w2*(X3+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*(X1+X3) + w2*(X2+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*(X1+X4) + w2*(X2+X3); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*(X2+X3) + w2*(X1+X4); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*(X2+X4) + w2*(X1+X3); i = i+1;
    G(:,[i,i+ng,i+2*ng]) = w1*(X3+X4) + w2*(X1+X2);

end
gx = G(:,1:ng); gy = G(:,ng+1:2*ng); gz = G(:,2*ng+1:3*ng);


function [pospeakind,negpeakind]=peakdetect(signal)

%	PEAKDETECT peak detection
%
%	[pospeakind,negpeakind]=peakdetect(signal)
%
%	The positive and negative polarity (concave down and up) peak index vectors are
%	generated from the signal vector and graphically displayed.  Positive and negative
%	polarity peaks occur at points of positive to negative and negative to positive
%	slope adjacency, respectively.  The typically rare contingencies of peaks
%	occurring at the lagging edges of constant intervals are supported.  Complex
%	signals are modified to the modulus of the elements.  If unspecified, the signal
%	vector is entered after the prompt from the keyboard.

%	Implemented using MATLAB 6.0.0
%
%	Examples:
%
%	ť [p,n]=peakdetect([-1 -1 0 1 0 1 0 -1 -1])
%
%	p =
%
%	     4     6
%
%	n =
%
%	     1     5     8
%
%	ť [p,n]=peakdetect(cos(2*pi*(0:999999)/500000))
%
%	p =
%
%	           1      500001     1000000
%
%	n =
%
%	      250001      750001
%
%	Copyright (c) 2001
%	Tom McMurray
%	mcmurray@teamcmi.com

%	if signal is not input, enter signal or return for empty outputs

if ~nargin
   signal=input('enter signal vector or return for empty outputs\n');
   if isempty(signal)
      pospeakind=[];
      negpeakind=[];
      return
   end
end
sizsig=size(signal);

%	while signal is unsupported, enter supported signal or return for empty outputs

while isempty(signal)|~isnumeric(signal)|~all(all(isfinite(signal)))...
      |length(sizsig)>2|min(sizsig)~=1
   signal=input(['signal is empty, nonnumeric, nonfinite, or nonvector:\nenter '...
         'finite vector or return for empty outputs\n']);
   if isempty(signal)
      pospeakind=[];
      negpeakind=[];
      return
   end
   sizsig=size(signal);
end

%	if signal is complex, modify to modulus of the elements

if ~isreal(signal)
   signal=abs(signal);
end

%	if signal is constant, return empty outputs

if ~any(signal-signal(1))
   pospeakind=[];
   negpeakind=[];
   disp('constant signal graph suppressed')
   return
end
sizsig1=sizsig(1);
lensig=sizsig1;

%	if signal is a row vector, modify to a column vector

if lensig==1
   signal=signal(:);
   lensig=sizsig(2);
end
lensig1=lensig-1;
lensig2=lensig1-1;

%	if signal length is 2, return max/min as positive/negative polarity peaks

if ~lensig2
   [sig,pospeakind]=max(signal);
   [sig,negpeakind]=min(signal);
   disp('2 element signal graph suppressed')
   return
end

%	generate difference signal

difsig=diff(signal);

%	generate vectors corresponding to positive slope indices

dsgt0=difsig>0;
dsgt00=dsgt0(1:lensig2);
dsgt01=dsgt0(2:lensig1);

%	generate vectors corresponding to negative slope indices

dslt0=difsig<0;
dslt00=dslt0(1:lensig2);
dslt01=dslt0(2:lensig1);

%	generate vectors corresponding to constant intervals

dseq0=difsig==0;
dseq01=dseq0(2:lensig1);
clear difsig

%	positive to negative slope adjacencies define positive polarity peaks

pospeakind=find(dsgt00&dslt01)+1;

%	negative to positive slope adjacencies define negative polarity peaks

negpeakind=find(dsgt01&dslt00)+1;

%	positive slope to constant interval adjacencies initiate positive polarity peaks

peakind=find(dsgt00&dseq01)+1;
lenpeakind=length(peakind);

%	determine positive polarity peak terminations

for k=1:lenpeakind
   peakindk=peakind(k);
   l=peakindk+1;

%	if end constant interval occurs, positive polarity peak exists

   if l==lensig
      pospeakind=[pospeakind;peakindk];

%	else l<lensig, determine next nonzero slope index

   else
      dseq0l=dseq0(l);
      while dseq0l&l<lensig1
         l=l+1;
         dseq0l=dseq0(l);
      end

%	if negative slope or end constant interval occurs, positive polarity peaks exist

      if dslt0(l)|dseq0l;
         pospeakind=[pospeakind;peakindk];
      end
   end
end

%	negative slope to constant interval adjacencies initiate negative polarity peaks

peakind=find(dslt00&dseq01)+1;
lenpeakind=length(peakind);
clear dseq01

%	determine negative polarity peak terminations

for k=1:lenpeakind
   peakindk=peakind(k);
   l=peakindk+1;

%	if end constant interval occurs, negative polarity peak exists

   if l==lensig
      negpeakind=[negpeakind;peakindk];

%	else l<lensig, determine next nonzero slope index

   else
      dseq0l=dseq0(l);
      while dseq0l&l<lensig1
         l=l+1;
         dseq0l=dseq0(l);
      end

%	if positive slope or end constant interval occurs, negative polarity peaks exist

      if dsgt0(l)|dseq0l;
         negpeakind=[negpeakind;peakindk];
      end
   end
end
clear dsgt0 peakind

%	if initial negative slope occurs, initial positive polarity peak exists

if dslt00(1)
   pospeakind=[1;pospeakind];

%	elseif initial positive slope occurs, initial negative polarity peak exists

elseif dsgt00(1)
   negpeakind=[1;negpeakind];

%	else initial constant interval occurs, determine next nonzero slope index

else
   k=2;
   dseq0k=dseq0(2);
   while dseq0k
      k=k+1;
      dseq0k=dseq0(k);
   end

%	if negative slope occurs, initial positive polarity peak exists

   if dslt0(k)
      pospeakind=[1;pospeakind];

%	else positive slope occurs, initial negative polarity peak exists

   else
      negpeakind=[1;negpeakind];
   end
end
clear dsgt00 dslt0 dslt00 dseq0

%	if final positive slope occurs, final positive polarity peak exists

if dsgt01(lensig2)
   pospeakind=[pospeakind;lensig];

%	elseif final negative slope occurs, final negative polarity peak exists

elseif dslt01(lensig2)
   negpeakind=[negpeakind;lensig];
end
clear dsgt01 dslt01

%	if peak indices are not ascending, order peak indices

if any(diff(pospeakind)<0)
   pospeakind=sort(pospeakind);
end
if any(diff(negpeakind)<0)
   negpeakind=sort(negpeakind);
end

%	if signal is a row vector, modify peak indices to row vectors

if sizsig1==1
   pospeakind=pospeakind.';
   negpeakind=negpeakind.';
end

%	plot signal peaks

plot(0:lensig1,signal,pospeakind-1,signal(pospeakind),'b^',negpeakind-1,...
   signal(negpeakind),'bv')
xlabel('Sample')
ylabel('Signal')
grid


function shepard_interp_2d_test01 ( prob, g, p )

%*****************************************************************************80
%
%% SHEPARD_INTERP_2D_TEST01 tests SHEPARD_INTERP_2D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 August 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer PROB, the problem number.
%
%    Input, integer G, the grid number.
%
%    Input, real P, the power used in the distance weighting.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SHEPARD_INTERP_2D_TEST01:\n' );
  fprintf ( 1, '  Interpolate data from TEST_INTERP_2D problem #%d\n', prob );
  fprintf ( 1, '  using grid #%d\n', g );
  fprintf ( 1, '  using Shepard interpolation with P = %g\n', p );

  nd = g00_size ( g );
  fprintf ( 1, '  Number of data points = %d\n', nd );

  [ xd, yd ] = g00_xy ( g, nd );

  zd = f00_f0 ( prob, nd, xd, yd );

  if ( p == 1.0 )
    r8vec3_print ( nd, xd, yd, zd, '  X, Y, Z data:' );
  end
%
%  #1:  Does interpolant match function at interpolation points?
%
  ni = nd;
  xi = xd;
  yi = yd;
  zi = shepard_interp_2d ( nd, xd, yd, zd, p, ni, xi, yi );

  int_error = norm ( zi - zd ) / ni;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  L2 interpolation error averaged per interpolant node = %g\n', int_error );
%
%  #2: Plot the linear interpolant.
%
  if ( p == 1.0 )
    figure ( 1 );
    clf
    hold on
    tri = delaunay ( xd, yd );
    h = trisurf ( tri, xd, yd, zd );
    ft = f00_title ( prob );
    title ( ft );
    grid on
    xlabel ( '<--- X --->' );
    ylabel ( '<--- Y --->' );
    zlabel ( '<---Z(X,Y)--->' );
    l = light ( 'Position', [ 0, 0, 25 ] );
    set ( gca, 'CameraPosition', [ -10, -10, 20 ] );
    lighting phong
    shading interp
    hold off
    filename = sprintf ( 'p%02d_data.png', prob );
    print ( '-dpng', filename );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Created plot file "%s".\n', filename );
  end
%
%  #3: Plot the Shepard interpolant.
%
  figure ( 2 );
  clf
  nix = 21;
  niy = 21;
  xmin = 0.0;
  xmax = 1.0;
  ymin = 0.0;
  ymax = 1.0;
  xi = linspace ( xmin, xmax, nix );
  yi = linspace ( ymin, ymax, niy );
  [XI,YI] = meshgrid ( xi, yi );
  NI = nix * niy;
  XI = reshape ( XI, NI, 1 );
  YI = reshape ( YI, NI, 1 );
  ZI = shepard_interp_2d ( nd, xd, yd, zd, p, NI, XI, YI );
  XI = reshape ( XI, nix, niy );
  YI = reshape ( YI, nix, niy );
  ZI = reshape ( ZI, nix, niy );
  surf ( XI, YI, ZI );
  xlabel ( '<--- X --->' );
  ylabel ( '<--- Y --->' );
  zlabel ( '<---Z(X,Y)--->' );
  title ( sprintf ( 'Shepard interpolant for problem %d with P = %g', prob, p ) )
  grid on
  hold off
  filename = sprintf ( 'p%02d_power%g.png', prob, p );
  print ( '-dpng', filename );
  fprintf ( 1, '  Created plot file "%s".\n', filename );

  return
end


function mesh = mshSphere(N,rad)
%+========================================================================+
%|                                                                        |
%|                 OPENMSH - LIBRARY FOR MESH MANAGEMENT                  |
%|           openMsh is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : mshSphere.m                                   |
%|    #    |   VERSION    : 0.40                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 14.03.2018                                    |
%| ( === ) |   SYNOPSIS   : Build uniform mesh for a sphere               |
%|  `---'  |                                                              |
%+========================================================================+

% Fibonnacci rules
or     = (1+sqrt(5))/2;
theta  = (mod((2*pi/or) .* (0:N-1),2*pi))';
phi    = asin( -1 + 2/(N-1) * (0:N-1))';

% Carthesian coordinates
[x,y,z] = sph2cart(theta,phi,rad);
X       = [0 0 0 ; x y z];

% Delaunay triangulation
DT        = delaunayTriangulation(X);
[elt,vtx] = freeBoundary(DT);

% Mesh
mesh = msh(vtx,elt);
end


function result = AngleAxisRotatePoint(angle_axis, pt)

angle_axis = angle_axis(1:3);

theta2 = dot(angle_axis,angle_axis);

if (theta2 > 0.0)
    % Away from zero, use the rodriguez formula
    %
    %   result = pt costheta + (w x pt) * sintheta + w (w . pt) (1 - costheta)
    %
    % We want to be careful to only evaluate the square root if the
    % norm of the angle_axis vector is greater than zero. Otherwise
    % we get a division by zero.

    theta = sqrt(theta2);
    w = angle_axis / theta;

    costheta = cos(theta);
    sintheta = sin(theta);

    % w_cross_pt = cross(w, pt);
    w_cross_pt = xprodmat(w) * pt;

    %w_dot_pt = dot(w, pt);
    w_dot_pt = w * pt;

    result = pt * costheta + w_cross_pt * sintheta + (w' * (1 - costheta)) * w_dot_pt;

else
    % Near zero, the first order Taylor approximation of the rotation
    % matrix R corresponding to a vector w and angle w is
    %
    %   R = I + hat(w) * sin(theta)
    %
    % But sintheta ~ theta and theta * w = angle_axis, which gives us
    %
    %  R = I + hat(w)
    %
    % and actually performing multiplication with the point pt, gives us
    % R * pt = pt + w x pt.
    %
    % Switching to the Taylor expansion at zero helps avoid all sorts
    % of numerical nastiness.

    %w_cross_pt = cross(angle_axis, pt);
    w_cross_pt = xprodmat(angle_axis) * pt; % vectorize version

    result = pt + w_cross_pt;

end


function [IF_phase_unwrap, GDD_phase, MGD_phase]=phase_feature_extraction(sp_complex, sp_delay)

% This function is using for different kinds of phase feature extraction,
% include instantaneous frequency deviation (IF), group delay deviation
% (GDD), modified group delay (MGD), phase distortion (PD), phase
% distortion standard deviation (PDD) and relative phase shift (RPS).
%
% input:
%     sp_complex:   [n*d] n is frame number (time), d is the feature dimension (frequency)
%                   STFT of target waveform x(n) which contain both magnitude and phase information
%     sp_delay:     [n*d] n is frame number, d is the feature dimension
%                   STFT of modified target waveform n*x(n)
%
% output:
%                   [n*d] n is frame number (time), d is the feature dimension (frequency)
%     IF_phase:     instantaneous frequency deviation
%     GDD_phase:    group delay deviation
%     MGD_phase:    modified group delay
%     PD_phase:     phase distortion
%     PDD_phase:    phase distortion standard deviation
%     RPS_phase:    relative phase shift
%

is_plot = 1;

phase = angle(sp_complex);
phase_unwrap = unwrap(phase);

% [~, IF_phase] = gradient(phase); % deviation along time aixs
[~, IF_phase_unwrap] = gradient(phase_unwrap); % deviation along time aixs

GDD_phase = group_delay_feature(sp_complex); % deviation along frequency aixs

MGD_phase = modified_group_delay_raw(sp_complex, sp_delay);

if is_plot
    figure
    subplot(5,1,1)
    imagesc(phase');colormap jet
    title('original phase')

    subplot(5,1,2)
    imagesc(phase_unwrap');colormap jet
    title('original unwraped phase')

    subplot(5,1,3)
    imagesc(IF_phase_unwrap');colormap jet
    title('instantaneous frequency deviation')

    subplot(5,1,4)
    gdd = GDD_phase.^ 0.4;
    imagesc(real(gdd'));colormap jet
    title('group delay deviation')

    subplot(5,1,5)
    imagesc(MGD_phase');colormap jet
    title('modified group delay')
end

% RPS_phase = relative_phase_shift(phase);
%
% PD_phase = phase_distortion(phase);

% PDD_phase = phase_distortion_std_deviation(phase)


% [mu,S] = mean_cov(x)
% x is n x d
function [mu,S] = mean_cov(x)

%[n,d] = size(x);

z = size(x,1);
mu = sum(x,1)/z;

diffs = bsxfun(@minus,x,mu);
S = (diffs'*diffs)/z;


function f = kappa_inf(tau, P)
%  This function computes approximation to kappa in the limit of n->inf
%
x0 = norminv( (tau+1)/2, 0, 1);
VP = 2 * (1 + 2*P);
VQ = 2 * (1 + P)^2;
f = 2*normcdf(sqrt(VP/VQ) * x0, 0, 1) - 1;


function [H] = rigidbody(f);

% RIGIDBODY creates the homogenous spatial transformation matrix
% for a 6 parameter rigid-body transformation
%
% Use as
%   [H] = rigidbody(f)
%
% The transformation vector f should contain the
%   x-shift
%   y-shift
%   z-shift
% followed by the
%   pitch (rotation around x-axis)
%   roll  (rotation around y-axis)
%   yaw   (rotation around z-axis)

% Copyright (C) 2000-2005, Robert Oostenveld
%
% 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

% $Log: rigidbody.m,v $
% Revision 1.1  2009/01/30 04:02:11  arno
% *** empty log message ***
%
% Revision 1.4  2006/04/13 10:37:38  roboos
% added a ; to the end of a line
%
% Revision 1.3  2005/08/15 08:15:32  roboos
% reimplemented the rotate function, which contained an error (the error is in the AIR technical reference)
% changed all functions to be dependent on the rotate, translate and scale function
% all functions now behave consistenly, which also means that they are not compleetly backward compatible w.r.t. the order of the rotations
%
% Revision 1.2  2004/05/19 09:57:07  roberto
% added GPL copyright statement, added CVS log item
%

% compute the homogenous transformation matrix for the translation
T = translate(f([1 2 3]));

% compute the homogenous transformation matrix for the rotation
R = rotate(f([4 5 6]));

% compute the homogenous transformation matrix for the combination
H = T*R;


%                        experimentImage2D.m
%
% Create a Fourier measurement operator and measurements for reconstructing
% an image.
%
% Inputs:
% numMasks: number of random octanary Fourier masks.
% imagePath: The path of the image to be recovered
%
% Outputs:
%  A     : A function handle: n1*n2 x 1 -> n1*n2*numMasks x 1. It returns
%          A*x.
%  At    : The transpose of A
%  b0    : A n1*n2*numMasks x 1 real, non-negative vector consists of the
%          measurements abs(A*x).
%  Xt    : The true signal - a vectorization of the image
%
%
%
% PhasePack by Rohan Chandra, Ziyuan Zhong, Justin Hontz, Val McCulloch,
% Christoph Studer, & Tom Goldstein
% Copyright (c) University of Maryland, 2017

%% -----------------------------START----------------------------------

function [A, At, b0, Xt, plotter] = experimentImage2D(numMasks, imagePath)
    %%  Read Image
    % Below Xt is n1 x n2 x 3; i.e. we have three n1 x n2 images, one for each of the 3 color channels
    image = rgb2gray(imread([imagePath]));
    image = double(image);
    dims = size(image);
    L = numMasks;

    %% Make octanary masks and linear sampling operators
    % Each mask has iid entries following the octanary pattern.
    % Entries have the form b = b1*b2 , where b1 is sampled
    % from {-1, 1, -i, i} with equal probability 1/4, and b2 from
    % { sqrt(1/2), sqrt(3)} with probability 4/5 and 1/5 respectively.
    b1 = [-1;1;-1i;1i];
    b2 = [repmat(sqrt(0.5),4,1); sqrt(3)];
    % Masks has size [n1,n2,L]
    masks = b1(randi(4,[dims,L])) .* b2(randi(5,[dims,L])); % Storage for L masks, each of dim n1 x n2

    %% Make linear operators that act on a vectorized image
    A = @(x) fourierMeasurementOperator(x, masks, dims);
    At = @(y) transposeOperator(y, masks, dims);

    Xt = image(:);
    b0 = abs(A(Xt(:)));

    % Set up plotting
    subplot(1,2,1);
    imagesc(image);
    title('original');
    subplot(1,2,2);

    plotter = @(x) imagesc(reshape(abs(x),dims));

end

function y = fourierMeasurementOperator(x, masks, dims)
    x = reshape(x, dims);   % image comes in as a vector.  Reshape to rectangle
    [n1,n2] = size(x);
    L = size(masks,3);              % get number of masks

    % Compute measurements
    copies = repmat(x,[1,1,L]);
    y = fft2(masks.*copies);
    y = y(:);
end

function x = transposeOperator(y, masks, dims)
    n1 = dims(1);
    n2 = dims(2);
    L = size(masks,3);              % get number of masks
    y = reshape(y, [n1,n2,L]);   % image comes in as a vector.  Reshape to rectangle

    x = n1*n2*ifft2(y).*conj(masks);
    x = sum(x,3);
    x = x(:);
end


  function [fw, hr, hl] = fwhm1(psfs, varargin)
%|function [fw, hr, hl] = fwhm1(psfs, options)
%|
%| compute fwhm of point-spread function centered at pixel imid
%| and half-right width and half-left width (fw = hr + hl)
%|
%| in
%|	psfs	[np nc]	column psf(s)
%|
%| option
%|	imid	[1]	which pixel is the middle (default: peak)
%|	dx	[1]	pixel size (default: 1)
%|	min0	0|1	if 1, then negative psf value set to 0 (default: 1)
%|	chat	0|1	if 1, then plot
%| out
%|	fw	[nc]	fwhm of each psf column
%|
%| 2012-09-13, added 'min0' option to avoid fwhm < 1

if ~nargin, ir_usage, end
if streq(psfs, 'test'), fwhm1_test, return, end

arg.chat = 0;
arg.dx = 1;
arg.imid = [];
arg.min0 = true;
arg = vararg_pair(arg, varargin);

psfs = squeeze(psfs); % in case [1 1 np]
[np nc] = size(psfs);
if (np == 1) % single row
	psfs = psfs';
	[np nc] = size(psfs);
end

warned = false;
for ic = 1:nc
	psf = psfs(:,ic);
	if isempty(arg.imid)
		imid = imax(psf);
	else
		imid = arg.imid;
	end

	% normalize
	psf = psf / psf(imid);
	if ~warned && (1 ~= max(psf))
		warn 'peak not at center'
		warned = true;
	end

	% right
	ir = sum(cumprod(double(psf((imid+1):np) >= 0.5)));
	if (imid + ir == np)
		hr(ic,1) = ir;
	else
		high = psf(imid + ir);
		ilow = imid + ir + 1;
		low = psf(ilow);
		if arg.min0 && low < 0
			warn('psf(%d) = %g < 0, so using 0', ilow, low)
			low = 0;
		end
		hr(ic,1) = ir + (high - 1/2) / (high-low);
	end

	% left
	il = sum(cumprod(double(psf((imid-1):-1:1) >= 0.5)));
	if (il == imid-1)
		hl(ic,1) = il;
	else
		high = psf(imid - il);
		ilow = imid - il - 1;
		low = psf(ilow);
		if arg.min0 && low < 0
			warn('psf(%d) = %g < 0, so using 0', ilow, low)
			low = 0;
		end
		hl(ic,1) = il + (high - 1/2) / (high-low);
	end
end

hr = hr * arg.dx;
hl = hl * arg.dx;
fw = hr + hl;

if arg.chat && im
	plot(([1:np]-imid)*arg.dx, psf, '-o', ...
	[-hl hr], [0.5 0.5], '-')
	xlabel 'x', ylabel 'psf(x)'
	title(sprintf('fwhm=%g', fw))
end

% fwhm1_test
function fwhm1_test
dx = 3;
nx = 100;
xx = [-nx/2:nx/2-1]' * dx;
fx = 30;
sx = fx / sqrt(log(256));
psf = exp(-((xx/sx).^2)/2);
fw = fwhm1(psf, 'dx', dx, 'chat', 1);

psf = zeros(7,1); psf(5) = 2; psf([4 6]) = -0.2;
fw = fwhm1(psf, 'dx', dx, 'chat', 1);


function x = r8sto_sl ( n, a, b )

%*****************************************************************************80
%
%% R8STO_SL solves a R8STO system.
%
%  Discussion:
%
%    The R8STO storage format is used for a symmetric Toeplitz matrix.
%    It stores the N elements of the first row.
%
%    For this routine, the matrix is also required to be positive definite.
%
%    This implementation of the algorithm assumes that the diagonal element
%    is 1.
%
%    The real symmetric Toeplitz matrix can be described by N numbers, which,
%    for convenience, we will label A(0:N-1).
%
%    Note that there is a typographical error in the presentation
%    of this algorithm in the reference, and another in the presentation
%    of a sample problem.  Both involve sign errors.  A minor error
%    makes the algorithm incorrect for the case N = 1.
%
%  Example:
%
%    To solve
%
%     1.0 0.5 0.2    x1    4.0
%     0.5 1.0 0.5 *  x2 = -1.0
%     0.2 0.5 1.0    x3    3.0
%
%    we input:
%
%      N = 3
%      A(0:N-1) = (/ 1.0, 0.5, 0.2 /)
%      B(1:3) = (/ 4.0, -1.0, 3.0 /)
%
%    with output:
%
%      X(1:3) = (/ 355, -376, 285 /) / 56
%             = (/ 6.339, -6.714, 5.089 /)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 March 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Gene Golub and Charles Van Loan,
%    Section 4.7.3, "The General Right Hand Side Problem",
%    Matrix Computations,
%    Third Edition,
%    Johns Hopkins, 1996.
%
%  Parameters:
%
%    Input, integer N, the order of the system.
%
%    Input, real A(N), the R8STO matrix.
%
%    Input, real B(N), the right hand side of the linear system.
%
%    Output, real X(N), the solution of the linear system.
%
  k = 0;

  beta = 1.0E+00;
  x(k+1) = b(k+1) / beta;

  if ( k < n-1 )
    y(k+1) = -a(k+2) / beta;
  end

  for k = 1 : n-1

    beta = ( 1.0E+00 - y(k) * y(k) ) * beta;

    x(k+1) = ( b(k+1) - a(2:k+1) * x(k:-1:1)' ) / beta;

    x(1:k) = x(1:k) + x(k+1) * y(k:-1:1);

    if ( k < n - 1 )
      y(k+1) = ( -a(k+2) - a(2:k+1) * y(k:-1:1)' ) / beta;
      y(1:k) = y(1:k) + y(k+1) * y(k:-1:1);
    end

  end

  return
end


function [ n_data, mu, sigma, a, x, fx ] = ...
  truncated_normal_a_pdf_values ( n_data )

%*****************************************************************************80
%
%% TRUNCATED_NORMAL_A_PDF_VALUES: values of the lower Truncated Normal AB PDF.
%
%  Discussion:
%
%    The Normal distribution, with mean Mu and standard deviation Sigma,
%    is truncated to the interval [A,+oo).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 September 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real MU, the mean of the distribution.
%
%    Output, real SIGMA, the standard deviation of the distribution.
%
%    Output, real A, the lower truncation limit.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 11;

  a_vec = [ ...
     50.0, ...
     50.0, ...
     50.0, ...
     50.0, ...
     50.0, ...
     50.0, ...
     50.0, ...
     50.0, ...
     50.0, ...
     50.0, ...
     50.0 ];

  fx_vec = [ ...
     0.01507373507401876, ...
     0.01551417047139894, ...
     0.01586560931024694, ...
     0.01612150073158793, ...
     0.01627701240029317, ...
     0.01632918226724295, ...
     0.01627701240029317, ...
     0.01612150073158793, ...
     0.01586560931024694, ...
     0.01551417047139894, ...
     0.01507373507401876 ];

  mu_vec = [ ...
     100.0, ...
     100.0, ...
     100.0, ...
     100.0, ...
     100.0, ...
     100.0, ...
     100.0, ...
     100.0, ...
     100.0, ...
     100.0, ...
     100.0 ];

  sigma_vec = [ ...
    25.0, ...
    25.0, ...
    25.0, ...
    25.0, ...
    25.0, ...
    25.0, ...
    25.0, ...
    25.0, ...
    25.0, ...
    25.0, ...
    25.0  ];

  x_vec = [ ...
     90.0, ...
     92.0, ...
     94.0, ...
     96.0, ...
     98.0, ...
    100.0, ...
    102.0, ...
    104.0, ...
    106.0, ...
    108.0, ...
    110.0 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    a = 0.0;
    mu = 0.0;
    sigma = 0.0;
    x = 0.0;
    fx = 0.0;
  else
    a = a_vec(n_data);
    mu = mu_vec(n_data);
    sigma = sigma_vec(n_data);
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function ihs_test03 ( )

%*****************************************************************************80
%
%% IHS_TEST03 tests the improved distributed hypercube sampling algorithm.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 February 2014
%
%  Author:
%
%    John Burkardt
%
  dim_num = 2;
  duplication = 5;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'IHS_TEST03' );
  fprintf ( 1, '  IHS implements the IHS Algorithm\n' );
  fprintf ( 1, '  (Improved Distributed Hypercube Sampling)\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Demonstrate the code for a fixed dimension\n' );
  fprintf ( 1, '  and duplication value, and increasing number of points.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Spatial dimension =        %d\n', dim_num );
  fprintf ( 1, '  Duplication factor =       %d\n', duplication );

  for i = 1 : 5

    point_num = 10 * 2^(i-1);

    opt = point_num / point_num^( 1.0 / dim_num );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Number of points =         %d\n', point_num );
    fprintf ( 1, '  Desired minimum distance = %f\n', opt );
%
%  Get the points.
%
    x = ihs ( dim_num, point_num, duplication );
%
%  Compute the covariance.
%
    [ average, sd, covc ] = covariance ( dim_num, point_num, x );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Average minimum distance %f\n', average );
    fprintf ( 1, '  Standard deviation:      %f\n', sd );
    fprintf ( 1, '  Covariance:              %f\n', covc );

    fprintf ( 1, '\n' );

    for j = 1 : point_num

      if ( j <= 10 | point_num - 10 <= j )
        fprintf ( 1, '%4d    ', j );
        for i = 1 : dim_num
          fprintf ( 1, '%4d  ', x(i,j) );
        end
        fprintf ( 1, '\n');
      elseif ( j == 11 )
        fprintf ( 1, '....  ........\n' );
      end
    end

  end

  return
end


function [x, fx, exitFlag] = bisection_s(f,lb,ub,target,options)%#codegen
% BISECTION Fast and robust root-finding method that handles n-dim arrays.
%   bisection_s => "s" = single or serial, for turning into a mex file
%   [x,fVal,ExitFlag] = BISECTION(f,LB,UB,target,options) finds x +/- TolX
%   (LB < x < UB) such that f(x) = target +/- TolFun.
%
%   x = BISECTION(f,LB,UB) finds the root(s) of function f on the interval
%   [LB, UB], i.e. finds x such that f(x) = 0 where LB <= x <= UB. f will
%   never be evaluated outside of the interval specified by LB and UB. f
%   should have only one root and f(UB) and f(LB) must bound it. Elements
%   of x are NaN for instances where a solution could not be found.
%
%   x = BISECTION(f,LB,UB,target) finds x such that f(x) = target.
%
%   x = BISECTION(f,LB,UB,target,TolX) will terminate the search when the
%   search interval is smaller than TolX (TolX must be positive).
%
%   x = BISECTION(f,LB,UB,target,options) solves with the default
%   parameters replaced by values in the structure OPTIONS, an argument
%   created with the OPTIMSET function. Used options are TolX and TolFun.
%   Note that OPTIMSET will not allow arrays for tolerances, so set the
%   fields of the options structure manually for non-scalar TolX or TolFun.
%
%   [x,fVal] = BISECTION(f,...) returns the value of f evaluated at x.
%
%   [x,fVal,ExitFlag] = BISECTION(...) returns an ExitFlag that describes
%   the exit condition of BISECTION. Possible values of elements of
%   ExitFlag and the corresponding exit conditions are
%
%       1   Search interval smaller than TolX.
%       2   Function value within TolFun of target.
%       3   Search interval smaller than TolX AND function value within
%           TolFun of target.
%      -1   No solution found.
%
%   Any or all of f(scalar), f(array), LB, UB, target, TolX, or TolFun may
%   be scalar or n-dim arrays. All non-scalar arrays must be the same size.
%   All outputs will be this size.
%
%   Default values are target = 0, TolX = 1e-6, and TolFun = 0.
%
%   There is no iteration limit. This is because BISECTION (with a TolX
%   that won't introduce numerical issues) is guaranteed to converge if f
%   is a continuous function on the interval [UB, LB] and f(x)-target
%   changes sign on the interval.
%
%   The bisection method is very robust root-finding method. The absolute
%   error is halved at each step so the method converges linearly. However,
%   Brent's method (such as implemented in FZERO) can converge
%   superlinearly and is as robust. FZERO also has more features and input
%   checking, so use BISECTION in cases where either the optimization
%   toolbox is unavailable or if FZERO would have to be implemented in a
%   loop to solve multiple cases, in which case BISECTION will be much
%   faster because of vectorization.
%
%   Define LB, UB, target, TolX, and TolFun for each specific application
%   using great care for the following reasons:
%     - There is no iteration limit, so given an unsolvable task, such as
%       TolX = TolFun = 0, BISECTION remains in an unending loop.
%     - Spacing between very large floating point numbers is likely to be
%       greater than TolX.
%     - There is no initial check to make sure that f(x) - target changes
%       sign between LB and UB.
%     - Very large or very small numbers can introduce numerical issues.
%
%   Example 1: Find cube root of array 'target' without using NTHROOT and
%   compare speed to using FZERO.
%       options = optimset('TolX', 1e-9);
%       target = [(-100:.1:100)' (-1000:1:1000)'];
%
%       tic;
%       xfz = zeros(size(target));
%       for ii = 1:numel(target)
%           xfz(ii) = fzero(@(x) x.^3-target(ii), [-20 20], options);
%       end
%       fzero_time = toc
%
%       tic;
%       xbis = bisection(@(x) x.^3, -20, 20, target, options);
%       bisection_time = toc
%
%       fprintf('FZERO took %0.0f times longer than BISECTION.\n',...
%                   fzero_time/bisection_time)
%
%   Example 2: Find roots by varying the function coefficients.
%       [A, B] = meshgrid(linspace(1,2,6), linspace(4,12,10));
%       f = @(x) A.*x.^0.2 + B.*x.^0.87 - 15;
%       xstar = bisection(f,0,5);
%
%   See also FZERO, FMINBND, OPTIMSET, FUNCTION_HANDLE.
%
%   [x,fVal,ExitFlag] = BISECTION(f,LB,UB,target,options)

%   Copyright 2010-2013 Sky Sartorius
%   Author  - Sky Sartorius
%   Contact - www.mathworks.com/matlabcentral/fileexchange/authors/101715

% --- Process inputs. ---
% Set default values
tolX    = 1e-6;
tolFun  = 0;
if nargin == 5
    if isstruct(options)
        if isfield(options,'TolX') && ~isempty(options.TolX)
            tolX = options.TolX;
        end
        if isfield(options,'TolFun') && ~isempty(options.TolFun)
            tolFun = options.TolFun;
        end
    else
        tolX = options;
    end
end
if nargin<4 || isempty(target); target=0; end

ub_in = ub; lb_in = lb;
f = @(x) f(x) - target;

% --- Flip UB and LB if necessary. ---
isFlipped = lb>ub;
if any(isFlipped(:))
    ub(isFlipped) = lb_in(isFlipped);
    lb(isFlipped) = ub_in(isFlipped);
    ub_in = ub; lb_in = lb;
end

% --- Make sure everything is the same size for a non-scalar problem. ---
if isscalar(lb) && isscalar(ub)
    % Test if f returns multiple outputs for scalar input.
    if ~isscalar(target)
        ub = ub + zeros(size(target));
    else
        jnk = f(ub);
        if ~isscalar(jnk)
            ub = ub + zeros(size(jnk));
        end
    end
end

% Check if lb and/or ub need to be made into arrays.
if isscalar(lb) && ~isscalar(ub)
    lb = lb + zeros(size(ub));
elseif ~isscalar(lb) && isscalar(ub)
    ub = ub + zeros(size(lb));
end

x = zeros(size(lb));
testconvergence
% --- Iterate ---
iterations = 0;
while any(stillNotDone(:))
    bigger  = fx.*f(ub) > 0;
    ub(bigger)= x(bigger);
    lb(~bigger)= x(~bigger);

    testconvergence;
    iterations = iterations + 1;

    if(iterations > 10000)
%         error('Too many iterations in bisection!');
        break;
    end
end

    function testconvergence
        x=(ub+lb)/2;
        fx=f(x);
        outsideTolFun =  abs(fx)  > tolFun;
        outsideTolX =   (ub - lb) > tolX;
        stillNotDone = outsideTolX & outsideTolFun;
    end

% --- Check that f(x+tolX) and f(x-tolX) have opposite sign. ---
fu = f(min(x+tolX,ub_in));
fl = f(max(x-tolX,lb_in));
unboundedRoot = (fu.*fl) > 0;

% Throw out unbounded results if not meeting TolFun convergence criteria.
x(unboundedRoot & outsideTolFun) = NaN;

% --- Catch NaN elements of UB, LB, target, or other funky stuff. ---
x(isnan(fx)) = NaN;

% --- Characterize results. ---
fx = fx + target;
if nargout > 2
    exitFlag                                    = +~outsideTolX;
    exitFlag(~outsideTolFun)                    =  2;
    exitFlag(~outsideTolFun & ~outsideTolX)     =  3;
    exitFlag(isnan(x))                          = -1;
end

end

% V2: July     2010
% V3: December 2012
% don't remember when
%   typo line 39; added fn handle to see also; made array in example 2
%   smaller; changed wording in example 1
% 2013-08-23
%  -changed scalar*ones(...) calls to scalar+zeros(...) calls based on
%   http://undocumentedmatlab.com/blog/allocation-performance-take-2/
%  -rearranged help block and formatted a tiny bit


%% OPT05_RUN
%
%  Modified:
%
%    09 January 2008
%
   %---------------------------------------------------------------------
   %  Running testcase, from D+S, pp. 157-8; x_* = [.1,10].
   %  This tests the scaling implementation.
   %---------------------------------------------------------------------
   fprintf('---------------------------------------------------------\n')
   fprintf('Running testcase_5 as scalar optimization problem:\n' );
   fprintf('Exact solution (0.1, 10)\n')
   fprintf('---------------------------------------------------------\n')
   fname = 'opt05_fgh';
   options = [];
   options.verbose            = 0;
   options.method             = 'newton';
   options.globalization      = 'trust_region';
   options.step_tolerance     = 1.e-14;
   options.gradient_tolerance = 1.e-14;
   options.max_fevals         = 151;
   options.max_iterations     = 151;

   alpha = 10;
   fprintf ( 'Program scale ALPHA = %f\n', alpha );
   x0 = [-1.2/alpha, alpha];
   fprintf('Unscaled:\n')
%
% should require ~94 iterations
%
   x = entrust(fname, x0, options, alpha);

   fprintf('Newton produced  (%10.7e, %10.7e)\n\n',x(1),x(2))
   f = opt05_fgh ( x, 'f', alpha );
   fprintf('Value of F(X) = %f\n', f );

   options.scale_x            = [ 1/alpha; alpha ];
   fprintf('Scaled:\n')

   % should require ~24 iterations
   x = entrust(fname, x0, options, alpha);

   fprintf('Newton produced  (%10.7e, %10.7e)\n\n',x(1),x(2))
   f = opt05_fgh ( x, 'f', alpha );
   fprintf('Value of F(X) = %f\n', f );

   %---------------------------------------------------------------------
   %  Test Gauss-Newton strategies.
   %---------------------------------------------------------------------
   fprintf('---------------------------------------------------------\n')
   fprintf('Running testcase_5 as least squares problem: \n')
   fprintf('Exact solution (0.1, 10)\n')
   fprintf('---------------------------------------------------------\n')
   fname = 'opt05_rj';
   options = [];
   options.verbose            = 0;
   options.method             = 'gauss_newton';
   options.step_tolerance     = 1.e-15;
   options.globalization      = 'none';
   options.gradient_tolerance = 1.e-10;
   options.max_iterations     = 40;

   alpha = 10;
   fprintf ( 'Program scale ALPHA = %f\n', alpha );
   x0 = [-1.2/alpha, alpha];
   fprintf('Unscaled:\n')

   x = entrust(fname, x0, options, alpha);

   fprintf('Gauss-Newton produced  (%10.7e, %10.7e)\n\n',x(1),x(2))
   [ res, jac ] = opt05_rj ( x, 'f', alpha );
   fprintf('Norm of RES(X) = %f\n', norm ( res ) );

   options.scale_x            = [ 1/alpha; alpha ];
   fprintf('Scaled:\n')

   x = entrust(fname, x0, options, alpha);

   fprintf('Gauss-Newton produced  (%10.7e, %10.7e)\n\n',x(1),x(2))
   [ res, jac ] = opt05_rj ( x, 'f', alpha );
   fprintf('Norm of RES(X) = %f\n', norm ( res ) );


function spiral_test03 ( )

%*****************************************************************************80
%
%% SPIRAL_TEST03 generates a field on a regular grid and plots it.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 January 2015
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPIRAL_TEST03:\n' );
  fprintf ( 1, '  Generate a spiral velocity field on a regular grid.\n' );
  fprintf ( 1, '  Store in GNUPLOT data and command files.\n' );

  x_lo = 0.0;
  x_hi = 1.0;
  x_num = 21;

  y_lo = 0.0;
  y_hi = 1.0;
  y_num = 21;

  [ x, y ] = grid_2d ( x_num, x_lo, x_hi, y_num, y_lo, y_hi );

  n = x_num * y_num;
  c = 1.0;

  [ u, v ] = uv_spiral ( n, x, y, c );

  header = 'spiral';
  s = 0.05;
  spiral_gnuplot ( header, n, x, y, u, v, s );

  return
end


%  Figure 3.30a      Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%

clf;
num=1;
a=10;

zeta =1;
den2=[1/(zeta*a) 1];
den1=[1 2*zeta 1];
den=conv(den1,den2);
t=0:.1:8;
y1=step(num,den,t);

zeta =.7;
den2=[1/(zeta*a) 1];
den1=[1 2*zeta 1];
den=conv(den1,den2);
y2=step(num,den,t);

zeta =.5;
den2=[1/(zeta*a) 1];
den1=[1 2*zeta 1];
den=conv(den1,den2);
y3=step(num,den,t);

axis([0 5 .1 .9])
plot(t,y1,'-',t,y2,'--',t,y3,'-.'),grid
title('Fig. 3.30a Step response with extra pole, \alpha= 10')
xlabel('\omega_n t')
ylabel('y(t)')


function value = r8_beta ( a, b )

%*****************************************************************************80
%
%% R8_BETA evaluates the beta function of R8 arguments.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real A, B, the arguments.
%
%    Output, real VALUE, the beta function of A and B.
%
  persistent alnsml
  persistent xmax

  if ( isempty ( xmax ) )
    [ xmin, xmax ] = r8_gaml ( );
    alnsml = log ( r8_mach ( 1 ) );
  end

  if ( a <= 0.0 || b <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_BETA - Fatal error!\n' );
    fprintf ( 1, '  A and B must be greater than 0.\n' );
    error ( 'R8_BETA - Fatal error!' )
  end

  if ( a + b < xmax )
    value = r8_gamma ( a ) * r8_gamma ( b ) / r8_gamma ( a + b );
    return
  end

  value = r8_lbeta ( a, b );

  value = exp ( value );

  return
end


%% MULTIGRID OF FOR THE STOKES EQNS IN 2D
%
% This example is to show the convergence of multigrid methods for various
% finite element approximation of the Stokes equation on the unit square:
%
%       -div(mu*grad u) + grad p = f in \Omega,
%                        - div u = 0  in \Omega,
%                              u = g_D  on \Gamma.
%
% with the pure Dirichlet boundary condition.
%

%% Setting
clear all; close all;
% [node,elem] = squaremesh([0,1,0,1],0.5);
[node,elem] = circlemesh(0,0,1,0.25);
% [node,elem] = uniformrefine(node,elem);
showmesh(node,elem);
pde = Stokesdata1;
%  pde = StokesZulehnerdata;
bdFlag = setboundary(node,elem,'Dirichlet');
option.L0 = 1;
option.maxIt = 3;
option.printlevel = 1;
option.plotflag = 0;
option.rateflag = 0;

%% MG options
option.solver = 'asmg';
option.smoothingstep = 2;
option.smootherbarSp = 'SGS';

%% CR-P0 element
display('CR-P0')
option.elemType = 'CRP0';
femStokes(node,elem,pde,bdFlag,option);

%% P2-P0 element
display('P2-P0')
option.elemType = 'P2P0';
femStokes(node,elem,pde,bdFlag,option);

%% P2-P1 element
display('P2-P1')
option.elemType = 'P2P1';
option.solver = 'asmg';
femStokes(node,elem,pde,bdFlag,option);

%% isoP2-P0 element
display('isoP2-P0')
option.elemType = 'isoP2P0';
femStokes(node,elem,pde,bdFlag,option);

%% isoP2-P1 element
display('isoP2-P1')
option.elemType = 'isoP2P1';
femStokes(node,elem,pde,bdFlag,option);

%% P1b-P1 element
display('P1b-P0')
option.elemType = 'P1bP1';
femStokes(node,elem,pde,bdFlag,option);

%% Conclusion
%


function [D,dir] = dimensions(points,varargin)

% Calculates the box-dimensions and dimension estimates of the point set
% "points". Returns also the corresponding direction vectors.

if nargin == 2
    P = varargin{1};
    points = P(points,:);
elseif nargin == 3
    P = varargin{1};
    Bal = varargin{2};
    I = vertcat(Bal{points});
    points = P(I,:);
end

if size(points,2) == 3
    X = cov(points);
    [U,S,~] = svd(X);

    dp1 = points*U(:,1);
    dp2 = points*U(:,2);
    dp3 = points*U(:,3);

    D = [max(dp1)-min(dp1) max(dp2)-min(dp2) max(dp3)-min(dp3) ...
        (S(1,1)-S(2,2))/S(1,1) (S(2,2)-S(3,3))/S(1,1) S(3,3)/S(1,1)];

    dir = [U(:,1)' U(:,2)' U(:,3)'];
else
    X = cov(points);
    [U,S,~] = svd(X);

    dp1 = points*U(:,1);
    dp2 = points*U(:,2);

    D = [max(dp1)-min(dp1) max(dp2)-min(dp2) ...
        (S(1,1)-S(2,2))/S(1,1) S(2,2)/S(1,1)];

    dir = [U(:,1)' U(:,2)'];
end


function result = simplex_nd ( func, n, v )

%*****************************************************************************80
%
%% SIMPLEX_ND approximates an integral inside a simplex in ND.
%
%  Integration region:
%
%    The simplex bounded by the origin and a convex combination of N points.
%
%  Discussion:
%
%    An N+1 point second degree formula is used.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 May 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Arthur H Stroud,
%    Approximate Calculation of Multiple Integrals,
%    Prentice Hall, 1971.
%
%  Parameters:
%
%    Input, external FUNC, the name of the user supplied
%    function which evaluates F(X) at the N-dimensional point
%    X, of the form
%      function value = func ( n, x )
%
%    Input, integer N, the dimension of the space.
%
%    Input, real V(N+1,N), each of the
%    N+1 rows of V contains the N coordinates of one of the
%    vertices of the simplex.
%
%    Output, real RESULT, the approximate integral of the function.
%
  c = 1.0E+00 / sqrt ( n + 2 );
  w = 1.0E+00 / ( n + 1 );

  for i = 1 : n
    x(i) = w * ( 1.0E+00 - c ) * sum ( v(1:n+1,i) );
  end

  quad = 0.0E+00;

  for i = 1 : n+1

    x(1:n) = x(1:n) + c * v(i,1:n);

    quad = quad + w * feval ( func, n, x );

    x(1:n) = x(1:n) - c * v(i,1:n);

  end

  volume = simplex_volume_nd ( n, v );
  result = quad * volume;

  return
end


function [ vTransVectorMean, vTransVectorCov ] = UnscentedTransform( hTransformFunction, vVectorMean, mVectorCov )
%UNTITLED4 Summary of this function goes here
%   Detailed explanation goes here

vectorOrder     = size(vVectorMean, 1);
numSigmaPoints  = (2 * vectorOrder) + 1;

mVectorCovSqrt = chol(mVectorCov);
if(vectorOrder <= 3) %<! Used as k Factor in literature
    unscentedTransformScalingFactor = 3 - vectorOrder;
else
    unscentedTransformScalingFactor = 3;
end

vSigmaPoints                = zeros(vectorOrder, numSigmaPoints);
vTransformedSigmaPoints     = zeros(vectorOrder, numSigmaPoints);
vUnscentedTransformWeights  = zeros(numSigmaPoints, 1);

vTransVectorMean    = zeros(vectorOrder, 1);
vTransVectorCov     = zeros(vectorOrder, vectorOrder);

mVectorCovSqrtNormalized = sqrt(vectorOrder + unscentedTransformScalingFactor) .* mVectorCovSqrt;

vSigmaPoints(1) = 0;
vUnscentedTransformWeights(1) = unscentedTransformScalingFactor / (unscentedTransformScalingFactor + vectorOrder);
vUnscentedTransformWeights(2:end) = 1 / (2 * (unscentedTransformScalingFactor + vectorOrder));

for ii = 2:(vectorOrder + 1)
    vSigmaPoints(:, ii) = mVectorCovSqrtNormalized(:, (ii - 1));
end

vSigmaPoints(:, (vectorOrder + 2):end) = -vSigmaPoints(:, 2:(vectorOrder + 1));

for ii = 1:numSigmaPoints
    vSigmaPoints(:, ii) = vVectorMean + vSigmaPoints(:, ii);
    vTransformedSigmaPoints(:, ii) = hTransformFunction(vSigmaPoints(:, ii));
end

for ii = 1:numSigmaPoints
    vTransVectorMean = vTransVectorMean + (vUnscentedTransformWeights(ii) * vTransformedSigmaPoints(:, ii));
end

for ii = 1:numSigmaPoints
    vTransVectorCov = vTransVectorCov + ...
        (vUnscentedTransformWeights(ii) * (([vTransformedSigmaPoints(:, ii) - vTransVectorMean]) * ([vTransformedSigmaPoints(:, ii) - vTransVectorMean].')));
end

end


function x = ellipse_rs ( r, s, i )

%*****************************************************************************80
%
%% ELLIPSE_RS maps the boundary of the unit square to an ellipse.
%
%  Discussion:
%
%    The ellipse is ( 3 * cos ( THETA ), 2 * sin ( THETA ) ).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real R, S, the coordinates of a point that lies on the
%    boundary of the square.
%
%    Input, integer I, the component of the data which is to be returned.
%
%    Output, real X, the I-th component of the data vector X, evaluated
%    at the point (R,S), which is on a corner, or edge, of the unit square.
%
  if ( r == 0.0 )
    theta = 0.25 * pi * ( 5.0 * ( 1.0 - s ) + 3.0 * s  );
  elseif ( r == 1.0 ) then
    theta = 0.25 * pi * ( - 1.0 * ( 1.0 - s ) + 1.0 * s );
  elseif ( s == 0.0 ) then
    theta = 0.25 * pi * ( 5.0 * ( 1.0 - r ) + 7.0 * r );
  elseif ( s == 1.0 ) then
    theta = 0.25 * pi * ( 3.0 * ( 1.0 - r ) + 1.0 * r );
  end

  if ( i == 1 )

    x = 3.0 * cos ( theta );

  elseif ( i == 2 ) then

    x = 2.0 * sin ( theta );

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, 'ELLIPSE_RS - Fatal error!\n' );
    fprintf ( 1, '  Illegal component index I = %d\n', i );
    error ( 'ELLIPSE_RS - Fatal error!' );

  end

  return
end


% SP_H1_ERROR: Evaluate the error in H^1 norm.
%
%   [errh1, errl2, errh1s, errh1_elem, errl2_elem, errh1s_elem] = sp_h1_error (space, msh, u, uex, graduex);
%
% INPUT:
%
%     space:    structure representing the space of discrete functions (see sp_scalar/sp_evaluate_col)
%     msh:      structure containing the domain partition and the quadrature rule (see msh_cartesian/msh_evaluate_col)
%     u:        vector of dof weights
%     uex:      function handle to evaluate the exact solution
%     graduex:  function handle to evaluate the gradient of the exact solution
%
% OUTPUT:
%
%     errh1:  error in H^1 norm
%     errl2:  error in L^2 norm
%     errh1s: error in H^1 seminorm
%     errh1_elem:  error in H^1 norm, for each single element
%     errl2_elem:  error in L^2 norm, for each single element
%     errh1s_elem: error in H^1 seminorm, for each single element
%
% Copyright (C) 2010 Carlo de Falco
% Copyright (C) 2011 Rafael Vazquez
% Copyright (C) 2015 Eduardo M. Garau, Rafael Vazquez
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 3 of the License, or
% (at your option) any later version.

% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with Octave; see the file COPYING.  If not, see
% <http://www.gnu.org/licenses/>.

function [errh1, errl2, errh1s, errh1_elem, errl2_elem, errh1s_elem] = sp_h1_error (sp, msh, u, uex, graduex)

  grad_valu = sp_eval_msh (u, sp, msh, 'gradient');
  grad_valu = reshape (grad_valu, sp.ncomp, msh.rdim, msh.nqn, msh.nel);

  for idir = 1:msh.rdim
    x{idir} = reshape (msh.geo_map(idir,:,:), msh.nqn*msh.nel, 1);
  end
  grad_valex  = reshape (feval (graduex, x{:}), sp.ncomp, msh.rdim, msh.nqn, msh.nel);

  w = msh.quad_weights .* msh.jacdet;

  [errl2, errl2_elem] = sp_l2_error (sp, msh, u, uex);
  errh1s_elem = sum (reshape (sum (sum ((grad_valu - grad_valex).^2, 1), 2), [msh.nqn, msh.nel]) .* w);
  errh1s = sqrt (sum (errh1s_elem));

  errh1  = sqrt (errl2^2 + errh1s^2);

  errh1_elem  = sqrt (errl2_elem.^2 + errh1s_elem);
  errh1s_elem = sqrt (errh1s_elem);

end


%% Gaussian Mixture Model (GMM)
%
% Demonstrates EM clustering, and also compares againt K-means clustering.
%
% Sources:
%
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/python/gaussian_mix.py>
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/python/kmeans.py>
%

function gaussian_mix_demo()
    %% Data
    % sample 2D points from a mixture distribution of K bivariate Gaussians
    K = 5;
    sz = 512;
    [pts, labels, mus, sigmas] = make_gaussian_mixture(K, sz);
    whos pts labels mus sigmas

    %%
    % draw points (color-coded) with the ground-truth mixtures
    clr = uint8(hsv(K) * 255);
    clr(:,4) = 0;
    img = zeros([sz sz 3], 'uint8');
    img0 = cv.circle(img, pts, 1, 'Colors',clr(labels,:), 'Thickness','Filled');
    for k=1:K
        img0 = draw_gaussian(img0, mus(k,:), sigmas{k}, [0 255 255]);
    end
    imshow(img0), title(sprintf('%d Samples, K=%d', numel(labels), K))

    %% EM
    % cluster using EM by fitting a Gaussian mixture model
    em = cv.EM();
    em.ClustersNumber = K;
    em.CovarianceMatrixType = 'Generic';
    tic
    [~,L] = em.trainEM(pts);
    toc

    %%
    % draw clustered points along with the estimated mixture model
    means = em.getMeans();
    covs = em.getCovs();
    img1 = cv.circle(img, pts, 1, 'Colors',clr(L+1,:), 'Thickness','Filled');
    for k=1:K
        img1 = draw_gaussian(img1, means(k,:), covs{k}, [0 255 255]);
    end
    figure, imshow(img1), title('Gaussian Mixtures')

    %% Kmeans
    % cluster using Kmeans
    crit = struct('type','Count+EPS', 'maxCount',30, 'epsilon',0.1);
    L = cv.kmeans(pts, K, 'Criteria',crit, 'Attempts',10);

    %%
    % draw points color-coded by assigned label
    img2 = cv.circle(img, pts, 1, 'Colors',clr(L+1,:), 'Thickness','Filled');
    figure, imshow(img2), title('Kmeans')
end

%% Helper functions

function [pts, labels, mus, sigmas] = make_gaussian_mixture(K, sz)
    %MAKE_GAUSSIAN_MIXTURE  Random points from Gaussian mixture distribution
    %
    %     [pts, labels, mus, sigmas] = make_gaussian_mixture(K, sz)
    %
    % ## Input
    % * __K__ number of components
    % * __sz__ image size, determines 2D points domain
    %
    % ## Output
    % * __pts__ matrix of 2D points
    % * __labels__ vector of corresponding component indices
    % * __mus__ K-by-2 mean of each component
    % * __sigmas__ cell array of length K, covariance of each component
    %
    % See also: gmdistribution.random
    %

    mus = zeros(K,2);
    sigmas = cell(K,1);
    pts = cell(K,1);
    labels = cell(K,1);
    for k=1:K
        mus(k,:) = (rand(1,2)*0.8 + 0.1) * sz;
        a = (rand(2,2) - 0.5) * sz * 0.1;
        sigmas{k} = (a.' * a) + sz*0.05*eye(2);
        n = 100 + randi([0 900]);
        pts{k} = my_mvnrnd(mus(k,:), sigmas{k}, n);
        labels{k} = k * ones(size(pts{k},1),1);
    end
    pts = single(cat(1, pts{:}));
    labels = int32(cat(1, labels{:}));
end

%%

function X = my_mvnrnd(mu, sigma, num)
    %MY_MVNRND  Random points from Gaussian distribution
    %
    %     X = my_mvnrnd(mu, sigma, num)
    %
    % ## Input
    % * __mu__ 1x2 mean vector
    % * __sigma__ 2x2 covariance matrix
    % * __num__ number of points to generate
    %
    % ## Output
    % * __X__ matrix of 2D points, num-by-2
    %
    % See also: mvnrnd, randn
    %

    if mexopencv.require('stats')
        X = mvnrnd(mu, sigma, num);
    else
        X = bsxfun(@plus, randn(num,numel(mu)) * cholcov(sigma), mu);
    end
end

%%

function img = draw_gaussian(img, mu, sigma, clr)
    %DRAW_GAUSSIAN  Draw a bivariate Gaussian
    %
    %     img = draw_gaussian(img, mu, sigma, clr)
    %
    % ## Input
    % * __img__ input image on which to draw
    % * __mu__ 1x2 mean vector
    % * __sigma__ 2x2 covariance matrix
    % * __clr__ color
    %
    % ## Output
    % * __img__ output image
    %
    % Represent a 2D Gaussian with an ellipse where:
    %
    % * its mean is the center of the ellipse
    % * its covariance matrix eigenvector corresponding to largest eigenvalue
    %   is the direction of the ellipse
    % * its (scaled) eigenvalues are the major/minor axis lengths of the
    %   ellipse
    %

    [w,u,~] = cv.SVD.Compute(sigma);
    ang = atan2(u(2,1), u(1,1)) * (180/pi);
    s = sqrt(w)*3;
    img = cv.ellipse(img, mu, s, 'Angle',ang, 'Color',clr, 'LineType','AA');
end


function [A,xy]= cut_grid_data

% Generate a cut-grid graph for the ICM 2006 talk example
%
% The graph has 64 nodes and 95 edges
% A is an n x m incidence matrix (n is number of nodes, m is number of edges)
% xy is the location data
%
% Original code by Arpita Ghosh, modified for ICM06 talk by Almir Mutapcic

n  = 8;
r1 = 2;
y  = ones(n,1) * (1:n);
x  = y';
x  = x(:);
y  = y(:);
dx = x * ones(1,n^2);
dy = y * ones(1,n^2);
xy = [ x, y ];

% Find the adjacency matrix, manually deleting edges to get down to size
Adj1 = tril( ( dx - dx' ) .^ 2 + ( dy - dy' ) .^2 < r1, -1 );
Adj1(49,41) = 0;
Adj1(50,42) = 0;
Adj1(16,8)  = 0;
Adj1(24,16) = 0;
Adj1(15,7)  = 0;
Adj1(23,15) = 0;
Adj1(10,1)  = 0;
Adj1(21,13) = 0;
Adj1(13,5)  = 0;
Adj1(22,14) = 0;
Adj1(14,6)  = 0;
Adj1(51,43) = 0;
Adj1(52,44) = 0;
Adj1(53,45) = 0;
Adj1(54,46) = 0;
Adj1(42,41) = 0;
Adj1(34,33) = 0;
Adj1(26,25) = 0;

% Build the incidence matrix
[i,j] = find(Adj1);
m = length(i);
A = sparse( [i;j], [1:m,1:m]', [ones(m,1);-ones(m,1)] );


function [rho,rhou,Ener] = Euler1D(rho, rhou, Ener, FinalTime)

% function [rho, rhou, Ener] = Euler1D(rho, rhou, Ener, FinalTime)
% Purpose  : Integrate 1D Euler equations until FinalTime starting with
%            initial conditions [rho, rhou, Ener]

Globals1D;

% Parameters
gamma = 1.4; CFL = 1.0; time = 0;

% Prepare for adaptive time stepping
mindx = min(x(2,:)-x(1,:));

% Limit initial solution
rho =SlopeLimitN(rho); rhou=SlopeLimitN(rhou); Ener=SlopeLimitN(Ener);

% outer time step loop
while(time<FinalTime)

  Temp = (Ener - 0.5*(rhou).^2./rho)./rho;
  cvel = sqrt(gamma*(gamma-1)*Temp);
  dt = CFL*min(min(mindx./(abs(rhou./rho)+cvel)));

  if(time+dt>FinalTime)
    dt = FinalTime-time;
  end

  % 3rd order SSP Runge-Kutta

  % SSP RK Stage 1.
  [rhsrho,rhsrhou,rhsEner]  = EulerRHS1D(rho, rhou, Ener);
  rho1  = rho  + dt*rhsrho;
  rhou1 = rhou + dt*rhsrhou;
  Ener1 = Ener + dt*rhsEner;

  % Limit fields
  rho1  = SlopeLimitN(rho1); rhou1 = SlopeLimitN(rhou1); Ener1 = SlopeLimitN(Ener1);

  % SSP RK Stage 2.
  [rhsrho,rhsrhou,rhsEner]  = EulerRHS1D(rho1, rhou1, Ener1);
  rho2   = (3*rho  + rho1  + dt*rhsrho )/4;
  rhou2  = (3*rhou + rhou1 + dt*rhsrhou)/4;
  Ener2  = (3*Ener + Ener1 + dt*rhsEner)/4;

  % Limit fields
  rho2  = SlopeLimitN(rho2); rhou2 = SlopeLimitN(rhou2); Ener2 = SlopeLimitN(Ener2);

  % SSP RK Stage 3.
  [rhsrho,rhsrhou,rhsEner]  = EulerRHS1D(rho2, rhou2, Ener2);
  rho  = (rho  + 2*rho2  + 2*dt*rhsrho )/3;
  rhou = (rhou + 2*rhou2 + 2*dt*rhsrhou)/3;
  Ener = (Ener + 2*Ener2 + 2*dt*rhsEner)/3;

  % Limit solution
  rho =SlopeLimitN(rho); rhou=SlopeLimitN(rhou); Ener=SlopeLimitN(Ener);

  % Increment time and adapt timestep
  time = time+dt;
end
return


function [A,B,D,E,I]=bpf1(X)
% Multiple Feedback Bandpass Filter
% Note that complex frequency variable s is not used here.
% These arrays are real, not complex.
%
R1=X(1);R2=X(2);R3=X(3);C1=X(4);C2=X(5);
%
N=2;M=1;U=1;Y=1;
%
A1=[0 1 1;
   1/R3 1 0;
   1 0 0];
%
B2=[-(1/R1+1/R2) 0 1/R1;
   0 0 0;
   1 -1 0];
%
P=diag([C1 C2]);
%
% The following statements never change from circuit to circuit.
%
V=A1\B2;H=V(U+1:U+N,1:N+M);I=eye(N);
AB=P\H;A=AB(1:N,1:N);B=AB(1:N,N+1:N+M);
D=V(Y:Y,1:N);E=V(Y:Y,N+1:N+M);


% the MWF beamformer
% r1MWF:    h = PhiN^-1 * PhiX * refVec / (mu + trace(PhiN^-1 * PhiX)
% SDWMWF:   h = (PhiX + mu * PhiN)^-1 * PhiX * refVec
% input:    PhiN, (Nch, Nch, Nbin) the noise covariance matrix
%           PhiX, (Nch, Nch, Nbin) the speech covariance matrix
%           mu, the speech distortion/noise reduction trade-off parameter
%           refMic, the reference microphone, default = 1
% output:   h, (Nch, Nbin)  the beamformer coefficients
% % Ziteng Wang @ 201812

function h = MWF(PhiX, PhiN, mu, refMic, choice)
if nargin < 3
    mu = 1;                 % typical value {0, 1}
elseif nargin < 4
    refMic = 1;
elseif nargin < 5
    choice = 'r1MWF';       % collected from 'r1MWF' 'SDWMWF'
end

[Nch, ~, Nbin] = size(PhiX);
refVec = zeros(Nch, 1);
refVec(refMic) = 1;
lambda = zeros(Nbin, 1);
h = zeros(Nch, Nbin);

for bin = 1:Nbin
    if rcond(PhiN(:,:,bin)) < eps
        disp(['bin ' num2str(bin) ': Noise covariance ill-conditioned.']);
        PhiN(:,:,bin) = PhiN(:,:,bin) + 1e-10 * eye(Nch);
    end
    if strcmp(choice, 'r1MWF')
        tmp = PhiN(:,:,bin) \ PhiX(:,:,bin);
        lambda(bin) = trace(tmp);
        h(:,bin) = tmp * refVec / (mu + lambda(bin));
        %%% Gain factor to normalize the steering vector. The results are
        %%% better. Needs further checking
        %gain_factor = norm(PhiX(:,refMic,bin) / PhiX(refMic,refMic,bin));
        %h(:,bin) = gain_factor * h(:,bin);
    elseif strcmp(choice, 'SDWMWF') % SDW-MWF is more sensitive to estimation errors.
        tmp = (PhiX(:,:,bin) + mu * PhiN(:,:,bin)) \ PhiX(:,:,bin);
        h(:,bin) = tmp * refVec;
    end
end


function [ deltaHD , deltaSEN , deltaPRO ] = CV2XMode4_common( lambda , Pt , distance, Psen , step_dB , noise , coding);

% CV2XMode4_common is a script that calculates the probability of packet
% loss due to half-duplex transmissions, the probability of packet loss
% due to a received signal power below the sensing power threshold and the
% probability of packet loss due to propagation effects for different
% Tx-Rx distances based on the models 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
%
% CV2XMode4_common is called from the main script, CV2XMode4.
%
% Input parameters:
%   lambda: packet transmission frequency in Hz. .
%   Pt: transmission power in dBm.
%   distance: distance between tramsmitter and receiver in meters. It can be a vector with multiple distances.
%   Psen: sensing threshold in dBm.
%   step_dB: discrete steps to compute the PDF of the SNR and SINR in dB.
%   noise: 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.
%   coding: ID of the coding used to identify the BLER curve
%
% Output metrics:
%    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
%
% 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.

    D = length(distance);

    deltaHD(1:D) = lambda/1000;  % Equation (7)

    [PL_E_R, std_dev_E_R] = get_PL_SH(distance);   % Obtains pathloss and shadowing for different Tx-Rx distances.
    deltaSEN = 0.5 * ( 1 - erf( (Pt - PL_E_R - Psen)./(std_dev_E_R*sqrt(2)) ) );         % Equation (10)

    [SNR, PDF_SNR] = get_SINRdistribution( Pt - PL_E_R , -180 , std_dev_E_R , 3 , noise , Psen , step_dB); % Obtains the PDF of the SNR experienced by Rx (without interference).
    deltaPRO = get_BLER( SNR , PDF_SNR , coding , step_dB);   % Equation (13)

end


function value = local_sum(input, window_size)
% LOCAL_SUM Fast computation of 2D local sum using integral images.
%
% Algorithm is described in Viola and Jones, "Robust real-time face
% detection", International Journal of Computer Vision, 2004.  Computation
% time is independent of window size. Precision may be an issue for very
% large images, but a few thousand by a few thousand images seems to work
% with no issues.
%
% Suprisingly, for small window sizes, MATLAB's conv2 actually seems to be
% a faster way to do this, but this implementation gets much faster as
% window sizes get larger.  MATLAB Central's fastrunmean demonstrates
% another way of computing fast means/sums.  Fastrunmean is not quite as
% fast as this integral image implementation, but doesn't have the same
% potential precision issues for very large images.
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

padsize = floor(window_size/2);
padded = padarray(input, padsize+1, 0, 'pre'); % Zeropad to handle local sums at edges
padded = padarray(padded, padsize, 0, 'post'); % Zeropad to handle local sums at edges
padded = cumsum(cumsum(double(padded),1),2); % Create integral image
value = padded(1:size(input,1),1:size(input,2))-... % Compute sum over local window
    padded(1:size(input,1),(window_size(2)+1):end)-...
    padded((window_size(1)+1):end,1:size(input,2))+...
    padded((window_size(1)+1):end,(window_size(2)+1):end);

end

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


function [ C ] = gsp_gwft(G,g,f, param )
%GSP_GWFT Generalized windowed Fourier transform
%   Usage:  C = gsp_gwft(G,g,f, param );
%           C = gsp_gwft(G,g,f);
%
%   Input parameters:
%         G     : Graph
%         g     : Window (graph signal or kernel)
%         f     : Graph signal (column vector)
%         param : Structure of optional parameter
%   Output parameters:
%         C     : Coefficient.
%
%   This function compute the graph windowed Fourier transform of a signal
%   *f* with the window *g*. The function returns a matrix of size N^2*N.
%
%   *param* a Matlab structure containing the following fields:
%
%     * *param.verbose* : 0 no log, 1 print main steps, 2 print all steps.
%       By default, it is 1.
%     * *param.lowmemory* : use less memory. By default, it is 1.
%
%   Reference: shuman2013windowed

% Author : Nathanael Perraudin
% Testing: test_gwft

% Optional input arguments
if nargin<4, param=struct; end
if ~isfield(param, 'verbose'), param.verbose=1 ; end
if ~isfield(param, 'lowmemory'), param.lowmemory=1 ; end

if ~isfield(G,'U')
   error(['GSP_GWFT: You need first to compute the Fourier basis. ',...
       'You can do it with the function gsp_compute_fourier_basis']);
end

if sum(G.U(:,1)<eps(2))
    error(['GSP_GWFT: The current implementation of this function is ',...
       'not working for disconnected graphs'])
end

Nf = size(f,2);

if iscell(g)
    g = gsp_igft(G,g{1}(G.e));
end

if isa(g, 'function_handle')
    g = gsp_igft(G,g(G.e));
end

if ~param.lowmemory
    % Compute the Frame into a big matrix
    Frame=gsp_gwft_frame_matrix(G,g,param);

    C=Frame'*f;

    C=reshape(C,G.N,G.N,Nf);
else
    % Compute the translate of g
    ghat=G.U'*g;
    Ftrans=sqrt(G.N)*G.U*(repmat(ghat,1,G.N).*G.U');
    C=zeros(G.N);
    for jj = 1:Nf
        for ii=1:G.N
             C(:,ii,jj)=(repmat(1./G.U(:,1),1,G.N) .* ...
                      G.U.*repmat(Ftrans(:,ii),1,G.N))'*f(:,jj);
        end
    end

end


function linpack_d_test11 ( )

%*****************************************************************************80
%
%% TEST11 tests DGEFA and DGESL.
%
%  Discussion:
%
%    In this example, we solve a relatively large linear system.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2009
%
%  Author:
%
%    John Burkardt
%
  n = 100;
  lda = n;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST11\n' );
  fprintf ( 1, '  For a general matrix,\n' );
  fprintf ( 1, '  DGEFA computes the LU factors;\n' );
  fprintf ( 1, '  DGESL solves a factored linear system;\n' );
  fprintf ( 1, '  The matrix size is N = %d\n', n );
%
%  Assign values to the matrix A and the right hand side B.
%
%  The problem is just an enlarged version of the
%  problem for N = 5, which is:
%
%  Matrix A is ( n -1 -1 -1 -1)    Right hand side B is  (1)
%              (-1  n -1 -1 -1)                          (1)
%              (-1 -1  n -1 -1)                          (1)
%              (-1 -1 -1  n -1)                          (1)
%              (-1 -1 -1 -1  n)                          (1)
%
%  Solution is   (1)
%                (1)
%                (1)
%                (1)
%                (1)
%
  b(1:n) = 1.0;

  a(1:n,1:n) = -1.0;
  for i = 1 : n
    a(i,i) = n;
  end
%
%  Factor the matrix.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Factor the matrix\n' );

  [ a, ipvt, info ] = dgefa ( a, lda, n );

  if ( info ~= 0 )
    fprintf ( 1, '  DGEFA returned an error flag INFO = %d\n', info );
    return
  end
%
%  Solve the linear system.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Solve the linear system.\n' );

  job = 0;
  b = dgesl ( a, lda, n, ipvt, b, job );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The first and last five entries of the solution:\n' );
  fprintf ( 1, '  (All of them should be 1.)\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    if ( i <= 5 | n-5 < i )
      fprintf ( 1, '  %6d  %14f\n', i, b(i) );
    end
    if ( i == 5 )
      fprintf ( 1, '  ......  ..............\n' );
    end
  end

  return
end


function d=center(x,op);

%  CENTER  Delay calculation for Wavelet transform alignment.
%
%          CENTER (X, OP) calculates the delay for filter in X
%          according to the alignment method indicated in OP.
%          This delay is used by Wavelet transform functions.
%          The value of OP can be:
%              0 : First Absolute Maxima Location
%              1 : Zero delay in analysis (Full for synthesis).
%              2 : Mass center (sum(m*d)/sum(m))
%              3 : Energy center (sum(m^2 *d)/sum(m^2))
%
%          If no output argument is given, then the vector X will
%          be plotted in the current figure, and a color line will be
%          marking the result.(red: OP=0; green: OP=1; cyan: OP=2;
%          blue: OP=4)
%
%          See also: WTCENTER, WTMETHOD

%--------------------------------------------------------
% Copyright (C) 1994, 1995, 1996, by Universidad de Vigo
%
%
% Uvi_Wave 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, or (at your option) any
% later version.
%
% Uvi_Wave 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 Uvi_Wave; see the file COPYING.  If not, write to the Free
% Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA.
%
%       Author: Sergio J. Garcia Galan
%       e-mail: Uvi_Wave@tsc.uvigo.es
%--------------------------------------------------------

lx=length(x);
l=1:lx;

if op==1
	d=0;
else
	if op==2
		xx=abs(x(:)');
		L=l;
	end
	if op==3
		xx=x(:)'.^2;
		L=l;
	end
	if op==0
		[mx,d]=max(abs(x));
	else

		d=sum(xx.*L)/sum(xx);
	end
end

if nargout==0,
  cad='rgcbk';
  plot(x)
  l=line([d,d],[min(x),max(x)]);
  set(l,'Color',cad(op+1));
end


 function [basis, Hw] = deconv_z(psf, fun, t)
% psf: 3-tap symmetric psf: [a _b_ a]
% H(z) = 1 / (az + b + a/z)

if numel(psf) == 1
	basis = fun(t) / psf;
	Hw = @(t) psf * ones(size(t));
return
end

if length(psf) ~= 3, error 'not done', end
a = psf(1);
b = psf(2);
c = b / a / 2;
p = -c + sign(c) * sqrt(c^2 - 1); % pole

scale = 1/a * 1/(p - 1/p);
basis = fun(t);
for n=1:9
	basis = basis + p^n * (fun(t-n) + fun(t+n));
end
basis = scale * basis;

Hw = @(om) 1 ./ (b + 2 * a * cos(om));


function img = discreteCube(varargin)
%DISCRETECUBE discretize a 3D cube
%
%   IMG = discreteCube(LX, LY, LZ, CUBE)
%   Creates a 3D image of a cube, given coordinates of voxel centers in LX,
%   LY and LZ, and parameters of the shape in CUBE.
%
%   CUBE has the format:
%   [XC YC ZC SIDE], with (XC YC ZC) being the coordinates of the center,
%   and SIDE being the side length of the cube.
%
%   Oriented cubes are represented as follow:
%   [XC YC ZC SIDE THETA PHI PSI], with (THETA, PHI) being the angle of the
%   normal of the cube (THETA is colatitude, PHI is longitude), and PSI is
%   rotation of the cube around the normal. All angles are in degrees.
%
%   IMG = discreteCube(DIM, ...)
%   DIM is the size of image, with the format [x0 dx x1;y0 dy y1;z0 dz z1]
%
%   IMG = discreteCube(DIM, CENTER, SIDE, THETA, PHI)
%   Also specify spherical angle of the normal of a face of the cube. THETA
%   is the angle from the vertical, and PHI is the angle from the 0x axis,
%   both between 0 and 360.
%
%   IMG = discreteCube(DIM, CENTER, SIDE, THETA, PHI, PSI)
%   Also consider 'roll' of the cube around its normal axis. PSI is an
%   angle given in degrees between 0 and 360.
%
%   IMG = discreteCube(DIM, CUBE)
%   send parameters in a row vector, where CUBE contains at least the
%   center coordinate, and possibly the other parameters.
%
%   Example
%   img = discreteCube(1:100, 1:100, 1:100, [50 50 50 30 10 60 45]);
%   img = discreteCube([1 1 100;1 1 100;1 1 100], [50 50 50], 30, 10);
%   img = discreteCube([1 1 100;1 1 100;1 1 100], [50 50 50 30 10]);
%
%   See Also
%   imShapes, discreteBall, discreteEllipsoid, discreteCylinder, drawCube
%

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

%   HISTORY
%   03/11/2006: concatenate transforms, add doc for PSI
%   04/03/2009: use meshgrid
%   30/04/2009: update transforms
%   29/05/2009: use more possibilities for specifying grid
%   22/01/2010: fix auto center with odd image size
%   2011-06-21 use degrees for angles

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

% default parameters
center  = [lx(ceil(end/2)) ly(ceil(end/2)) lz(ceil(end/2))];
side    = center;
theta   = 0; phi = 0; psi=0;

% process input parameters
if length(varargin)==1
    var = varargin{1};
    center = var(:,1:3);
    if size(var, 2)>3
        side = var(:,4);
    end
    if size(var, 2)>4
        theta = var(:,5);
    end
    if size(var, 2)>5
        phi = var(:,6);
    end
    if size(var, 2)>6
        psi = var(:,7);
    end

elseif ~isempty(varargin)
    center = varargin{1};
    if length(varargin)>1
        side = varargin{2};
    end
    if length(varargin)>2
        theta = varargin{3};
    end
    if length(varargin)>3
        phi = varargin{4};
    end
    if length(varargin)>4
        psi = varargin{5};
    end
end

if length(side) == 1
    side = [side side side];
end

% compute coordinate of image voxels in cube reference system
trans = composeTransforms3d(...
    createTranslation3d(-center),...
    createRotationOz(-deg2rad(phi)),...
    createRotationOy(-deg2rad(theta)), ...
    createRotationOz(-deg2rad(psi)));
[x, y, z] = transformPoint3d(x, y, z, trans);

% create image: simple threshold over 3 dimensions
img = abs(x) <= side(1)/2 & abs(y) <= side(2)/2 & abs(z) <= side(3)/2;


function [mi entropy fd_bins] = mutualinformationx(x,y,fd_bins,permtest)
% MUTUALINFORMATIONX   Compute mutual information between two vectors
%
%   Inputs:
%       x,y     :  data matrices of equal size
%
%   Optional inputs:
%       bins    :  number of bins to use for distribution discretization
%      permtest :  perform permutation test and return mi in standard-Z values
%
%   Outputs:
%       mi      :  mutual information in bits
%       entropy :  entropy of x, y, and joint
%       nbins   :  number of bins used for discretization
%                  (based on Freedman-Diaconis rule)
%
%  Mike X Cohen (mikexcohen@gmail.com)

if nargin<2, error('Specify two inputs.'); end
if length(x)~=length(y), error('X and Y must have equal length'); end

%% determine the optimal number of bins for each variable

% vectorize in the case of matrices
x=x(:); y=y(:);

if nargin<3 || isempty(fd_bins)
    n            = length(x);
    maxmin_range = max(x)-min(x);
    fd_bins1     = ceil(maxmin_range/(2.0*iqr(x)*n^(-1/3))); % Freedman-Diaconis

    n            = length(y);
    maxmin_range = max(y)-min(y);
    fd_bins2     = ceil(maxmin_range/(2.0*iqr(y)*n^(-1/3)));

    % and use the average...
    fd_bins = ceil((fd_bins1+fd_bins2)/2);
end

%% bin data

edges = linspace(min(x),max(x),fd_bins+1);
[nPerBin1,bins1] = histc(x,edges);

edges = linspace(min(y),max(y),fd_bins+1);
[nPerBin2,bins2] = histc(y,edges);

%% compute entropies

% recompute entropy with optimal bins for comparison
hdat1 = hist(x,fd_bins);
hdat1 = hdat1./sum(hdat1);
hdat2 = hist(y,fd_bins);
hdat2 = hdat2./sum(hdat2);

% convert histograms to probability values
for i=1:2
    eval([ 'entropy(' num2str(i) ') = -sum(hdat' num2str(i) '.*log2(hdat' num2str(i) '+eps));' ]);
end

%% compute joint probabilities

jointprobs = zeros(fd_bins);
for i1=1:fd_bins
    for i2=1:fd_bins
        jointprobs(i1,i2) = sum(bins1==i1 & bins2==i2);
    end
end
jointprobs=jointprobs./sum(jointprobs(:));

entropy(3) = -sum(jointprobs(:).*log2(jointprobs(:)+eps));

%% mutual information

mi = sum(entropy(1:2)) - entropy(3);

%% optional permutation testing

if nargin==4

    npermutes = 500;
    n = length(bins2);

    perm_mi = zeros(1,npermutes);

    for permi=1:npermutes

        jointprobs = zeros(fd_bins);

        % shuffle bins
        binbreak = randsample(round(n*.8),1,1)+round(n*.1);
        switch mod(permi,4)
            case 0, bins2 = [ bins2(binbreak:end);    bins2(1:binbreak-1) ];
            case 1, bins2 = [ bins2(end:-1:binbreak); bins2(1:binbreak-1) ];
            case 2, bins2 = [ bins2(binbreak:end);    bins2(binbreak-1:-1:1) ];
            case 3, bins2 = [ bins2(end:-1:binbreak); bins2(binbreak-1:-1:1) ];
        end

        for i1=1:fd_bins
            for i2=1:fd_bins
                jointprobs(i1,i2) = sum(bins1==i1 & bins2==i2);
            end
        end
        jointprobs=jointprobs./sum(jointprobs(:));

        perm_jentropy = -sum(jointprobs(:).*log2(jointprobs(:)+eps));

        % mutual information
        perm_mi(permi) = sum(entropy(1:2)) - perm_jentropy;
    end

    mi = (mi-mean(perm_mi))/std(perm_mi);
end

%%
% simplified replacement for randsample
function y = randsample(x,n,junk)
y=randperm(x);
y=y(1:n);


function res = reconWUpyr(pyr, pind, daub_order);

% RES = reconWUpyr(PYR, INDICES, DAUB_ORDER)

% Reconstruct image from its separable undecimated orthonormal QMF/wavelet pyramid
% representation, as created by buildWUpyr.
%
% PYR is a vector containing the N pyramid subbands, ordered from fine
% to coarse.  INDICES is an Nx2 matrix containing the sizes of
% each subband.
%
% DAUB_ORDER: specifies the order of the daubechies wavelet filter used

% JPM, Univ. de Granada, 03/2003, based on Rice Wavelet Toolbox
% functions "mrdwt" and  "mirdwt", and on Matlab Pyrtools from Eero Simoncelli.

Nor = 3;
Nsc = (size(pind,1)-2)/Nor-1;
h = daubcqf(daub_order);

yh = [];

nband = 1;
last = prod(pind(1,:)); % empty "high pass residual band" for compatibility with full steerpyr 2
for nsc = 1:Nsc+1,  % The number of scales corresponds to the number of pyramid levels (also for compatibility)
    for nor = 1:Nor,
        nband = nband +1;
        first = last + 1;
        last = first + prod(pind(nband,:)) - 1;
        band = pyrBand(pyr,pind,nband);
        sh = (daub_order/2 - 1)*2^nsc;  % approximate phase compensation
        if nsc > 2,
            band = expand(band, 2^(nsc-2));
        end
        if nor == 1,        % horizontal
            band = shift(band, [-sh -2^(nsc-1)]);
        elseif nor == 2,    % vertical
            band = shift(band, [-2^(nsc-1) -sh]);
        else
            band = shift(band, [-sh -sh]);    % diagonal
        end
        yh = [yh band];
    end
end

nband = nband + 1;
band = pyrBand(pyr,pind,nband);
lpr = expand(band,2^Nsc);

res= mirdwt(lpr,yh,h,Nsc+1);