function [y,dzdx2] = vl_nnscosinesim(x1, x2, varargin)
% VL_NNCOSINESIM Compute the cosine similariy between features
%   Y = VL_NNCOSINESIM(X1, X2) computes the cosine similarity between X1
%   and X2 where X1 has shape H x W x C x N, X2 has shape
%   H x W x C x N and Y is has shape 1 x 1 x 1 x N (producing a scalar
%   per batch element).
%
%   [DZDX1, DZDX2] = VL_NNCOSINESIM(X1, X2, DZDY) computes the
%   derivatives of the block projected onto DZDY. DZDX1 and DZDX2
%   have the same dimensions as X1, X2 and B respectively.
%
%   NOTES: The cosine similarity is defined between vectors A and B of
%   dimension N x 1 to be the scalar given by:
%   cos_sim = a'b / (norm(a,2) * norm(b,2))
%   To operate with tensors, the first three channels (H, W and C) are
%   treated as the vector elements, while the fourth dimension denotes
%   batch indicies.
%
%   VL_NNCOSINELOSS(.., 'option', value, ...) accepts the following options:
%
%   `eps`:: 1e-6
%    A small value to avoid possible division by zero.
%
% Copyright (C) 2018 Samuel Albanie.
% Licensed under The MIT License [see LICENSE.md for details]

  opts.eps = 1e-6 ;
  [~, dzdy] = vl_argparsepos(struct(), varargin) ;

  sz1 = size(x1) ; sz2 = size(x2) ;
  assert(all(sz1 == sz2), 'tensor sizes do not match') ;
  x1 = reshape(x1, [], sz1(4)) ;
  x2 = reshape(x2, [], sz1(4)) ;
  dots = dot(x1, x2, 1) ;
  x1_norms = sqrt(sum(x1 .* x1, 1)) ;
  x2_norms = sqrt(sum(x2 .* x2, 1)) ;

  if isempty(dzdy)
    y = dots ./ max(x1_norms .* x2_norms, opts.eps) ;
    y = reshape(y, 1, 1, 1, sz1(4)) ;
  else
    dzdy = dzdy{1} ; dsize = size(dzdy) ;
    assert(numel(dsize) == 4 & all(dsize(1:3) == ones(1,3)) ...
           & dsize(4) == sz1(4), 'DZDY has an unexpected size') ;
    t1 = bsxfun(@rdivide, x2, max(x1_norms .* x2_norms, opts.eps)) ;
    t2 = bsxfun(@rdivide, bsxfun(@times, x1, dots), ...
                              max(x1_norms .^3 .* x2_norms, opts.eps)) ;
    dzdx1 = t1 - t2 ;

    t1 = bsxfun(@rdivide, x1, max(x2_norms .* x1_norms, opts.eps)) ;
    t2 = bsxfun(@rdivide, bsxfun(@times, x2, dots), ...
                              max(x2_norms .^3 .* x1_norms, opts.eps)) ;
    dzdx2 = t1 - t2 ;

    % reshape to match input and compute projected derivatives
    dzdx1 = reshape(dzdx1, sz1) ;
    dzdx2 = reshape(dzdx2, sz2) ;
    dzdx1 = bsxfun(@times, dzdx1, dzdy) ;
    dzdx2 = bsxfun(@times, dzdx2, dzdy) ;
    y = dzdx1 ;
  end


function [Dt]=patchVectorTangent(F,V,D,N)

%Derive tangent contribution through dot-product with normal vectors

if isempty(N)
    [~,~,N]=patchNormal(F,V); %Get current vertex normals
end

Dn_mag=dot(D,N,2); %Allong normal displacement magnitudes
Dn=Dn_mag(:,ones(1,3)).*N; %Normal direction displacement vectors
Dt=D-Dn; %Tranverse or tangential only displacement vectors

%%
% _*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 k = lfmvKernDiagCompute(lfmKern, t)

% LFMVKERNDIAGCOMPUTE Compute diagonal of LFMV kernel.
% FORMAT
% DESC computes the diagonal of the kernel matrix for velocity- velocity in
% the switching dynamical latent force kernel.
% ARG kern : the kernel structure for which the matrix is computed.
% ARG t : input data matrix in the form of a design matrix.
% RETURN k : a vector containing the diagonal of the kernel matrix
% computed at the given points.
%
% COPYRIGHT : Mauricio A. Alvarez, 2010

% KERN

% Get length scale out.
sigma2 = 2/lfmKern.inverseWidth;
sigma = sqrt(sigma2);

% Parameters of the kernel
alpha(1) = lfmKern.damper./(2*lfmKern.mass);
omega(1) = sqrt(lfmKern.spring./lfmKern.mass - alpha(1)*alpha(1));

% Precomputations to increase the speed
preExp1 = zeros(length(t),2);
gamma1_p = alpha(1) + j*omega(1);
gamma1_m = alpha(1) - j*omega(1);
preGamma(1) = gamma1_p + gamma1_p;
preGamma(2) = gamma1_p + gamma1_m;
preGamma(3) = gamma1_m + gamma1_p;
preGamma(4) = gamma1_m + gamma1_m;
preConst = 1./preGamma;
preFactors(1) = preConst(2) - preConst(1);
preFactors(2) = preConst(3) - preConst(4);
preFactors(3) = preConst(3) - preConst(1);
preFactors(4) = preConst(2) - preConst(4);
preExp1(:,1) = gamma1_p.*exp(-gamma1_p*t);
preExp1(:,2) = gamma1_m.*exp(-gamma1_m*t);
% Actual computation of the kernel
sk =lfmDiagComputeH3VV(gamma1_p, gamma1_m, sigma2, t, preFactors([1 2]), 1) + ...
    lfmDiagComputeH3VV(gamma1_p, gamma1_m, sigma2, t, preFactors([3 4]), 0) + ...
    lfmDiagComputeH4VV(gamma1_p, gamma1_m, sigma2, t, preGamma([1 2 4 3]), preExp1 ) + ...
    lfmDiagComputeH4VV(gamma1_p, gamma1_m, sigma2, t, preGamma([1 3 4 2]), preExp1 );

if lfmKern.isNormalised
    k0 = lfmKern.sensitivity^2/(8*sqrt(2)*lfmKern.mass^2*omega^2);
else
    k0 = sqrt(pi)*sigma*lfmKern.sensitivity^2/(8*lfmKern.mass^2*omega^2);
end
k = k0*sk;

function h = lfmDiagComputeH3VV(gamma1_p, gamma1_m, sigma2, t, preFactor, mode)

h = preFactor(1)*lfmvvComputeUpsilonDiagVector(gamma1_p, sigma2, t, mode) ...
    + preFactor(2)*lfmvvComputeUpsilonDiagVector(gamma1_m,sigma2, t, mode);

function h = lfmDiagComputeH4VV(gamma1_p, gamma1_m, sigma2, t, ...
    preFactor, preExp)

h =   lfmvpComputeUpsilonVector(gamma1_p,sigma2, t, 0).*( preExp(:,2)/preFactor(2) - preExp(:,1)/preFactor(1)) ...
    + lfmvpComputeUpsilonVector(gamma1_m,sigma2, t, 0).*( preExp(:,1)/preFactor(4) - preExp(:,2)/preFactor(3));


function S=streakbar(X,Y,U,V,unit)

% H=streakbar(X,Y,U,V,unit) creates a colorbar for (but not exclusively)
% the function streakarrow.
% The arrays X and Y defines the coordinates for U and V.
% U and V are the same arrays used for streakarrow.
% The string variable unit is the unit of the vector magnitude
% Example:
%   streakbar(X,Y,U,V,'m/s')

