function [dx] = spm_dx(dfdx,f,t)
% returns dx(t) = (expm(dfdx*t) - I)*inv(dfdx)*f
% FORMAT [dx] = spm_dx(dfdx,f,[t])
% dfdx   = df/dx
% f      = dx/dt
% t      = integration time: (default t = Inf);
%          if t is a cell (i.e., {t}) then t is set to:
%          exp(t - log(diag(-dfdx))
%
% dx     = x(t) - x(0)
%--------------------------------------------------------------------------
% Integration of a dynamic system using local linearization.  This scheme
% accommodates nonlinearities in the state equation by using a functional of
% f(x) = dx/dt.  This uses the equality
%
%             expm([0   0     ]) = (expm(t*dfdx) - I)*inv(dfdx)*f
%                  [t*f t*dfdx]
%
% When t -> Inf this reduces to
%
%              dx(t) = -inv(dfdx)*f
%
% These are the solutions to the gradient ascent ODE
%
%            dx/dt   = k*f = k*dfdx*x =>
%
%            dx(t)   = expm(t*k*dfdx)*x(0)
%                    = expm(t*k*dfdx)*inv(dfdx)*f(0) -
%                      expm(0*k*dfdx)*inv(dfdx)*f(0)
%
% When f = dF/dx (and dfdx = dF/dxdx), dx represents the update from a
% Gauss-Newton ascent on F.  This can be regularised by specifying {t}
% A heavy regularization corresponds to t = -4 and a light
% regularization would be t = 4. This version of spm_dx uses an augmented
% system and the Pade approximation to compute requisite matrix
% exponentials
%
% references:
%
% Friston K, Mattout J, Trujillo-Barreto N, Ashburner J, Penny W. (2007).
% Variational free energy and the Laplace approximation. NeuroImage.
% 34(1):220-34
%
% Ozaki T (1992) A bridge between nonlinear time-series models and
% nonlinear stochastic dynamical systems: A local linearization approach.
% Statistica Sin. 2:113-135.
%
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_dx.m 7144 2017-07-31 13:55:55Z karl $

% defaults
%--------------------------------------------------------------------------
nmax  = 512;                        % threshold for numerical approximation
if nargin < 3, t = Inf; end         % integration time
xf    = f; f = spm_vec(f);          % vectorise
n     = length(f);                  % dimensionality

% t is a regulariser
%--------------------------------------------------------------------------
sw  = warning('off','MATLAB:log:logOfZero');
if iscell(t)

    % relative integration time
    %----------------------------------------------------------------------
    t      = t{:};
    if isscalar(t)
        t  = exp(t - spm_logdet(dfdx)/n);
    else
        t  = exp(t - log(diag(-dfdx)));
    end

end
warning(sw);

% use a [pseudo]inverse if all t > TOL
%==========================================================================
if min(t) > exp(16)

    dx = -spm_pinv(dfdx)*f;

else

    % ensure t is a scalar or matrix
    %----------------------------------------------------------------------
    if isvector(t), t = diag(t); end

    % augment Jacobian and take matrix exponential
    %======================================================================
    J = spm_cat({0  []      ;
                t*f t*dfdx});

    % solve using matrix expectation
    %----------------------------------------------------------------------
    if n  <= nmax
        dx    = spm_expm(J);
        dx    = dx(:,1);
    else
        x     = sparse(1,1,1,n + 1,1);
        dx    = expv(1,J,x);
    end

    % recover update
    %----------------------------------------------------------------------
    dx    = dx(2:end);

end
dx = spm_unvec(real(dx),xf);

return

%==========================================================================
%  Roger B. Sidje (rbs@maths.uq.edu.au)
%  EXPOKIT: Software Package for Computing Matrix Exponentials.
%  ACM - Transactions On Mathematical Software, 24(1):130-156, 1998

function  [w, err, hump] = expv( t, A, v, tol, m )
%  FOTMAT [w, err, hump] = expv( t, A, v, tol, m )
%  EXPV computes an approximation of w = exp(t*A)*v for a
%  general matrix A using Krylov subspace  projection techniques.
%  It does not compute the matrix exponential in isolation but instead,
%  it computes directly the action of the exponential operator on the
%  operand vector. This way of doing so allows for addressing large
%  sparse problems. The matrix under consideration interacts only
%  via matrix-vector products (matrix-free method).
%
%  w = expv( t, A, v )
%  computes w = exp(t*A)*v using a default tol = 1.0e-7 and m = 30.
%
%  [w, err] = expv( t, A, v )
%  renders an estimate of the error on the approximation.
%
%  [w, err] = expv( t, A, v, tol )
%  overrides default tolerance.
%
%  [w, err, hump] = expv( t, A, v, tol, m )
%  overrides default tolerance and dimension of the Krylov subspace,
%  and renders an approximation of the `hump'.
%
%  The hump is defined as:
%          hump = max||exp(sA)||, s in [0,t]  (or s in [t,0] if t < 0).
%  It is used as a measure of the conditioning of the matrix exponential
%  problem. The matrix exponential is well-conditioned if hump = 1,
%  whereas it is poorly-conditioned if hump >> 1. However the solution
%  can still be relatively fairly accurate even when the hump is large
%  (the hump is an upper bound), especially when the hump and
%  ||w(t)||/||v|| are of the same order of magnitude (further details in
%  reference below).
%
%  Example 1:
%  ----------
%    n = 100;
%    A = rand(n);
%    v = eye(n,1);
%    w = expv(1,A,v);
%
%  Example 2:
%  ----------
%    % generate a random sparse matrix
%    n = 100;
%    A = rand(n);
%    for j = 1:n
%        for i = 1:n
%            if rand < 0.5, A(i,j) = 0; end;
%        end;
%    end;
%    v = eye(n,1);
%    A = sparse(A); % invaluable for a large and sparse matrix.
%
%    tic
%    [w,err] = expv(1,A,v);
%    toc
%
%    disp('w(1:10) ='); disp(w(1:10));
%    disp('err =');     disp(err);
%
%    tic
%    w_matlab = expm(full(A))*v;
%    toc
%
%    disp('w_matlab(1:10) ='); disp(w_matlab(1:10));
%    gap = norm(w-w_matlab)/norm(w_matlab);
%    disp('||w-w_matlab|| / ||w_matlab|| ='); disp(gap);
%
%  In the above example, n could have been set to a larger value,
%  but the computation of w_matlab will be too long (feel free to
%  discard this computation).
%
%  See also MEXPV, EXPOKIT.

%  Roger B. Sidje (rbs@maths.uq.edu.au)
%  EXPOKIT: Software Package for Computing Matrix Exponentials.
%  ACM - Transactions On Mathematical Software, 24(1):130-156, 1998
%__________________________________________________________________________

[n,n] = size(A);
if nargin == 3,
    tol = 1.0e-7;
    m = min(n,30);
end;
if nargin == 4,
    m = min(n,30);
end;

anorm = norm(A,'inf');
mxrej = 10;  btol  = 1.0e-7;
gamma = 0.9; delta = 1.2;
mb    = m; t_out   = abs(t);
nstep = 0; t_new   = 0;
t_now = 0; s_error = 0;
rndoff= anorm*eps;

k1    = 2; xm = 1/m; normv = norm(v); beta = normv;
fact  = (((m+1)/exp(1))^(m+1))*sqrt(2*pi*(m+1));
t_new = (1/anorm)*((fact*tol)/(4*beta*anorm))^xm;
s     = 10^(floor(log10(t_new))-1); t_new = ceil(t_new/s)*s;
sgn   = sign(t); nstep = 0;

w     = v;
hump  = normv;
while t_now < t_out
    nstep = nstep + 1;
    t_step = min( t_out-t_now,t_new );
    V = zeros(n,m+1);
    H = zeros(m+2,m+2);

    V(:,1) = (1/beta)*w;
    for j = 1:m
        p = A*V(:,j);
        for i = 1:j
            H(i,j) = V(:,i)'*p;
            p = p-H(i,j)*V(:,i);
        end;
        s = norm(p);
        if s < btol,
            k1 = 0;
            mb = j;
            t_step = t_out-t_now;
            break;
        end;
        H(j+1,j) = s;
        V(:,j+1) = (1/s)*p;
    end;
    if k1 ~= 0,
        H(m+2,m+1) = 1;
        avnorm = norm(A*V(:,m+1));
    end;
    ireject = 0;
    while ireject <= mxrej,
        mx = mb + k1;
        F = expm(sgn*t_step*H(1:mx,1:mx));
        if k1 == 0,
            err_loc = btol;
            break;
        else
            phi1 = abs( beta*F(m+1,1) );
            phi2 = abs( beta*F(m+2,1) * avnorm );
            if phi1 > 10*phi2,
                err_loc = phi2;
                xm = 1/m;
            elseif phi1 > phi2,
                err_loc = (phi1*phi2)/(phi1-phi2);
                xm = 1/m;
            else
                err_loc = phi1;
                xm = 1/(m-1);
            end;
        end;
        if err_loc <= delta * t_step*tol,
            break;
        else
            t_step = gamma * t_step * (t_step*tol/err_loc)^xm;
            s = 10^(floor(log10(t_step))-1);
            t_step = ceil(t_step/s) * s;
            if ireject == mxrej,
                error('The requested tolerance is too high.');
            end;
            ireject = ireject + 1;
        end;
    end;
    mx = mb + max( 0,k1-1 );
    w = V(:,1:mx)*(beta*F(1:mx,1));
    beta = norm( w );
    hump = max(hump,beta);

    t_now = t_now + t_step;
    t_new = gamma * t_step * (t_step*tol/err_loc)^xm;
    s = 10^(floor(log10(t_new))-1);
    t_new = ceil(t_new/s) * s;

    err_loc = max(err_loc,rndoff);
    s_error = s_error + err_loc;
end;
err  = s_error;
hump = hump / normv;

return

function E = padm( A, p )
%  FORMAT E = padm( A, p )
%  PADM computes the matrix exponential exp(A) using the irreducible
%  (p,p)-degree rational Pade approximation to the exponential function.
%
%  E = padm( A )
%  p is internally set to 6 (recommended and generally satisfactory).
%
%  See also CHBV, EXPOKIT and the MATLAB supplied functions EXPM and EXPM1.

%  Roger B. Sidje (rbs@maths.uq.edu.au)
%  EXPOKIT: Software Package for Computing Matrix Exponentials.
%  ACM - Transactions On Mathematical Software, 24(1):130-156, 1998
%__________________________________________________________________________

if nargin == 1, p = 6; end;
[n,n] = size(A);

% Pade coefficients (1-based instead of 0-based as in the literature)
%--------------------------------------------------------------------------
c(1) = 1;
for k = 1:p
    c(k+1) = c(k)*((p+1-k)/(k*(2*p+1-k)));
end;

% Scaling
%--------------------------------------------------------------------------
s = norm(A,'inf');
if s > 0.5,
    s = max(0,fix(log(s)/log(2))+2);
    A = 2^(-s)*A;
end;

% Horner evaluation of the irreducible fraction (see ref. above)
%--------------------------------------------------------------------------
I = eye(n);
A2 = A*A;
Q = c(p+1)*I;
P = c(p)*I;
odd = 1;
for k = p-1:-1:1,
    if odd == 1,
        Q = Q*A2 + c(k)*I;
    else
        P = P*A2 + c(k)*I;
    end;
    odd = 1-odd;
end;
if odd == 1
    Q = Q*A;
    Q = Q - P;
    E = -(I + 2*(Q\P));
else
    P = P*A;
    Q = Q - P;
    E = I + 2*(Q\P);
end;

% Squaring
%--------------------------------------------------------------------------
for k = 1:s,
    E = E*E;
end;

return


function  z = embedding(x,types,positions,w,dropout)
% embedding   The BERT embeddings of encoded tokens, token types and token
% positions.
%
%   Z = embedding(X,types,positions,weights,dropoutProbability) computes
%   the embedding of encoded tokens X, token types specified by types, and
%   token positions. Inputs X, types and positions are
%   1-by-numInputTokens-by-numObs unformatted dlarray-s. The types take
%   values 1 or 2. The weights input is a struct of embedding weights such
%   as mdl.Parameters.Weights.embeddings where mdl = bert(). The
%   dropoutProbability is a scalar double between 0 and 1 corresponding to
%   the post-embedding dropout probability.

% Copyright 2021 The MathWorks, Inc.
wordEmbedding = embed(x,w.word_embeddings,'DataFormat','CTB');
typeEmbedding = embed(types,w.token_type_embeddings,'DataFormat','CTB');
positionEmbedding = embed(positions,w.position_embeddings,'DataFormat','CTB');
z = wordEmbedding+typeEmbedding+positionEmbedding;
z = transformer.layer.normalization(z,w.LayerNorm.gamma,w.LayerNorm.beta);
z = transformer.layer.dropout(z,dropout);
end


function pass = test_integral( )
% Test INTEGRAL()

tol = 1000*chebfunpref().cheb2Prefs.chebfun2eps;

%test empty diskfun
f = diskfun();
pass(1) = (integral(f) == 0);
pass(2) = (integral(f, chebfun(@(x) x)) == 0);

%test unitcircle feature
f = diskfun(@(x,y) sin(x.^2+3*y));
pass(3) = ( abs(sum(f(:, 1))-integral(f, 'unitcircle')) < tol);
pass(4) = ( abs(integral(diskfun.harmonic(3,2, 'neumann'), 'unitcircle')) < tol);

% test integral along a contour (need more rigorous tests when complex-valued
% diskfuns are supported)
z = chebfun(@(x) .5*exp(1i*pi*x));
g = diskfun(@(x, y) exp(-2*x.^2-2*y.^2));
pass(5) = ( abs(.5*sum(g(:,.5))-integral(g, z)) < tol);
z = chebfun(@(x) x*exp(1i*pi/4));
pass(6) = ( abs(sum(g(pi/4, :))-integral(g, z) )  < tol );
end


 function y = Gtomo_nufft_filter(omega, ob, do_phase)
%function y = Gtomo_nufft_filter(omega, ob, do_phase)
% build the sinogram-spectrum-sized matrix
% that is .* multiplied after 2D FT, before iFFT or NUiFFT
% in
%	omega	[M 2]	frequency sample locations (radians)
% out
%	y	[M 2]	filter
% Copyright 2001-1, Jeff Fessler, The University of Michigan
% Extend to fan-beam geometry, 2003-11, Yingying Zhang

%
% effect of image-domain shift
%
y = exp(1i * (omega * ob.nxy_shift(:)));

%
% effect of image basis function (extension to non-rect by S. Matej)
% basis: b(x/Dx,y/Dy) <-FT-> Dx*Dy * B(Dx*u,Dy*v)
%
y = y .* (ob.dx).^2;

if isempty(ob.basis.type) | streq(ob.basis.type, 'pixel')
	% sinc_2 due to square pixels
	if ob.chat, printf('pixel basis, dx=%g', ob.dx), end
	y = y .* sinc(omega(:,1)/(2*pi)) .* sinc(omega(:,2)/(2*pi));

elseif streq(ob.basis.type, 'no')
	if ob.chat, printf('no image basis modeled'), end

elseif streq(ob.basis.type, 'KB')
	if ob.chat, printf('KB basis: J=%g, alpha=%g, m=%g, n=%g', ...
		ob.basis.diam, ob.basis.shape, ob.basis.m, ob.basis.dim); end
	norm = kaiser_bessel_ft(0, ...
		ob.basis.diam, ob.basis.shape, ob.basis.m, ob.basis.dim);
	nd = sqrt(omega(:,1)/(2*pi) .* omega(:,1)/(2*pi) + ...
		omega(:,2)/(2*pi) .* omega(:,2)/(2*pi));
	y = y .* kaiser_bessel_ft(nd, ...
		ob.basis.diam, ob.basis.shape, ob.basis.m, ob.basis.dim) / norm;

elseif streq(ob.basis.type, 'Gauss')
	error('Gaussian basis not yet implemented')

else
	error(sprintf('basis function %s not implemented', ob.basis.type))
end

% form into sinogram shape
K = ob.Krho;
y = reshape(y, K, ob.na);

%
% detector radial response resolution-loss effect in frequency domain
% extension to parallel-beam non-rect case by S. Matej
%

kk = [-K/2 : K/2-1]';
if isempty(ob.beam.type) | streq(ob.beam.type, 'rect')

	% Trick: in fan-beam, the detector blur is not shift-invariant.
	% We approximate by the blur at the center of object.
	% The effective strip_width / ds at the center is the same.

	strip_ray = ob.strip_width / ob.ds;
	blur = sinc(strip_ray * kk / K);
	if ob.chat
		printf('strip integrals, relative beam width=%g', strip_ray)
	end

elseif streq(ob.beam.type, 'line')
	blur = ones(size(kk));
	if ob.chat, printf('line integrals modeled'), end

elseif streq(ob.beam.type, 'KB')
	if ob.chat, printf('KB beam shape: J=%g, alpha=%g, m=%g', ...
		ob.beam.diam, ob.beam.shape, ob.beam.m), end
	rel_bdiam = ob.beam.diam;	% KB_diameter relative to ob.ds
	norm = kaiser_bessel_ft(0, rel_bdiam, ob.beam.shape, ob.beam.m, 1);
	blur = kaiser_bessel_ft(...
		kk/K, rel_bdiam, ob.beam.shape, ob.beam.m, 1) / norm;

elseif streq(ob.beam.type, 'Gauss')
	g_sigma = ob.beam.shape / sqrt(8*log(2));
	blur = exp(-(2*pi*kk/K).^2 .* g_sigma^2/2);

else
	error(sprintf('beam shape %s not implemented', ob.beam.type))

end

y = y .* repmat(blur, [1 ob.na]);	% include blur effect

%
% phase "shift" to effect a half-pixel shift in each row
% corresponding to "offset=0" in tomographic projection.
% build in the post-fft shift too while at it.
%
%if streq(ob.geometry, 'par') % do we need this in fan-beam ???
if do_phase % do we need this in fan-beam ???
	phase = exp(1i*2*pi*(K+ob.is.shift0)/2 * kk / K);
	y = y .* repmat(phase, [1 ob.na]);
	y = y ./ ob.ds;	% see JF tech. report
end

% trick: for parallel case, build in the phase shift due to offset_s
% added 2005-8-24 since it had been omitted previously
if ~ob.is.fan
	phase = exp(-2i*pi * ob.offset_s * kk / K);
	y = y .* repmat(phase, [1 ob.na]);
end


function c = mvn_vbcost(x,L,mu,L_mu,L_K,logdet_K)
warning('This function is deprecated')

% c = mvnvbcost(x,Covx,mu,Covmu,pCholCov,pLogDetCov)
%
% Calculates c = -KL(q||p) = <log p(X)> - <log q(X)>
% where the expectation <.> is taken over q(X). This is used for
% calculating the lower bound of the marginal loglikelihood in VB
% models.
%
% p(X) = N(M,K)
% q(X) = N(x,Covx)
%
% Rest of the parameters are defined as:
% q(M) = N(mu,Covmu)
% <inv K> = inv(L_K * L_K')
% <log det K> = logdet_K
%
% If Covx==[], then the term <log q(X)> is not calculated.
% This is useful when X is, e.g., observations.

c = 0;
d = length(x);

% Cost from q-posterior
if ~isempty(L)
  c = mnorm_entropy(L);
else
  L = 0;
end

% Use Cholesky of the prior covariance
z = solve_tril(L_K, x-mu);

% Cost from prior

% Below: (x-mu)' * Cov^(-1) * (x-mu) + trace(Cov^(-1)*(Covx+Covmu))
V = linsolve_chol(L_K, L*L'+L_mu*L_mu')
err2 = z'*z + trace(V, V); % TODO: Optimize!
% $$$ err2 = z'*z + trace(solve_triu(pCholCov',solve_tril(pCholCov,Covx+Covmu)));
c = c - 0.5*logdet_K - 0.5*err2 - 0.5*d*log(2*pi);


function [overallFitness,mostFit,f] = getOverallFitness(cbalColinPowerWeights,fitnessMatrix)
% function [overallFitness,mostFit,f] = getOverallFitness(cbalColinPowerWeights,fitnessMatrix)
%
%
% Tor Wager, 11/17/01

   	% Determine overall fitness
   	for i = 1:size(cbalColinPowerWeights,2)				% convert to z scores
      if not(sum(fitnessMatrix(i,:)) == 0)              % only for rows with > 0 weight
         if std(fitnessMatrix(i,:)) == 0
            % disp(['Warning: All fitness scores for row ' num2str(i) ' are the same. Sum ' num2str(sum(fitnessMatrix(i,:)))])
            zfitnessMatrix(i,:) = (fitnessMatrix(i,:) - mean(fitnessMatrix(i,:))) / 1;
         else
      		zfitnessMatrix(i,:) = (fitnessMatrix(i,:) - mean(fitnessMatrix(i,:))) / std(fitnessMatrix(i,:));
         end
      end
   	end
   zfitnessMatrix((cbalColinPowerWeights == 0),:) = 0;
   overallFitness = (cbalColinPowerWeights * zfitnessMatrix) / sum(cbalColinPowerWeights);  % weighted avg sum z-scores

   % determine the most fit organism overall
   mostFit = find(overallFitness == max(overallFitness));

   % avoid duplicates
   mostFit = mostFit(1);

   % return fitness scores for most fit
   f = fitnessMatrix(:,mostFit);

return


%*****************************************************************************************************
%NAME: esemilogy.m
%AUTHOR: Andri M. Gretarsson
%DATE: 01/21/98
%
%SYNTAX:	esemilogy(X,Y, 'colour')
%
%Note that 'colour' is NOT an optional argument.
%
%This function acts just like the built-in function 'semilogy' but plots error-bars. The error-bars
%are plotted in the colour given by 'colour'.  'X' and 'Y' are Nx2 matrixes, the first column
%representing the values of the coordinate (x or y), the second column representing the uncertainty
%('error') of those values.  'colour' is a one-letter string which must be one of the letters allowed
%in the built-in matlab function 'plot', to specify the plot colour.  Note that the function exits
%with "hold" set to "off".
%
%This function does not print points in addition to the error bars.  Where the error bars cross, is
%the coordinate point.  This means that if both error bars are exceedingly small complared to the
%coordinate values, the mark will be correspondingly small.  In such situations, it may be better to
%use 'semilogy' directly and specify that the error is smaller than the size of the mark.
%
%EXAMPLE:
%
%X=[1.0	0.2
%   2.0	0.2]
%Y=[1.0 	0.25
%   2.0	0.25]
%esemilogy(X,Y,'g')
%
%plots a green cross of width 0.2 and height 0.25 at coordinate (1.0,1.0), and a cross of width 0.2
%and height 0.25 at coordinate( (2.0,2.0), on a linear x, log y scale
%
%LAST MODIFIED:  01/21/98
%*****************************************************************************************************

function esemilogx=semilogxplot(x,y,colourstring)

    xvalue=x(:,1);												%For clarity
    xerror=x(:,2);
    yvalue=y(:,1);
    yerror=y(:,2);

    semilogy(xvalue-xerror,yvalue-yerror,'w-',xvalue+xerror,yvalue+yerror,'w-'); hold on;
    %Sets appropriate axes but otherwise invisible on a white background.

    for i=1:length(xvalue)
        semilogy([xvalue(i)-xerror(i) xvalue(i)+xerror(i)], [yvalue(i) yvalue(i)], colourstring);
        semilogy([xvalue(i) xvalue(i)], [yvalue(i)-yerror(i) yvalue(i)+yerror(i)], colourstring);
    end
    hold off;


function [varargout]=rhombicDodecahedronMesh(varargin)

% function [E,V,C,F,CF,CFF]=rhombicDodecahedronMesh(r,nCopies)
% ------------------------------------------------------------------------
% Creates a rhombic dodecahedron mesh where r sets the radias and nCopies
% (a 1x3 vector) sets the number of copies in the x, y, and z direction.
% The output consists of:
%
% Fc_Q, Fc_T: the quadrilateral and triangular face cell arrays (1 cell
% entry per element).
%
% Ft_Q, Ft,T: the quadrilateral and triangular face arrays
%
% Ct_Q, Ct,T: color/label data for the face arrays
%
% Vt: the vertex array
%
% Kevin Mattheus Moerman
% gibbon.toolbox@gmail.com
%
% 2019/02/08 Created
% 2019/10/13 Changed orientation for gridding to create more regular grid
% ------------------------------------------------------------------------

%% Parse input

switch nargin
    case 1
        r=varargin{1};
        nCopies=2;
    case 2
        r=varargin{1};
        nCopies=varargin{2};
end

if isempty(nCopies)
    nCopies=2;
end

if numel(nCopies)==1
    nCopies=nCopies*ones(1,3);
end

%%

% Get single rhombic dodecahedron
[~,Vs]=rhombicDodecahedron(r);
V_offsets=2*r*eye(3,3); %Offset vectors

% Copy over rhombic dodecahedron in grid like fashion 4 times and shift
% some of the copies to nest them together.
nCopies1=nCopies;
[Vg1]=gridClone(Vs,nCopies1,V_offsets);

nCopies2=nCopies;
nCopies2([1,2])=nCopies2([1,2])-1;
[Vg2]=gridClone(Vs,nCopies2,V_offsets);

nCopies3=nCopies;
nCopies3([1,3])=nCopies3([1,3])-1;
[Vg3]=gridClone(Vs,nCopies3,V_offsets);

nCopies4=nCopies;
nCopies4([2,3])=nCopies4([2,3])-1;
[Vg4]=gridClone(Vs,nCopies4,V_offsets);

%%

Vg2(:,1)=Vg2(:,1)+r;
Vg2(:,2)=Vg2(:,2)+r;

Vg3(:,1)=Vg3(:,1)+r;
Vg3(:,3)=Vg3(:,3)+r;

Vg4(:,2)=Vg4(:,2)+r;
Vg4(:,3)=Vg4(:,3)+r;

V=[Vg1;Vg2;Vg3;Vg4];

%% Create element and face arrays
E=reshape((1:1:size(V,1)),14,size(V,1)/14)';
C=(1:1:size(E,1))'; %Element colors
[F,CF,CFF]=element2patch(E,C);

%% Merging point and fix element and face indices
[F,V,~,indFix]=mergeVertices(F,V);
E=indFix(E);

%% Collect output
varargout{1}=E;
varargout{2}=V;
varargout{3}=C;
varargout{4}=F;
varargout{5}=CF;
varargout{6}=CFF;

end

%%

function [Vg]=gridClone(Vs,nCopies,V_offsets)

nTotal=prod(nCopies); %Total number of copies
indC=ones(size(Vs,1),1)*(1:1:nTotal);
indC=indC(:);
[I,J,K] = ind2sub(nCopies,indC);
I=I-1; J=J-1; K=K-1;

%Defining offsets
D1=I*V_offsets(1,:);
D2=J*V_offsets(2,:);
D3=K*V_offsets(3,:);

%Defining vertices matrix
Vg=repmat(Vs,nTotal,1)+(D1+D2+D3);

end

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function [x,y] = allPairs(x,y)
% all pairs of elements of x and y modulo permutation
%

if nargin == 1

  [x,y] = meshgrid(x,x);

  x = x(tril(ones(size(x)))>0);
  y = y(tril(ones(size(y)))>0);

else

  %
  x = x(:);
  y = y(:);
  iseq = bsxfun(@eq,x,y.');

  [ix,iy] = find(iseq);

  x = [x(ix);x(~any(iseq,2))];
  y = [y(iy);y(~any(iseq,1))];

  % all pairs
  [x,y] = meshgrid(x,y);

  % remove double pars
  A = zeros(size(x));
  A(1:length(ix),1:length(ix)) = 1;
  A = ~triu(A,1);

  x = x(A);
  y = y(A);

end

if nargout < 2, x = [x(:),y(:)]; end


function value = scasum ( n, x, incx )

%*****************************************************************************80
%
%% SCASUM takes the sum of the absolute values of a complex vector.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 April 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1,
%    LC: QA214.L56.
%
%    Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
%    Basic Linear Algebra Subprograms for FORTRAN usage,
%    ACM Transactions on Mathematical Software,
%    Volume 5, Number 3, pages 308-323, 1979.
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Input, complex X(*), the vector.
%
%    Input, integer INCX, the increment between successive entries of X.
%
%    Output, real VALUE, the sum of the absolute values.
%
  value = sum ( abs ( real ( x(1:incx:1+(n-1)*incx) ) ) ...
              + abs ( imag ( x(1:incx:1+(n-1)*incx) ) ) );

  return
end


%% DML Toolbox Functions
%
% * <analysis.html |dml.analysis|> - multivariate analysis class
% * <blogreg.html |dml.blogreg|> - Bayesian logistic regression
% * <bootstrap.html |dml.bootstrap|> - bootstrapping to determine parameter relevance
% * <circreg.html |dml.circreg|> - circular regression method
% * <corclas.html |dml.corclas|> - template matching correlation based classifier
% * <crossvalidator.html |dml.crossvalidator|> - crossvalidation class
% * <enet.html |dml.enet|> - efficient elastic net algorithm
% * <filterer.html |dml.filterer|> - filtering approach to feature selection
% * <garrote.html |dml.garrote|> - variational garrote
% * <gp.html |dml.gp|> - Gaussian process
% * <graphnet.html |dml.graphnet|> - native implementation of graphnet algorithm
% * <gridsearch.html |dml.gridsearch|> - grid search method
% * <hmm.html |dml.hmm|> - Hidden Markov model with continuous observations
% * <lds.html |dml.lds|> - linear dynamical system
% * <method.html |dml.method|> - abstract class for multivariate methods
% * <naive.html |dml.naive|> - gaussian naive Bayes classifier
% * <ndata.html |dml.ndata|> - wrapper class to make methods handle multiple datas
% * <noutput.html |dml.noutput|> - wrapper class to make methods handle multiple outpu
% * <one_against_one.html |dml.one_against_one|> - one-against-one binary classification
% * <one_against_rest.html |dml.one_against_rest|> - one-against-rest binary classification
% * <permutation.html |dml.permutation|> - permutation testing class
% * <prior.html |dml.prior|> - used to created smoothing and shrinkage priors
% * <searchlight.html |dml.searchlight|> - searchlight analysis
% * <sincos.html |dml.sincos|> - circular regression by decomposing into sine and co
% * <slda.html |dml.slda|> - shrinkage linear discriminant analysis
% * <sopls.html |dml.sopls|> - sparse orthonormalized partial least squares
% * <standardizer.html |dml.standardizer|> - takes zscores
% * <statistic.html |dml.statistic|> - test statistics
% * <svm.html |dml.svm|> - support vector machine
% * <whiten.html |dml.whiten|> - whitens the data


function hermite_polynomial_test15 ( )

%*****************************************************************************80
%
%% HERMITE_POLYNOMIAL_TEST15 tests HE_POLYNOMIAL_COEFFICIENTS.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 March 2012
%
%  Author:
%
%    John Burkardt
%
  n = 10;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'HERMITE_POLYNOMIAL_TEST15\n' );
  fprintf ( 1, '  HE_POLYNOMIAL_COEFFICIENTS determines the probabilist''s Hermite \n' );
  fprintf ( 1, '  polynomial coefficients.\n' );

  c = he_polynomial_coefficients ( n );

  for i = 0 : n
    fprintf ( 1, '\n' );
    fprintf ( 1, '  He(%d) = \n', i );
    fprintf ( 1, '\n' );
    for j = i : -1 : 0
      if ( c(i+1,j+1) == 0.0 )

      elseif ( j == 0 )
        fprintf ( 1, '  %f\n', c(i+1,j+1) );
      elseif ( j == 1 )
        fprintf ( 1, '  %f * x\n', c(i+1,j+1) );
      else
        fprintf ( 1, '  %f * x^%d\n', c(i+1,j+1), j );
      end
    end
  end

  return
end


function centroids = computeNewCentroids(X, idx, K)

% computeNewCentroids computes the new centroids of each cluster based on
% the mean value of the all the points belonging to that cluster.

% Initialize variables
[m n] = size(X);
centroids = zeros(K, n);

for i=1:K
    temp = find(idx==i);
    Xtemp = X(temp,:); % Get all points belonging to that cluster
    centroids(i,:) = (sum(Xtemp,1))./length(Xtemp); % Assign new centroid based on mean
end


function center = findCenter( PSF )
%
%       center = findCenter( PSF );
%
%  Given (possibly) several PSF images, this function sets the center
%  of the PSFs (location of point source) to be the location of max
%  entry of the PSFs.
%
%  Input:
%         PSF  -  cell array containing the PSF image(s)
%
%  Output:
%         center - array [row_index, col_index] containing the
%                  location of the center of the PSF.
%

%  J.Nagy 1/8/02

center = cell(size(PSF));

%
%  Note that this should work for 2D as well as 3D images.  We simply
%  find the maximum entry in the center of the image.
%
for k = 1:size(PSF, 3)
  for i = 1:size(PSF, 1)
    for j = 1:size(PSF, 2)
       P = PSF{i,j,k};
       [m, n, l] = size(P);
       idx_row = floor((m+1)/2):ceil((m+1)/2);
       idx_col = floor((n+1)/2):ceil((n+1)/2);
       idx_depth = floor((l+1)/2):ceil((l+1)/2);
       P2 = zeros(size(P));
       P2(idx_row, idx_col, idx_depth) = P(idx_row, idx_col, idx_depth);
       [ci, cj, ck] = ind2sub(size(P2), find( P2 == max( P2(:) ) ));
       c = [min(ci), min(cj), min(ck)];
       center{i,j,k} = c(1:length(size(P2)));
    end
  end
end


function results = vl_test_cummax(varargin)
% VL_TEST_CUMMAX
vl_test_init ;

function test_basic()
vl_assert_almost_equal(...
  vl_cummax(1), 1) ;
vl_assert_almost_equal(...
  vl_cummax([1 2 3 4], 2), [1 2 3 4]) ;

function test_multidim()
a = [1 2 3 4 3 2 1] ;
b = [1 2 3 4 4 4 4] ;
for k=1:6
  dims = ones(1,6) ;
  dims(k) = numel(a) ;
  a = reshape(a, dims) ;
  b = reshape(b, dims) ;
  vl_assert_almost_equal(...
    vl_cummax(a, k), b) ;
end

function test_storage_classes()
types = {@double, @single, ...
         @int32, @uint32, ...
         @int16, @uint16, ...
         @int8, @uint8} ;
if vl_matlabversion() > 71000
  types = horzcat(types, {@int64, @uint64}) ;
end
for a = types
  a = a{1} ;
  for b = types
    b = b{1} ;
    vl_assert_almost_equal(...
      vl_cummax(a(eye(3))), a(toeplitz([1 1 1], [1 0 0 ]))) ;
  end
end


function fMI = fusionMI(imgA,imgB,imgF)
% the overall mutual information between source images and fused image
%--------------------------------------------------------------------
[pAF, pA, pF] = estpab(imgA,imgF);
MIAF = estmutualinfo(pAF, pA, pF);

[pBF, pB, pF] = estpab(imgB,imgF);
MIBF = estmutualinfo(pBF, pB, pF);

fMI = MIAF + MIBF;
return


function  eta = conftim1(a, om, omr, omt, h)
% calculate the conformal time today for a universe with a cosmological constant
% oml = ratio of dark energy and matter = ohm_l/ohm_m
%
% D Vangheluwe 2 oct 2004
% modified for curvature of space with extra variable : omt (dd 31 mrt 2005)

global  GL_cmb_c  GL_cmb_h0;

% velocity of light in m/s
%c = 2.998e8;
c = GL_cmb_c;
% the hubble constant at present in h Mpc^-1, see my notes p71
%h0 = 1e5/c;
h0 = GL_cmb_h0;

eta = 1 ./ (h0 * h * sqrt(omr + om * a + (omt - omr - om) * a.^4 + (1 - omt) * a.^2));


function [R] = averageRotationTransform(R1s,R2s)
%AVERAGEROTATION Summary of this function goes here
%   takes from R1s to R2s
[R1s,R2s] = removeNanRots(R1s,R2s);
R = eye(3);
N = length(R1s);
maxDiff = pi/3;

for i=1:N
    diffs{i} = R1s{i}'*R2s{i};
end
err = Inf;
iter = 0;

while(err > 1e-4 && iter < 50)
    iter = iter+1;
    delta = zeros(3,3);ct = 0;
    for i=1:N
        erri = norm(logm(R'*diffs{i}),'fro')/sqrt(2);
        if(iter < 5 || erri < maxDiff)
            delta = delta + logm(R'*diffs{i});
            ct = ct+1;
        end
    end
    delta = (delta/ct);
    err = (norm(delta,'fro')/sqrt(2));
    %disp(err);
    delta = expm(delta);
    R = R*delta;
    iter = iter + 1;
end
R = real(R);

end

function [nR1s,nR2s]=removeNanRots(R1s,R2s)
    n1 = cellfun(@(x)(any(isnan(x(:)))),R1s(:));
    n2 = cellfun(@(x)(any(isnan(x(:)))),R2s(:));
    n12 = n1 | n2;
    nR1s = R1s(~n12);
    nR2s = R2s(~n12);
end


function desc = calGradient(rgb_im, seg, numRegion)

    G = fspecial('gaussian',[4 4],2);
    Ig = imfilter(rgb_im,G,'same');
    [Gmag,Gdir] = imgradient(rgb2gray(Ig));

    binNum = 20;
    inter = max(max(Gmag)) / binNum;
    Gmag = int32(Gmag / inter);

    binVal = 1:binNum;
    desc = zeros([numRegion binNum]);

    cnt = 0;
    ind={};
    for iReg=1:numRegion
        ind{iReg} = seg(:)==iReg;
    end

    for bin=1:binNum
        cnt = cnt + 1;
        I =  (Gmag(:)==binVal(bin)) ;
        for iReg=1:numRegion
            desc(iReg,cnt) = sum(I(ind{iReg}));
        end
    end

    tmp = sum(desc, 2);
    desc = desc ./ repmat(tmp(:,:), [1 size(desc,2)]);

end


% Calculate gridness score for an autocorrelogram
%
% Calculates a gridness score by expanding a circle around the centre field and
% calculating a correlation value of the expanded circle with it's rotated versions.
% The expansion is done up until the smallest side of the autocorrelogram.
% Can also calculate grid statistics.
%
% Gridness score value by itslef is calculated as a maximum over sliding mean
% of expanded circles. The widtg of the sliding window is given by variable
% numGridnessRadii. This is done in order to keep the values the same with
% historical development of gridness score.
%
% NB! The function assumes that the provided input is indeed an autocorrelgoram.
% This means that the center pixel must have the maximum value. This must be true
% for both square and rectangular autocorrelogram. This will be important if you
% manually create square matrix from a rectangular one. Imagine you have a rectangular
% matrix aCorr of size 309x305, aCorr(155, 153) == 1. If you make it rectangular
% like  this `aSquare = aCorr(1:305, :);`, then the centre pixel will not have
% the highest values, i.e. `aSquare(153, 153) ~= 1`. You should do it like this
% `aSquare = aCorr(3:309-2, :)`, then `aSquare(153, 153)` yields `1`.
%
%  USAGE
%   [score, <stats>] = analyses.gridnessScore(aCorr, <options>)
%   aCorr       A 2D autocorrelogram. aCorr can be square or rectangular. See the note (NB) above!
%   <options>   Optional list of property-value pairs (see table below)
%
%   ==============================================================================================
%    Properties         Values
%   ----------------------------------------------------------------------------------------------
%    'minOrientation'   Value of minimal difference of inner fields orientation (in degrees). If
%                       there are fields that differ in orientation for less than minOrientation,
%                       then only the closest to the centre field are left. Default value is 15.
%   'debug'             True or False. If set to True, the function produces some debug output.
%   ==============================================================================================
%   score       Gridness score. Ranges from -2 to 2. 2 is more a theoretical bound for a perfect grid.
%               More practical value is around 1.3.
%   stats       If this variable is requested, then it is a structure with the following statistics:
%       spacing         3-element vector with distances from the centre field to neighbour fields.
%       orientation     3-element vector with orientations between the centre field and neighbour fields.
%       ellipse         Ellipse fitted to the grid. Contains the centre, radii and orientation in
%                       radians, stored as [Cx, Cy, Rx, Ry, theta].
%       ellipseTheta    Radius of the ellipse in degrees wrapped in range [0..180].
%
function [gscore, varargout] = gridnessScore(aCorr, varargin)
    nout = max(nargout, 1) - 1;
    inp = inputParser;
    defaultMinOrientation = 15;
    gridStat.orientation = [];
    gridStat.spacing = [];
    gridStat.ellipse = [];
    gridStat.ellipseTheta = nan;

    % input argument check functions
    checkDScalar = @(x) helpers.isdscalar(x, '>0');

    % fill input parser object
    addRequired(inp, 'aCorr');
    addParameter(inp, 'minOrientation', defaultMinOrientation, checkDScalar);
    addParameter(inp, 'debug', false);

    parse(inp, aCorr, varargin{:});

    % get parsed arguments
    minOrientation = inp.Results.minOrientation;
    isDebug = inp.Results.debug;

    halfSize = ceil(size(aCorr)/2);
    half_height = halfSize(1);
    half_width = halfSize(2);
    aCorrRad = min(halfSize);
    aCorrSize = size(aCorr);

    if aCorrSize(1) == 1 || aCorrSize(2) == 1
        gscore = nan;
        if nout > 0
            varargout{1} = gridStat;
            varargout{2} = 0;
            varargout{3} = nan(6, 2);
            varargout{4} = 0;
        end
        return;
    end

    % contourc is efficient if aCorr is normalized
    maxValue = max(max(aCorr));
    if maxValue ~= 1
        aCorr = aCorr / maxValue;
    end

    cFieldRadius = findCentreRadius(aCorr, half_width, half_height);
    if isDebug
        fprintf('Center radius is %f\n', cFieldRadius);
    end
    if cFieldRadius == 0 || cFieldRadius == 1 || cFieldRadius == -1 || cFieldRadius >=min(halfSize)
        gscore = nan;
        if nout > 0
            varargout{1} = gridStat;
            varargout{2} = 0;
            varargout{3} = nan(6, 2);
            varargout{4} = 0;
        end
        return;
    end

    % Meshgrid for expanding circle
    [rr, cc] = meshgrid(1:size(aCorr, 2), 1:size(aCorr, 1));

    % Define iteration radius step size for the gridness score
%     if cFieldRadius>=aCorrRad  % modified by Weijian Zong,20201012
%     cFieldRadius=aCorrRad;  % modified by Weijian Zong,20201012
%     else% modified by Weijian Zong,20201012
%     end % modified by Weijian Zong,20201012
    radSteps = cFieldRadius:aCorrRad;
    radSteps(1) = [];
    numSteps = length(radSteps);

    GNS = zeros(numSteps, 2);
    rotCorr = zeros(1, 5);
    rotAngles_deg = 30*(1:5);

    % aCorr is rotated outside the loop for speed
    rotatedCorr = cell(1, length(rotAngles_deg));
    for i = 1:length(rotAngles_deg)
        rotatedCorr{i} = imrotate(aCorr, rotAngles_deg(i), 'bilinear', 'crop');
    end

    mainCircle = sqrt((cc - half_height).^2 + (rr - half_width).^2);
    innerCircle = mainCircle > cFieldRadius;

    % Define expanding ring of autocorrellogram and do x30 correlations
    for i = 1:numSteps
        ind = (innerCircle & (mainCircle < radSteps(i)));
        tempCorr = reshape(aCorr(ind), 1, [])';
        for j = 1:5
            rotatedCircle = reshape(rotatedCorr{j}(ind), 1, [])';
            rotCorr(j) =  corr(tempCorr, rotatedCircle);
            if isDebug
                fprintf('Step %u, angle %u, corr value %f\n', i-1, rotAngles_deg(j), rotCorr(j));
            end
        end
        GNS(i, 1) = min(rotCorr([2, 4])) - max(rotCorr([1, 3, 5]));
        GNS(i, 2) = radSteps(i);
    end

    % Find the biggest gridness score and radius
    [~, gscoreLoc] = max(GNS(:, 1));
    % See function help about numGridnessRadii
    numGridnessRadii = 3;
    numStep = numSteps - numGridnessRadii;
    if numStep < 1
        numStep = 1;
    end

    if numStep == 1
        gscore = nanmean(GNS(:, 1));
    else
        meanGridnessArray = zeros(numStep, 1);
        for ii = 1:numStep
            meanGridnessArray(ii) = nanmean(GNS(ii:ii + numGridnessRadii-1, 1));
        end

        [gscore, gInd] = max(meanGridnessArray);
        gscoreLoc = gInd + (numGridnessRadii-1)/2;
    end

    varargout{4} = radSteps(gscoreLoc);

    % Return if we do not need to calculate grid statistics
    if nout < 1
        return;
    end

    %% Calculate gridness score statistics
    bestCorr = (mainCircle < radSteps(gscoreLoc) * 1.25) .* aCorr;
    regionalMaxMap = imregionalmax(bestCorr, 4);
    se = strel('square', 3);
    im2 = imdilate(regionalMaxMap, se); % dilate map to eliminate fragmentation
    cc = bwconncomp(im2, 8);
    stats = regionprops(cc, 'Centroid');

    if length(stats) < 5
        warning('BNT:numFields', 'Not enough inner fields has been found. Can''t calculate grid properties');

        varargout{1} = gridStat;
        varargout{2} = cFieldRadius;
        varargout{3} = nan(6, 2);
        varargout{4} = radSteps(gscoreLoc);
        return;
    end

    allCoords = [stats(:).Centroid];
    centresOfMass(:, 1) = allCoords(1:2:end);
    centresOfMass(:, 2) = allCoords(2:2:end);

    % Calculate orientation for each field relative to the centre field
    orientation = (atan2(centresOfMass(:, 2) - half_height, centresOfMass(:, 1) - half_width)); % atan2(Y, X)
    peaksToCentre = sqDistance(centresOfMass', [half_width half_height]');
    zeroInd = find(orientation == 0, 1);
    orientation(zeroInd) = []; % remove zero value, so that we do not have a side effect with minOrientation
    stats(zeroInd) = [];
    peaksToCentre(zeroInd) = [];
    centresOfMass(zeroInd, :) = [];

    % filter fields that have similar orientation
    orientDistSq = CircStat2012a.circ_dist2(orientation);
    closeFields = abs(orientDistSq) < deg2rad(minOrientation);
    [rows, cols] = size(closeFields);
    closeFields(1:(rows+1):rows*cols) = 0; % assign zero to diagonal elements
    closeFields(tril(true(rows))) = 0; % assign zero to lower triangular of a matrix. Matrix is
                                       % symmetric and we do not need these values.
    [rows, cols] = find(closeFields); % find non-empty elements, they correspond to indices of close fields
    if ~isempty(rows)
        indToDelete = zeros(1, length(rows));
        for i = 1:length(rows)
            % fieldPeaks = [fields([rows(i) cols(i)]).peakX; fields([rows(i) cols(i)]).peakY];
            % peaksToCentre = sqDistance(fieldPeaks, [half_width; half_height]);
            if peaksToCentre(rows(i)) > peaksToCentre(cols(i))
                indToDelete(i) = rows(i);
            else
                indToDelete(i) = cols(i);
            end
        end
        indToDelete = unique(indToDelete);
        stats(indToDelete) = [];
        peaksToCentre(indToDelete) = [];

        if length(stats) < 4
            warning('BNT:numFields', 'Not enough inner fields has been found. Can''t calculate grid properties');

            varargout{1} = gridStat;
            varargout{2} = cFieldRadius;
            varargout{3} = nan(6, 2);
            varargout{4} = radSteps(gscoreLoc);
            return;
        end

        allCoords = [stats(:).Centroid];
        clear centresOfMass;
        centresOfMass(:, 1) = allCoords(1:2:end);
        centresOfMass(:, 2) = allCoords(2:2:end);
    end

    % % get fields peak coordinates
    % fieldPeaks = zeros(length(stats), 2);
    % for i = 1:length(stats)
    %     [~, maxInd] = max(bestCorr(stats(i).PixelIdxList));
    %     fieldPeaks(i, :) = stats(i).PixelList(maxInd, :);
    % end
    % % fieldPeaks = [fields(:).peakX; fields(:).peakY]'; %
%     peaksToCentre = sqDistance(centresOfMass', [half_width half_height]');
    [~, sortInd] = sort(peaksToCentre);
    stats = stats(sortInd);
    centresOfMass = centresOfMass(sortInd, :);

    % leave only 6 closest neighbours (if available)
    if length(stats) > 5
%         stats = stats(1:6);
        centresOfMass = centresOfMass(1:6, :);
    else
%         stats = stats(1:end);
        centresOfMass = centresOfMass(1:end, :);
    end

    % centresOfMass = [fields(:).x; fields(:).y]';
    % Calculate orientation for each field relative to the centre field
    orientation = rad2deg(atan2(centresOfMass(:, 2) - half_height, centresOfMass(:, 1) - half_width)); % atan2(Y, X)

    % Calculate distances between centre of masses for each field and the centre field
    spacing = sqrt((centresOfMass(:, 1) - half_width).^2 + (centresOfMass(:, 2) - half_height).^2);

%     % Plot grid polygon points
%     figure, plot.colorMap(bestCorr), hold on;
%     plot(centresOfMass(:, 1), centresOfMass(:, 2), '+k', 'markersize', 8);

    ell = general.fitEllipse(centresOfMass(:, 1), centresOfMass(:, 2));
    ellipseTheta = rad2deg(general.wrap(ell(end)) + pi);
%     drawEllipse(ell, 'linewidth', 2, 'color', [1 1 1]);

    % Determine axes orientation, spacing and deviation
    [~, bBC] = sort(abs(orientation));
    [~, bBC2] = sort(abs(orientation - orientation(bBC(1))));

    % leave only three values, because autocorrelogram is symmetric
    orientation = orientation(bBC2(1:3));
    spacing = spacing(bBC2(1:3));
    [orientation, orientSortInd] = sort(orientation);

    spacing = spacing(orientSortInd);

    gridStat.orientation = orientation;
    gridStat.spacing = spacing;
    gridStat.ellipse = ell;
    gridStat.ellipseTheta = ellipseTheta;

    varargout{1} = gridStat;
    varargout{2} = cFieldRadius;
    varargout{3} = centresOfMass;
    varargout{4} = radSteps(gscoreLoc);
end

function D = sqDistance(X, Y)
    D = bsxfun(@plus,dot(X,X,1)',dot(Y,Y,1))-2*(X'*Y);
end

function cFieldRadius = findCentreRadius(aCorr, half_width, half_height)
    cFieldRadius = 0;

    % Search for fields only around the centre
    peakCoords = [half_width, half_height];
    [~, fields] = SpatialTuning_BNT.placefieldAdaptive(aCorr, 'minPeak', 0, 'minBins', 2, ...
        'peakCoords', peakCoords);
    if isempty(fields)
        return;
    end

    peakLoc = [fields(:).peakX; fields(:).peakY]';

    % get all distances and check two minimums of them
    allDistances = sqDistance(peakLoc', [half_width half_height]'); % point should be in format [x y]
    [~, sortIndices] = sort(allDistances);
    if length(sortIndices) >= 2
        % this is a bit leagacy code. By using peakCoords with placefieldAdaptive, we
        % should always get just a single field. Keeping it as I did not test the version without it properly.
        twoMinIndices = sortIndices(1:2);

        if abs(allDistances(twoMinIndices(1)) - allDistances(twoMinIndices(2))) < 2
            % two fields with close middle points. Let's select one with minimum square
            [~, minInd] = min([fields(twoMinIndices).area]);
            closestFieldInd = twoMinIndices(minInd);
        else
            closestFieldInd = twoMinIndices(1); % get the first minimum
        end
    else
        closestFieldInd = sortIndices(1);
    end
    cFieldRadius = floor(sqrt(fields(closestFieldInd).area / pi));
end


function [h,hg,htick]=terplot(number)
%FUNCTION [h,hg,htick]=TERPLOT perpares a ternary axis system that is
% needed for the ternaryc function. It returns three handels:
% - h:      to modify the patch created by the fill function;
% - hg:     to change each grid line separately (must probably be
% modified);
% - htick:  to edit the tick labels (probably very inconvinient)
%
% Uli Theune, Geophysics, University of Alberta
%
if nargin<1
    number=11;
end
h=fill([0 1 0.5 0],[0 0 0.866 0],'w','linewidth',2);
%set(h,'facecolor',[0.7 0.7 0.7],'edgecolor','w')
%set(gcf,'color',[0 0 0.3])
d1=cos(pi/3);
d2=sin(pi/3);
l=linspace(0,1,number);
hold on
for i=2:length(l)-1
   hg(i-1,3)=plot([l(i)*d1 1-l(i)*d1],[l(i)*d2 l(i)*d2],':k','linewidth',0.25);
   hg(i-1,1)=plot([l(i) l(i)+(1-l(i))*d1],[0 (1-l(i))*d2],':k','linewidth',0.25);
   hg(i-1,2)=plot([(1-l(i))*d1 1-l(i)],[(1-l(i))*d2 0],':k','linewidth',0.25);
end
hold off
axis image
axis off
% Make x-tick labels
for i=1:number
    htick(i,1)=text(l(i),-0.025,num2str(l(i)));
    htick(i,3)=text(1-l(i)*cos(pi/3)+0.025,l(i)*sin(pi/3)+0.025,num2str(l(i)));
    htick(i,2)=text(0.5-l(i)*cos(pi/3)-0.06,sin(pi/3)*(1-l(i)),num2str(l(i)));
end

set(gcf,'WindowButtonDownFcn','InitTerExpl');
global hx;


function [Ncoef, L]=frameclength(F,Ls)
%FRAMECLENGTH  Number of coefficients from length of signal
%   Usage: Ncoef=frameclength(F,Ls);
%          [Ncoef,L]=frameclength(...);
%
%   `Ncoef=frameclength(F,Ls)` returns the total number of coefficients
%   obtained by applying the analysis operator of frame *F* to a signal
%   of length *Ls* i.e. `size(frana(F,f),1)` for `Ls=length(f)`.
%
%   `[Ncoef,L]=frameclength(F,Ls)` additionally returns *L*, which is the
%   same as returned by |framelength|.
%
%   If the frame length *L* is longer than the signal length *Ls*, the
%   signal will be zero-padded to *L* by |frana|.
%
%   See also: frame, framelengthcoef

callfun = upper(mfilename);
complainif_notposint(Ls,'Ls',callfun);
complainif_notvalidframeobj(F,callfun);

L = F.length(Ls);

% Some frames need special function
if isfield(F,'clength')
    Ncoef = F.clength(L);
else
    % Generic, works for any non-realonly frame and for
    % all representaions not having any extra coefficients

    Ncoef = L*F.red;

    if F.realinput
        Ncoef=Ncoef/2;
    end

    assert(abs(Ncoef-round(Ncoef))<1e-3,...
           sprintf('%s: There is a bug. L=%d should be an integer.',...
           upper(mfilename),Ncoef));

    Ncoef=round(Ncoef);
end


function y = filtdn(x, f, dim, extmod, shift)
% FILTDN   Filter and downsample (by 2) along a dimension
%
%       y = filtdn(x, f, dim, extmod, shift)
%
% Input:
%   x:      input signal
%   f:      1-D filter
%   dim:    the processing dimension
%   extmod: extension mode (e.g. 'per' or 'sym')
%   shift:  specifies the window over which filtering occurs
%
% Output:
%   y:      filtered and dowsampled signal
%
% Note:
%   The origin of the filter f is assumed to be floor(size(f)/2) + 1.
%   Amount of shift should be no more than floor((size(f)-1)/2).

% Skip singleton dimension
if size(x, dim) == 1
    y = x;
    return
end

% Cell array of indexes for each dimension
nd = ndims(x);
I = cell(1, nd);
for d = 1:nd
    I{d} = 1:size(x,d);
end

% Border extend
n = size(x, dim);
hlf = (length(f) - 1) / 2;
% Amount of extension at two ends
e1 = floor(hlf) + shift;
e2 = ceil(hlf) - shift;

switch extmod
    case 'per'
        I{dim} = [ly-e1+1:n , 1:n , 1:e2];

    case 'sym'
        I{dim} = [e1+1:-1:2 , 1:n , n-1:-1:e2];

    otherwise
        error('Invalid input for EXTMOD')

end
y = x(I{:});

% Filter, downsample, and return only the 'valid' part
y = filter(f, 1, y, [], dim);

I{dim} = (1:2:n) + length(f) - 1;
y = y(I{:});


function [beta,Mu_c,Sigma_y_tmp] = GMC(Priors, Mu, Sigma, x, in, out)
%GMC Gaussian Mixture Conditional. Returns the conditional P(out|in=x) of
% the Gaussian Mixture Model P(out,in) at the point in=x.
%
%
%
% Inputs -----------------------------------------------------------------
%   o Priors:  1 x K array representing the prior probabilities of the K GMM
%              components.
%   o Mu:      D x K array representing the centers of the K GMM components.
%   o Sigma:   D x D x K array representing the covariance matrices of the
%              K GMM components.
%   o x:       P x N array representing N datapoints of P dimensions.
%   o in:      1 x P array representing the dimensions to consider as
%              inputs.
%   o out:     1 x Q array representing the dimensions to consider as
%              outputs (D=P+Q)
%
% Output------------------------------------------------------------------------
%
%   o beta      :   N x K
%
%   o Mu_c      :   Q x N x K
%
%   o Sigma_c   :   Q x Q x K
%
%
%

nbData = size(x,2);
nbVar = size(Mu,1);
nbStates = size(Sigma,3);

%% Compute the influence of each GMM component, given input x
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:nbStates
%     if i == 1
%         Mu(in,i)
%         Sigma(in,in,i)
%         inv(Sigma(in,in,i))
%         det(Sigma(in,in,i))
%         gaussPDF(x, Mu(in,i), Sigma(in,in,i))
%     end
    Pxi(:,i) = Priors(i).*gaussPDF(x, Mu(in,i), Sigma(in,in,i));
end
beta = Pxi./repmat(sum(Pxi,2)+realmin,1,nbStates);

%% Compute expected means y, given input x
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for j=1:nbStates
    %   (N x D x K)
    %                   (D x N)                        (D x D)                               (D x N)
    Mu_c(:,:,j) = (repmat(Mu(out,j),1,nbData) + Sigma(out,in,j)*inv(Sigma(in,in,j)) * (x-repmat(Mu(in,j),1,nbData)))';
   %  Mu_c(:,:,j) = (repmat(Mu(out,j),1,nbData))';

end

%% Compute Marginal covariance matrices Sigma_y
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for j=1:nbStates
    Sigma_y_tmp(:,:,j) = Sigma(out,out,j) - (Sigma(out,in,j)*inv(Sigma(in,in,j))*Sigma(in,out,j));
    Sigma_y_tmp(:,:,j) = 0.5 * (Sigma_y_tmp(:,:,j) + Sigma_y_tmp(:,:,j)');
end

end


function [mi3] = km32mi3(km3)
% Convert volume from cubic kilometers to cubic miles.
% Chad Greene 2012
mi3 = km3*0.23991275858;


function [avgArray] = calculateAvgArray(tau, sPeriod, readings)
%This function uses raw readings and returns an array of readings for the
%input Tau argument. readings can either be freq or time

avgCount = tau / sPeriod; %gets count for averaging
loopCount = numel(readings) / avgCount; %get number of loop iterations needed
loopCount = floor(loopCount); %convert loopCount to integer in case it is a non int

avgArray = zeros(1,loopCount); %allocate array
iter = 0;
iter = int32(iter);

%loop to build array
for i = 1:loopCount
    temp = mean(readings((iter+1):(iter+avgCount)));
    avgArray(i) = temp;
    iter = iter + avgCount;
end


function [pdf,X1,X2]=akde(X,grid,gam)
%% adaptive kernel density estimation in high dimensions;
%  optimal accuracy/speed tradeoff, controlled via parameter "gam";
% INPUTS:   X  - data as a 'n' by 'd' vector;
%
%         grid - 'm' points of dimension 'd' over which pdf is computed;
%                default provided only for 2-dimensional data;
%                see example on how to construct it in higher dimensions
%
%          gam - cost/accuracy tradeoff parameter, where gam<n;
%                default value is gam=ceil(n^(1/2)); larger values
%                may result in better accuracy, but always reduce speed;
%                to speedup the code, reduce the value of "gam";
%
% OUTPUT: pdf   - the value of the estimated density at 'grid'
%         X1,X2 - grid only for 2 dimensional data
%
%%  EXAMPLE in 2 dimensions:
%   L=chol([1,-0.999;-0.999,1],'lower');L1=chol([1,0.999;0.999,1],'lower');
%   data=[(L1*randn(10^3,2)')';(L*randn(10^3,2)')'*2;rand(10^4,2)*5-2.5];
%   [pdf,X1,X2]=akde(data);pdf=reshape(pdf,size(X1));contour(X1,X2,pdf,20)
%
%%  EXAMPLE in 3 dimensions:
%  data=[randn(10^3,3);randn(10^3,3)/2+2]; % three dimensional data
%  [n,d]=size(data); ng=100; % total grid points = ng^d
%  MAX=max(data,[],1); MIN=min(data,[],1); scaling=MAX-MIN;
%  % create meshgrid in 3-dimensions
%  [X1,X2,X3]=meshgrid(MIN(1):scaling(1)/(ng-1):MAX(1),...
%      MIN(2):scaling(2)/(ng-1):MAX(2),MIN(3):scaling(3)/(ng-1):MAX(3));
%  grid=reshape([X1(:),X2(:),X3(:)],ng^d,d); % create points for plotting
%  pdf=akde(data,grid); % run adaptive kde
%  pdf=reshape(pdf,size(X1)); % reshape pdf for use with meshgrid
%  for iso=[0.005:0.005:0.015] % isosurfaces with pdf = 0.005,0.01,0.015
%      isosurface(X1,X2,X3,pdf,iso),view(3),alpha(.3),box on,hold on
%      colormap cool
%  end
%
%%  Reference:
%  Kernel density estimation via diffusion
%  Z. I. Botev, J. F. Grotowski, and D. P. Kroese (2010)
%  Annals of Statistics, Volume 38, Number 5, pages 2916-2957.
[n,d]=size(X);
% begin scaling preprocessing
MAX=max(X,[],1);MIN=min(X,[],1);scaling=MAX-MIN;
MAX=MAX+scaling/10;MIN=MIN-scaling/10;scaling=MAX-MIN;
X=bsxfun(@minus,X,MIN);X=bsxfun(@rdivide,X,scaling);
if (nargin<2)|isempty(grid) % failing to provide grid
    warning('Assuming data is 2 dimensional. For higher dimensions, provide a grid as in example.')
    % create meshgrid in 2-dimensions
    [X1,X2]=meshgrid(MIN(1):scaling(1)/(2^7-1):MAX(1),...
           MIN(2):scaling(2)/(2^7-1):MAX(2));
    grid=reshape([X1(:),X2(:)],2^14,d); % create grid for plotting
end
mesh=bsxfun(@minus,grid,MIN);mesh=bsxfun(@rdivide,mesh,scaling);
if nargin<3 % failing to provide speed/accuracy tradeoff
    gam=ceil(n^(1/2));
end
% end preprocessing
% algorithm initialization
del=.1/n^(d/(d+4));perm=randperm(n);mu=X(perm(1:gam),:);w=rand(1,gam);
w=w/sum(w);Sig=bsxfun(@times,rand(d,d,gam),eye(d)*del);ent=-Inf;
for iter=1:1500 % begin algorithm
    Eold=ent;
    [w,mu,Sig,del,ent]=regEM(w,mu,Sig,del,X); % update parameters
    err=abs((ent-Eold)/ent); % stopping condition
    fprintf('Iter.    Tol.      Bandwidth \n');
    fprintf('%4i    %8.2e   %8.2e\n',iter,err,del);
    fprintf('----------------------------\n');
    if (err<10^-4)|(iter>200), break, end
end
% now output density values at grid
pdf = probfun(mesh,w,mu,Sig)/prod(scaling); % evaluate density
del=del*scaling; % adjust bandwidth for scaling
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function pdf=probfun(x,w,mu,Sig)
[gam,d]=size(mu);
pdf=0;
for k=1:gam
    L=chol(Sig(:,:,k));s=diag(L);
    logpdf=-.5*sum(( bsxfun(@minus,x,mu(k,:))/L).^2,2)+log(w(k))...
        -sum(log(s))-d*log(2*pi)/2;
    pdf=pdf+exp(logpdf);
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [w,mu,Sig,del,ent]=regEM(w,mu,Sig,del,X)
[gam,d]=size(mu);[n,d]=size(X);
log_lh=zeros(n,gam); log_sig=log_lh;
for i=1:gam
    L=chol(Sig(:,:,i));
    Xcentered = bsxfun(@minus, X, mu(i,:));
    xRinv = Xcentered /L; xSig = sum((xRinv /L').^2,2)+eps;
    log_lh(:,i)=-.5*sum(xRinv.^2, 2)-sum(log(diag(L)))...
        +log(w(i))-d*log(2*pi)/2-.5*del^2*trace((eye(d)/L)/L');
    log_sig(:,i)=log_lh(:,i)+log(xSig);
end
maxll = max (log_lh,[],2); maxlsig = max (log_sig,[],2);
p= exp(bsxfun(@minus, log_lh, maxll));
psig=exp(bsxfun(@minus, log_sig, maxlsig));
density = sum(p,2);  psigd=sum(psig,2);
logpdf=log(density)+maxll; logpsigd=log(psigd)+maxlsig;
p = bsxfun(@rdivide, p, density);
ent=sum(logpdf); w=sum(p,1);
for i=find(w>0)
    mu(i,:)=p(:,i)'*X/w(i);  %compute mu's
    Xcentered = bsxfun(@minus, X,mu(i,:));
    Xcentered = bsxfun(@times,sqrt(p(:,i)),Xcentered);
    Sig(:,:,i)=Xcentered'*Xcentered/w(i)+del^2*eye(d); % compute sigmas;
end
w=w/sum(w);curv=mean(exp(logpsigd-logpdf)); % estimate curvature
del=1/(4*n*(4*pi)^(d/2)*curv)^(1/(d+2));
end


global_fp = 0;
global_fn = 0;
global_tn = 0;
global_tp = 0;
sample = 90;

for i = 1:sample
    close all;
    load(['Detection/img' num2str(i) '/img' num2str(i) '_detection.mat']);
    img = rgb2gray(im2single(imread(['Detection/img' num2str(i) '/img' num2str(i) '.bmp'])));

    % -------------------------------------------------------------------------
    % Process the ground true data
    % -------------------------------------------------------------------------
    temp = zeros(500);
    for j=1:size(detection, 1)
        temp(max(floor(detection(j, 2)), 1), max(floor(detection(j, 1)), 1)) = 1;
    end
    neg = ~imdilate(temp, strel('disk', 5, 0)) ; % draw a circle around the pixel
    pos = ~neg;

%     Plot the ground true
    figure('Name',['Image: ' num2str(i)]) ; clf ;
    subplot(1,3,1) ; imagesc(img) ; axis equal ; title('image') ;
    hold on;
    plot(detection(:, 1), detection(:, 2), 's', 'MarkerSize',10, 'Color', 'g');
    subplot(1,3,2) ; imagesc(pos) ; axis equal ; title('positive points (blob centres)') ;
    subplot(1,3,3) ; imagesc(neg) ; axis equal ; title('negative points (not a blob)') ;
    colormap gray ;

    % -------------------------------------------------------------------------
    % Detection code
    % -------------------------------------------------------------------------
    min_blob_size = 40;

    img = vl_imsmooth(img,2); % Blob pixels would melt together
    mask = img < 0.6; % Leave only the darker blob

    % Label blob
    mask=imfill(mask,'holes');
    map=bwlabel(mask);
    labels = setdiff(unique(map),0)';

    result = zeros(size(img,1),size(img,2));

    % Filter blob size
    for j=labels

        blob_pixel = (map==j);
        blob_area = sum(sum(blob_pixel));

        if blob_area > min_blob_size
            result = result | blob_pixel;
        end

    end

    % -------------------------------------------------------------------------
    % Measurement
    % -------------------------------------------------------------------------
    y = zeros(size(pos),'single') ;
    y(pos) = +1 ;
    y(neg) = -1 ;

    fp = result > 0 & y < 0 ;
    fn = result < 1 & y > 0 ;
    tn = result <= 0 & y < 0 ;
    tp = result >= 1 & y > 0 ;

    global_fp = global_fp + sum(sum(fp>0));
    global_fn = global_fn + sum(sum(fn>0));
    global_tn = global_tn + sum(sum(tn>0));
    global_tp = global_tp + sum(sum(tp>0));

end

recall = global_tp/(global_tp+global_fn);
precision = global_tp/(global_tp+global_fp);

f1_score = 2*((precision * recall)/(precision + recall));


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% ldlup.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function [L,d,p]=ldlup(L,d,j,g)
% updates LDL^T factorization when a unit j-th row and column
% are replaced by column g
% if the new matrix is definite (signalled by p=[]);
% otherwise, the original L,d and
% a direction p of null or negative curvature are returned
%
% d contains diag(D) and is assumed positive
% Note that g must have zeros in other unit rows!!!
%
function [L,d,p]=ldlup(L,d,j,g);

p=[];

test=0;
if test,
  disp('enter ldlup')
  A=L*diag(d)*L';A(:,j)=g;A(j,:)=g';
end;

n=size(d,1);
I=1:j-1;K=j+1:n;
if j==1,
  v=zeros(0,1);
  del=g(j);
  if del<=n*eps,
    p=[1;zeros(n-1,1)];
    if test,
      A,p
      Nenner=abs(p)'*abs(A)*abs(p);
      if Nenner==0, indef1=0 ,else indef1=(p'*A*p)/Nenner, end;
      disp('leave ldlup at 1')
    end;
    return;
  end;
  w=g(K)/del;
  L(j,I)=v';
  d(j)=del;
  if test,
    A1=L*diag(d)*L',A
    quot=norm(A1-A,1)/norm(A,1),
    disp('leave ldlup at 3')
  end;
  return;
end;

% now j>1, K nonempty
LII=L(I,I);
u=LII\g(I);
v=u./d(I);
del=g(j)-u'*v;
if del<=n*eps,
  p=[LII'\v;-1;zeros(n-j,1)];
  if test,
    A,p
    indef1=(p'*A*p)/(abs(p)'*abs(A)*abs(p))
    disp('leave ldlup at 2')
  end;
  return;
end;
LKI=L(K,I);
w=(g(K)-LKI*u)/del;
[LKK,d(K),q]=ldlrk1(L(K,K),d(K),-del,w);
if isempty(q),
  % work around expensive sparse L(K,K)=LKK
  L=[L(I,:);
     v', 1,L(j,K);
     LKI,w,LKK];
  d(j)=del;
  if test,
    A1=L*diag(d)*L',A
    quot=norm(A1-A,1)/norm(A,1),
    disp('leave ldlup at 4')
  end;
else
  % work around expensive sparse L(K,K)=LKK
  L=[L(1:j,:);
     LKI,L(K,j),LKK];
  pi=w'*q;
  p=[LII'\(pi*v-LKI'*q);-pi;q];
  if test,
    indef2=(p'*A*p)/(abs(p)'*abs(A)*abs(p)),
    disp('leave ldlup at 5')
  end;
end;


function path_length = iTreePathLength(x, treenode, current_path_length)
%UNTITLED7 Summary of this function goes here
%   Detailed explanation goes here

% if strcmp(class(treenode), 'iTreeLeaf')
%    path_length = current_path_length + adjustment(treenode.Size);
% else
%    if x(treenode.SplitAttribute) < treenode.SplitValue
%        path_length = iTreePathLength(x, treenode.Left, current_path_length+1);
%    else
%        path_length = iTreePathLength(x, treenode.Right, current_path_length+1);
%    end
% end

path_length = 0;
while ~strcmp(class(treenode), 'iTreeLeaf')
    if x(treenode.SplitAttribute) < treenode.SplitValue
        treenode = treenode.Left;
    else
        treenode = treenode.Right;
    end
    path_length = path_length + 1;
end
path_length = path_length + adjustment(treenode.Size);
end

function value=adjustment(n)
%The average path length of an unsuccessful BST search
    if n<=1
        value = 0;
    else
        value = 2 * (log(n-1)+0.5772156649) - 2*(n-1) / n;
    end
end


function results = vl_test_fisher(varargin)
% VL_TEST_FISHER
vl_test_init ;

function s =  setup()
randn('state',0) ;
dimension = 5 ;
numData = 21 ;
numComponents = 3 ;
s.x = randn(dimension,numData) ;
s.mu = randn(dimension,numComponents) ;
s.sigma2 = ones(dimension,numComponents) ;
s.prior = ones(1,numComponents) ;
s.prior = s.prior / sum(s.prior) ;

function test_basic(s)
phi_ = simple_fisher(s.x, s.mu, s.sigma2, s.prior) ;
phi = vl_fisher(s.x, s.mu, s.sigma2, s.prior) ;
vl_assert_almost_equal(phi, phi_, 1e-10) ;

function test_norm(s)
phi_ = simple_fisher(s.x, s.mu, s.sigma2, s.prior) ;
phi_ = phi_ / norm(phi_) ;
phi = vl_fisher(s.x, s.mu, s.sigma2, s.prior, 'normalized') ;
vl_assert_almost_equal(phi, phi_, 1e-10) ;

function test_sqrt(s)
phi_ = simple_fisher(s.x, s.mu, s.sigma2, s.prior) ;
phi_ = sign(phi_) .* sqrt(abs(phi_)) ;
phi = vl_fisher(s.x, s.mu, s.sigma2, s.prior, 'squareroot') ;
vl_assert_almost_equal(phi, phi_, 1e-10) ;

function test_improved(s)
phi_ = simple_fisher(s.x, s.mu, s.sigma2, s.prior) ;
phi_ = sign(phi_) .* sqrt(abs(phi_)) ;
phi_ = phi_ / norm(phi_) ;
phi = vl_fisher(s.x, s.mu, s.sigma2, s.prior, 'improved') ;
vl_assert_almost_equal(phi, phi_, 1e-10) ;

function enc = simple_fisher(x, mu, sigma2, pri)
sigma = sqrt(sigma2) ;
for i = 1:size(mu,2)
  delta{i} = bsxfun(@times, bsxfun(@minus, x, mu(:,i)), 1./sigma(:,i)) ;
  q(i,:) = log(pri(i)) - 0.5 * log(sigma2(i)) - 0.5 * sum(delta{i}.^2,1) ;
end
q = exp(bsxfun(@minus, q, max(q,[],1))) ;
q = bsxfun(@times, q, 1 ./ sum(q,1)) ;
n = size(x,2) ;
for i = 1:size(mu,2)
  u{i} = delta{i} * q(i,:)' / n / sqrt(pri(i)) ;
  v{i} = (delta{i}.^2 - 1) * q(i,:)' / n / sqrt(2*pri(i)) ;
end
enc = cat(1, u{:}, v{:}) ;


% -------------------------------------------------------------------------
%   Description:
%       Demo script to calculate the Kendall coefficient of agreement
%       This script reproduces the results of Figure 4 in our paper.
%
%   Citation:
%       A Comparative Study for Single Image Blind Deblurring
%       Wei-Sheng Lai, Jia-Bin Huang, Zhe Hu, Narendra Ahuja, and Ming-Hsuan Yang
%       IEEE Conference on Computer Vision and Pattern Recognition (CVPR), 2016
%
%   Contact:
%       Wei-Sheng Lai
%       wlai24@ucmerced.edu
%       University of California, Merced
% -------------------------------------------------------------------------

%% input dataset and attributes
dataset = 'real';
% dataset = 'uniform';
% dataset = 'nonuniform';

attribute = {};
attribute{end+1} = 'manmade';
attribute{end+1} = 'natural';
attribute{end+1} = 'people';
attribute{end+1} = 'saturated';
attribute{end+1} = 'text';
attribute{end+1} = 'all';

num_method = 14; % total number of evaluated methods

fprintf(' %10s | %10s | %8s\n', 'dataset', 'attribute', 'kendall');
fprintf('------------------------------------\n')

for a = 1:length(attribute)

    %% load votes
    vote_filename = fullfile('votes', sprintf('votes_%s_balance_%s.csv', dataset, attribute{a}));
    M = csvread(vote_filename, 1, 0); % offset the first row to skip header

    %% convert M to winning matrix
    C = construct_winning_matrix(M, num_method);

    %% compute kendall coefficient of agreement
    k = coefficient_of_agreement(C);
    fprintf(' %10s | %10s | %f\n', dataset, attribute{a}, k);

end


% Local Regression and Likelihood, Figure 11.1
%
% Variable degree fit, uses module 'vord'. Note there is
% no `lower' degree here; it defaults to 0.
%
% Author: Catherine Loader

load ethanol;

figure('Name','fig11_1a: Variable degree fit');
fit = locfit(E,NOx,'deg',3,'nn',0.3,'module','vord');
lfplot(fit);

figure('Name','fig11_1b: Variable degree fit');
x = fit.fit_points.evaluation_points';
z = predict(fit,'fitp','what','deg');
plot(x,z,'o');
xlabel('Fitting Point');
ylabel('Degree');


% [INPUT]
% eq = A vector of floats [0,Inf) of length k representing the market value of equity.
% db = A float or a vector of floats [0,Inf) of length k representing the default barrier.
% r = A float or a vector of floats (-Inf,Inf) of length k representing the annualized risk-free interest rate.
% t = A float or a vector of floats (0,Inf) of length k representing the time to maturity of default barrier.
% op = A string representing the option pricing model used by the Systemic CCA framework (optional, default='BSM'):
%   - 'BSM' for Black-Scholes-Merton;
%   - 'GC' for Gram-Charlier.
%
% [OUTPUT]
% va = A column vector of floats of length k representing the value of assets.
% vap = Output argument representing the distributional parameters of assets whose type depends on the chosen option pricing model:
%   - for Black-Scholes-Merton, a float [0,Inf) representing the annualized volatility of assets;
%   - for Gram-Charlier, a row vector of floats (-Inf,Inf) of length 3 whose values represent respectively the annualized volatility, skewness and excess kurtosis of assets.

function [va,vap] = kmv_structural(varargin)

    persistent ip;

    if (isempty(ip))
        ip = inputParser();
        ip.addRequired('eq',@(x)validateattributes(x,{'double'},{'real' 'finite' 'nonnegative' 'vector' 'nonempty'}));
        ip.addRequired('db',@(x)validateattributes(x,{'double'},{'real' 'finite' 'nonnegative' 'vector' 'nonempty'}));
        ip.addRequired('r',@(x)validateattributes(x,{'double'},{'real' 'finite' 'vector' 'nonempty'}));
        ip.addRequired('t',@(x)validateattributes(x,{'double'},{'real' 'finite' '>' 0 'vector' 'nonempty'}));
        ip.addRequired('op',@(x)any(validatestring(x,{'BSM' 'GC'})));
    end

    ip.parse(varargin{:});

    ipr = ip.Results;
    [eq,db,r,t] = validate_input(ipr.eq,ipr.db,ipr.r,ipr.t);
    op = ipr.op;

    nargoutchk(1,2);

    [va,vap] = kmv_structural_internal(eq,db,r,t,op);

end

function [va,vap] = kmv_structural_internal(eq,db,r,t,op)

    df = exp(-r .* t);

    k = numel(r);
    sk = sqrt(k);

    va = eq + (db .* df);
    va_r = diff(log(va));
    va_s = sqrt(252) * std(va_r);

    sst = va_s .* sqrt(t);
    d1 = (log(va ./ db) + ((r + (0.5 * va_s^2)) .* t)) ./ sst;
    d2 = d1 - sst;
    n1 = normcdf(d1);
    n2 = normcdf(d2);

    va_old = va;
    va = eq + ((va .* (1 - n1)) + (db .* df .* n2));

    count = 0;
    error = norm(va - va_old) / sk;

    while ((count < 10000) && (error > 1e-8))
        sst = va_s .* sqrt(t);
        d1 = (log(va ./ db) + ((r + (0.5 * va_s^2)) .* t)) ./ sst;
        d2 = d1 - sst;
        n1 = normcdf(d1);
        n2 = normcdf(d2);

        va_old = va;
        va = eq + ((va .* (1 - n1)) + (db .* df .* n2));
        va_r = diff(log(va));
        va_s = sqrt(252) * std(va_r);

        count = count + 1;
        error = norm(va - va_old) / sk;
    end

    if (strcmp(op,'BSM'))
        vap = va_s;
    else
        va_g = skewness(va_r,0) / sqrt(252);
        va_k = (kurtosis(va_r,0) - 3) / 252;

        vap = [va_s va_g va_k];
    end

end

function [eq,db,r,t] = validate_input(eq,db,r,t)

    eq = eq(:);
    eq_len = numel(eq);

    if (eq_len < 5)
        error('The value of ''eq'' is invalid. Expected input to be a vector containing at least 5 elements.');
    end

    data = {db(:) r(:) t(:)};

    l = unique(cellfun(@numel,data));
    l_scalar = (l == 1);

    if (any(l_scalar))
        if (any(l(~l_scalar) ~= eq_len))
            error(['The number of elements of ''db'', ''r'' and ''t'' must be either 1 or equal to ' num2str(eq_len) '.']);
        end
    else
        if (any(l ~= eq_len))
            error(['The number of elements of ''db'', ''r'' and ''t'' must be either 1 or equal to ' num2str(eq_len) '.']);
        end
    end

    for i = 1:numel(data)
        data_i = data{i};

        if (numel(data_i) == 1)
            data{i} = repmat(data_i,eq_len,1);
        end
    end

    [db,r,t] = deal(data{:});

end


function q = affparaminv(p,q)
% function q = affparaminv(p[, q])
%
%    p(6,n) : [dx dy sc th sr phi]'
%    q(6,n) : [q(1) q(3) q(4); q(2) q(5) q(6)]

% Copyright (C) Jongwoo Lim and David Ross.  All rights reserved.

if (length(p) == 6)
  p = p(:);
end
if (nargin > 1)
  q = inv([p(3) p(4); p(5) p(6)]) * [q(1)-p(1) q(3:4); q(2)-p(2) q(5:6)];
else
  q = inv([p(3) p(4); p(5) p(6)]) * [-p(1) 1 0; -p(2) 0 1];
end
q = q([1,2,3,5,4,6]);


function A = warmUpExercise()
%WARMUPEXERCISE Example function in octave
%   A = WARMUPEXERCISE() is an example function that returns the 5x5 identity matrix

A = [];
% ============= YOUR CODE HERE ==============
% Instructions: Return the 5x5 identity matrix
%               In octave, we return values by defining which variables
%               represent the return values (at the top of the file)
%               and then set them accordingly.

% ===========================================
A = eye(5);

end


function [ave, nvals] = calc_average(structure,field)
%CALC_AVERAGE Calculate the average of values in a field.
% Function CALC_AVERAGE calculates the average value
% of the elements in a particular field of a structure
% array.  It returns the average value and (optionally)
% the number of items averaged.

% Define variables:
%   arr       -- Array of values to average
%   ave       -- Average of arr
%   ii        -- Index variable
%
%  Record of revisions:
%      Date       Programmer          Description of change
%      ====       ==========          =====================
%    03/04/07    S. J. Chapman        Original code
%
% Check for a legal number of input arguments.
msg = nargchk(2,2,nargin);
error(msg);

% Create an array of values from the field
arr = [];
for ii = 1:length(structure)
   arr = [arr structure(ii).(field)];
end

% Calculate average
ave = mean(arr);

% Return number of values averaged
if nargout == 2
   nvals = length(arr);
end


function coeffs3D = vals2coeffs(F)
%VALS2COEFFS   Convert tensor of values to tensor of coefficients.
%
% See also CHEBFUN3/COEFFS2VALS.

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

[m, n, p] = size(F);

%% Step 1:
% Mode-1 unfolding of F to get a matrix of size m x n*p:
F1 = chebfun3.unfold(F, 1);

% Apply 1D "vals2coeffs" in the X direction i.e., the 1st dimension of F1:
F1 = chebtech2.vals2coeffs(F1);

% Tensorize F1 back to its original m x n x p size:
F1 = chebfun3.fold(F1, [m, n, p], 1, [2 3]);
% This is simply F1 = reshape(V1, m, n, p);

%% Step 2:
% Mode-2 unfolding of (the tensorized) F1 to get a matrix of size m x n*p:
F2 = chebfun3.unfold(F1, 2);

% Apply 1D "vals2coeffs" in the Y direction, i.e. to the 1st direction of
% F2:
F2 = chebtech2.vals2coeffs(F2);

% Tensorize F2 back to its original m x n x p size:
F2 = chebfun3.fold(F2, [m, n, p], 2, [1 3]);

%% Step 3:
% Mode-3 unfolding of (the tensorized) F2 to get a matrix of size p x m*n:
F3 = chebfun3.unfold(F2, 3);

% Now, vals2coeffs is applied in the Z direction.
F3 = chebtech2.vals2coeffs(F3);

% Reshape F3 back to the original m x n x p size:
coeffs3D = chebfun3.fold(F3, [m, n, p], 3, [1 2]);
end


%%
% WARNING: challenge data from FAUST challenge contain outlier vertices.
% This demo filters them out and works with cleaned-up meshes; the
% resulting matches must then be mapped back to the original (corrupted)
% raw domains before submitting to the challenge!

clear all
close all
clc

addpath('./tools/')
addpath('./tools/flann/')
addpath(genpath('../../matlab/manopt/manopt/'))

options = struct;
options.k = 100;
options.icp_iters = 0;     % 0 for nearest neighbors
options.use_svd   = true;  % false for basic least squares
options.refine_iters = 0;  % 0 for no refinement

%% Load raw FAUST scans

% full resolution shapes
M = load_ply('./data/tr_scan_000.ply');
N = load_ply('./data/tr_scan_001.ply');

% NOTE: raw faust data contains outliers
[M, M.is_outlier] = cleanup(M);
[N, N.is_outlier] = cleanup(N);

%% Load sparse matches (these may come from some matching pipeline)

load('./data/sparse_matches.mat')
[i,j,~] = find(P);
sparse_matches = [i j];
n_matches = size(sparse_matches,1);

colors = create_colormap(N,N);
figure, colormap([1 1 1])
subplot(121)
plot_scalar_map(N, ones(N.n,1)); hold on
plot_cloud_color(N.VERT(sparse_matches(:,2),:), colors(sparse_matches(:,2),:), 5)
axis off; view([0 90])
subplot(122)
plot_scalar_map(M, ones(M.n,1)); hold on
plot_cloud_color(M.VERT(sparse_matches(:,1),:), colors(sparse_matches(:,2),:), 5)
axis off; view([0 90])

%% Compute LBO eigenfunctions

[M.W, ~, M.S] = calc_LB_FEM(M);
[M.evecs, M.evals] = eigs(M.W, M.S, options.k, -1e-5);
M.evals = diag(M.evals);
[M.evals, idx] = sort(M.evals);
M.evecs = M.evecs(:,idx);

[N.W, ~, N.S] = calc_LB_FEM(N);
[N.evecs, N.evals] = eigs(N.W, N.S, options.k, -1e-5);
N.evals = diag(N.evals);
[N.evals, idx] = sort(N.evals);
N.evecs = N.evecs(:,idx);

%% Refine and upscale matches

F = sparse(sparse_matches(:,1), 1:n_matches, 1, M.n, n_matches);
G = sparse(sparse_matches(:,2), 1:n_matches, 1, N.n, n_matches);

if options.refine_iters > 0

    A_init = M.evecs'*(M.S*F);
    B_init = N.evecs'*(N.S*G);
    [u,~,v] = svd(A_init*B_init');
    C_init = u*v';
    C_init = C_init';

    % fps among the input sparse matches
    fps = fps_euclidean(M.VERT(sparse_matches(:,1),:), 1e3, 1);

    matches_upscaled = refine_matches(...
        M, N, F(:,fps), G(:,fps), C_init, options);

    % do a final svd step
    G_svd = sparse(matches_upscaled, 1:M.n, 1, N.n, M.n);
    B_svd = M.evecs'*M.S;
    A_svd = N.evecs'*(N.S*G_svd);
    [u,~,v] = svd(A_svd*B_svd');
    [~, matches_upscaled_svd] = run_icp_fixed(N, M, v*u', options.icp_iters);

else

    B = M.evecs'*(M.S*F);
    A = N.evecs'*(N.S*G);

    if ~options.use_svd
        C_upscaled = A'\B';
        C_upscaled = C_upscaled';
    else
        [u,~,v] = svd(A*B');
        C_upscaled = u*v';
        C_upscaled = C_upscaled';
    end

    [~, matches_upscaled] = run_icp_fixed(N, M, C_upscaled, options.icp_iters);

end

figure
colors_hires = create_colormap(N,N);
subplot(141), colormap(colors_hires), plot_scalar_map(N, 1:N.n); axis off, view([0 90]), freeze_colors
subplot(142), colormap(colors_hires(matches_upscaled,:)), plot_scalar_map(M, 1:M.n); axis off, view([0 90]), freeze_colors
subplot(143), colormap(colors_hires), plot_scalar_map(N, 1:N.n); axis off, view([180 -90]), freeze_colors
subplot(144), colormap(colors_hires(matches_upscaled,:)), plot_scalar_map(M, 1:M.n); axis off, view([180 -90]), freeze_colors


function [  net,res,opts ] = rmsprop(  net,res,opts )
% Modified RMSProp using second-order information.
%   1.Tieleman, T. and Hinton, G. Lecture 6.5 - RMSProp, COURSERA: Neural Networks for Machine Learning.
%   Technical report, 2012.
%   2.Ye, C., Yang, Y., Fermuller, C., & Aloimonos, Y. (2017).
%   On the Importance of Consistency in Training Deep Neural Networks. arXiv preprint arXiv:1708.00631.

    if ~isfield(opts.parameters,'second_order')
        opts.parameters.second_order=0;
    end
    if opts.parameters.second_order
        [  net,res,opts ] = gradient_decorrelation(  net,res,opts );
    end

    if ~isfield(opts.parameters,'weightDecay')
        opts.parameters.weightDecay=1e-4;
    end

    if ~isfield(opts.parameters,'clip')
        opts.parameters.clip=1e0;
    end

    if ~isfield(opts.parameters,'eps')
        opts.parameters.eps=1e-6;
    end

    if ~isfield(net,'iterations')||(isfield(opts,'reset_mom')&&opts.reset_mom==1)
        net.iterations=0;
    end

    net.iterations=net.iterations+1;

    mom_factor=(1-opts.parameters.mom.^net.iterations);

    for layer=1:numel(net.layers)
        if isfield(net.layers{layer},'weights')
            if ~isfield(net.layers{layer},'momentum')||(isfield(opts,'reset_mom')&&opts.reset_mom==1)
                net.layers{layer}.momentum{1}=zeros(size(net.layers{layer}.weights{1}),'like',net.layers{layer}.weights{1});
                net.layers{layer}.momentum{2}=zeros(size(net.layers{layer}.weights{2}),'like',net.layers{layer}.weights{2});

            end

            net.layers{layer}.momentum{1}=opts.parameters.mom.*net.layers{layer}.momentum{1}+(1-opts.parameters.mom).*(res(layer).dzdw.^2);
            normalized_grad=res(layer).dzdw./(net.layers{layer}.momentum{1}.^0.5+opts.parameters.eps)./mom_factor;
            if isfield(opts.parameters,'clip')&&opts.parameters.clip>0
                mask=abs(normalized_grad)>opts.parameters.clip;
                normalized_grad(mask)=sign(normalized_grad(mask)).*opts.parameters.clip;
            end
            net.layers{layer}.weights{1}=net.layers{layer}.weights{1}-opts.parameters.lr*normalized_grad- opts.parameters.weightDecay * net.layers{layer}.weights{1};

            net.layers{layer}.momentum{2}=opts.parameters.mom.*net.layers{layer}.momentum{2}+(1-opts.parameters.mom).*(res(layer).dzdb.^2);
            normalized_grad=res(layer).dzdb./(net.layers{layer}.momentum{2}.^0.5+opts.parameters.eps)./mom_factor;
            if isfield(opts.parameters,'clip')&&opts.parameters.clip>0
                mask=abs(normalized_grad)>opts.parameters.clip;
                normalized_grad(mask)=sign(normalized_grad(mask)).*opts.parameters.clip;
            end
            net.layers{layer}.weights{2}=net.layers{layer}.weights{2}-opts.parameters.lr*normalized_grad;
        end
    end

   if ~isfield(opts,'reset_mom')||opts.reset_mom==1
        opts.reset_mom=0;
    end
end


function c = gain_cond_check(model_param, B, psi_1, evmax, c1, c2, kx, kv, kr, kw)

m = model_param.mass;
J = model_param.I;

alpha = sqrt(psi_1*(2-psi_1));
W1 = [ c1*kx/m -c1*kv/(2*m)*(1+alpha);-c1*kv/(2*m)*(1+alpha) kv*(1-alpha)-c1 ];
W12 = [kx*evmax+c1/m*B 0;B 0];
W2 = [c2*kr/max(eig(J)) -c2*kw/(2*min(eig(J)));-c2*kw/(2*min(eig(J))) kw-c2];

c = min(eig(W2))-4*norm(W12)^2/min(eig(W1));


function  [ gx,dgdx,dgdP ] = g_softmax(x,P,u,in )
% softmax decision rule for Q-learning (2-armed bandit task)
% function  [ gx,dgdx,dgdP ] = g_softmax(x,P,u,in )
% IN:
%   - x : Q-values
%   - P : inverse (log-) temperature and bias
%   - u : [useless]
%   - in : [useless]
% OUT:
%   - gx : P(a=1|x)

% inverse temperature
% -------------------------------------------------------------------------
beta = exp(P(1)); % exp: [-Inf,Inf] -> [0 Inf]

dQ = (x(1)-x(2));
if length(P)>1
    gx = VBA_sigmoid( beta*dQ + P(2));
else
    gx = VBA_sigmoid( beta*dQ );
end
dgdx = zeros(size(x,1),1);
dgdx(1) = beta*gx*(1-gx);
dgdx(2) = -beta*gx*(1-gx);
if length(P)>1
    dgdP = [beta*dQ*gx*(1-gx),gx*(1-gx)];
else
    dgdP = [beta*dQ*gx*(1-gx)];
end


%% Check rate of convergence for 3D Hcurl interface problem
%     curl(A curl u) + B u  = f,    x\in \Omega
%      where A and B are piecewise constants on Omega^+ and Omega^-.
%
% Domain: Rectangular domain: [xmin,xmax] X [ymin,ymax] X [zmin,zmax]
% Mesh: Cartesian triangular mesh.
% Method: FE
% for this method, need to mannualy import a mesh data

%% Geometry and Boundary Conditions
%clear
%close all
%clc

%path(pathdef)
addpath(genpath(pwd),'-begin');
rmpath(genpath('./.git'));
rmpath(genpath('./docs'));
savepath;

domain = [-1,1,-1,1,-1,1];
bc = [1,1,1,1,1,1]; % Dirichelet BC

%% Finite Element Type
femtype = '1st kind Nedelec';
disp(['FEM Type =  Conforming ', femtype]);

%% Initial Partition
nx0 = 10;
ny0 = nx0;
nz0 = nx0;

%% Task
showErr = 0;
showMesh = 0;
computErr = 1;

%% PDE
test = 3;
switch test
    case 0
        pde = poissonNonPoly3D;
    case 1 % circular interface
        r = pi/5; bm = 1; bp = 1; am = 1; ap = 1;
        x0 = 0; y0 = 0; z0 = 0; a11 = 1; a12 = 1; a = 1;
        pde = elli3DcircIntf2(am,ap,bm,bp,r,x0,y0,z0,a11,a12,a);
    case 2
        r = pi/5; bm = 1; bp = 1;
        x0 = 0; y0 = 0; z0 = 0; coef = 1;
        pde = constant_fun(bm,bp,r,x0,y0,z0,coef);
    case 3
        r = pi/5; bm = 1; bp = 1; am = 1; ap = 1;
        x0 = 0; y0 = 0; z0 = 0; coef = 1;
        pde = linear_fun(am,ap,bm,bp,r,x0,y0,z0,coef);
end

%% Max Iteration

%% 1. Generate Mesh
disp('*******************************************************************************')
disp('Import mesh');
load('BoxTorusMesh.mat')
mesh = enrichMesh3D(mesh,0); % Mesh detail level = 1 (for IFE).

%% 2. Generate FEM DoF
fem = genNedFEM3D(mesh,bc);
disp(['number of DoF =  ', int2str(length(fem.p))]);

%% 3. Assemble Matrix
disp(' '); disp('Start Assembling Matrix');
S = globMatrixNedFit3D(am,ap,1,mesh,fem,fem);
M = globMatrixNedFit3D(bm,bp,0,mesh,fem,fem);
rhsF1 = globNedFitRHS3D(pde.fm1,pde.fp1, mesh, fem, 0, 1);
rhsF2 = globNedFitRHS3D(pde.fm2,pde.fp2, mesh, fem, 0, 2);
rhsF3 = globNedFitRHS3D(pde.fm3,pde.fp3, mesh, fem, 0, 3);
rhsF = rhsF1 + rhsF2 + rhsF3;
Atotal = S + M;
tu1 = sum(feval(pde.exactu1,fem.gex,fem.gey,fem.gez).*fem.gew,2);
tu2 = sum(feval(pde.exactu2,fem.gex,fem.gey,fem.gez).*fem.gew,2);
tu3 = sum(feval(pde.exactu3,fem.gex,fem.gey,fem.gez).*fem.gew,2);
tgt = mesh.p(mesh.e(:,2),:) - mesh.p(mesh.e(:,1),:);
tgt = tgt./sum(tgt.^2,2).^(1/2);
tu = tu1.*tgt(:,1) + tu2.*tgt(:,2) + tu3.*tgt(:,3);

Ndof = size(mesh.e,1);
bdidx = zeros(Ndof,1);
isBdEdge = true(Ndof,1);
isBdEdge(fem.mapper) = false;
bdidx(isBdEdge) = 1;
Tbd = spdiags(bdidx,0,Ndof,Ndof);
T = spdiags(1-bdidx,0,Ndof,Ndof);
A = T*Atotal*T + Tbd;
ub = tu;
ub(fem.mapper) = 0;
rhsB = Atotal*ub;
f = rhsF - rhsB;
f(isBdEdge) = tu(isBdEdge);

%% 3. Solve the linear system Au = f
option.outsolver = 'cg';
tID1 = (mesh.tLoc == 1);
tID2 = (mesh.tLoc == 2);
e1tmp = unique(reshape(mesh.t_e(tID1,:),[],1));
e2tmp = unique(reshape(mesh.t_e(tID2,:),[],1));
eInt = intersect(e1tmp,e2tmp);
%eIntp1 = mesh.p(mesh.e(eInt,1),:);
%eIntp2 = mesh.p(mesh.e(eInt,2),:);
eid1 = setdiff(e1tmp,eInt);
eid2 = setdiff(e2tmp,eInt);
alpha = am*ones(size(mesh.e,1),1);
alpha(eid2) = ap;
alpha(eInt) = (am+ap)/2;
beta = bm*ones(size(mesh.e,1),1);
beta(eid2) = bp;
beta(eInt) = (bm+bp)/2;
option.alpha = alpha;
option.beta = beta;
option.solver = 'amg';
edge = mesh.e;
option.isBdEdge = isBdEdge;
option.smoother = 'BD';
NEdof = size(A,1);
option.blklevel = 0;
option.blkId = NEdof;
option.fact = 'chol';
[x,info] = amgMaxwellinterface(A,f,mesh.p,edge,option);
uh = x;

%% 4. Postprocess: Calculating Errors
tic
errND = max(abs(uh - tu)); % Error on nodes
err.nd = errND; err.inf = 0; err.l2 = 0; err.h1 = 0;
if computErr == 1
    disp(' ');  disp('Start computing error in L2 norm');
    eNorm = 'L2'; disp(['Start computing error in ',eNorm,'  norm']);
    [errL2,errL2K1,errL2K2,errL2K3] = getCurlErr3D(uh, pde, fem, eNorm);

    eNorm = 'Curl'; disp(['Start computing error in ',eNorm,' norm']);
    [errCurl,errCurl1,errCurl2,errCurl3] = getCurlErr3D(uh, pde, fem, eNorm);
    err.l2 = errL2;
    err.curl = errCurl;
end
time(6) = toc;
time(7) = 1e6*sum(time(2:6))/length(mesh.t);

%% 5: Output

disp(' ')
disp('Errors')
disp('Node   L2 norm     H1 norm')
formatSpec = '%6.4e %6.4e  %6.4e\n';
fprintf(formatSpec, err.nd, err.l2, err.curl)

err0 = err; h0 = h;

disp(' '); disp('CPU Time')
disp('   N     Mesh     FEM      Matrix   Solve    Error    Time/1M cell')
formatSpec = '%4i  %7.2f  %7.2f  %7.2f  %7.2f  %7.2f   %7.2f\n';
fprintf(formatSpec, time)

%% 6. Plot Solution and Error
if showErr == 1
end


classdef testStressRotationInVoigtNotation < handle

    properties (Access = public)
        tol =  1e-14;
    end

    properties (Access = protected)
        direction
        stress
        stressVoigt
        rotatedStressByVoigt
        rotatedStress
    end

    properties (Access = private)
        angle
    end

    methods (Access = public)

        function error = computeError(obj)
            rotStre        = obj.rotatedStress.getValue();
            rotStreByVoigt = obj.rotatedStressByVoigt.getValue();
            error = norm(double(rotStre) - double(rotStreByVoigt));
        end

    end

    methods (Access = protected)

        function compute(obj)
            obj.createAngle()
            obj.createDirection()
            obj.createStress()
            obj.createRotatedStress()
            obj.createRotatedStressWithVoigtNotation()
        end

        function createStress(obj)
            obj.stress = Stress3DTensor;
            obj.stress.createRandomTensor();
            obj.stressVoigt = Tensor2VoigtConverter.convert(obj.stress);
        end

        function createRotatedStress(obj)
            rotS = obj.rotateStress();
            obj.rotatedStress = Tensor2VoigtConverter.convert(rotS);
        end

        function rotS = rotateStress(obj)
            a  = obj.angle;
            d  = obj.direction;
            rotS = Rotator.rotate(obj.stress,a,d);
        end

    end

    methods (Access = private)

        function createAngle(obj)
            obj.angle = 2*pi*rand(1);
        end

        function createRotatedStressWithVoigtNotation(obj)
            theta = obj.angle;
            dir   = obj.direction;
            stre = obj.stressVoigt;
            rotStre = Rotator.rotate(stre,theta,dir);
            obj.rotatedStressByVoigt = rotStre;
        end

    end

    methods (Abstract,Access = protected)
        createDirection(obj)
    end
end


function Ex_GFK()
% This shows how to use GFK in a 1-nearest neighbor classifier.

% ref: Geodesic Flow Kernel for Unsupervised Domain Adaptation.
% B. Gong, Y. Shi, F. Sha, and K. Grauman.
% Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR), Providence, RI, June 2012.

% Contact: Boqing Gong (boqinggo@usc.edu)

%-------------------------I. setup source/target domains----------------------
% Four domains: { Caltech10, amazon, webcam, dslr }
src = 'webcam';
tgt = 'Caltech10';

d = 20; % subspace dimension, the following dims are used in the paper:
% webcam-dslr: 10
% dslr-amazon: 20
% webcam-amazon: 10
% caltech-webcam: 20
% caltech-dslr: 10
% caltech-amazon: 20
% Note the dim from X to Y is the same as that from Y to X.

nPerClass = 20;
% 20 per class when Caltech/Amazon/Webcam is the source domain, and
% 8 when DSLR is the source domain.

%--------------------II. prepare data--------------------------------------
load(['data/' src '_SURF_L10.mat']);     % source domain
fts = fts ./ repmat(sum(fts,2),1,size(fts,2));
Xs = zscore(fts,1);    clear fts
Ys = labels;           clear labels
Ps = princomp(Xs);  % source subspace

load(['data/' tgt '_SURF_L10.mat']);     % target domain
fts = fts ./ repmat(sum(fts,2),1,size(fts,2));
Xt = zscore(fts,1);     clear fts
Yt = labels;            clear labels
% Pt = princomp(Xt);  % target subspace
Pt = pca(Xt);  % target subspace
% Pt = ftProc_pca_tr(Xt);  % target subspace

fprintf('\nsource (%s) --> target (%s):\n', src, tgt);
fprintf('round     accuracy\n');
%--------------------III. run experiments----------------------------------
round = 20; % 20 random trials
tot = 0;
for iter = 1 : round
    fprintf('%4d', iter);

    inds = split(Ys, nPerClass);
    Xr = Xs(inds,:);
    Yr = Ys(inds);

    %---------------III.A. PLS --------------------------------------------
    % Ps = PLS(Xr, OneOfKEncoding(Yr), 3*d);
    % PLS generally leads to better performance.
    % A nice implementation is publicaly available at http://www.utd.edu/~herve/

    G = GFK([Ps,null(Ps')], Pt(:,1:d));
    [~, accy] = my_kernel_knn(G, Xr, Yr, Xt, Yt);
    fprintf('\t\t%2.2f%%\n', accy*100);
    tot = tot + accy;
end
fprintf('mean accuracy: %2.2f%%\n\n', tot/round*100);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [prediction accuracy] = my_kernel_knn(M, Xr, Yr, Xt, Yt)
dist = repmat(diag(Xr*M*Xr'),1,length(Yt)) ...
    + repmat(diag(Xt*M*Xt')',length(Yr),1)...
    - 2*Xr*M*Xt';
[~, minIDX] = min(dist);
prediction = Yr(minIDX);
accuracy = sum( prediction==Yt ) / length(Yt);

function [idx1 idx2] = split(Y,nPerClass, ratio)
% [idx1 idx2] = split(X,Y,nPerClass)
idx1 = [];  idx2 = [];
for C = 1 : max(Y)
    idx = find(Y == C);
    rn = randperm(length(idx));
    if exist('ratio')
        nPerClass = floor(length(idx)*ratio);
    end
    idx1 = [idx1; idx( rn(1:min(nPerClass,length(idx))) ) ];
    idx2 = [idx2; idx( rn(min(nPerClass,length(idx))+1:end) ) ];
end


function [ x, y, z, w ] = ld0350 ( )

%*****************************************************************************80
%
%% LD0350 computes the 350 point Lebedev angular grid.
%
%  Modified:
%
%    14 September 2010
%
%  Author:
%
%    Dmitri Laikov
%
%  Reference:
%
%    Vyacheslav Lebedev, Dmitri Laikov,
%    A quadrature formula for the sphere of the 131st
%    algebraic order of accuracy,
%    Russian Academy of Sciences Doklady Mathematics,
%    Volume 59, Number 3, 1999, pages 477-481.
%
%  Parameters:
%
%    Output, real X(N), Y(N), Z(N), W(N), the coordinates
%    and weights of the points.
%
  n = 0;
  x = zeros(350,1);
  y = zeros(350,1);
  z = zeros(350,1);
  w = zeros(350,1);
  a = 0.0;
  b = 0.0;
  v = 0.3006796749453936E-02;
  [ n, x, y, z, w ] = gen_oh ( 1, n, a, b, v, x, y, z, w );
  v = 0.3050627745650771E-02;
  [ n, x, y, z, w ] = gen_oh ( 3, n, a, b, v, x, y, z, w );
  a = 0.7068965463912316;
  v = 0.1621104600288991E-02;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.4794682625712025;
  v = 0.3005701484901752E-02;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.1927533154878019;
  v = 0.2990992529653774E-02;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.6930357961327123;
  v = 0.2982170644107595E-02;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.3608302115520091;
  v = 0.2721564237310992E-02;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.6498486161496169;
  v = 0.3033513795811141E-02;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.1932945013230339;
  v = 0.3007949555218533E-02;
  [ n, x, y, z, w ] = gen_oh ( 5, n, a, b, v, x, y, z, w );
  a = 0.3800494919899303;
  v = 0.2881964603055307E-02;
  [ n, x, y, z, w ] = gen_oh ( 5, n, a, b, v, x, y, z, w );
  a = 0.2899558825499574;
  b = 0.7934537856582316;
  v = 0.2958357626535696E-02;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.9684121455103957E-01;
  b = 0.8280801506686862;
  v = 0.3036020026407088E-02;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.1833434647041659;
  b = 0.9074658265305127;
  v = 0.2832187403926303E-02;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );

  return
end


function I = shift_reconstruct(Y,shifts,diffphase,us_fac,Nr,Nc,Np,method,add_value)

% applies 3-d sub-pixel shifts to an input image
% INPUTS:
% Y:            input image (double 3d tensor) in space (real) or frequency (complex) domain
% shifts:       shifts
% diffphase:    phase difference
% us_fac:       upsampling factor for subpixel shifts
% method:       method for treating boundaries
% add_value:    value to add when zero-ing out boundaries

% OUTPUT:
% I:        output image

% Written by Eftychios A. Pnevmatikakis, Simons Foundation, 2016

if isreal(Y);
    buf2ft = fftn(Y);
else
    buf2ft = Y;
end

if nargin < 9 || isempty(add_value); add_value = 0; end
if nargin < 8 || isempty(method); method = 'zero'; end
if nargin < 4 || isempty(us_fac); us_fac = 50; end

[nr,nc,np]=size(Y);
if any(shifts)
    if nargin < 5 || isempty(Nr); Nr = ifftshift(-fix(nr/2):ceil(nr/2)-1); end
    if nargin < 6 || isempty(Nc); Nc = ifftshift(-fix(nc/2):ceil(nc/2)-1); end
    if nargin < 7 || isempty(Np); Np = ifftshift(-fix(np/2):ceil(np/2)-1); end

    %shifts = shiftdim(shifts,3);

    row_shift = shifts(1);
    col_shift = shifts(2);
    if ismatrix(Y); pln_shift = 0; else pln_shift = shifts(3); end
    shifts = [row_shift,col_shift,pln_shift];

    if us_fac > 0
        if isvector(Nc); [Nc,Nr,Np] = meshgrid(Nc,Nr,Np); end
        Greg = buf2ft.*exp(1i*2*pi*(-row_shift*Nr/nr-col_shift*Nc/nc-pln_shift*Np/np));
    elseif us_fac == 0
        Greg = buf2ft;
    end
    Greg = Greg*exp(1i*diffphase);
    I = real(ifftn(Greg));
    I = remove_boundaries(I,shifts,method,add_value);
else
    if isreal(Y)
        I = Y;
    else
        I = real(ifftn(Y));
    end
end

end


function p = bst_cross(x,y,dim)
% BST_CROSS: Cross product between two sets, each set with three columns
%
% USAGE:  p = bst_cross(x,y)
%         p = bst_cross(x,y,dim)
%
% INPUT:
%     - x   : [1,3] double or single
%     - y   : [1,3] double or single
%     - dim : dimension along which to compute the cross product
%
% NOTE:
%     - Does exactly then same as the Matlab 'cross' function, but much faster.

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

% Default: dim=1
if (nargin < 3) || isempty(dim)
    dim = 1;
end
% Check size
if (size(x,dim)~=3) || (size(y,dim)~=3)
    error(' Must have three columns ');
end
% Compute cross product
if (dim == 2)
    p = [x(:,2).*y(:,3) - x(:,3).*y(:,2),...
         x(:,3).*y(:,1) - x(:,1).*y(:,3),...
         x(:,1).*y(:,2) - x(:,2).*y(:,1)];
else
    p = [x(2,:).*y(3,:) - x(3,:).*y(2,:);...
         x(3,:).*y(1,:) - x(1,:).*y(3,:);...
         x(1,:).*y(2,:) - x(2,:).*y(1,:)];
end


function [mus,sigmas,R] = restore_from_projection(mus_p,sigmas_p,R_p,c,E)
% restore from projection.
% input:
%   mus_p - 1 by M cell array (each cell is a K_j by B matrix)
%   sigmas_p - 1 by M cell array (each cell is a B by B by K_j matrix)
%   R_p - M by d projected data or M by d by N projected data
%   c - 1 by B center of the PCA
%   E - B by d projection matrix
% output:
%   mus, sigmas, R - restored versions of mus_p, sigmas_p and R_p

if ~isempty(mus_p) && ~isempty(sigmas_p)
    M = length(mus_p);
elseif ~isempty(R_p)
    M = size(R_p,1);
end
mus = cell(1,M);
sigmas = cell(1,M);
B = size(E,1);
R = zeros(M,B);

if ~isempty(mus_p) && ~isempty(sigmas_p)
    for j = 1:M
        K_j = size(mus_p{j},1);
        mus{j} = zeros(K_j,B);
        sigmas{j} = zeros(B,B,K_j);

        for k = 1:size(mus_p{j},1)
    %         mus{j}(k,:) = solve_linsys_mu(E,mus_p{j}(k,:)')' + c;
    %         sigmas{j}(:,:,k) = solve_linsys_sigma(E, sigmas_p{j}(:,:,k));
            mus{j}(k,:) = recover_mu(E,c,mus_p{j}(k,:));
            sigmas{j}(:,:,k) = recover_sigma(E,sigmas_p{j}(:,:,k));
        end
    end
end

if ~isempty(R_p)
    if ndims(R_p) == 2
        R = recover_mu(E,c,R_p);
    elseif ndims(R_p) == 3
        R = zeros(M,B,size(R_p,3));
        for i = 1:size(R_p,3)
            R(:,:,i) = recover_mu(E,c,R_p(:,:,i));
        end
    end
end

function mu = recover_mu(E,c,mu0)
mu = E*mu0' + repmat(c',[1, size(mu0,1)]);
mu = mu';

function sigma = recover_sigma(E, sigma0)
sigma = E*sigma0*E' + 1e-9*eye(size(E,1));

function x = solve_linsys_mu(E,mu0)
EE1 = E*E';
B = size(EE1,1);
EE1 = EE1 + 1e-9*eye(B);
x = EE1 \ (E* mu0);

function sigma = solve_linsys_sigma(E,sigma0)
[B,d] = size(E);
w = 1e-9;
EE = kron(E,E);
tmp1 = EE * sigma0(:);
tmp2 = (eye(d^2) + (1/w)*EE'*EE) \ (EE' * tmp1);
sigma = (1/w) * tmp1 - (1/w^2) * EE * tmp2;
sigma = reshape(sigma,B,B);