Vmag=sqrt(U.^2+V.^2);
Vmin=min(Vmag(:)); Vmax=max(Vmag(:));

 P=get(gca,'position');
 %axes('position',[P(1)+P(3)+.02  P(2)+0.01  .01  P(4)-0.02]')
 axes('position',[P(1)+P(3)+.02  P(2)  .01  P(4)]')
 [X,Y]=meshgrid( [0 1], linspace(Vmin,Vmax,64));
 Q= [1:64; 1:64];
 S=pcolor(X', Y',Q); shading flat; set(gca,'XTickLabel',[],  'Yaxislocation', 'right')
 title(unit)


function [transformationMatrix] = ...
    phase_difference_linear_registration3d(moving, movingCertainty, ...
    fixed, fixedCertainty, varargin)
% PHASE_DIFFERENCE_LINEAR_REGISTRATION3D Estimates a transformation matrix using phase-difference
%
% [transformationMatrix] = ...
%     phase_difference_linear_registration3d(moving, movingCertainty, ...
%     fixed, fixedCertainty)
%
% INPUT ARGUMENTS
% moving                    - Moving image
% movingCertainty           - Certainty mask of moving image
% fixed                     - Fixed image
% fixedCertainty            - Certainty mask of fixed image
%
% OPTIONAL INPUT ARGUMENTS
% 'transformationModel'     - Transformation model for estimating the
%                             displacement field
%                             'translation', 'affine' (default)
%
% OUTPUT ARGUMENTS
% transformationMatrix      - Estimated transformation matrix

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

%% Setup default parameters
% translation, affine, non-rigid
transformationModel = 'affine';

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

% Initialize transformation matrix
transformationMatrix = eye(4);

% Load quadrature filters
load quadratureFiltersLinearRegistration3D

% Perform quadrature filtering
q21 = imfilter(moving,f1,'same','conv');
q22 = imfilter(moving,f2,'same','conv');
q23 = imfilter(moving,f3,'same','conv');

q11 = imfilter(fixed,f1,'same','conv');
q12 = imfilter(fixed,f2,'same','conv');
q13 = imfilter(fixed,f3,'same','conv');

filterSize = (size(real(f1),1)-1)/2;
q11 = q11(filterSize+1:end-filterSize,filterSize+1:end-filterSize,filterSize+1:end-filterSize);
q12 = q12(filterSize+1:end-filterSize,filterSize+1:end-filterSize,filterSize+1:end-filterSize);
q13 = q13(filterSize+1:end-filterSize,filterSize+1:end-filterSize,filterSize+1:end-filterSize);
q21 = q21(filterSize+1:end-filterSize,filterSize+1:end-filterSize,filterSize+1:end-filterSize);
q22 = q22(filterSize+1:end-filterSize,filterSize+1:end-filterSize,filterSize+1:end-filterSize);
q23 = q23(filterSize+1:end-filterSize,filterSize+1:end-filterSize,filterSize+1:end-filterSize);

% Compute phase-difference, certainties and phase-gradients
dphi(:,:,:,1) = angle(q11.*conj(q21));
dphi(:,:,:,2) = angle(q12.*conj(q22));
dphi(:,:,:,3) = angle(q13.*conj(q23));

% Estimate certainties
certainty(:,:,:,1) = abs(q11.*q21).*((cos(dphi(:,:,:,1)/2)).^2);
certainty(:,:,:,2) = abs(q12.*q22).*((cos(dphi(:,:,:,2)/2)).^2);
certainty(:,:,:,3) = abs(q13.*q23).*((cos(dphi(:,:,:,3)/2)).^2);

% Add certainty masks
if ~isempty(fixedCertainty)
    fixedCertainty = fixedCertainty(...
        filterSize+1:end-filterSize,filterSize+1:end-filterSize,filterSize+1:end-filterSize);
    certainty = bsxfun(@times,certainty,fixedCertainty);
end
if ~isempty(movingCertainty)
    movingCertainty = movingCertainty(...
        filterSize+1:end-filterSize,filterSize+1:end-filterSize,filterSize+1:end-filterSize);
    certainty = bsxfun(@times,certainty,movingCertainty);
end

% Estimate gradients of phi
grad_x_dphi_n1 = zeros(size(q11));
grad_x_dphi_n1(:,2:end-1,:) = ...
    angle(q11(:,3:end,:).*conj(q11(:,2:end-1,:)) + ...
    q11(:,2:end-1,:).*conj(q11(:,1:end-2,:)) + ...
    q21(:,3:end,:).*conj(q21(:,2:end-1,:)) + ...
    q21(:,2:end-1,:).*conj(q21(:,1:end-2,:)));

grad_y_dphi_n2 = zeros(size(q11));
grad_y_dphi_n2(2:end-1,:,:) = ...
    angle(q12(3:end,:,:).*conj(q12(2:end-1,:,:)) + ...
    q12(2:end-1,:,:).*conj(q12(1:end-2,:,:)) + ...
    q22(3:end,:,:).*conj(q22(2:end-1,:,:)) + ...
    q22(2:end-1,:,:).*conj(q22(1:end-2,:,:)));

grad_z_dphi_n3 = zeros(size(q11));
grad_z_dphi_n3(:,:,2:end-1) = ...
    angle(q13(:,:,3:end).*conj(q13(:,:,2:end-1)) + ...
    q13(:,:,2:end-1).*conj(q13(:,:,1:end-2)) + ...
    q23(:,:,3:end).*conj(q23(:,:,2:end-1)) + ...
    q23(:,:,2:end-1).*conj(q23(:,:,1:end-2)));

% Save all the phase gradients nicely...
phaseGradient(:,:,:,1) = grad_x_dphi_n1;
phaseGradient(:,:,:,2) = grad_y_dphi_n2;
phaseGradient(:,:,:,3) = grad_z_dphi_n3;

% Build A and h for A p = h
[A, h] = build_A_h_3d(dphi, certainty, phaseGradient, transformationModel);

% Solve the equation system
switch transformationModel
    case 'translation'
        d = A \ h;
        transformationMatrix(1:3,4) = d;
    case {'rigid','affine'}
        p = A \ h;
        transformationMatrix(1:3,1:4) = [1+p(1) p(2) p(3) p(10);...
            p(4) 1+p(5) p(6) p(11);...
            p(7) p(8) 1+p(9) p(12)];
end


function [w,u] = biharmonicP2(node,elem,pde,bdFlag,option)
%   [w,u] = biharmonicP1(node,elem,pde,bdFlag) produces the mixed cubic finite element
%   approximation of the biharmonic equation, where w = laplace u
%   See also biharmonicP1, biharmonicP2, biharmonicP3.
%   Created by Jie Zhou.
%   Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if nargin<5, option = []; end
tic;
%% Construct Data Structure
[elem2dof,edge,bdDof] = dofP2(elem);
N = size(node,1);  NT = size(elem,1);  Ndof = N+size(edge,1);

%% Compute geometric quantities and gradient of local basis
[Dlambda,area] = gradbasis(node,elem);

%% Assemble stiffness matrix
% Since Dphi_i*Dphi_j is quadratic, numerical quadrature rule is used here
if ~isfield(option,'quadorder')
    option.quadorder = 4;   % default order
end
[lambda, weight] = quadpts(option.quadorder);
nQuad = size(lambda,1);
ii = zeros(21*NT,1); jj = zeros(21*NT,1); sA = zeros(21*NT,1);sB = zeros(21*NT,1);
index = 0;
for i = 1:6
    for j = i:6
        Bij = 0;
        Aij = 0;
        for p = 1:nQuad
                 Bij = Bij + weight(p)*dot(Dphi(p,i),Dphi(p,j),2);
                 Aij = Aij + weight(p)*dot(phi(p,i),phi(p,j),2);
        end
        Bij = Bij.*area;
        Aij = Aij.*area;

        ii(index+1:index+NT) = double(elem2dof(:,i));
        jj(index+1:index+NT) = double(elem2dof(:,j));
        sB(index+1:index+NT) = Bij;
        sA(index+1:index+NT) = Aij;
        index = index + NT;
    end
end
clear Aij Bij
diagIdx = (ii == jj);   upperIdx = ~diagIdx;
B = sparse(ii(diagIdx),jj(diagIdx),sB(diagIdx),Ndof,Ndof);
A = sparse(ii(diagIdx),jj(diagIdx),sA(diagIdx),Ndof,Ndof);
% A = spdiags(accumarray(ii(diagIdx),sA(diagIdx),[Ndof 1]),0,Ndof,Ndof);
BU = sparse(ii(upperIdx),jj(upperIdx),sB(upperIdx),Ndof,Ndof);
AU = sparse(ii(upperIdx),jj(upperIdx),sA(upperIdx),Ndof,Ndof);
B = B + BU + BU';
A = A + AU + AU';

%% boundary condition
%%
    fixedDof = [];
    isFixedDof = false(Ndof,1);
    if ~isempty(bdFlag)
        elem2edge = elem2dof(:,4:6)-N;
        isDirichlet(elem2edge(bdFlag(:)==1)) = true;
        isFixedDof(edge(isDirichlet,:)) = true;
        isFixedDof(N + find(isDirichlet')) = true;
        fixedDof = find(isFixedDof);
        freeDof = find(~isFixedDof);
    end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunction Dphi
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function s = Dphi(p,i) % gradient of basis phi
    switch i
        case 1
            s = (4*lambda(p,1)-1).*Dlambda(:,:,1);
        case 2
            s = (4*lambda(p,2)-1).*Dlambda(:,:,2);
        case 3
            s = (4*lambda(p,3)-1).*Dlambda(:,:,3);
        case 4
            s = 4*(lambda(p,2)*Dlambda(:,:,3)+lambda(p,3)*Dlambda(:,:,2));
        case 5
            s = 4*(lambda(p,3)*Dlambda(:,:,1)+lambda(p,1)*Dlambda(:,:,3));
        case 6
            s = 4*(lambda(p,1)*Dlambda(:,:,2)+lambda(p,2)*Dlambda(:,:,1));
    end
    end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunction Dphi
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function s = phi(p,i) % gradient of basis phi
    switch i
        case 1
            s = (2*lambda(p,1)-1).*lambda(p,1);
        case 2
            s = (2*lambda(p,2)-1).*lambda(p,2);
        case 3
            s = (2*lambda(p,3)-1).*lambda(p,3);
        case 4
            s = 4*lambda(p,3).*lambda(p,2);
        case 5
            s = 4*lambda(p,1).*lambda(p,3);
        case 6
            s = 4*lambda(p,1).*lambda(p,2);
    end
    end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Assemble right hand side by high order quadrature rule
% To reduce the effect of the error introduced by the numerical quadrature,
% the load term is computed using the 3rd order qudrature rule.
b = zeros(Ndof,1);
u=zeros(Ndof,1);
w=zeros(Ndof,1);

if ~isfield(option,'fquadorder')
    option.fquadorder = 3;   % default order
end
if ~isfield(pde,'f') || (isreal(pde.f) && (pde.f==0))
    pde.f = [];
end
if ~isempty(pde.f)
    % quadrature points in the barycentric coordinate
    [lambda,w] = quadpts(option.fquadorder);
    nQuad = size(lambda,1);
%     phi(:,1) = lambda(:,1).*(2*lambda(:,1)-1);
%     phi(:,2) = lambda(:,2).*(2*lambda(:,2)-1);
%     phi(:,3) = lambda(:,3).*(2*lambda(:,3)-1);
%     phi(:,4) = 4*lambda(:,2).*lambda(:,3);
%     phi(:,5) = 4*lambda(:,3).*lambda(:,1);
%     phi(:,6) = 4*lambda(:,1).*lambda(:,2);
    bt = zeros(NT,6);
    for p = 1:nQuad
        % quadrature points in the x-y coordinate
        pxy = lambda(p,1)*node(elem(:,1),:) ...
            + lambda(p,2)*node(elem(:,2),:) ...
            + lambda(p,3)*node(elem(:,3),:);
        if isfield(pde,'f') && isnumeric(pde.f)
            fp = pde.f;        % piecewise constant
        else
            fp = pde.f(pxy);   % function handle
        end
        for j = 1:6
            bt(:,j) = bt(:,j) + w(p)*phi(p,j)*fp;
        end
    end
    bt = bt.*repmat(area,1,6);
    b = accumarray(elem2dof(:),bt(:),[Ndof 1]);
end

 function [b1,u] = getbdP2(b)
    %% Boundary conditions for Poisson equation: P2 quadratic FEM.
    %
    % The set up of boundary condition consists of two parts:
    %

    %
    %  Modify the right hand side b. The Neumann boundary integral is added
    %  to b. For Dirichlet boundary ndoes, b(fixedDof) is the evaluation of
    % pde.g_D.
    %

    u = zeros(Ndof,1);

    %% Part 1: Find boundary edges and modify the load b
    % Neumann boundary condition
        if ~isfield(option,'gNquadorder')
            option.gNquadorder = 6;
        end
                idxN = (bdFlag(:) == 1);      % all Neumann edges in bdFlag
        Neumannidx = elem2edge(idxN ); % index of Neumann and Robin edges
        % since boundary integral is also needed for Robin edges
        Neumann   = edge(Neumannidx,:);
        b1 = zeros(Ndof,1);
        [lambdagN,weightgN] = quadpts1(option.gNquadorder);
        nQuadgN = size(lambdagN,1);
        % quadratic bases (1---3---2)
        bdphi = zeros(nQuadgN,3);
        bdphi(:,1) = (2*lambdagN(:,1)-1).*lambdagN(:,1);
        bdphi(:,2) = (2*lambdagN(:,2)-1).*lambdagN(:,2);
        bdphi(:,3) = 4*lambdagN(:,1).*lambdagN(:,2);
        % length of edge
        el = sqrt(sum((node(Neumann(:,1),:) - node(Neumann(:,2),:)).^2,2));
        ge = zeros(size(Neumann,1),3);
        for pp = 1:nQuadgN
            ppxy = lambdagN(pp,1)*node(Neumann(:,1),:) ...
                 + lambdagN(pp,2)*node(Neumann(:,2),:);
            gNu = pde.g_N(ppxy);
            ge(:,1) = ge(:,1) + weightgN(pp)*gNu*bdphi(pp,1);
            ge(:,2) = ge(:,2) + weightgN(pp)*gNu*bdphi(pp,2);
            ge(:,3) = ge(:,3) + weightgN(pp)*gNu*bdphi(pp,3); % interior bubble
        end
        % update RHS
        ge = ge.*repmat(el,1,3);
        b1(1:N) = accumarray(Neumann(:), [ge(:,1); ge(:,2)],[N,1]);
        b1(N+Neumannidx) = b1(N+Neumannidx) + ge(:,3);

    %% Part 2: Find Dirichlet boundary edges and compute the boundary value
    % Dirichlet boundary conditions

        isDirichlet(elem2edge(bdFlag(:)==1)) = true;
        % interpolation
                idx = (fixedDof > N);                         % index of edge nodes
        u(fixedDof(~idx)) = pde.g_D(node(fixedDof(~idx),:)); % bd value at vertex dofs
        bdEdgeIdx = fixedDof(idx) - N;
        bdEdgeMid = (node(edge(bdEdgeIdx,1),:) + node(edge(bdEdgeIdx,2),:))/2;
        u(fixedDof(idx)) = pde.g_D(bdEdgeMid);
        b1 = b1 - B*u;

    end % end of getbdP2

[b1,u] = getbdP2(b);
B(:,fixedDof)=[];
Nu=size(freeDof,1);

b(fixedDof)=[];
     bigA = [A, B; ...
             B', sparse(Nu,Nu)];
     bigF = [b1; -b];

% Solver
tic;

%bigU=PFGMRES(bigA, bigF,sparse(Ndof+Nu,1), Ndof, 10, 1e-6, [],[]);

   bigU=bigA\bigF;
   w=bigU(1:Ndof);
   u(freeDof)=bigU(Ndof+1:Ndof+Nu);
toc
end                 % end of function PoissonP2


function [x, xdot, x2dot] = modifiednewmarkint(M, C, K, R, x0, xdot0, ...
    t, varargin)
%Newmark's Direct Integration Method
%--------------------------------------------------------------------------
% Code written by : - Siva Srinivas Kolukula                              |
%                     Senior Research Fellow                              |
%                     Structural Mechanics Laboratory                     |
%                     Indira Gandhi Center for Atomic Research            |
%                     INDIA
%                   - Hamed Nokhostin                                     |
%                     B.S. Student                                        |
%                     Mechanical Engineering Faculty                      |
%                     K.N.Toosi Uiversity of Technoloygy                  |
%                     I.R.Iran                                            |
% E-mail : allwayzitzme@gmail.com
%          h_nokhostin@yahoo.com                                          |
%-------------------------------------------------------------------------
% PURPOSE
%        ???
% SYNTAX
%        [x, xdot, x2dot] = newmarkint(M, C, K, R, x0, xdot0, t, varargin)
% INPUT
%        [M] :       System Mass              [n,n]
%        [C] :       System Damping           [n,n]
%        [K] :       System Stiffness         [n,n]
%        [R] :       Externally Applied Load  [n,nt]
%        [x0] :      Initial Position         [n,1]
%        [xdot0] :   Initial Velocity         [n,1]
%        [t] :       Time Vector              [1,nt]
%        [varargin]: Options
%
% OUTPUT
%       [x]:        Displacemente Response   [n,nt]
%       [xdot]:     Velocity                 [n,nt]
%       [x2dot]:    Acceleration             [n,nt]
%
%
%  nt = number of time steps
%  n = number of nodes
% The options include changing the value of the "gamma" and "beta"
% coefficient which appear in the formulation of the method. By default
% these values are set to gamma = 1/2 and beta = 1/4.
%
% EXAMPLE
% To change nemark's coefficients, say to gamma = 1/3 and beta = 1/5,
% the syntax is:
%       [u, udot, u2dot] = newmark_int(t,p,u0,udot0,m,k,xi, 1/3, 1/5)
%
%-------------------------------------------------------------------------
if nargin == 7
    disp('Using default values:');
    disp('    gamma = 1/2');
    disp('    beta  = 1/4');
    gamma = 1 / 2;
    beta = 1 / 4;
elseif nargin == 9
    gamma = varargin{1};
    beta = varargin{2};
    disp('Using user''s values:');
    disp(['    gamma = ', num2str(alpha)]);
    disp(['    beta  = ', num2str(delta)]);
else
    error('Incorrect number of imput arguments');
end

dt = t(2) - t(1);
nt = fix((t(length(t) )- t(1)) / dt);
n = length(M);

% Constants used in Newmark's integration
a1 = gamma / (beta * dt);
a2 = 1 / (beta * dt ^ 2);
a3 = 1 / (beta * dt);
a4 = gamma / beta;
a5 = 1/(2 * beta);
a6 = (gamma / (2 * beta) - 1) * dt;

x = zeros(n,nt);
xdot = zeros(n,nt);
x2dot = zeros(n,nt);

% Initial Conditions
x(:, 1) = x0;
xdot(:, 1) = xdot0;
%R0 = zeros(n,1);
x2dot(:,1) = M \ (R(:, 1) - C * xdot(:, 1) - K * x(:, 1)) ;

Kcap = K + a1 * C + a2 * M;

a = a3 * M + a4 * C;
b = a5 * M + a6 * C;

% Tme step starts
for i = 1 : nt
    delR = R(:, i) + a * xdot(:, i) + b * x2dot(:, i);
    delx = Kcap \ delR ;
    delxdot = a1 * delx - a4 * xdot(:, i) - a6 * x2dot(:, i);
    delx2dot = a2 * delx - a3 * xdot(:, i) - a5 * x2dot(:, i);
    x(:, i + 1) = x(:, i) + delx;
    xdot(:, i + 1) = xdot(:, i) + delxdot;
    x2dot(:, i + 1) = x2dot(:, i) + delx2dot;
end


function [CVA] = spm_cva_compare (Y,X,c)
% Model comparison for probabilistic CVA
% FORMAT [CVA] = spm_cva_compare (Y,X,c)
%
% Y  [N x d1] data matrix
% X  [N x d2] design matrix
% c  Contrast vector (if specified)
%
% CVA has fields:
%
% .order        number of canonical vectors (latent space dimension)
% .bic          BIC for each order
% .aic          AIC for each order
%
% and
%
% .U1,.U2      Canonical vectors
% .W1,.W2      Factor matrices
%
% for the highest order model.
%
% See spm_cva_prob.m for more details
%___________________________________________________________________________
% Copyright (C) 2011 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_cva_compare.m 4687 2012-03-14 18:15:49Z will $

if nargin > 2
    % get null-space of contrast
    X0  = X - X*c*pinv(c);
    X   = full(X*c);
    X0  = spm_svd(X0);

    Y     = Y - X0*(X0'*Y);
    X     = X - X0*(X0'*X);
end

[N1,p1]=size(Y);
[N2,p2]=size(X);
if ~(N1==N2)
    disp('X and Y are of incompatible size');
    return
end

m=min([p1,p2]);
for i=1:m+1,
    order(i)=i-1;
    CVA = spm_cva_prob (Y',X',order(i));
    bic(i)=CVA.bic;
    aic(i)=CVA.aic;
    L(i)=CVA.L;
end

CVA.order=order;
CVA.bic=bic;
CVA.aic=aic;
CVA.L=L;


      satis_distort = 0;

      while ~satis_distort,

          k_g_save = k_g;

	 k_g = input(['Guess for distortion factor kc ([]=' num2str(k_g_save) '): ']);

	 if isempty(k_g), k_g = k_g_save; end;

x_n = (x - c_g(1))/f_g;
y_n = (y - c_g(2))/f_g;

[x_pn] = comp_fisheye_distortion([x_n' ; y_n'],[k_g;0;0;0]);

% Compute the inside points through computation of the planar homography (collineation)

a00 = [x_pn(1,1);x_pn(2,1);1];
a10 = [x_pn(1,2);x_pn(2,2);1];
a11 = [x_pn(1,3);x_pn(2,3);1];
a01 = [x_pn(1,4);x_pn(2,4);1];

% Compute the planar collineation: (return the normalization matrix as well)
[Homo,Hnorm,inv_Hnorm] = compute_homography([a00 a10 a11 a01],[0 1 1 0;0 0 1 1;1 1 1 1]);

% Build the grid using the planar collineation:

x_l = ((0:n_sq_x)'*ones(1,n_sq_y+1))/n_sq_x;
y_l = (ones(n_sq_x+1,1)*(0:n_sq_y))/n_sq_y;
pts = [x_l(:) y_l(:) ones((n_sq_x+1)*(n_sq_y+1),1)]';

XXpn = Homo*pts;
XXpn = XXpn(1:2,:) ./ (ones(2,1)*XXpn(3,:));

XX = apply_fisheye_distortion(XXpn,[k_g;0;0;0]);

XX(1,:) = f_g*XX(1,:) + c_g(1);
XX(2,:) = f_g*XX(2,:) + c_g(2);

	 figure(2);
	 image(I);
	 colormap(map);
	 zoom on;
	 hold on;
	 plot(XX(1,:),XX(2,:),'r+');
	 title('The red crosses should be on the grid corners...');
	 hold off;

	 satis_distort = input('Satisfied with distortion? ([]=no, other=yes) ');

	 satis_distort = ~isempty(satis_distort);

      end;


% The static class geometricTools encapsulates the most used functions for surface processing
% used to solve EEG forward or inverse problems.
%
% Author: Alejandro Ojeda, SCCN/INC/UCSD, 2012
%
% Contributors:
% nonrigid_version23    -> D.Kroon, University of Twente, August
%                             2010 (http://www.mathworks.com/matlabcentral/fileexchange/20057)
% getSurfaceLaplacian   -> Nelson Trujillo Barreto and Pedro Antonio Valdes
%                             Hernandez, Cuban Neuroscience Center
% iso2mesh dependencies -> Qianqian Fang, http://iso2mesh.sourceforge.net/cgi-bin/index.cgi

classdef geometricTools
    methods
        function obj = geometricTools()
        end
        %%
        function disp(obj)
            disp(['Static class: ' class(obj)])
            disp('Static classes work like a namespace, they are used to create data and functions that can be accessed without creating an instance of the class.')
            methods(obj);
        end
    end
    methods(Static)
        %%
        function Xcentered = correctOrigin(X)
            [m,n] = size(X);
            K = ones(m,n);
            B = pinv(K'*K)*K'*X;
            X0 = K*B;
            Aff = eye(4);
            Aff([1 2],4) = X0(1,[1 2])';
            Xcentered = Aff\[X ones(m,1)]';
            Xcentered = Xcentered(1:3,:)';
        end
        %%
        function [Aff,Sn, scale] = affineMapping(S,T)
            % S: source space
            % T: target space
            % S = [sx1 sy1 sz1; sx2 sy2 sz2; ... sxk syn szk]
            % T = [tx1 ty1 tz1; tx2 ty2 tz2; ... txk tyn tzk]
            %
            % d(Aff) = min frobenius(T -S* Aff')
            % Sn = S*Aff';

            [~,~,transform] = procrustes(T,S);
            scale = transform.b;
            Aff = [[transform.b*transform.T;transform.c(1,:)] [0;0;0;1]]';
            Sn = geometricTools.applyAffineMapping(S,Aff);
        end
        %%
        function T = applyAffineMapping(S,M)
            % [T 1] = [S 1]*M';
            T = [S ones(size(S,1),1)]*M';
            T(:,4) = [];
        end
        %%
        function [def,spacing,offset,SgridWarped] = bSplineMapping(S,T,Sgrid,options)
            if nargin < 4,
                options.Verbose = false;
                options.MaxRef = 5;
            end
            mn = min(Sgrid);
            Smn = bsxfun(@minus,S,mn);
            dim = max(Sgrid) - mn;
            Tmn = bsxfun(@minus,T,mn);
            [def,spacing,SgridWarped] = point_registration(dim,Smn,Tmn,options);
            offset = mn;
            SgridWarped = bsxfun(@plus,SgridWarped,offset);
        end
        %%
        function SgridWarped = applyBSplineMapping(def,spacing,offset,Sgrid)
            Smn = bsxfun(@minus,Sgrid,offset);
            SmnWarped = bspline_trans_points_double(def,spacing,Smn);
            SgridWarped = bsxfun(@plus,SmnWarped,offset);
        end
        %%
        function [neighbors,D,loc] = nearestNeighbor(vertices,T)
            if exist('DelaunayTri','file')
                 dt = DelaunayTri(vertices(:,1),vertices(:,2),vertices(:,3)); %#ok
            else dt = delaunayTriangulation(vertices(:,1),vertices(:,2),vertices(:,3));
            end
            try [loc,D] = nearestNeighbor(dt, T);
            catch
                loc = nearestNeighbor(dt, T);
                D = sqrt(sum((vertices(loc,:)-T).^2,2));
            end
            neighbors = vertices(loc,:);
        end
        %%
        function Yi = ridgeInterpolation(vertices,faces,elec,Y)
            L = geometricTools.getSurfaceLaplacian(vertices,faces);
            K = geometricTools.localGaussianInterpolator(vertices,elec,1);
            K = full(K);
            Yi = ridgeGCV(Y,K,L,100,0);
        end
        %%
        function J = simulateGaussianSource(X,X0,h)
            if nargin < 3, h = 0.1;end
            J = geometricTools.localGaussianInterpolator(X,X0,h)';
        end
        function W = localGaussianInterpolator(X,Xi,h,normalize)
            if nargin < 3, h = 0.1;end
            if nargin < 4, normalize = false;end
            N = size(Xi,1);
            M = size(X,1);
            W = zeros(N,M);
            for it=1:N
                d = sum(bsxfun(@minus,X,Xi(it,:)).^2,2);
                W(it,:) = exp(-d/(2*h^2));
            end
            if normalize, W = bsxfun(@rdivide,W,sum(W,2)+eps);end
        end
        %%
        function D = isInConvexHull(X,Xi)

            X = geometricTools.correctOrigin(X);
            X = bsxfun(@rdivide,X,sqrt(sum(X.^2,2)));

            [x,y,z] = sphere(64);

            figure;
            surf(x,y,z,'FaceColor','g','FaceAlpha',0.7,'EdgeColor','none');
            set(gca,'Projection','perspective','DataAspectRatio',[1 1 1]); hold on;axis tight;camlight

            plot3(X(:,1),X(:,2),X(:,3),'.')

            X = geometricTools.correctOrigin(X);
            X = bsxfun(@rdivide,X,sqrt(sum(X.^2,2)));

            N = size(Xi,1);
            M = size(X,1);
            D = zeros(M,N);
            for it=1:N
                d = bsxfun(@minus,X,Xi(it,:));
                D(:,it) = sqrt(sum(( d ).^2,2));
            end
        end
        %%
        function Yi = localGaussianScatterInterpolator(X,Y,Xi)
            n = size(Y,2);
            if n==1
                F = TriScatteredInterp(X(:,1),X(:,2),X(:,3),Y,'nearest');
                Yi = F(Xi(:,1),Xi(:,2),Xi(:,3));
            else
                Yi = zeros(size(Xi,1),n);
                for it=1:n
                    F = TriScatteredInterp(X(:,1),X(:,2),X(:,3),Y(:,it),'nearest');
                    Yi(:,it) = F(Xi(:,1),Xi(:,2),Xi(:,3));
                end
            end
            W = geometricTools.localGaussianInterpolator(Xi,Xi,16);
            Yi = W*Yi;
        end
        %%
        function [rVertices,rFaces] = resampleSurface(vertices,faces,decimationPercent)
            if nargin < 2, error('Not enough input arguments.');end
            if nargin < 3, decimationPercent = 0.1;end
            if isempty(which('meshresample')), error('This function uses Iso2Mesh toolbox, you can download it for free fom: http://iso2mesh.sourceforge.net');end
            [rVertices,rFaces]=meshresample(vertices,faces,decimationPercent);
        end
        %%
        function sVertices = smoothSurface(vertices,faces,lambda,method)
            if nargin < 2, error('Not enough input arguments.');end
            if nargin < 3, lambda = 0.2;end
            if nargin < 4, method = 'lowpass';end

            maxIter = 20;
            N = size(vertices,1);

            if isempty(which('meshresample'))
                warning('MoBILAB:noIso2Mesh','This function uses Iso2Mesh toolbox if is installed, you can download it for free fom: http://iso2mesh.sourceforge.net');
                sVertices = vertices;
                for it=1:N
                    ind = any(faces==it,2);
                    indices = faces(ind,:);
                    indices = indices(:);
                    indices(indices==it) = [];
                    W = geometricTools.localGaussianInterpolator(vertices(indices,:),vertices(it,:),10);
                    sVertices(it,:) = sum((1./W)*vertices(indices,:),1)./sum(1./W);
                end
                return;
            end
            conn = neighborelem(faces,size(vertices,1));
            for it=1:N
                tmp = faces(conn{it},:);
                conn{it} = unique(tmp(:)');
            end
            sVertices = smoothsurf(vertices,[],conn,maxIter,lambda,method);
        end
        %%
        function [rVertices,rFaces] = refineSurface(vertices,faces,decimationRate,maxIter)
            if nargin < 3, decimationRate = 0.5;end
            if nargin < 4, maxIter = 3;end
            if isempty(which('meshresample')), error('This function uses Iso2Mesh toolbox, you can download it for free fom: http://iso2mesh.sourceforge.net');end

            tmpVertices = vertices;
            tmpFaces = faces;
            for it=1:maxIter
                [tmpVertices,tmpFaces] = geometricTools.resampleSurface(tmpVertices,tmpFaces,decimationRate);
                tmpVertices = geometricTools.smoothSurface(tmpVertices,tmpFaces);
                disp(it)
            end
            rVertices = tmpVertices;
            rFaces = tmpFaces;
        end
        %%
        function verticesExt = repareIntersectedSurface(surfInt,surfOut,dmax)
            if nargin < 3, dmax = 8;end
            verticesInt = surfInt.vertices;
            verticesExt = surfOut.vertices;
            [nVerticesInt,d] = geometricTools.nearestNeighbor(verticesExt,verticesInt);
            I = d < dmax;
            while any(I)
                I2 = ismember(verticesExt,nVerticesInt(I,:),'rows');
                verticesExt(I2,:) = 1.005*verticesExt(I2,:);
                [nVerticesInt,d] = geometricTools.nearestNeighbor(verticesExt,verticesInt);
                I = d < dmax;
            end
            if any(verticesExt(:) ~= surfOut.vertices(:))
                verticesExt = geometricTools.smoothSurface(verticesExt,surfOut.faces);
            end
        end
        %%
        function [normals,faces] = getSurfaceNormals(vertices,faces,normalsIn)
            if nargin < 3, normalsIn = true;end
            h = figure('visible','off');
            h2 = patch('vertices',vertices,'faces',fliplr(faces));
            normals = get(h2,'vertexnormals');close(h);
            if isempty(normals)
                normals = vertices;
            end
            normals = normals./(sqrt(sum(normals.^2,2))*[1 1 1]);
            area1 = geometricTools.getSurfaceArea(vertices,faces);
            area2 = geometricTools.getSurfaceArea(vertices+normals,faces);
            if area2 < area1% && normalsIn
                faces = fliplr(faces);
                h = figure('visible','off');h2 = patch('vertices',vertices,'faces',fliplr(faces));
                normals = get(h2,'vertexnormals');close(h);
                normals = normals./(sqrt(sum(normals.^2,2))*[1 1 1]);
            end
            if normalsIn
                faces = fliplr(faces);
                h = figure('visible','off');h2 = patch('vertices',vertices,'faces',fliplr(faces));
                normals = get(h2,'vertexnormals');close(h);
                normals = normals./(sqrt(sum(normals.^2,2))*[1 1 1]);
            end
        end
        %%
        function [area,areas] = getSurfaceArea(vertices,faces)
            x1= vertices(faces(:,1),1);
            y1= vertices(faces(:,1),2);
            z1= vertices(faces(:,1),3);
            x2= vertices(faces(:,2),1);
            y2= vertices(faces(:,2),2);
            z2= vertices(faces(:,2),3);
            x3= vertices(faces(:,3),1);
            y3= vertices(faces(:,3),2);
            z3= vertices(faces(:,3),3);
            area = sqrt(((y2-y1).*(z3-z1)-(y3-y1).*(z2-z1)).^2+((z2-z1).*(x3-x1)-(z3-z1).*(x2-x1)).^2+...
                ((x2-x1).*(y3-y1)-(x3-x1).*(y2-y1)).^2)/2;
            areas = area;
            area = sum(area);
        end
        function L = getSurfaceLaplacian1(vertices,faces)
            % LAPLACES Calculates a Discrete Surface Laplacian Matrix
            %          for a triangulated surface

            Nv = size(vertices,1);
            Nf = size(faces,1);
            L = spalloc(Nv,Nv,3*Nf);
            %L = speye(Nvtx);
            for fi=1:Nf
                for k=1:3
                    kk = mod(k,3)+1;
                    L(faces(fi,k),faces(fi,kk)) = sqrt( sum((vertices(faces(fi,k),:)-vertices(faces(fi,kk),:)).^2,2));
                end
            end
            L(L>0) = 1./L(L>0);
            L = (L+L')/2;
            L = L - spdiags(sum(L,2),0,Nv,Nv);
        end
        function L = getSurfaceLaplacian(vertices,faces)
            % LAPLACES Calculates a Discrete Surface Laplacian Matrix
            %          for a triangulated surface
            %
            % Reference:
            % [1] Huiskamp, G., 1991, Difference formulas for the surface laplacian
            %     on a triangulated surface, Journal of Computational Physics 95,
            %     477-496.
            %
            % Nelson Trujillo Barreto
            % Pedro antonio Valdes Hernandez
            % Cuban Neuroscience Center

            Cortex.vertices = vertices;
            Cortex.faces = faces;
            vtx = Cortex.vertices;
            tri = Cortex.faces;
            [nei,nei_tri] = geometricTools.get_neis(Cortex);
            Nvtx = size(vtx,1);
            L = speye(Nvtx);
            for j=1:Nvtx,
                nei_tri_j = tri(nei_tri{j},:);
                nei_j = nei{j};
                PHI_jk = [];
                rj = vtx(j,:);
                Nj = length(nei_j);
                for k=1:Nj,

                    [indi,indj]=find(nei_tri_j==nei_j(k)); %#ok
                    nei_tri_jk = nei_tri_j(indi,:);
                    if size(nei_tri_jk,1) < 2,
                        break;
                    end
                    nei_k_lr = setxor(nei_tri_jk(1,:),nei_tri_jk(2,:));

                    rk2 = sum((rj-vtx(nei_j(k),:)).^2);
                    rl2 = sum((rj-vtx(nei_k_lr(1),:)).^2);
                    rr2 = sum((rj-vtx(nei_k_lr(2),:)).^2);
                    rkl2 = sum((vtx(nei_k_lr(1),:)-vtx(nei_j(k),:)).^2);
                    rkr2 = sum((vtx(nei_k_lr(2),:)-vtx(nei_j(k),:)).^2);

                    cos_phi_kl = (rk2+rl2-rkl2)./sqrt(rk2.*rl2)./2;
                    cos_phi_kr = (rk2+rr2-rkr2)./sqrt(rk2.*rr2)./2;
                    sin_phi_kl = sqrt(1-cos_phi_kl.^2);
                    sin_phi_kr = sqrt(1-cos_phi_kr.^2);

                    PHI_jk = [PHI_jk (1-cos_phi_kl)./(sin_phi_kl+eps)+(1-cos_phi_kr)./(sin_phi_kr+eps)]; %#ok
                end
                if ~isempty(PHI_jk)
                    rjk = sqrt(sum((vtx(nei_j,:)-repmat(rj,Nj,1)).^2,2));
                    rj_bar = mean(rjk);

                    theta_jk = 4*PHI_jk'./(rj_bar*sum(PHI_jk)*rjk);
                    L(j,nei_j) = theta_jk'; %#ok
                else
                    L(j,nei_j) = 0;     %#ok
                    L(j,j) = 1;         %#ok
                end
            end;

            L = L - speye(Nvtx,Nvtx);
            L = L - spdiags(sum(L,2),0,Nvtx,Nvtx);
            d = diag(L);
            ind = find(d==0);
            if ~isempty(ind), for it=1:length(ind), L(ind(it),ind(it)) = 1;end;end
            %th = prctile(nonzeros(L),[0.1 99.9]);
            %L(L>th(2)) = 0;
            %L(L<th(1)) = 0;
        end
        %%
        function [nei,nei_tri] = get_neis(P)
            % helper function for getSurfaceLaplacian
            % Nelson Trujillo Barreto
            % Pedro antonio Valdes Hernandez
            % Cuban Neuroscience Center
            n = size(P.vertices,1);
            nei_tri = cell(n,1);
            nei = cell(n,1);
            for i = 1:n
                [r,c] = find(P.faces == i); %#ok
                nei_tri{i} = r;
                nei{i} = setdiff(unique(P.faces(r,:)),i);
            end
        end
        function [Nei_faces,Nei_vertices] = get_neis1(P)
            n = size(P.vertices,1);
            Nei_faces = cell(n,1);
            Nei_vertices = cell(n,1);
            % hbar = waitbar(0,'calculating neigs...');
            for i = 1:n
                [r,c] = find(P.faces == i); %#ok
                tmp = P.faces(r,:)';
                tmp(tmp == i) = [];
                m = length(tmp)/2;
                Nei_faces{i} = reshape(tmp,2,m)';
                for j = 1:m
                    Nei_vertices{i}{j} = P.vertices(Nei_faces{i}(j,:),:);
                end
                waitbar(i/n,hbar);
            end
            close(hbar);
        end
       %%
        function [nVertices,nFaces] = openSurface(vertices,faces,rmIndices)
            nVertices = vertices;
            vertices(rmIndices,:) = [];
            [~,rm_1] = ismember(faces(:,1),rmIndices);
            [~,rm_2] = ismember(faces(:,2),rmIndices);
            [~,rm_3] = ismember(faces(:,3),rmIndices);
            rm_faces = rm_1 | rm_2 | rm_3;
            faces(rm_faces,:) = [];
            [~,J] = ismember(nVertices,vertices,'rows');
            nFaces = J(faces);
            nVertices = vertices;
        end
        %%
        function [nVertices,nFaces] = getSurfaceROI(vertices,faces,roiIndices)
            rmIndices = setdiff(1:size(vertices,1),roiIndices);
            [nVertices,nFaces] = geometricTools.openSurface(vertices,faces,rmIndices);
        end
        %%
        function yi = interpOnSurface(vertices,faces,elec,y,method)
            if nargin < 5, method = 'spline';end
            switch method
                case 'ridge'
                    yi = geometricTools.ridgeInterpolation(vertices,faces,elec,y);
                case 'linear'
                    W = geometricTools.localGaussianInterpolator(elec,vertices,32);
                    yi = W*y;
                case 'spline'
                    yi = geometricTools.spSplineInterpolator(elec,y,vertices);
                otherwise
                    yi = geometricTools.spSplineInterpolator(elec,y,vertices);
            end
        end
        %%
        function atlas = labelSurface(Surf,imgAtlasfile, txtAtlasLabel,maxColorValue)
            if nargin < 4, maxColorValue = 90;end
            % Atlas
            v =spm_vol(imgAtlasfile); % atlas
            A = spm_read_vols(v);
            A(A>maxColorValue) = 0;
            indNonZero = A(:)~=0;
            colorTable = A(indNonZero);
            [x,y,z] = ndgrid(1:v.dim(1),1:v.dim(2),1:v.dim(3));
            M = v.mat;
            X = [x(:) y(:) z(:) ones(numel(x),1)]*M';
            X = X(indNonZero,1:3);
            clear x y z
            F = TriScatteredInterp(X,colorTable,'nearest');
            n = size(Surf.vertices,1);
            labelsValue = F(Surf.vertices);
            colorTable = labelsValue;
            hwait = waitbar(0,'Atlas correction...');
            for it=1:n
                neigInd = any(Surf.faces == it,2);
                vertexInedex = Surf.faces(neigInd,:);
                vertexInedex = vertexInedex(:);
                [y,x] = hist(labelsValue(vertexInedex));
                [~,loc] = max(y);
                [~,loc] = min(abs(labelsValue(vertexInedex) - x(loc)));
                labelsValue(it) = colorTable(vertexInedex(loc));
                waitbar(it/n,hwait);
            end
            waitbar(1,hwait);
            close(hwait);
            atlas.colorTable = labelsValue;
            atlas.label = textfile2cell(txtAtlasLabel);
            atlas.label = atlas.label(1:max(atlas.colorTable));
            for it=1:length(atlas.label)
                ind = find(atlas.label{it} == ' ');
                atlas.label{it} = atlas.label{it}(ind(1)+1:ind(end)-1);
            end
        end
        %%
        function X = projectOntoUnitarySphere(X)
            [~,X(:,1),X(:,2),X(:,3)] = geometricTools.projectOnSphere(X(:,1),X(:,2),X(:,3));
            [azimuth,elevation,r] = cart2sph(X(:,1),X(:,2),X(:,3));
            [X(:,1),X(:,2),X(:,3)] = sph2cart(azimuth,elevation,elevation*0+1);
        end
        %%
        function [Yi,W] = spSplineInterpolator(X,Y,Xi,plotFlag)
            % Computes the spherical spline interpolator based on Perrin, F.,
            % Pernier, J., Bertrand, O., Echallier, J.F. (1990). Corrigenda
            % EEG 02274. Electroencephalography and Clinical Neurophysiology, 76, 565.

            if nargin < 4, plotFlag = false;end
            %X0 = mean(X);
            %X  = bsxfun(@minus,X,X0);
            %Xi  = bsxfun(@minus,Xi,X0);
            X  = geometricTools.projectOntoUnitarySphere(X);
            Xi = geometricTools.projectOntoUnitarySphere(Xi);
            %X  = bsxfun(@rdivide,X,sqrt(sum(X.^2,2)));
            %Xi = bsxfun(@rdivide,Xi,sqrt(sum(Xi.^2,2)));

            %--
            M = size(X,1);
            One = ones(size(Xi,1),1);
            %--

            % Solving eq. 4 of Perrin et al. (1989)
            COS_X  = geometricTools.cosines(X,X);
            COS_Xi = geometricTools.cosines(Xi,X);

            % Solving eq. 3 of Perrin et al. (1989)
            Gx  = geometricTools.sphericalSpline(COS_X);
            Gxi = geometricTools.sphericalSpline(COS_Xi);

            % Solving eq. 2 Perrin et al. (1989)
            [C,~,~,T] = ridgeGCV([Y;0],[Gx ones(M,1);ones(1,M) 0],eye(M+1));

            % Interpolating with the spherical harmonics
            Yi = [Gxi One]* C;

            W = [Gxi One]* T(:,1:end-1);

            % Plot the input & projected electrode positions on a sphere
            if plotFlag
                geometricTools.plot_on_sphere(X,Y,Xi,Yi);
            end
        end
        %%
        function Gx = sphericalSpline(x)
            % sphericalSpline solves eq. 3 of Perrin et al. (1989)
            % g(COS) = 1/4pi * sum[n=1:inf] (( (2*n+1)/( n^m * (n+1)^m ) ) * Pn(COS));

            m = 4;
            N = 16;    % gives accuracy of 10^-6

            P = cat(3, ones(size(x)), x);
            Gx = 3 / 2 ^ m * P(:, :, 2);
            for n = 2:N
                P(:, :, 3) = ((2 * n - 1) * x .* P(:, :, 2) - (n - 1) * P(:, :, 1)) / n;
                P = P(:,:,[2 3 1]);
                Gx = Gx + (2 * n + 1) / (n ^ m * (n + 1) ^ m) * P(:, :, 2);
            end
            Gx = Gx / (4 * pi);
        end
        %%
        function [r,x,y,z] = projectOnSphere(X,Y,Z,xo,yo,zo)
            % projectOnSphere - calculates projections of xyz positions
            % onto the unitary sphere
            %
            % Usage: [r,x,y,z] = projectOnSphere(X,Y,Z,xo,yo,zo,plotFlag)
            %
            % Notes:    The general formula for a sphere, with radius r is given by:
            %
            %           (x - xo)^2  +  (y - yo)^2  +  (z - zo)^2  =  r^2
            %
            %           This function takes arguments for cartesian co-ordinates
            %           of X,Y,Z (assume Z > 0) and the center of the sphere (xo,yo,zo).
            %           If (xo,yo,zo) is not provided a cnter at (0,0,0) is assumed.
            %
            %           Returned values are the fitted radius 'r' (constant)
            %           and the (x,y,z) Cartesian coordinates of the projected points
            %
            %
            % $Revision: 1.3 $ $Date: 2005/07/12 22:16:48 $
            % Licence:  GNU GPL, no express or implied warranties
            % History:  02/2002, Darren.Weber_at_radiology.ucsf.edu
            %                    adapted from elec_fit_sph
            %
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

            % initialise centroid, unless input parameters defined
            if nargin < 4, xo = 0;end
            if nargin < 5, yo = 0;end
            if nargin < 6, zo = 0;end

            % Initialise r0 as a rough guess at the sphere radius
            rX = (max(X) - min(X)) / 2;
            rY = (max(Y) - min(Y)) / 2;
            rZ =  max(Z) - zo;
            r0 = mean([ rX rY rZ ]);

            % perform least squares estimate of spherical radius (r)
            options = optimset('fminsearch');
            r = fminsearch(@geometricTools.fit2sphere,r0, options, X, Y, Z, xo, yo, zo);

            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            % Find the projection point of X,Y,Z to the fitted sphere radius r

            % Convert Cartesian X,Y,Z to spherical (radians)
            theta = atan2( (Y-yo), (X-xo) );
            phi = atan2( sqrt( (X-xo).^2 + (Y-yo).^2 ), (Z-zo) );
            % do not recalc: r = sqrt( (X-xo).^2 + (Y-yo).^2 + (Z-zo).^2);

            %   Recalculate X,Y,Z for constant r, given theta & phi.
            R = ones(size(phi)) * r;
            x = R .* sin(phi) .* cos(theta);
            y = R .* sin(phi) .* sin(theta);
            z = R .* cos(phi);
        end
    end
    methods(Static,Hidden=true)
        %%
        function f = fit2sphere(r, X, Y, Z, xo, yo, zo)
            S = (X-xo).^2  +  (Y-yo).^2  +  (Z-zo).^2  -  r^2;
            f = sum( S.^2 );
        end
        %%
        function Cos = cosines(A,B)
            Na = size(A,1);
            Nb = size(B,1);
            One1 = ones(1,Nb);
            One2 = ones(Na,1);
            Xe = A(:,1)*One1;
            Ye = A(:,2)*One1;
            Ze = A(:,3)*One1;
            Xf = One2*B(:,1)';
            Yf = One2*B(:,2)';
            Zf = One2*B(:,3)';
            Cos = (Xe-Xf).^2 + (Ye-Yf).^2 + (Ze-Zf).^2;
            Cos = 1-Cos/2;
            Cos(Cos > 1) = 1-eps;
            Cos(Cos < -1) = -1+eps;
        end
        function plot_on_sphere(X,Y,Xi,Yi)
            [~,X(:,1), X(:,2), X(:,3)]  = geometricTools.projectOnSphere(X(:,1), X(:,2), X(:,3));
            [~,Xi(:,1),Xi(:,2),Xi(:,3)] = geometricTools.projectOnSphere(Xi(:,1),Xi(:,2),Xi(:,3));
            X  = bsxfun(@rdivide,X,sqrt(sum(X.^2,2)));
            Xi = bsxfun(@rdivide,Xi,sqrt(sum(Xi.^2,2)));

            Xt = [X;Xi];
            Yt = [Y;Yi];
            Xt = bsxfun(@rdivide,Xt,sqrt(sum(Xt.^2,2)));
            Xi = bsxfun(@rdivide,Xi,sqrt(sum(Xi.^2,2)));
            Ne = size(X,1);
            Nf = 72;
            [Xs,Ys,Zs]=sphere(Nf);
            Xsp = [Xs(:) Ys(:) Zs(:)];
            Fsp = geometricTools.localGaussianInterpolator(Xt,Xsp,0.2);

            %[J,lambdaOpt,~,iFsp] = ridgeGCV(Yt,Fsp',eye(size(Xsp,1)),100,1);
            %J = iFsp*Yt;
            Ysp = Fsp*Yt;
            figure('NumberTitle','off','Name','Electrode Placements');
            set(gca,'Projection','perspective','DataAspectRatio',[1 1 1]); hold on
            %plot3(x,y,z,'b.');
            plot3(X(:,1),X(:,2),X(:,3),'ro');
            plot3(Xi(:,1),Xi(:,2),Xi(:,3),'k.')
            legend('input xyz','projected head','Location','BestOutside');
            surf(Xs,Ys,Zs,reshape(Ysp,[Nf Nf]+1),'specularstrength',0.1,'facealpha',0.9,'linestyle','none');
            camlight
            camlight headlight
            view(2); rotate3d;
            axis vis3d
        end
    end
end


function f=comp_idgt_fac(coef,gf,L,a,M)
%COMP_IDGT_FAC  Full-window factorization of a Gabor matrix.
%   Usage:  f=comp_idgt_fac(c,g,a,M)
%
%   Input parameters:
%         c     : M x N array of coefficients.
%         gf    : Factorization of window (from facgabm).
%         a     : Length of time shift.
%         M     : Number of frequency shifts.
%   Output parameters:
%         f     : Reconstructed signal.
%
%   Do not call this function directly, use IDGT.
%   This function does not check input parameters!
%
%   If input is a matrix, the transformation is applied to
%   each column.
%
%   This function does not handle multidimensional data, take care before
%   you call it.
%
%   References: so07-2 st98-8

%   AUTHOR : Peter L. Søndergaard.
%   TESTING: OK
%   REFERENCE: OK

% Calculate the parameters that was not specified.
N=L/a;
b=L/M;

R=prod(size(gf))/L;

W=prod(size(coef))/(M*N*R);

N=L/a;
b=L/M;

[c,h_a,h_m]=gcd(a,M);
h_a=-h_a;
p=a/c;
q=M/c;
d=N/q;

ff=zeros(p,q*W,c,d,assert_classname(coef,gf));
C=zeros(q*R,q*W,c,d,assert_classname(coef,gf));
f=zeros(L,W,assert_classname(coef,gf));

% Apply ifft to the coefficients.
%coef=ifft(reshape(coef,M,N*W))*sqrt(M);
coef=ifft(coef)*sqrt(M);

% Set up the small matrices

coef=reshape(coef,M,N,R,W);

if p==1

  for rw=0:R-1
    for w=0:W-1
      for s=0:d-1
	for l=0:q-1
	  for u=0:q-1
	    C(u+1+rw*q,l+1+w*q,:,s+1)=coef((1:c)+l*c,mod(u+s*q+l,N)+1,rw+1,w+1);
	  end;
	end;
      end;
    end;
  end;
else
  % Rational oversampling
  for rw=0:R-1
    for w=0:W-1
      for s=0:d-1
	for l=0:q-1
	  for u=0:q-1
	    C(u+1+rw*q,l+1+w*q,:,s+1)=coef((1:c)+l*c,mod(u+s*q-l*h_a,N)+1,rw+1,w+1);
	  end;
	end;
      end;
    end;
  end;
end;

% FFT them
if d>1
  C=fft(C,[],4);
end;

% Multiply them
for r=0:c-1
  for s=0:d-1
    CM=reshape(C(:,:,r+1,s+1),q*R,q*W);
    GM=reshape(gf(:,r+s*c+1),p,q*R);

    ff(:,:,r+1,s+1)=GM*CM;
  end;
end;

% Inverse FFT
if d>1
  ff=ifft(ff,[],4);
end;

% Place the result
if p==1

  for s=0:d-1
    for w=0:W-1
      for l=0:q-1
	f((1:c)+mod(s*M+l*a,L),w+1)=reshape(ff(1,l+1+w*q,:,s+1),c,1);
      end;
    end;
  end;

else
  % Rational oversampling
  for w=0:W-1
    for s=0:d-1
      for l=0:q-1
	for k=0:p-1
	  f((1:c)+mod(k*M+s*p*M-l*h_a*a,L),w+1)=reshape(ff(k+1,l+1+w*q,:,s+1),c,1);
	end;
      end;
    end;
  end;

end;


function B = transform_image_LO_TO(I, corners, interporation)

% B = transform_image3(I, corners, interporation)
% calculate the transformation matrix for each area

B = nan(128,128);

if notDefined('interporation'), interporation = [];  end;

% LO1
input_points = corners{1};
% base_points =  [40 20;40 35; 80 35; 80 20];
base_points =  [30 80; 45 80; 45 40; 30 40];
udata = [1 121];  vdata = [1 121];  % input coordinate system
tform = maketform('projective',input_points, base_points);
[tmp,xdata,ydata] = imtransform(I,tform,interporation,'udata',udata,...
                                                'vdata',vdata,...
                                                'xdata',udata,...
                                                'ydata',vdata,...
                                                'size',size(I),...
                                                'fill',128);

B(round(20/120*128):round(100/120*128), round(20/120*128):round(45/120*128))=tmp(round(20/120*128):round(100/120*128),round(20/120*128):round(45/120*128));

% LO2
input_points = corners{2};
% base_points =  [40 50;40 35; 80 35; 80 50];
base_points =  [60 80; 45 80; 45 40; 60 40];
udata = [1 121];  vdata = [1 121];  % input coordinate system
tform = maketform('projective',input_points, base_points);
[tmp,xdata,ydata] = imtransform(I,tform,interporation,'udata',udata,...
                                                'vdata',vdata,...
                                                'xdata',udata,...
                                                'ydata',vdata,...
                                                'size',size(I),...
                                                'fill',128);
B(round(20/120*128):round(100/120*128), round(45/120*128):round(60/120*128))=tmp(round(20/120*128):round(100/120*128),round(45/120*128):round(60/120*128));

% TO1
input_points = corners{3};
% base_points =  [40 50;40 65; 80 65; 80 50];
base_points =  [60 80; 75 80; 75 40; 60 40];
udata = [1 121];  vdata = [1 121];  % input coordinate system
tform = maketform('projective',input_points, base_points);
[tmp,xdata,ydata] = imtransform(I,tform,interporation,'udata',udata,...
                                                'vdata',vdata,...
                                                'xdata',udata,...
                                                'ydata',vdata,...
                                                'size',size(I),...
                                                'fill',128);

B(round(20/120*128):round(100/120*128), round(60/120*128):round(75/120*128))=tmp(round(20/120*128):round(100/120*128),round(60/120*128):round(75/120*128));

% TO2
input_points = corners{4};
base_points =  [90 80; 75 80; 75 40; 90 40];
udata = [1 121];  vdata = [1 121];  % input coordinate system
tform = maketform('projective',input_points, base_points);
[tmp,xdata,ydata] = imtransform(I,tform,interporation,'udata',udata,...
                                                'vdata',vdata,...
                                                'xdata',udata,...
                                                'ydata',vdata,...
                                                'size',size(I),...
                                                'fill',128);

B(round(20/120*128):round(100/120*128), round(75/120*128):round(90/120*128))=tmp(round(20/120*128):round(100/120*128),round(75/120*128):round(90/120*128));

% TO2 side (for the control of noise average)
input_points = corners{6};
base_points =  [90 80; 105 80; 105 40; 90 40];
udata = [1 121];  vdata = [1 121];  % input coordinate system
tform = maketform('projective',input_points, base_points);
[tmp,xdata,ydata] = imtransform(I,tform,interporation,'udata',udata,...
                                                'vdata',vdata,...
                                                'xdata',udata,...
                                                'ydata',vdata,...
                                                'size',size(I),...
                                                'fill',128);

B(round(20/120*128):round(100/120*128), round(90/120*128):round(105/120*128))=tmp(round(20/120*128):round(100/120*128),round(90/120*128):round(105/120*128));


function fem2d_pack_test22 ( )

%*****************************************************************************80
%
%% TEST22 tests SPHERE_GRID_T3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  element_order = 3;
  nelemx = 8;
  nelemy = 8;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST22\n' );
  fprintf ( 1, '  SPHERE_GRID_T3_ELEMENT sets up a grid of T3 triangles\n' );
  fprintf ( 1, '    on a sphere.\n' );
  fprintf ( 1, '  SPHERE_GRID_T3_ELEMENT_NUM returns the number\n' );
  fprintf ( 1, '    of elements in the grid\n' );
  fprintf ( 1, '  SPHERE_GRID_T3_NODE_NUM returns the number\n' );
  fprintf ( 1, '    of nodes in the grid.\n' );
  fprintf ( 1, '  SPHERE_GRID_T3_NODE_XYZ returns the coordinates\n' );
  fprintf ( 1, '    of nodes in the grid.\n' );

  element_num = sphere_grid_t3_element_num ( nelemx, nelemy );
  node_num = sphere_grid_t3_node_num ( nelemx, nelemy );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Expected number of nodes =    %d\n', node_num );
  fprintf ( 1, '  Expected number of elements = %d\n', element_num );

  element_node = sphere_grid_t3_element ( nelemx, nelemy );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The elements and their nodes:\n' );
  fprintf ( 1, '\n' );

  for element = 1 : element_num
    fprintf ( 1, '%4d  ', element );
    for order = 1 : element_order
      fprintf ( 1, '%4d', element_node(order,element) );
    end
    fprintf ( 1, '\n' );
  end

  node_xyz = sphere_grid_t3_node_xyz ( nelemx, nelemy );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The node coordinates:\n' );
  fprintf ( 1, '\n' );

  for node = 1 : node_num
    fprintf ( 1, '  %4d  %12f  %12f  %12f\n', node, node_xyz(1:3,node) );
  end
%
%  Write the elements and nodes to files.
%
  r8mat_write ( 'sphere_t3_nodes.txt', 3, node_num, node_xyz );

  i4mat_write ( 'sphere_t3_elements.txt', element_order, element_num, ...
    element_node );

  return
end


function map= cmap_solarizedColors(base)
%CMAP_SOLARIZEDCOLORS - Popular colormap 'solarized' by Ethan Schoonover
%
%Synposis:
% MAP= cmap_solarizedColors(<BASE>)
%
%Input:
% BASE: [BOOL] if true, the base colors BASE03, ... are returned as colormap.
%       Otherwise the colors YELLOW, ORANGE, ..., GREEN are returned.
%Output:
% MAP: A colormap matrix of size [M 3]
%
%Example:
% clf; imagesc(toeplitz(1:8)); colorbar;
% colormap(cmap_solarizedColors);
%
%Reference:
%  http://ethanschoonover.com/solarized
%
%See also COLORMAP, HSV2RGB, CMAP_RAINBOW

% 06-2015 Benjamin Blankertz

if nargin>0 && base,
  map= [  0  43  54
          7  54  66
         88 110 117
        101 123 131
        131 148 150
        147 161 161
        238 232 213
        253 246 227]/255;
else
  map= [181 137   0;
        203  75  22;
        220  50  47;
        211  54 130;
        108 113 196;
         38 139 210;
         42 161 152;
        133 153   0]/255;
end


%typ==0 -> nav=geodetic_frame, typ==1 -> nav=wander_frame
%although this implementation can be used as wander frame mechanization,
%this is still singular as it explicitly computes the wander angle for
%transport rate computation.
%For non-singular implementation see strapdown_wander_quat (which uses
%geowander to compute earth curvature)

function [Cbn_new, Vn_new, Cen_new, h_new]=strapdown_Cen_dcm(Cbn, Vn, Cen, h, a, w, dt, typ)

%%Geo Params
[ll, wander]=dcm2llh_v000(Cen);
Cgn=[wander(2) wander(1) 0;-wander(1) wander(2) 0;0 0 1];
llh=[ll;h];
[Rn, Re, g, sL, cL, WIE_E]=geoparam_v000(llh);

wie_n=Cen*[0; 0; WIE_E];
wen_n=Cgn*[0 1/(Re+llh(3)) 0; -1/(Rn+llh(3)) 0 0;0 -sL/cL/(Re+llh(3)) 0]*Cgn'*Vn;

if (typ==1) %wander transport rate
    wen_n(3)=0;
end

%%%Update attitude
%Part I:Body frame update
rot=w*dt;
rot_norm=norm(rot);
sr_a=1-(rot_norm^2/6)+(rot_norm^4/120);
sr_b=(1/2)-(rot_norm^2/24)+(rot_norm^4/720);
mx_a=eye(3)+sr_a*skew(rot)+sr_b*skew(rot)*skew(rot);

%Part II:Nav Frame update
rot=-(wen_n+wie_n)*dt;
rot_norm=norm(rot);
sr_a=1-(rot_norm^2/6)+(rot_norm^4/120);
sr_b=(1/2)-(rot_norm^2/24)+(rot_norm^4/720);
mx_b=eye(3)+sr_a*skew(rot)+sr_b*skew(rot)*skew(rot);

Cbn_new=mx_b*Cbn*mx_a;

%%%Update Velocity
vel_inc1=(Cbn*(a*dt))+[0;0;g]*dt;
vel_inc2=(cross(Vn,2*wie_n+wen_n))*dt;
Vn_new=Vn+vel_inc1+vel_inc2;

%%%Update Cen (position + wander_angle)
rot=-wen_n*dt;
rot_norm=norm(rot);
sr_a=1-(rot_norm^2/6)+(rot_norm^4/120);
sr_b=(1/2)-(rot_norm^2/24)+(rot_norm^4/720);
mx_b=eye(3)+sr_a*skew(rot)+sr_b*skew(rot)*skew(rot);
Cen_new=mx_b*Cen;

%update height
h_new=h-Vn(3)*dt;


function [ xa, xb, fxa, fxb ] = secant ( fatol, step_max, prob, xatol, xmin, ...
  xmax, xa, xb, fxa, fxb )

%*****************************************************************************80
%
%% SECANT carries out the secant method to seek a root of F(X) = 0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 May 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real FATOL, an absolute error tolerance for the
%    function value of the root.  If an approximate root X satisfies
%      ABS ( F ( X ) ) <= FATOL, then X will be accepted as the
%    root and the iteration will be terminated.
%
%    Input, integer STEP_MAX, the maximum number of steps allowed
%    for an iteration.
%
%    Input, integer PROB, the index of the function whose root is
%    to be sought.
%
%    Input, real XATOL, an absolute error tolerance for the root.
%
%    Input, real XMAX, XMIN, the interval in which the root should
%    be sought.
%
%    Input/output, real XA, XB, two points at which the
%    function differs in sign.  On output, these values have been adjusted
%    to a smaller interval.
%
%    Input/output, real FXA, FXB, the value of the function
%    at XA and XB.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SECANT\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Step             X             F(X)\n' );
  fprintf ( 1, '\n' );

  step_num = -1;
  fprintf ( 1, '  %4d  %14g  %14g\n', step_num, xa, fxa );

  if ( abs ( fxa ) <= fatol )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Function small enough for convergence.\n' );
    return
  end

  step_num = 0;
  fprintf ( 1, '  %4d  %14g  %14g\n', step_num, xb, fxb );

  for step_num = 1 : step_max

    if ( abs ( fxb ) <= fatol )
      fprintf ( 1, '\n' );
      fprintf ( 1, '  Function small enough for convergence.\n' );
      return
    end

    if ( abs ( xa - xb ) < xatol )
      fprintf ( 1, '\n' );
      fprintf ( 1, '  Interval small enough for convergence.\n' );
      return
    end

    if ( xb < xmin || xmax < xb )
      fprintf ( 1, '\n' );
      fprintf ( 1, '  Iterate has left the region [XMIN,XMAX].\n' );
      return
    end

    if ( fxa == fxb )
      fprintf ( 1, '\n' );
      fprintf ( 1, '  F(A) = F(B), algorithm fails.\n' );
      return
    end

    xc = ( fxa * xb - fxb * xa ) / ( fxa - fxb );

    fxc = p00_fx ( prob, xc );

    xa = xb;
    fxa = fxb;
    xb = xc;
    fxb = fxc;
    fprintf ( 1, '  %4d  %14g  %14g\n', step_num, xb, fxb );

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Took maximum number of steps.\n' );

  return
end


function y = sum_square_abs( x, dim )

%SUM_SQUARE_ABS   Sum of the squares of absolute values.
%   For real arrays, SUM_SQUARE_ABS(X) computes the same result as
%   SUM_SQUARE(X). For complex arrays, SUM_SQUARE(X) first computes the
%   magnitudes of the elements of X, so it compute SUM_SQUARE_ABS(X).
%
%   Similarly, SUM_SQUARE_ABS(X,DIM) implements SUM_SQUARE(ABS(X),DIM).
%
%   Disciplined convex programming information:
%       SUM_SQUARE_ABS(X,...) is convex and nonmonotonic in X. Thus, when
%       used in CVX expressions, X must be affine. DIM must be constant.

narginchk(1,2);
y = conj( x ) .* x;
if nargin == 2,
    y = sum( y, dim );
else
    y = sum( y );
end

% Copyright 2005-2016 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function [V]=ellipseCoord3(e,t)

V=[e.radii(1).*cos(t(:)) e.radii(2).*sin(t(:)) zeros(numel(t),1)];
V=(e.axes*V')';
V=V+e.centre(ones(numel(t),1),:);

%%
% _*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/>.


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

% Cleaning
clear all
close all
clc

% Gypsilab path
run('../../addpathGypsilab.m')

% Create mesh
Nvtx = 1e3;
mesh = mshSquare(Nvtx,[2 2]);

% Colours
mesh.col(1:20)                 = -1;
mesh.col(length(mesh)+(-20:0)) = 1;

% Graphical representation
figure
plot(mesh)
hold on
plotNrm(mesh,'r')
axis equal
xlabel('X'); ylabel('Y'); zlabel('Z')

% Refine all element with midpoint algorithm
tic
meshr = midpoint(mesh);
toc

% Graphical representation
figure
plot(meshr)
hold on
plotNrm(meshr,'r')
axis equal
xlabel('X'); ylabel('Y'); zlabel('Z')

% Midpoint algorithm for selected indices
I = find(mesh.col~=0);
tic
meshr = midpoint(mesh,I);
toc

% Graphical representation
figure
plot(meshr)
hold on
plotNrm(meshr,'r')
axis equal
xlabel('X'); ylabel('Y'); zlabel('Z')

% Refinement with recursive midpoint algorithm and fixed order
ord = (1 + mesh.vtx(mesh.elt(:,1),1))/2;
ord = floor(4*ord);
tic
meshr = refine(mesh,ord);
toc

% Graphical representation
figure
plot(meshr)
hold on
plotNrm(meshr,'r')
axis equal
xlabel('X'); ylabel('Y'); zlabel('Z')

% Refinement with recursive midpoint algorithm and function order
fct = @(X) floor(5*(1+X(:,1))/2);
tic
meshr = refine(mesh,fct);
toc

% Graphical representation
figure
plot(meshr)
hold on
plotNrm(meshr,'r')
axis equal
xlabel('X'); ylabel('Y'); zlabel('Z')

% Refinement with recursive midpoint algorithm and fixed edge length
stp    = meshr.stp;
lambda = stp(2)/4;
tic
meshr = refine(meshr,lambda);
toc

% Graphical representation
figure
plot(meshr)
hold on
plotNrm(meshr,'r')
axis equal
xlabel('X'); ylabel('Y'); zlabel('Z')

disp('~~> Michto gypsilab !')


function backDiffUx = backdiffx(u)
% backward diff in x direction in equation (2.9a)
% \Delta^x_{+}u_{ij} = u_{i+1,j} - u_{ij}

rows = size(u,1);
% u([2:rows rows], :) is u_{i+1,j} in (2.9a)
backDiffUx = u([2:rows rows], :) - u;

github-repos/MATLAB/YimianDai-Image-Processing-Codes-for-Easier-Understanding/Image-Processing-Codes-for-Easier-Understanding-874302799e48852624bc3760b58b46bd9360f238/src/(Physica D 1992) Nonlinear Total Variation based noise removal algorithms/version_1/support/backdiffx.m


function ROCout=roc(varargin)
% ROC - Receiver Operating Characteristics.
% The ROC graphs are a useful tecnique for organizing classifiers and
% visualizing their performance. ROC graphs are commonly used in medical
% decision making.
%
% Syntax: ROCout=roc(x,thresholds,alpha,verbose)
%
% Input: x - This is a Nx2 data matrix. The first column is the column of the data value;
%            The second column is the column of the tag: unhealthy (1) and
%            healthy (0).
%        Thresholds - If you want to use all unique values in x(:,1)
%            then set this variable to 0 or leave it empty;
%            else set how many unique values you want to use (min=3);
%        alpha - significance level (default 0.05)
%        verbose - if you want to see all reports and plots (0-no; 1-yes by
%        default);
%
% Output: if verbose = 1
%         the ROCplots, the sensitivity and specificity at thresholds; the Area
%         under the curve with Standard error and Confidence interval and
%         comment.
%         if ROCout is declared, you will have a struct:
%         ROCout.AUC=Area under the curve (AUC);
%         ROCout.SE=Standard error of the area;
%         ROCout.ci=Confidence interval of the AUC
%         ROCout.co=Cut off points
%         ROCdata.xr and ROCdata.yr points for ROC plot
%
% USING roc WITHOUT ANY DATA, IT WILL RUN A DEMO
%
%           Created by Giuseppe Cardillo
%           giuseppe.cardillo-edta@poste.it
%
% To cite this file, this would be an appropriate format:
% Cardillo G. (2008) ROC curve: compute a Receiver Operating Characteristics curve.
% http://www.mathworks.com/matlabcentral/fileexchange/19950

%Input Error handling
args=cell(varargin);
nu=numel(args);
if isempty(nu)
    error('Warning: almost the data matrix is required')
elseif nu>4
    error('Warning: Max four input data are required')
end
default.values = {[165 1;140 1;154 1;139 1;134 1;154 1;120 1;133 1;150 1;...
146 1;140 1;114 1;128 1;131 1;116 1;128 1;122 1;129 1;145 1;117 1;140 1;...
149 1;116 1;147 1;125 1;149 1;129 1;157 1;144 1;123 1;107 1;129 1;152 1;...
164 1;134 1;120 1;148 1;151 1;149 1;138 1;159 1;169 1;137 1;151 1;141 1;...
145 1;135 1;135 1;153 1;125 1;159 1;148 1;142 1;130 1;111 1;140 1;136 1;...
142 1;139 1;137 1;187 1;154 1;151 1;149 1;148 1;157 1;159 1;143 1;124 1;...
141 1;114 1;136 1;110 1;129 1;145 1;132 1;125 1;149 1;146 1;138 1;151 1;...
147 1;154 1;147 1;158 1;156 1;156 1;128 1;151 1;138 1;193 1;131 1;127 1;...
129 1;120 1;159 1;147 1;159 1;156 1;143 1;149 1;160 1;126 1;136 1;150 1;...
136 1;151 1;140 1;145 1;140 1;134 1;140 1;138 1;144 1;140 1;140 1;159 0;...
136 0;149 0;156 0;191 0;169 0;194 0;182 0;163 0;152 0;145 0;176 0;122 0;...
141 0;172 0;162 0;165 0;184 0;239 0;178 0;178 0;164 0;185 0;154 0;164 0;...
140 0;207 0;214 0;165 0;183 0;218 0;142 0;161 0;168 0;181 0;162 0;166 0;...
150 0;205 0;163 0;166 0;176 0;],0,0.05,1};
default.values(1:nu) = args;
[x threshold alpha verbose] = deal(default.values{:});
if isvector(x)
    error('Warning: X must be a matrix')
end
if ~all(isfinite(x(:))) || ~all(isnumeric(x(:)))
    error('Warning: all X values must be numeric and finite')
end
x(:,2)=logical(x(:,2));
if all(x(:,2)==0)
    error('Warning: there are only healthy subjects!')
end
if all(x(:,2)==1)
    error('Warning: there are only unhealthy subjects!')
end
if nu>=2
    if isempty(threshold)
        threshold=0;
    else
        if ~isscalar(threshold) || ~isnumeric(threshold) || ~isfinite(threshold)
            error('Warning: it is required a numeric, finite and scalar THRESHOLD value.');
        end
        if threshold ~= 0 && threshold <3
            error('Warning: Threshold must be 0 if you want to use all unique points or >=2.')
        end
    end
    if nu>=3
        if isempty(alpha)
            alpha=0.05;
        else3
            if ~isscalar(alpha) || ~isnumeric(alpha) || ~isfinite(alpha)
                error('Warning: it is required a numeric, finite and scalar ALPHA value.');
            end
            if alpha <= 0 || alpha >= 1 %check if alpha is between 0 and 1
                error('Warning: ALPHA must be comprised between 0 and 1.')
            end
        end
    end
    if nu==4
        verbose=logical(verbose);
    end
end
clear args default nu

tr=repmat('-',1,100);
lu=length(x(x(:,2)==1)); %number of unhealthy subjects
lh=length(x(x(:,2)==0)); %number of healthy subjects
z=sortrows(x,1);
if threshold==0
    labels=unique(z(:,1));%find unique values in z
else
    K=linspace(0,1,threshold+1); K(1)=[];
    labels=quantile(unique(z(:,1)),K)';
end
labels(end+1)=labels(end)+1;
ll=length(labels); %count unique value
a=zeros(ll,2); b=a; c=zeros(ll,1);%array preallocation
ubar=mean(x(x(:,2)==1),1); %unhealthy mean value
hbar=mean(x(x(:,2)==0),1); %healthy mean value
for K=1:ll
    if hbar<ubar
        TP=length(x(x(:,2)==1 & x(:,1)>labels(K)));
        FP=length(x(x(:,2)==0 & x(:,1)>labels(K)));
        FN=length(x(x(:,2)==1 & x(:,1)<=labels(K)));
        TN=length(x(x(:,2)==0 & x(:,1)<=labels(K)));
    else
        TP=length(x(x(:,2)==1 & x(:,1)<labels(K)));
        FP=length(x(x(:,2)==0 & x(:,1)<labels(K)));
        FN=length(x(x(:,2)==1 & x(:,1)>=labels(K)));
        TN=length(x(x(:,2)==0 & x(:,1)>=labels(K)));
    end
    M=[TP FP;FN TN];
    a(K,:)=diag(M)'./sum(M); %Sensitivity and Specificity
    b(K,:)=[a(K,1)/(1-a(K,2)) (1-a(K,1))/a(K,2)]; %Positive and Negative likelihood ratio
    c(K)=trace(M)/sum(M(:)); %Efficiency
end
b(isnan(b))=Inf;

if hbar<ubar
    xroc=flipud(1-a(:,2)); yroc=flipud(a(:,1)); %ROC points
    labels=flipud(labels);
else
    xroc=1-a(:,2); yroc=a(:,1); %ROC points
end

st=[1 mean(xroc) 1]; L=[0 0 0]; U=[Inf 1 Inf];
fo_ = fitoptions('method','NonlinearLeastSquares','Lower',L,'Upper',U,'Startpoint',st);
ft_ = fittype('1-1/((1+(x/C)^B)^E)',...
     'dependent',{'y'},'independent',{'x'},...
     'coefficients',{'B', 'C', 'E'});
cfit = fit(xroc,yroc,ft_,fo_);

xfit=linspace(0,1,500);
yfit=feval(cfit,xfit);
Area=trapz(xfit,yfit); %estimate the area under the curve
%standard error of area
Area2=Area^2; Q1=Area/(2-Area); Q2=2*Area2/(1+Area);
V=(Area*(1-Area)+(lu-1)*(Q1-Area2)+(lh-1)*(Q2-Area2))/(lu*lh);
Serror=realsqrt(V);
%confidence interval
ci=Area+[-1 1].*(realsqrt(2)*erfcinv(alpha)*Serror);
if ci(1)<0; ci(1)=0; end
if ci(2)>1; ci(2)=1; end
m=zeros(1,4);
%z-test
SAUC=(Area-0.5)/Serror; %standardized area
p=1-0.5*erfc(-SAUC/realsqrt(2)); %p-value

if verbose
    %Performance of the classifier
    if Area==1
        str='Perfect test';
    elseif Area>=0.90 && Area<1
        str='Excellent test';
    elseif Area>=0.80 && Area<0.90
        str='Good test';
    elseif Area>=0.70 && Area<0.80
        str='Fair test';
    elseif Area>=0.60 && Area<0.70
        str='Poor test';
    elseif Area>=0.50 && Area<0.60
        str='Fail test';
    else
        str='Failed test - less than chance';
    end
    %display results
    disp('ROC CURVE ANALYSIS')
    disp(' ')
    disp(tr)
    str2=['AUC\t\t\tS.E.\t\t\t\t' num2str((1-alpha)*100) '%% C.I.\t\t\tComment\n'];
    fprintf(str2)
    disp(tr)
    fprintf('%0.5f\t\t\t%0.5f\t\t\t%0.5f\t\t%0.5f\t\t\t%s\n',Area,Serror,ci,str)
    disp(tr)
    fprintf('Standardized AUC\t\t1-tail p-value\n')
    if p<1e-4
        fprintf('%0.4f\t\t\t\t%0.4e',SAUC,p)
    else
        fprintf('%0.4f\t\t\t\t%0.4f',SAUC,p)
    end
    if p<=alpha
        fprintf('\t\tThe area is statistically greater than 0.5\n')
    else
        fprintf('\t\tThe area is not statistically greater than 0.5\n')
    end
    disp(' ')
    %display graph
    H=figure;
    set(H,'Position',[4 402 560 420])
    hold on
    plot([0 1],[0 1],'k');
    plot(xfit,yfit,'marker','none','linestyle','-','color','r','linewidth',2);
    H1=plot(xroc,yroc,'bo');
    set(H1,'markersize',6,'markeredgecolor','b','markerfacecolor','b')
    hold off
    xlabel('False positive rate (1-Specificity)')
    ylabel('True positive rate (Sensitivity)')
    title(sprintf('ROC curve (AUC=%0.4f)',Area))
    axis square
end

if p<=alpha
    ButtonName = questdlg('Do you want to input the true prevalence?', 'Prevalence Question', 'Yes', 'No', 'Yes');
    if strcmp(ButtonName,'Yes')
        ButtonName = questdlg('Do you want to input the true prevalence as:', 'Prevalence Question', 'Ratio', 'Probability', 'Ratio');
        switch ButtonName
            case 'Ratio'
                prompt={'Enter the Numerator or the prevalence ratio:','Enter the denominator or the prevalence ratio:'};
                name='Input for Ratio prevalence';
                Ratio=str2double(inputdlg(prompt,name));
                POD=Ratio(1)/diff(Ratio); %prior odds
            case 'Probability'
                prompt={'Enter the prevalence probability comprised between 0 and 1:'};
                name='Input for prevalence';
                pr=str2double(inputdlg(prompt,name));
                POD=pr/(1-pr); %prior odds
        end
        d=[1./(1+1./(b(:,1).*POD)) 1./(1+(b(:,2).*POD))];
        d((a(:,1)==0 & a(:,2)==1),1)=NaN;
        d((a(:,1)==1 & a(:,2)==0),2)=NaN;
        table=[labels'; a(:,1)'; a(:,2)';c';d(:,1)'.*100; d(:,2)'.*100;]';
        if verbose
            disp('ROC CURVE DATA')
            disp(tr)
            fprintf('Cut-off \tSensitivity\tSpecificity\tEfficiency\tPos.Pred.\tNeg.Pred.\n')
            fprintf('%0.2f\t\t%0.4f\t\t%0.4f\t\t%0.4f\t\t%0.2f\t\t%0.2f\n',table')
            disp(tr)
            disp(' ')
        end
    else
        table=[labels'; a(:,1)'; a(:,2)';c']';
        if verbose
            disp('ROC CURVE DATA')
            disp(tr)
            fprintf('Cut-off \tSensitivity\tSpecificity\tEfficiency\n')
            fprintf('%0.2f\t\t%0.4f\t\t%0.4f\t\t%0.4f\n',table')
            disp(tr)
            disp(' ')
        end
    end
    CSe=find(table(:,2)==max(table(:,2)),1,'first'); %Max sensitivity cut-off
    CSp=find(table(:,3)==max(table(:,3)),1,'last'); %Max specificity cut-off
    CEff=find(table(:,4)==max(table(:,4)),1,'first'); %Max efficiency cut-off
    d=realsqrt(xroc.^2+(1-yroc).^2); %apply the Pitagora's theorem
    [~,CE]=min(d); %Cost-effective cut-off
    xg=linspace(0,max(table(:,1)),500);
    st=[1 mean(table(:,1)) 1]; U=[Inf max(table(:,1)) Inf];
    fo_ = fitoptions('method','NonlinearLeastSquares','Lower',L,'Upper',U,'Startpoint',st);
    fitSe = fit(table(:,1),table(:,2),ft_,fo_);
    st=[-1 mean(table(:,1)) 1]; L=[-Inf 0 0]; U=[0 max(table(:,1)) Inf];
    fo_ = fitoptions('method','NonlinearLeastSquares','Lower',L,'Upper',U,'Startpoint',st);
    fitSp=fit(table(:,1),table(:,3),ft_,fo_);
    st=[min(table(:,4)) 1 mean(table(:,1)) max(table(:,4)) 1]; L=[0 0 0 0 0]; U=[1 Inf max(table(:,1)) 1 Inf];
    ft_ = fittype('D+(A-D)/((1+(x/C)^B)^E)','dependent',{'y'},'independent',{'x'},'coefficients',{'A', 'B', 'C', 'D', 'E'});
    fo_ = fitoptions('method','NonlinearLeastSquares','Lower',L,'Upper',U,'Startpoint',st);
    fitEff=fit(table(:,1),table(:,4),ft_,fo_);
    if verbose
        H2=figure;
        set(H2,'Position',[570 402 868 420])
        hold on
        HSE = plot(xg,feval(fitSe,xg),'marker','none','linestyle','-','color','r','linewidth',2);
        HCSe=plot([table(CSe,1) table(CSe,1)],[0 1],'marker','none','linestyle','--','color','r','linewidth',2);
        HSP = plot(xg,feval(fitSp,xg),'marker','none','linestyle','-','color','g','linewidth',2);
        HCSp=plot([table(CSp,1) table(CSp,1)],[0 1],'marker','none','linestyle','--','color','g','linewidth',2);
        HEFF = plot(xg,feval(fitEff,xg),'marker','none','linestyle','-','color','b','linewidth',2);
        HCEff=plot([table(CEff,1) table(CEff,1)],[0 1],'marker','none','linestyle','--','color','b','linewidth',2);
        HCO=plot([table(CE,1) table(CE,1)],[0 1],'marker','none','linestyle','--','color','m','linewidth',2);
        hold off
        legend([HSE HCSe HSP HCSp HCO HEFF HCEff],...
            'Sensitivity',sprintf('Max Sensitivity cutoff: %0.4f',table(CSe,1)),...
            'Specificity',sprintf('Max Specificity cutoff: %0.4f',table(CSp,1)),...
            sprintf('Cost effective cutoff: %0.4f',table(CE,1)),...
            'Efficiency',sprintf('Max Efficiency cutoff: %0.4f',table(CEff,1)),...
            'Location','BestOutside')
        axis([xg(1) xg(end) 0 1.1])

        fprintf('1) Max Sensitivity Cut-off point= %0.2f\n',table(CSe,1))
        fprintf('2) Max Specificity Cut-off point= %0.2f\n',table(CSp,1))
        fprintf('3) Cost effective Cut-off point= %0.2f\n',table(CE,1)),
        fprintf('4) Max Efficiency Cut-off point= %0.2f\n',table(CEff,1))
        m=table([CSe CSp CE CEff],1);
    end
else
    table=NaN;
end

if nargout
    ROCout.AUC=Area; %Area under the curve
    ROCout.SE=Serror; %standard error of the area
    ROCout.ci=ci; % 95% Confidence interval
    ROCout.co=m; % cut off points
    ROCout.xr=xroc; %graphic x points
    ROCout.yr=yroc; %graphic y points
    ROCout.table=table;
end


function linpack_d_test25 ( )

%*****************************************************************************80
%
%% TEST25 tests DSIFA and DSISL.
%
%  Discussion:
%
%    DSIFA and DSISL are for symmetric indefinite matrices.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2009
%
%  Author:
%
%    John Burkardt
%
  n = 100;
  lda = n;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST25\n' );
  fprintf ( 1, '  For a symmetric indefinite matrix,\n' );
  fprintf ( 1, '  DSIFA factors the matrix,\n' );
  fprintf ( 1, '  DSISL solves a factored linear system,\n' );
  fprintf ( 1, '  The matrix size is N = %d\n', n );
%
%  Assign values to the matrix A and the right hand side B.
%
  b(1:n-1) = 0.0;
  b(n)= n + 1;
%
%  Force B to be a column vector.
%
  b = b';

  a(1:n,1:n) = 0.0;

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

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

  if ( info ~= 0 )
    fprintf ( 1, '  Error!  DSIFA returns INFO = %d\n', info );
    return
  end
%
%  Solve the linear system.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Solve the linear system.\n' );

  b = dsisl ( a, lda, n, ipvt, b );
%
%  Print the result.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The first and last five entries of the solution:\n' );
  fprintf ( 1, '  (Should be 1,2,3,4,5,...,n-1,n):\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    if ( i <= 5 | n-5 < i )
      fprintf ( 1, '  %6d  %14f\n', i, b(i) );
    end
    if ( i == 5 )
      fprintf ( 1, '  ......  ..............\n' );
    end
  end

  return
end


function varargout= procutil_catFilters(varargin)
% PROCUTIL_CATFILTERS - Concatenate several filters into one
%
%Synopsis:
% [B, A]= procutil_catFilters(FILT1, <FILT2>, ...)
% HD= procutil_catFilters(FILT1, <FILT2>, ...)
%
%Arguments:
% FILTx - Filter specified as struct with fields b and a
%
%Output:
% B, A - filter coefficients
% HD   - filter specified as discrete-time filter object

sos= zeros(0, 6);
g= [];

for ii= 1:length(varargin),
  [sos0, g0]= tf2sos(varargin{ii}.b, varargin{ii}.a);
  sos= cat(1, sos, sos0);
  g= cat(1, g, g0);
end

if nargout==2,
  [b, a]= sos2tf(sos, g);
  varargout= {b, a};
elseif nargout==1,
  Hd= dfilt.df2sos(sos, g);
  varargout= {Hd};
end


% Copyright (C) Daphne Koller, Stanford University, 2012

function [num mv] = randi(arg1,arg2,arg3)

num = -1;
persistent x_i;
persistent p1;
persistent p2;
if(isempty(x_i))
  x_i = 1;
  p1 = 160481183;
  p2 = 179424673;
end
mv=p2;
if(ischar(arg1)==1)
  if(strcmp(arg1,'seed'))
    if(nargin>1)
      x_i = arg2;
      num = 0;
    else
      x_i=1;
      num=0;
    end
  else
    'Unrecognized option. The only accepted option to this random library is -seed-.'
  end
else
  if(arg1>p2)
    'Max too high, range cutoff at 1 million'
  end
  if(nargin>1)
    if(nargin==2)
      arg3=arg2;
    end
    num = zeros(arg2,arg3);
    for i=1:arg2
      for j = 1:arg3
        x_i = mod(x_i*(p1+1)+p1,p2);
        num(i,j) = mod(x_i,arg1)+1;
      end
    end
  else
    x_i = mod(x_i*(p1+1)+p1,p2);
    num=mod(x_i,arg1)+1;
  end
end


function [newVectors, whiteningMatrix, dewhiteningMatrix] = whitenv ...
    (vectors, E, D, s_verbose);
%WHITENV - Whitenv vectors.
%
% [newVectors, whiteningMatrix, dewhiteningMatrix] = ...
%                               whitenv(vectors, E, D, verbose);
%
% Whitens the data (row vectors) and reduces dimension. Returns
% the whitened vectors (row vectors), whitening and dewhitening matrices.
%
% ARGUMENTS
%
% vectors       Data in row vectors.
% E             Eigenvector matrix from function 'pcamat'
% D             Diagonal eigenvalue matrix from function 'pcamat'
% verbose       Optional. Default is 'on'
%
% EXAMPLE
%       [E, D] = pcamat(vectors);
%       [nv, wm, dwm] = whitenv(vectors, E, D);
%
%
% This function is needed by FASTICA and FASTICAG
%
%   See also PCAMAT

% @(#)$Id: whitenv.m,v 1.3 2003/10/12 09:04:43 jarmo Exp $

% ========================================================
% Default value for 'verbose'
if nargin < 4, s_verbose = 'on'; end

% Check the optional parameter verbose;
switch lower(s_verbose)
 case 'on'
  b_verbose = 1;
 case 'off'
  b_verbose = 0;
 otherwise
  error(sprintf('Illegal value [ %s ] for parameter: ''verbose''\n', s_verbose));
end

% ========================================================
% In some cases, rounding errors in Matlab cause negative
% eigenvalues (elements in the diagonal of D). Since it
% is difficult to know when this happens, it is difficult
% to correct it automatically. Therefore an error is
% signalled and the correction is left to the user.
if any (diag (D) < 0),
  error (sprintf (['[ %d ] negative eigenvalues computed from the' ...
		   ' covariance matrix.\nThese are due to rounding' ...
		   ' errors in Matlab (the correct eigenvalues are\n' ...
		   'probably very small).\nTo correct the situation,' ...
		   ' please reduce the number of dimensions in the' ...
		   ' data\nby using the ''lastEig'' argument in' ...
		   ' function FASTICA, or ''Reduce dim.'' button\nin' ...
		   ' the graphical user interface.'], ...
		  sum (diag (D) < 0)));
end

% ========================================================
% Calculate the whitening and dewhitening matrices (these handle
% dimensionality simultaneously).
whiteningMatrix = inv (sqrt (D)) * E';
dewhiteningMatrix = E * sqrt (D);

% Project to the eigenvectors of the covariance matrix.
% Whiten the samples and reduce dimension simultaneously.
if b_verbose, fprintf ('Whitening...\n'); end
newVectors =  whiteningMatrix * vectors;

% ========================================================
% Just some security...
if ~isreal(newVectors)
  error ('Whitened vectors have imaginary values.');
end

% Print some information to user
if b_verbose
  fprintf ('Check: covariance differs from identity by [ %g ].\n', ...
    max (max (abs (cov (newVectors', 1) - eye (size (newVectors, 1))))));
end


function [rowAssignments,colAssignments,blocks] = findNonInteractingRowsAndColumns(A)
% Factors a matrix into non-interacting blocks of rows and columns. Each
% block consists of the smallest set of rows and the smallest set of
% columns such that entries of the matrix outside the columns in the rows
% are zero and vice-versa.
%
% Author: Jonathan Karr
% Affiliation: Covert Lab, Depatment of Bioengineering, Stanford University
% Last updated: 11/5/2008

%Test Cases
% A = [1 0 1;
%      0 0 0;
%      1 0 0];
%
% A = [0 0 1;
%      0 1 0;
%      1 0 0];
%
% A = [1 0 1;
%      0 1 0;
%      1 0 0];

%assign rows, columns to blocks
nBlocks=0;
rowAssignments=zeros(size(A,1),1);
colAssignments=zeros(size(A,2),1);

%assign empty rows and columns to single block
for i=1:size(A,1)
    if(isempty(find(A(i,:),1)))
        nBlocks=1;
        rowAssignments(i)=nBlocks;
    end
end
for i=1:size(A,2)
    if(isempty(find(A(:,i),1)))
        nBlocks=1;
        colAssignments(i)=nBlocks;
    end
end

%assign remaining rows and columns
for i=1:size(A,1)
    if(rowAssignments(i)~=0); continue; end;
    nBlocks=nBlocks+1;
    rowAssignments(i)=nBlocks;
    [rowAssignments,colAssignments]=assignRecursively_row(A,i,nBlocks,rowAssignments,colAssignments);
end
for i=1:size(A,2)
    if(colAssignments(i)~=0); continue; end;
    nBlocks=nBlocks+1;
    colAssignments(i)=nBlocks;
    [rowAssignments,colAssignments]=assignRecursively_col(A,i,nBlocks,rowAssignments,colAssignments);
end

%assemble blocks
blocks=cell(nBlocks,1);
for i=1:nBlocks
    blocks{i}=A(rowAssignments==i,colAssignments==i);
end

% helper function
function [rowAssignments,colAssignments]=assignRecursively_row(A,row,nBlocks,rowAssignments,colAssignments)
for i=1:size(A,2)
    if(colAssignments(i)==0 && A(row,i)~=0)
        colAssignments(i)=nBlocks;
        [rowAssignments,colAssignments]=assignRecursively_col(A,i,nBlocks,rowAssignments,colAssignments);
    end
end

%helper function
function [rowAssignments,colAssignments]=assignRecursively_col(A,col,nBlocks,rowAssignments,colAssignments)
for i=1:size(A,1)
    if(rowAssignments(i)==0 && A(i,col)~=0)
        rowAssignments(i)=nBlocks;
        [rowAssignments,colAssignments]=assignRecursively_row(A,i,nBlocks,rowAssignments,colAssignments);
    end
end


function test_bug1408

% MEM 4gb
% WALLTIME 00:10:00
% DEPENDENCY ft_preprocessing preproc ft_preproc_bandpassfilter ft_preproc_bandstopfilter ft_preproc_dftfilter ft_preproc_highpassfilter ft_preproc_lowpassfilter

nchans   = 200;
nsamples = 1e6;

dat = rand(nchans,nsamples); datmean = rand(nchans,1);
tic
dat = dat - datmean(:,ones(1,nsamples));
t(1) = toc;

dat = rand(nchans,nsamples); datmean = rand(nchans,1);
tic
dat = dat - repmat(datmean,[1 nsamples]);
t(2) = toc;

dat = rand(nchans,nsamples); datmean = rand(nchans,1);
tic
for ichan = 1:nchans
  dat(ichan,:) = dat(ichan,:) - datmean(ichan);
end
t(3) = toc;

dat = rand(nchans,nsamples); datmean = rand(nchans,1);
tic
dat = dat';
for ichan = 1:nchans
  dat(:,ichan) = dat(:,ichan) - datmean(ichan);
end
dat = dat';
t(4) = toc;

dat = rand(nchans,nsamples); datmean = rand(nchans,1);
tic
dat = bsxfun(@minus, dat, datmean);
t(5) = toc;

dat = rand(nchans,nsamples); datmean = rand(nchans,1);
tic
for isample = 1:nsamples
  dat(:,isample) = dat(:, isample) - datmean;
end
t(6) = toc;

[minval, minindx] = min(t);
if minindx~=6
  warning('unexpected winner of the speed test');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

data          = [];
data.label    = {'1', '2', '3'};
data.trial{1} = rand(3,1000);
data.time{1}  = (0:999)/1000;
data.fsample  = 1000;

cfg = [];
cfg.demean = 'yes';
dataout = ft_preprocessing(cfg, data);

cfg = [];
cfg.detrend = 'yes';
dataout = ft_preprocessing(cfg, data);

cfg = [];
cfg.polyremoval = 'yes';
cfg.polyorder = 3;
dataout = ft_preprocessing(cfg, data);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
dat = rand(10,300)+ 30;
Fs  = 1000;
Fl  = 50;
Flp = 10;
Fhp = 70;
Fbp = [40 70];

filt = ft_preproc_bandpassfilter(dat,Fs,Fbp); assert(all(mean(filt,2)<1)); % the DC component should disappear
filt = ft_preproc_bandstopfilter(dat,Fs,Fbp); assert(all(mean(filt,2)>0));
filt = ft_preproc_dftfilter(dat,Fs,Fl);       assert(all(mean(filt,2)>0));
filt = ft_preproc_highpassfilter(dat,Fs,Fhp); assert(all(mean(filt,2)<1)); % the DC component should disappear
filt = ft_preproc_lowpassfilter(dat,Fs,Flp);  assert(all(mean(filt,2)>0));


function [nodes edges] = createTestGraph02(varargin)
%CREATETESTGRAPH02  Test graph for geodesic functions
%
%   [nodes edges] = createTestGraph02;
%
%   Example
%     [nodes edges] = createTestGraph02;
%     figure;
%     axis([0 100 10 90]);
%     axis equal;
%     drawGraph(nodes, edges);
%
%   See also
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2011-05-19,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2011 INRA - Cepia Software Platform.

nodes = [ ...
   10 40; ...
   10 60; ...
   20 50; ...
   40 50; ...
   50 20; ...
   50 40; ...
   50 60; ...
   50 80; ...
   60 50; ...
   80 50; ...
   90 40; ...
   90 60];

edges = [...
    1  3; ...
    2  3; ...
    3  4; ...
    4  6; ...
    4  7; ...
    5  6; ...
    6  9; ...
    7  8; ...
    7  9; ...
    9 10; ...
   10 11; ...
   10 12];


function K = squared_levenstein(kern,dat1,dat2,ind1,ind2,kerParam)
%-------- convert matrix to cell array ----
D= [];
Xf = get_x(dat1);
Xs = get_x(dat2);
X1 = {};
X2 = {};

for i = 1:size(Xf,1)
  tmp = Xf(i,:);
  X1{i} = tmp(tmp > 0);
end

for i = 1:size(Xs,1)
  tmp = Xs(i,:);
  X2{i} = tmp(tmp > 0);
end

%----- get distance matrix -----------
for i = 1:length(ind1)
    disp(['D row ' num2str(i)])
    for j = 1:length(ind2)
        D(i,j) = d(X1{i},X2{j});
    end
end
%---get kernel matrix from distance matrix--
K = [];
m = size(D);

for i = 1:length(ind1)
    disp(['D->K row ' num2str(i)])
    for j = 1:length(ind2)
        si = length(X1{i});
        sj = length(X2{j});
        p = (si + sj + D(i,j))/2;
        K(i,j) = cos(2*atan(sqrt(((p-si)*(p-sj))/(p*(p-D(i,j))))));
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function s = d(a,b)
    %----------initializing-------------
    C = zeros(length(a)+1,length(b)+1);
    C(:,1) = [0:length(a)]';
    C(1,:) = [0:length(b)];
    %------compute distance-------------
    for i = 2:size(C,1)
        for j = 2:size(C,2)
            delta = 1-abs(sign(a(i-1)-b(j-1)));
            C(i,j) = min([C(i-1,j)+1,C(i,j-1)+1,C(i-1,j-1) + 1 - delta]);
        end
    end

    s = C(end,end);


function g = log(f, pref)
%LOG   Natural logarithm of a CHEBFUN.
%   LOG(F) returns the natural logarithm of F. If F has any roots in its domain,
%   then the representation is likely to be inaccurate.
%
% See also LOG1P, LOG2, LOG10, EXP, REALLOG.

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

if ( nargin < 2 )
    pref = chebfunpref();
end

% Add breaks at the roots of f:
f = addBreaksAtRoots(f);

% Call COMPOSE():
g = compose(f, @(x) log(x), [], pref);

end


function weight = randInitializeWeights(inputs, outputs)
    % Initialize the weights randomly.
    % This will break the symmetry while training the neural network.
    epsilon = 0.12;
    weight = rand(outputs, inputs + 1) * (2 * epsilon) - epsilon;
end