function [isConsistent, m, model] = checkStoichiometricConsistency(model, printLevel, method)
% Verification of stoichiometric consistency by checking for at least one
% strictly positive basis in the left nullspace of `S`.
% If `S` is not stoichiometrically consistent detect conserved and unconserved
% metabolites, by returning a maximal conservation vector, which is
% a non-negative basis with as many strictly positive entries as possible.
% This omits rows of `S` that are entirely zero, when any exchange reactions
% are removed.
% The strictly positive and zero entries in `m` correspond to the conserved
% and unconserved metabolites respectively.
%
% Verification of stoichiometric consistency is as initially described in:
% `A. Gevorgyan, M. G. Poolman, and D. A. Fell
% Detection of stoichiometric inconsistencies in biomolecular
% models. Bioinformatics, 24(19):2245–2251, 2008`.
%
% Detection of conserved and unconserved metabolites based on a new
% implementation by Nikos Vlassis & Ronan Fleming.
%
% USAGE:
%
%    [isConsistent, m, model] = checkStoichiometricConsistency(model, printLevel, method)
%
% INPUT:
%    model:         structure with fields:
%
%                     * model.S - `m` x `n` stoichiometric matrix
%                     * model.mets - if exists, but `SIntRxnBool does not exist, then
%                       `findSExRxnBool` will attempt to identify exchange
%                       reactions. (optional)
%                     * model.SIntMetBool - `m` x 1 boolean vector indicating the metabolites that
%                       are thought to be exclusively involved in non-exchange
%                       reactions (optional)
%                     * model.SIntRxnBool  `n` x 1 boolean vector indicating the reactions that are
%                       thought to be non-exchange reactions. If this is not present,
%                       then we will attempt to identify such reactions,
%                       `model.rxns` exists, otherwise, we will assume all
%                       reactions are supposed to be non-exchange reactions. (optional)
%
% OPTIONAL INPUTS:
%    printLevel:    {(0), 1}
%    method:        structure with fields:
%
%                     * method.interface - {('SDCCO'),'LP', 'MILP', 'DCCO'} interface called to do the consistency check
%                     * method.solver - {(default solver as specified by CBT_LP_SOLVER), or any other CBT compatible LP solver}
%                     * method.param - solver specific parameter structure
%
% OUTPUTS:
%    isConsistent:        Solver status in standardized form:
%
%                     * 1 - Optimal solution (Stoichiometrically consistent)
%                     * 2 - Unbounded solution (Should never happen)
%                     * 0 - Infeasible (Stoichiometrically INconsistent)
%                     * -1 - No solution reported (timelimit, numerical problem etc)
%
%    m:             `m` x 1 strictly positive vector in left nullspace
%                   (empty if it does not exist)
%    model:         structure with fields:
%
%                     * .SConsistentMetBool - m x 1 boolean vector indicating metabolites involved
%                       in the maximal consistent vector
%                     * .SConsistentRxnBool - `n` x 1 boolean vector non-exchange reaction involving
%                       a stoichiometrically consistent metabolite
%
% .. Author: - Ronan Fleming   2012 initial coding
%                              2013 update with detection of conserved metabolites based on an algorithm by Nikos Vlassis.
%                              2014 update to omit trivial rows corresponding to S row that are all zero
%                              2022 interface with findStoichConsistentSubset

if ~exist('printLevel','var')
    printLevel=0;
end

resetSolver = 0;
if exist('method','var')
    if ~isfield(method,'interface')
        method.interface = 'SDCCO';
    end
    if isfield(method,'solver')
        global CBT_LP_SOLVER
        oldLPSolver = CBT_LP_SOLVER;
        resetSolver = 1;
        %set the solver and solver parameters
        solverOK = changeCobraSolver(method.solver,'LP');
    else
        global CBT_LP_SOLVER
        method.solver=CBT_LP_SOLVER;
    end
else
    method.interface='SDCCO';
    global CBT_LP_SOLVER
    method.solver=CBT_LP_SOLVER;
end

%set parameters according to feastol
feasTol = getCobraSolverParams('LP', 'feasTol');
epsilon = feasTol*10;

[nMet,nRxn]=size(model.S);
if ~isfield(model,'mets')
    %assume all reactions are internal
    model.SIntRxnBool=true(nRxn,1);
    intR='';
else
    if ~isfield(model,'SIntRxnBool') || ~isfield(model,'SIntMetBool')
        %Requires the openCOBRA toolbox
        model=findSExRxnInd(model);
        intR='internal reaction ';
    else
        intR='';
    end
end

% Check the stoichiometric consistency of the network by
% solving the following linear problem
%       min sum(m_i)
%           s.t     S'*m = 0
%                   m >= 1
% where l  is is a  mx1 vector of the molecular mass of m molecular species
N=model.S(:,model.SIntRxnBool);
LPproblem.A=N';
LPproblem.b=zeros(size(LPproblem.A,1),1);
LPproblem.lb=ones(size(LPproblem.A,2),1);
LPproblem.ub=inf*ones(size(LPproblem.A,2),1);
LPproblem.c=1*ones(size(LPproblem.A,2),1);
LPproblem.osense=1;
LPproblem.csense(1:size(LPproblem.A,1),1)='E';

%Requires the openCOBRA toolbox
solution = solveCobraLP(LPproblem,'printLevel',printLevel);

%OUTPUT
% solution Structure containing the following fields describing a LP
% solution
%  full     Full LP solution vector
%  obj      Objective value
%  rcost    Reduced costs
%  dual     Dual solution
%  solver   Solver used to solve LP problem
%
%  stat     Solver status in standardized form
%            1   Optimal solution
%            2   Unbounded solution
%            0   Infeasible
%           -1   No solution reported (timelimit, numerical problem etc)

isConsistent=solution.stat;

% If the network is not stoichiometrically consistent then one maximizes
% the number of  positive component of the molecular masses vector
if isConsistent~=1
    switch method.interface
        case 'none'
            warning(['Stoichiometrically INconsistent ' intR 'stoichiometry.']);
        case 'SDCCO'
            tic
            massBalanceCheck=0;
            fileName=[];
%             [SConsistentMetBool, SConsistentRxnBool, SInConsistentMetBool, SInConsistentRxnBool, unknownSConsistencyMetBool, unknownSConsistencyRxnBool, model, stoichConsistModel] =...
%                 findStoichConsistentSubset(model, massBalanceCheck, printLevel, fileName, epsilon)
             [~, ~, ~, ~, ~, ~, model, ~] = findStoichConsistentSubset(model, massBalanceCheck, printLevel,fileName, epsilon);
            %TODO
            m = 1*model.SConsistentMetBool;
            solution = [];
            timetaken=toc;
        case 'cvx'
            cvx_solver(method.solver)

            [nMet,~]=size(N);
            %maximal conservation vector
            %cvx code from Nikos Vlassis
            tic
            cvx_begin quiet

            variable m(nMet);
            variable z(nMet);

            maximize( ones(1,nMet) * z );

            z>=0; z<=epsilon;

            m>=z; m<=(1/epsilon);

            N'*m==0;

            cvx_end
            timetaken=toc;
            if any(isnan(m))
                error('NaN in maximal conservation vector')
            end
            %boolean indicating metabolites involved in the maximal consistent vector
            model.SConsistentMetBool=m>epsilon & model.SIntMetBool;
        case 'LP'

            [nMet,~]=size(N);

            % Solve the linear problem
            %   max sum(z_i)
            %       s.t S'*m = 0
            %           z <= m
            %           0 <= m <= 1/epsilon
            %           0 <= z <= epsilon
            nInt=nnz(model.SIntRxnBool);
            LPproblem.A=[N'      , sparse(nInt,nMet);
                         speye(nMet),      -speye(nMet)];

            LPproblem.b=zeros(nInt+nMet,1);

            LPproblem.lb=[zeros(nMet,1);zeros(nMet,1)];
            LPproblem.ub=[ones(nMet,1)*(1/epsilon);ones(nMet,1)*epsilon];

            LPproblem.c=zeros(nMet+nMet,1);
            LPproblem.c(nMet+1:2*nMet,1)=1;
            LPproblem.osense=-1;
            LPproblem.csense(1:nInt,1)='E';
            LPproblem.csense(nInt+1:nInt+nMet,1)='G';

            %Requires the COBRA toolbox
            tic
            if isfield(method,'param')
                solution = solveCobraLP(LPproblem,'printLevel',printLevel-1,method.param);
            else
                solution = solveCobraLP(LPproblem,'printLevel',printLevel-1);
            end
            timetaken=toc;

            if solution.stat==1
                m=solution.full(1:nMet,1);
                z=solution.full(nMet+1:end,1);
                if isfield(model,'SIntMetBool') && 0
                    %boolean indicating metabolites involved in the maximal consistent vector
                    model.SConsistentMetBool=m>epsilon & model.SIntMetBool;
                else
                    %boolean indicating metabolites involved in the maximal consistent vector
                    model.SConsistentMetBool=m>epsilon;
                end
            else
                disp(solution)
                error('solve for maximal conservation vector failed')
            end

            %mosek
            algorithm=solution.algorithm;
            %change back the solver
%            solverOK = changeCobraSolver(oldSolver,'LP');
        case 'DCCO'

            tic
            [~, ~, solution] = maxCardinalityConservationVector(N);
            timetaken=toc;
            method.solver='cappedL1';
            if solution.stat==1
                m = solution.l;
                %dummy z
                z = zeros(nMet,1);
                if isfield(model,'SIntMetBool')  && 0
                    %boolean indicating metabolites involved in the maximal consistent vector
                    model.SConsistentMetBool=m>epsilon & model.SIntMetBool;
                else
                    %boolean indicating metabolites involved in the maximal consistent vector
                    model.SConsistentMetBool=m>epsilon;
                end
            else
                disp(solution)
                error('solve for maximal conservation vector failed')
            end
        case 'MILP'
            [nMet,~]=size(N);

            % Solve the MILP problem
            %   max sum(z_i)
            %       s.t S'*m = 0
            %           z <= m
            %           z binary
            nInt=nnz(model.SIntRxnBool);
            MILPproblem.A=[N'      , sparse(nInt,nMet);
                         speye(nMet),      -speye(nMet)];

            MILPproblem.b=zeros(nInt+nMet,1);

            MILPproblem.lb=[zeros(nMet,1);zeros(nMet,1)];
            MILPproblem.ub=[ones(nMet,1)*(1/epsilon);ones(nMet,1)];

            MILPproblem.c=zeros(nMet+nMet,1);
            MILPproblem.c(nMet+1:2*nMet,1)=1;
            MILPproblem.osense=-1;
            MILPproblem.csense(1:nInt,1)='E';
            MILPproblem.csense(nInt+1:nInt+nMet,1)='G';
            MILPproblem.vartype(1:nMet,1)='C';
            MILPproblem.vartype(nMet+1:nMet+nMet,1)='B';
            MILPproblem.x0 = zeros(nMet+nMet,1);

%             solverOK = changeCobraSolver('gurobi','MILP');
            %Requires the COBRA toolbox
            tic
            if isfield(method,'param')
                solution = solveCobraMILP(MILPproblem,'printLevel',printLevel-1,method.param);
            else
                solution = solveCobraMILP(MILPproblem,'printLevel',printLevel-1);
            end
            timetaken=toc;

            if solution.stat==1
                m=solution.full(1:nMet,1);
                z=solution.full(nMet+1:end,1);
                if isfield(model,'SIntMetBool')  && 0
                    %boolean indicating metabolites involved in the maximal consistent vector
                    model.SConsistentMetBool=m>epsilon & model.SIntMetBool;
                else
                    %boolean indicating metabolites involved in the maximal consistent vector
                    model.SConsistentMetBool=m>epsilon;
                end
            else
                disp(solution)
                error('solve for maximal conservation vector failed')
            end
        case 'maxEnt'
            %does not work very well
            tic
            m=maxEntConsVector(N,printLevel);
            timetaken=toc;
            if isfield(model,'SIntMetBool')  && 0
                %boolean indicating metabolites involved in the maximal consistent vector
                model.SConsistentMetBool=m>epsilon & model.SIntMetBool;
            else
                %boolean indicating metabolites involved in the maximal consistent vector
                model.SConsistentMetBool=m>epsilon;
            end
            z=zeros(nMet,1);
        otherwise
            error(['unregognised method.interface = ' method.interface]);
    end
    if any(m < -feasTol)
        error('m should be greater than or equal to zero')
    end
    m(m<0)=0;
    if printLevel>0
        if isfield(method,'param')
            fprintf('%s%s%s%s%s%s%s%g%s\n','Maximal conservation vector, using ', method.interface, ' ', method.solver,' ',algorithm,', in time ',timetaken,' sec.')
        else
            fprintf('%s%s%s%s%s%g%s\n','Maximal conservation vector, using ', method.interface, ' ', method.solver,', in time ',timetaken,' sec.')
        end
        fprintf('%10f%s\n',ones(1,nMet) * m,' = Optimal objective (i.e. 1''*m)')
        fprintf('%10d%s\n', nnz(model.SConsistentMetBool),' = Number of stoichiometrically consistent rows')
        fprintf('%10g%s\n',norm(m'*N),' = || S''*m ||_inf for non-exchange reactions of S')
    end
else
    m=solution.full;
    %The only consistent rows are those corresponding to non-exchange reactions
    model.SConsistentMetBool=model.SIntMetBool;
    if printLevel>0
        fprintf('%s\n','--- Summary of stoichiometric consistency ----')
        fprintf('%6s\t%6s\n','#mets','#rxns')
        fprintf('%6u\t%6u\t%s\n',nMet,nRxn,' totals.')
        fprintf('%6u\t%6u\t%s\n',nnz(~model.SIntMetBool),nnz(~model.SIntRxnBool),' heuristically external.')
        fprintf('%6u\t%6u\t%s\n',nnz(model.SIntMetBool),nnz(model.SIntRxnBool),' heuristically internal:')
        fprintf('%6u\t%6u\t%s\n',nnz(model.SConsistentMetBool),nnz(model.SIntRxnBool),' ... of which are stoichiometrically consistent.')
    end
end

isConsistent = all(model.SConsistentMetBool==1);

if ~isfield(model,'SConsistentRxnBool')

    %OLD - incorrect way July 14th 2016 - Ronan.
    % %find every non-exchange reaction involving a stoichiometrically consistent metabolite
    % model.SConsistentRxnBool =(sum(model.S(model.SConsistentMetBool,:)~=0,1)~=0)';
    % model.SConsistentRxnBool(~model.SIntRxnBool)=0;

    %corresponding reactions exclusively involving consistent metabolites
    model.SConsistentRxnBool = ~any(model.S(~model.SConsistentMetBool, :), 1)' & model.SIntRxnBool;

    model.SConsistentRxnBool = getCorrespondingCols(model.S,model.SConsistentMetBool,model.SIntRxnBool,'inclusive');

end

if resetSolver
    %reset the solver
    solverOK = changeCobraSolver(oldLPSolver,'LP');
end


function pde = elasticitydata(para)
%% ELASTICITYDATA data for elasticity problem
%
% - mu \Delta u - (mu + lambda) grad(div u) = f    in \Omega
%                                         u = g_D  on \partial \Omega
%
% Created by Huayi Wei Monday, 27 June 2011.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if nargin == 0
    lambda = 1;
    mu = 1;
else
    if ~isstruct(para)
        exit('we need a struct data');
    end
    if ~isfield(para,'lambda') || isempty(para.lambda)
        lambda = 1;
    else
        lambda = para.lambda;
    end
    if ~isfield(para,'mu') || isempty(para.mu)
        mu = 1;
    else
        mu = para.mu;
    end
end

pde = struct('lambda',lambda,'mu',mu, 'f', @f, 'exactu',@exactu,'g_D',@g_D);
%%%%%% subfunctions %%%%%%
    function z = f(p)
       z = mu*2*pi^2.*exactu(p);
    end
    function z = exactu(p)
       x = p(:,1); y = p(:,2);
       z = [cos(pi*x).*cos(pi*y), sin(pi*x).*sin(pi*y)];
    end
    function z = g_D(p)
       z = exactu(p);
    end
end


function [K, dK_logtheta, dK_x2] = gpcovPeriodic(logtheta, x1, x2, distfun)

if nargin == 0
  % Return the number of hyperparameters
  K = 2;
  return
end

if nargin < 3
  x2 = [];
end
if nargin < 4
%  distfun = @sqdistEuclidean;
end

% 2-norm distances
if ~isempty(x2)
  D = bsxfun(@minus, x1(:), x2(:)');
  %dD_dx2 = reshape(-D, [1,length(x1),length(x2)]);
%  [D, dD_dx2] = feval(distfun, x1, x2, false);
else
  % Distances for variances
  n = cols(x1);
  D = zeros(n,1);
end

% TODO:
% helper, inverse squared length scale:
invl2 = exp(-2*logtheta(1));
wavel = exp(logtheta(2));

% Covariance matrix

angle = pi*D/wavel;
exponent = -2*sin(angle).^2*invl2;
K = exp(exponent);

% Gradient for hyperparameters
if nargout >= 2
  dK_logtheta = zeros([size(D), 2]);
  dK_logtheta(:,:,1) = K .* exponent .* (-2);
  dK_logtheta(:,:,2) = K.*(-2*invl2*2*sin(angle)).*cos(angle).*(-pi*D/wavel);
end

% Gradients for inputs x2
if nargout >= 3
  if isempty(x2)
    error('Can''t calculate gradient: x2 not given');
  end
  d = rows(x2); % dimensionality of inputs
  m = cols(x1); % number of other inputs
  n = cols(x2); % number of inputs
  dK_x2 = zeros([d,m,n]);
  dK_x2 = bsxfun(@times, ...
                 reshape(K,[1,m,n]), ...
                 reshape((-2*invl2*2*sin(angle)) .* ...
                 cos(angle) .* pi/wavel .* (-1), [1,m,n]));
% $$$   for j=1:n
% $$$     dK_x2(:,:,j) = bsxfun(@times, K(:,j)', (-0.5*invl2) * dD2_dx2(:,:,j));
% $$$     %2*bsxfun(@minus, x2(:,j),x1));
% $$$   end
end


function hrf = boyntonHIRF(t, n, tau, delay)
%
%    hrf = boyntonHIRF(t, [n=3], [tau=1.08], [delay=2.05]])
%
%Purpose:
%   Compute the Boynton et al. HIRF function.  The return values include
% both the hrf and the values of time and the parameters used in the
% computation.
%
% Equation:  (Eq 3 from Boynton & Heeger, J Neurosci 1996)
%   h(t) = [(t/tau) ^ (n-1) * exp(-t/tau)] / [tau(n-1)!]
%
% Inputs:
%   t: t should be in units of SECONDS, and reflect the time window in
%   which to estimate the HRF.
%
%   n: exponent in eq. above. (corresponds to z in standard gamma
%   functions).
%
%   tau: time constant in eq.
%
%   delay: additional delay before onset of gamma function. This is an
%   added heuristic, which may vary from subject to subject.
%
%
% Outputs:
%   hrf: estimate of the HRF sampled at t seconds.
%   IMPORTANT: If you are going to convolve
%   this for fMRI analysis, and the TR of your data is not 1, you will
%   need to resample both the input t and hrf to match the MR frames.
%
% written 2005 by wandell.
% ras, 01/2007: heavily modified: no parms struct, each arg can be
% specified separately; clarifies difference between seconds and frames,
% doesn't modify t.
if notDefined('n'),     n = 3;          end
if notDefined('tau'),   tau = 1.08;     end
if notDefined('delay'), delay = 2.05;   end

% ras 01/07: this is sometimes nice, but let's just turn it off
% altogether for now
% verbose = prefsVerboseCheck;
% if verbose,
%     disp('Boynton HIRF')
% end

% initialize the HRF to be zeros, the same size as t:
hrf = zeros( size(t) );

% The HRF is not specified for t < 0 secs. In addition, any
% values before the delay should also be zero.
% So, we only sample the HRF below, for time points after [0+delay].
% We call this sampling vector x to distinguish it from t.
x = t - delay;
x = x(x>0);  % not defined for x<0

% main computation (per Boynton & Heeger, 1996)
tmpHrf = (x/tau).^(n-1) .* exp(-(x/tau)) / (tau*(factorial(n-1)));

% paste in tmpHRF into appropriate indices in hrf, corresponding
% to the (shifted after delay) time points:
hrf(end-length(tmpHrf)+1:end) = tmpHrf;

return;


function [X]=missmult(A,B)

%MISSMULT product of two matrices containing NaNs
%
%[X]=missmult(A,B)
%This function determines the product of two matrices containing NaNs
%by finding X according to
%     X = A*B
%If there are columns in A or B that are pur missing values,
%then there will be entries in X that are missing too.
%
%The result is standardized, that is, corrected for the lower
%number of contributing terms.
%
%Missing elements should be denoted by 'NaN's

% Copyright (C) 1995-2006  Rasmus Bro & Claus Andersson
% Copenhagen University, DK-1958 Frederiksberg, Denmark, rb@life.ku.dk
%
% This program is free software; you can redistribute it and/or modify it under
% the terms of the GNU General Public License as published by the Free Software
% Foundation; either version 2 of the License, or (at your option) any later version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more details.
% You should have received a copy of the GNU General Public License along with
% this program; if not, write to the Free Software Foundation, Inc., 51 Franklin
% Street, Fifth Floor, Boston, MA  02110-1301, USA.

%INBOUNDS
%REALONLY

[ia ja]=size(A);
[ib jb]=size(B);
X=zeros(ia,jb);

one_arry=ones(ia,1);
for j=1:jb,
   p=one_arry*B(:,j)';
   tmpMat=A.*p;
   X(:,j)=misssum(tmpMat')';
end;


function err = getHcurlerror3NE(node,elem,curlE,Eh,markedElem)
%% GETHCURLERROR3NE Hcurl norm of approximation error for the lowest order Nedelect element in 3-D.
%
% err = getHcurlerror3NE(node,elem,curlE,Eh,markedElem);
%
% Example
%
%     node = [-1,-1,-1; 1,-1,-1; 1,1,-1; -1,1,-1; -1,-1,1; 1,-1,1; 1,1,1; -1,1,1];
%     elem = [1,2,3,7; 1,6,2,7; 1,5,6,7; 1,8,5,7; 1,4,8,7; 1,3,4,7];
%     maxIt = 4;
%     HcurlErr = zeros(maxIt,1);
%     N = zeros(maxIt,1);
%     for k = 1:maxIt
%         [node,elem] = uniformbisect3(node,elem);
%         [elem2dof,edge] = dof3edge(elem);
%         pde = Maxwelldata2;
%         uI = edgeinterpolate(pde.exactu,node,edge);
%         HcurlErr(k) = getHcurlerror3NE(node,elem,pde.curlu,uI);
%         N(k) = length(uI);
%     end
%     r = showrate(N,HcurlErr,1,'b-+');
%     legend('||u-u_I||_{curl}',['N^{' num2str(r) '}'],'LOCATION','Best');
%
% See also getHcurlerror3NE1, getHcurlerror3NE2, getL2error3NE
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

%% Sort elem to ascend ordering
elem = sort(elem,2);

%% Construct Data Structure
elem2dof = dof3edge(elem);
NT = size(elem,1);% Ndof = max(elem2dof(:)); %N = size(node,1);
% compute curl phi
[Dlambda,volume] = gradbasis3(node,elem);
% curl phi = 2*Dlambda_i cross Dlambda_j;
curlPhi(:,:,6) = 2*cross(Dlambda(:,:,3),Dlambda(:,:,4),2);
curlPhi(:,:,1) = 2*cross(Dlambda(:,:,1),Dlambda(:,:,2),2);
curlPhi(:,:,2) = 2*cross(Dlambda(:,:,1),Dlambda(:,:,3),2);
curlPhi(:,:,3) = 2*cross(Dlambda(:,:,1),Dlambda(:,:,4),2);
curlPhi(:,:,4) = 2*cross(Dlambda(:,:,2),Dlambda(:,:,3),2);
curlPhi(:,:,5) = 2*cross(Dlambda(:,:,2),Dlambda(:,:,4),2);
curlEhp = zeros(NT,3);
for k = 1:6
    curlEhp = curlEhp + ...
    repmat(Eh(elem2dof(:,k)),1,3).*curlPhi(:,:,k);
end

%% compute Hcurl error element-wise
[lambda,w] = quadpts3(2);
nQuad = size(lambda,1);
err = zeros(NT,1);
for p = 1:nQuad
    % quadrature points in the x-y-z coordinate
    pxy = lambda(p,1)*node(elem(:,1),:) ...
        + lambda(p,2)*node(elem(:,2),:) ...
        + lambda(p,3)*node(elem(:,3),:) ...
        + lambda(p,4)*node(elem(:,4),:);
    if isnumeric(curlE) % a constant vector
        curlEp = repmat(curlE,NT,1);
    else % function handel
        curlEp = curlE(pxy);
    end
%     curlEp = curlE(pxy);
    % compute Ehp at quadrature points
    err = err + w(p)*sum((curlEp - curlEhp).^2,2);
end
err = err.*volume;
% modify the error
err(isnan(err)) = 0; % remove the singular part
if (nargin == 5) && ~isempty(markedElem)
    err = err(markedElem); % error on some marked region
end
err = sqrt(sum(err));
%% TODO write more M-lint


% TESTFITLINE - demonstrates RANSAC line fitting
%
% Usage: testfitline(outliers, sigma, t, feedback)
%
% Arguments:
%               outliers - Fraction specifying how many points are to be
%                          outliers.
%               sigma    - Standard deviation of inlying points from the
%                          true line.
%               t        - Distance threshold to be used by the RANSAC
%                          algorithm for deciding whether a point is an
%                          inlier.
%               feedback - Optional flag 0 or 1 to turn on RANSAC feedback
%                          information.
%
%  Try using:  testfit3Dline(0.3, 0.05, 0.05)
%
% See also: RANSACFITPLANE, FITPLANE

% Copyright (c) 2003-2006 Peter Kovesi and Felix Duvallet (CMU)
% School of Computer Science & Software Engineering
% The University of Western Australia
% http://www.csse.uwa.edu.au/
%
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in
% all copies or substantial portions of the Software.
%
% The Software is provided "as is", without warranty of any kind.

% August 2006  testfitline created from testfitplane
%              author: Felix Duvallet

function testfit3Dline(outliers, sigma, t, feedback)

    close all;

    if nargin == 3
        feedback = 0;
    end

    % Hard wire some constants - vary these as you wish

    npts = 100;  % Number of 3D data points

    % Define a line:
    %    Y = m*X
    %    Z = n*X + Y + b
    % This definition needs fixing, but it works for now

    m = 6;
    n = -3;
    b = -4;

    outsigma = 30*sigma;  % outlying points have a distribution that is
                          % 30 times as spread as the inlying points

    vpts = round((1-outliers)*npts);  % No of valid points
    opts = npts - vpts;               % No of outlying points

    % Generate npts points in the line
    X = rand(1,npts);

    Y = m*X;
    Z = n*X + Y + b;
    Z = zeros(size(Y));

    XYZ =  [X
    	    Y
    	    Z];

    % Add uniform noise of +/-sigma
    XYZ = XYZ + (2*rand(size(XYZ))-1)*sigma;

    % Generate opts random outliers

    n = length(XYZ);
    ind = randperm(n);  % get a random set of point indices
    ind = ind(1:opts);  % ... of length opts

    % Add uniform noise of outsigma to the points chosen to be outliers.
    XYZ(:,ind) = XYZ(:,ind)  +   sign(rand(3,opts)-.5).*(rand(3,opts)+1)*outsigma;

    % Perform RANSAC fitting of the line
    [V, P, inliers] = ransacfit3Dline(XYZ, t, feedback);

    if(feedback)
        disp(['Number of Inliers: ' num2str(length(inliers)) ]);
    end

    % We want to plot the inlier points blue, with the outlier points in
    % red.  In order to do that, we must find the outliers.
    % Use setxor on all the points, and the inliers to find outliers
    %  (plotting all the points in red and then plotting over them in blue
    %  does not work well)
    oulier_points = setxor(transpose(XYZ), transpose(XYZ(:, inliers)), 'rows');
    oulier_points = oulier_points';

    % Display the cloud of outlier points
    figure(1); clf
    hold on;
    plot3(oulier_points(1,:),oulier_points(2,:),oulier_points(3,:), 'r*');

    % Plot the inliers as blue points
    plot3(XYZ(1,inliers), XYZ(2, inliers), XYZ(3, inliers), 'b*');

    % Display the line formed by the 2 points that gave the
    % line of maximum consensus as a green line
    line(P(1,:), P(2,:), P(3,:), 'Color', 'green', 'LineWidth', 4);

    %Display the line formed by the covariance fitting in magenta
    line(V(1,:), V(2, :), V(3,:), 'Color', 'magenta', 'LineWidth', 5);

    box('on'), grid('on'), rotate3d('on')


function window = mandelbrotViewer()
%mandelbrotViewer  view and explore the Mandelbrot set using a GPU
%
%   mandelbrotViewer() opens a MATLAB figure window showing the Mandelbrot
%   set. Use the usual zoom and pan controls from the figure window toolbar
%   to navigate around and explore, or click "animate" to see a pre-defined
%   path through the set. You can move back to the initial view at any time
%   by clicking the "reset" button or add the current view to the animation
%   list using "add".
%
%   The control panel can be hidden using the right-hand toolbar button.
%
%   A selector allows a choice of four ways to calculate each frame:
%
%   1. CPU: All calculations are performed by MATLAB on the host CPU. The
%   algorithm is fully vectorized and avoids indexing to give an efficient
%   calculation. Even so, this may take a few seconds to calculate each
%   frame.
%
%   2. GPU (simple): The same algorithm as (1) is used but the input
%   coordinates are switched to being stored on the GPU. This causes
%   MATLAB to operate on the resulting data array on the GPU with no other
%   code change. This gives some speedup at virtually no coding cost.
%
%   3. GPU Arrayfun: Now we change the code so that instead of many
%   operations running on the full data matrix, we now specify a single
%   "calculateElement" operation and run it for each element. Roughly
%   speaking, MATLAB translates one element into one thread on the GPU,
%   giving huge speedups for large arrays. Note that all code is still
%   written in MATLAB and the user needs no knowledge of how GPU kernels
%   are constructed and executed.
%
%   4. CUDAKernel: Taking things to the limit, we now hand-craft the
%   element-wise algorithm used in (3) in CUDA C++. The resulting kernel is
%   called from MATLAB using the CUDAKernel system, requiring the user to
%   specify the thread and block arrangements to use.
%
%   Note that version 3 gets us most of the speedup achieved by the
%   hand-crafted CUDA (version 4) but without any need to leave the comfort
%   of MATLAB!
%
%   See also:  gpuArray, mandelbrotViewerProcessElement

%   Copyright 2010-2011 The Mathworks, Inc.

% Check that we are running in R2011a or above and have a GPU
matlabVersionCheck();
gpuCheck();

% Define some global (to this file) data structures so that they can be
% used by all the helper functions.
data = createData();
gui = createGUI();
% Make sure the image is updated now that the window is onscreen
redraw();

% Return the window handle if requested
if nargout
    window = gui.Window;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Here are the four different Mandelbrot Set computations

    function logCount = computeMandelbrotCPU( xlim, numx, ylim, numy, maxIters )
        % Create a view of the Mandelbrot set using only the CPU.
        % This is the base-line version of the algorithm that is adapted to
        % run on the GPU in different ways in the functions below.

        % Create the input arrays
        escapeRadius2 = 400; % Square of escape radius
        x = linspace( xlim(1),  xlim(2), numx );
        y = linspace( ylim(1),  ylim(2), numy );
        [x0,y0] = meshgrid(x, y);
        count = zeros( size( x0 ) );
        z0 = complex( x0, y0 );
        z = z0;

        % Calculate
        for n = 0:maxIters
            inside = ((real(z).^2 + imag(z).^2) <= escapeRadius2);
            count = count + inside;
            z = inside.*(z.*z + z0) + (1-inside).*z;
        end
        magZ2 = real(z).^2 + imag(z).^2;
        logCount = log( count + 1 - log( log( max(magZ2,escapeRadius2) ) / 2 ) / log(2) );
    end % computeMandelbrotCPU

    function logCount = computeMandelbrotGPU( xlim, numx, ylim, numy, maxIters )
        % Compute using GPUArray overloads.
        % In this version the main calculation is exactly as it was for the
        % CPU version, we have simply changed the input grid to be on the
        % GPU. When MATLAB encounters GPU data it tries to run any
        % functions on the GPU. This provides a simple way to see if the
        % GPU helps without altering your code.

        % Setup the input grid on the GPU
        escapeRadius2 = 400; % Square of escape radius
        x = parallel.gpu.GPUArray.linspace( xlim(1),  xlim(2), numx );
        y = parallel.gpu.GPUArray.linspace( ylim(1),  ylim(2), numy );
        count = parallel.gpu.GPUArray.zeros( numy, numx );
        [x0,y0] = meshgrid(x, y);
        z0 = complex( x0, y0 );

        % Calculate
        z = z0;
        for n = 0:maxIters
            inside = ((real(z).^2 + imag(z).^2) <= escapeRadius2);
            count = count + inside;
            z = inside.*(z.*z + z0) + (1-inside).*z;
        end
        magZ2 = real(z).^2 + imag(z).^2;
        logCount = log( count + 1 - log( log( max(magZ2,escapeRadius2) ) / 2 ) / log(2) );

        % Gather the result back to the CPU
        logCount = gather( logCount );
    end % computeMandelbrotGPU

    function logCount = computeMandelbrotArrayFun( xlim, numx, ylim, numy, maxIters )
        % Compute using GPU arrayfun.
        % The second way in which MATLAB can use the GPU is by placing your
        % algorithm inside a helper function and calling it using ARRAYFUN
        % with some GPU data as input. The helper function is converted
        % into native GPU code (PTX) and each element of the input array is
        % processed in a separate GPU thread. The helper function must
        % operate only one scalars. Here we have taken the "calculate" code
        % above and put it in a helper "mandelbrotViewerProcessElement"
        % that will be converted into native code and run on the GPU.

        % Create the input arrays
        escapeRadius2 = 400;
        x = parallel.gpu.GPUArray.linspace( xlim(1),  xlim(2), numx );
        y = parallel.gpu.GPUArray.linspace( ylim(1),  ylim(2), numy );
        [x0,y0] = meshgrid(x, y);

        % Calculate
        [logCount] = arrayfun( @mandelbrotViewerProcessElement, x0, y0, ...
            escapeRadius2, maxIters );

        % Gather the result back to the CPU
        logCount = gather( logCount );
    end % computeMandelbrotCPU

    function logCount = computeMandelbrotCUDAKernel( xlim, numx, ylim, numy, maxIters )
        % Use pre-existing CUDA/C++ code.
        % The final way in which MATLAB can use the GPU is by calling some
        % hand-written CUDA code. The "CUDAKernel" interface allows the
        % function to be specified along with the number of threads and
        % blocks to use. This requires some knowledge of how GPUs work, but
        % does allow you to easily use existing CUDA kernels with MATLAB
        % data.

        % Create the input arrays
        escapeRadius = 20;
        x = parallel.gpu.GPUArray.linspace( xlim(1),  xlim(2), numx );
        y = parallel.gpu.GPUArray.linspace( ylim(1),  ylim(2), numy );
        [x0,y0] = meshgrid(x, y);

        % Make sure we have sufficient blocks to cover the whole array
        numElements = numel( x0 );
        data.Kernel.ThreadBlockSize = [data.Kernel.MaxThreadsPerBlock,1,1];
        data.Kernel.GridSize = [ceil(numElements/data.Kernel.MaxThreadsPerBlock),1];

        % Call the kernel
        logCount = parallel.gpu.GPUArray.zeros( size( x0 ) );
        logCount = feval( data.Kernel, logCount, ...
            x0, y0, ...
            escapeRadius, maxIters, numElements );
        logCount = gather( logCount );
    end % computeMandelbrotCUDAKernel

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Everything else is a callback or helper function.

    function out = createData()
        out = struct( ...
            'MaxIterations', 5000, ...
            'OrigXLim', [-2 1], ...
            'OrigY', 0, ...
            'XLim', [-2 1], ...
            'Y', 0, ...
            'CalculationMethods', {{
            'CPU'
            'GPU (simple)'
            'GPU ArrayFun'
            'CUDAKernel'
            }}, ...
            'IsAnimating', false, ...
            'NextLocation', 1, ...
            'LastFrameTime', now(), ...
            'WindowPixelSize', [100 100], ...
            'SelectedCalculationMethod', 'CUDAKernel', ...
            'ControlsVisible', true, ...
            'WriteVideo', false, ...
            'VideoWriter',  [] );

        % open writer
        if out.WriteVideo
            out.VideoWriter = VideoWriter('out.avi');
            out.VideoWriter.FrameRate = 20;
            out.VideoWriter.Quality = 90;
            open( out.VideoWriter );
        end

        % Read the location list from a local file
        out.LocationList = readLocationList();

        % Load the CUDA kernel
        out.Kernel = loadKernel();
    end % createData

    function out = createGUI()
        % Create the GUI, storing handles in the global GUI structure
        out.Window = figure( ...
            'Name', 'Mandelbrot viewer v1.1', ...
            'NumberTitle', 'off', ...
            'HandleVisibility', 'off', ...
            'MenuBar', 'none', ...
            'ToolBar', 'figure', ...
            'Renderer', 'ZBuffer' ); % Can't use painters as colormaps are broken for >256 colors!
        out.MainAxes = axes( ...
            'Parent', out.Window, ...
            'Position', [0 0 1 1], ...
            'XLim', data.XLim, ...
            'YLim', [-1 1], ...
            'CLim', log([1 data.MaxIterations]), ...
            'XTick', [], 'YTick', [], ...
            'DataAspectRatio', [1 1 1] );
        out.Image = image( ...
            'XData', [0 1], ...
            'YData', [0 1], ...
            'XLimInclude', 'off', ...
            'YLimInclude', 'off', ...
            'CData', nan, ...
            'CDataMapping', 'Scaled', ...
            'HandleVisibility', 'off', ...
            'Parent', out.MainAxes );
        % Add a line so that zooming works. Strange but true.
        line( 'Parent', out.MainAxes, 'XData', [-2 2], 'YData', [-2 2], ...
            'Visible', 'off', ...
            'HitTest', 'off' );
        set( out.MainAxes, 'XLimMode', 'manual', 'YLimMode', 'manual', ...
            'CLim', [0 1] );
        colormap( out.MainAxes, jet2(1000) );

        out.ControlPanel = uipanel( ...
            'Parent', out.Window, ...
            'BackgroundColor', 'k', ...
            'Units', 'Pixels', ...
            'Position', [10 10 155 78] );

        % Some text for showing compute time
        out.ComputeText = uicontrol( ...
            'Style', 'Text', ...
            'String', 'Computed in 0ms', ...
            'BackgroundColor', 'k', ...
            'ForegroundColor', 'g', ...
            'FontSize', 7, ...
            'Parent', out.ControlPanel, ...
            'Position', [5 16 145 14] );
        out.FrameRateText = uicontrol( ...
            'Style', 'Text', ...
            'String', 'Displaying at 0fps', ...
            'BackgroundColor', 'k', ...
            'ForegroundColor', 'g', ...
            'FontSize', 7, ...
            'Parent', out.ControlPanel, ...
            'Position', [5 2 145 14] );

        % Create a drop-down for selecting the calculation method
        out.MethodSelector = uicontrol( ...
            'Style', 'PopupMenu', ...
            'String', data.CalculationMethods, ...
            'Value', numel( data.CalculationMethods ), ...
            'FontSize', 7, ...
            'Parent', out.ControlPanel, ...
            'BackgroundColor', 0.8*[1 1 1], ...
            'Position', [5 54 145 16], ...
            'Callback', @onCalculationMethodChanged );
        out.AddButton = uicontrol( ...
            'Style', 'ToggleButton', ...
            'String', 'Add', ...
            'FontSize', 7, ...
            'Parent', out.ControlPanel, ...
            'BackgroundColor', 0.6*[1 1 1], ...
            'Position', [5 30 45 16], ...
            'TooltipString', 'Add the current location to the animation list', ...
            'Callback', @onAddPressed );
        out.ResetButton = uicontrol( ...
            'Style', 'ToggleButton', ...
            'String', 'Reset', ...
            'FontSize', 7, ...
            'Parent', out.ControlPanel, ...
            'BackgroundColor', 0.6*[1 1 1], ...
            'Position', [55 30 45 16], ...
            'TooltipString', 'Reset the view to the top', ...
            'Callback', @onResetPressed );
        out.AnimateButton = uicontrol( ...
            'Style', 'ToggleButton', ...
            'String', 'Animate', ...
            'FontSize', 7, ...
            'Parent', out.ControlPanel, ...
            'BackgroundColor', 0.6*[1 1 1], ...
            'Position', [105 30 45 16], ...
            'TooltipString', 'Start/stop animating between stored locations', ...
            'Callback', @onPlayPressed );

        % Remove some things we don't want from the toolbar and add a
        % toggle to the toolbar to hide the controls
        tb = findall( out.Window, 'Type', 'uitoolbar' );
        delete( findall( tb, 'Tag', 'Standard.FileOpen' ) );
        delete( findall( tb, 'Tag', 'Standard.NewFigure' ) );
        delete( findall( tb, 'Tag', 'Standard.EditPlot' ) );
        delete( findall( tb, 'Tag', 'Exploration.Brushing' ) );
        delete( findall( tb, 'Tag', 'Exploration.DataCursor' ) );
        delete( findall( tb, 'Tag', 'Exploration.Rotate' ) );
        delete( findall( tb, 'Tag', 'DataManager.Linking' ) );
        delete( findall( tb, 'Tag', 'Plottools.PlottoolsOn' ) );
        delete( findall( tb, 'Tag', 'Plottools.PlottoolsOff' ) );
        out.AnimateToggle = uitoggletool( ...
            'Parent', tb, ...
            'CData', readIcon( 'icon_play.png' ), ...
            'TooltipString', 'Start/stop animating between stored locations', ...
            'State', 'off', ...
            'Separator', 'on', ...
            'ClickedCallback', @onPlayToolbarPressed );
        out.ShowControlsToggle = uitoggletool( ...
            'Parent', tb, ...
            'CData', readIcon( 'icon_mandelControls.png' ), ...
            'TooltipString', 'Show/hide the Mandelbrot control panel', ...
            'State', 'on', ...
            'ClickedCallback', @onControlsTogglePressed );

        % Add listeners so that we can redraw when the axes are moved
        axHandle = handle( out.MainAxes );
        out.Listeners = [
            handle.listener( axHandle, findprop( axHandle, 'YLim' ), 'PropertyPostSet', @onLimitsChanged )
            ]; %#ok<NBRAK>
        % Also redraw if resized
        set( out.Window, 'ResizeFcn', @onFigureResize, ...
            'CloseRequestFcn', @onFigureClose );
    end % createGUI

    function onLimitsChanged( ~, ~ )
        redraw();
    end % onLimitsChanged

    function onFigureResize( ~, ~ )
        % Change the axes limits to exactly fit the figure
        pos = get( gui.Window, 'Position' );
        xlim = get( gui.MainAxes, 'XLim' );
        ylim = get( gui.MainAxes, 'YLim' );
        delta_ylim = ( diff( xlim )*pos(4)/pos(3) - diff( ylim ) ) / 2;
        data.WindowPixelSize = pos(3:4);
        % Set the YLim to give the correct aspect. This will trigger a
        % redraw
        set( gui.MainAxes, 'YLim', ylim + delta_ylim*[-1 1] );
    end % onFigureResize

    function onFigureClose( ~, ~ )
        % Clear up
        data.IsAnimating = false;
        if data.WriteVideo
            close( data.VideoWriter );
        end
        delete( gui.Window );
    end % onFigureClose

    function onCalculationMethodChanged( ~, ~ )
        idx = get( gui.MethodSelector, 'Value' );
        data.SelectedCalculationMethod = data.CalculationMethods{idx};
        redraw();
    end % onCalculationMethodChanged

    function onAddPressed( ~, ~ )
        disp('Add')
        fprintf( 'Adding location [%1.15f, %1.15f], %1.15f\n', ...
            data.XLim, data.Y );
        idx = numel( data.LocationList ) + 1;
        data.LocationList(idx).XLim = data.XLim;
        data.LocationList(idx).Y = data.Y;
        writeLocationList( data.LocationList );
        % Release the button
        set( gui.AddButton, 'Value', 0 );
    end % onAddPressed

    function onResetPressed( ~, ~ )
        disp('Reset')
        fprintf( 'Leaving location [%1.15f, %1.15f], %1.15f\n', ...
            data.XLim, data.Y );
        pos = get( gui.Window, 'Position' );
        aspect = pos(4)/pos(3);
        ylim = diff( data.OrigXLim ) * aspect / 2 * [-1 1];
        set( gui.MainAxes, 'XLim', data.OrigXLim, 'YLim', data.OrigY + ylim );
        set( gui.ResetButton, 'Value', 0 );
    end % onResetPressed

    function onPlayPressed( ~, ~ )
        disp('Play')
        if get( gui.AnimateButton, 'Value' )==1
            updateAnimationControls(true);
            while ishandle(gui.AnimateButton) && (get( gui.AnimateButton, 'Value' )==1)
                newXLim = data.LocationList(data.NextLocation).XLim;
                newY    = data.LocationList(data.NextLocation).Y;
                animatedMove( newXLim, newY );
                if numel( data.LocationList )>1
                    % Choose a random location
                    thisLocation = data.NextLocation;
                    while data.NextLocation == thisLocation
                        data.NextLocation = randi( numel( data.LocationList ), 1 );
                    end
                    fprintf( 'Next location: %d\n', data.NextLocation )
                else
                    % Only one location, so stop
                    data.IsAnimating = false;
                    updateAnimationControls( false );
                end
            end
        else
            data.IsAnimating = false;
            updateAnimationControls( false );
        end
    end % onPlayPressed

    function onPlayToolbarPressed( ~, evt )
        ison = strcmpi(get( gui.AnimateToggle, 'State' ), 'on');
        updateAnimationControls( ison );
        onPlayPressed(gui.AnimateButton, evt);
    end % onPlayToolbarPressed

    function updateAnimationControls( isAnimating )
        if isAnimating
            set( gui.AnimateButton, 'Value', 1 );
            set( gui.AnimateToggle, 'State', 'on' );
        else
            set( gui.AnimateButton, 'Value', 0 );
            set( gui.AnimateToggle, 'State', 'off' );
        end
        drawnow();
    end % updateAnimationControls

    function onControlsTogglePressed( ~, ~ )
        % Toggle the control panel on and off
        disp('Toggle controls')
        pos = get( gui.ControlPanel, 'Position' );
        if strcmpi( get( gui.ShowControlsToggle, 'State' ), 'off' )
            % Turn it off (move offscreen)
            pos(1) = -pos(3)-10;
        else
            % Turn it on (move onscreen)
            pos(1) = 10;
        end
        set( gui.ControlPanel, 'Position', pos );

    end

    function animatedMove( targetXLim, targetY )
        % Form a zoom path between the two
        data.IsAnimating = true;
        if isequal( data.XLim, targetXLim ) && isequal( data.Y, targetY )
            data.IsAnimating = false;
            return;
        end

        % Perform a zoom and translate arc
        maxNumSteps = 1000;
        distTravelled = sqrt( (mean( data.XLim ) - mean( targetXLim )).^2 ...
            + (data.Y - targetY).^2 );
        adjustRatio = exp( -10*linspace(-3,3,maxNumSteps).^2 );
        adjustRatio = adjustRatio - min(adjustRatio);
        ratio = cumsum( adjustRatio ); ratio = ratio / ratio(end);
        minXPath = interp1( [0,1], [data.XLim(1),targetXLim(1)], ratio );
        maxXPath = interp1( [0,1], [data.XLim(2),targetXLim(2)], ratio );
        maxXRange = max( maxXPath - maxXPath );
        xlimAdjust = max(0, 0.3*distTravelled - maxXRange);
        minXPath = minXPath - xlimAdjust*adjustRatio;
        maxXPath = maxXPath + xlimAdjust*adjustRatio;

        % Cull the ends if there's negligable motion. This helps to keep
        % things smooth but without long periods of no apparant motion.
        tolerance = 0.001;
        xRange = maxXPath - minXPath;
        firstGood = find( (xRange > (1+tolerance)*xRange(1)) | (xRange < (1-tolerance)*xRange(1)), 1, 'first' );
        if ~isempty( firstGood ) && firstGood > 2
            toCull = 2:firstGood-1;
        else
            toCull = [];
        end
        lastGood = find( (xRange > (1+tolerance)*xRange(end)) | (xRange < (1-tolerance)*xRange(end)), 1, 'last' );
        if ~isempty( lastGood ) && lastGood < numel(xRange)-1
            toCull = [toCull, lastGood:numel(xRange)-1];
        end
        ratio(toCull) = [];
        minXPath(toCull) = [];
        maxXPath(toCull) = [];
        xRange(toCull) = [];

        if ~isempty( ratio )
            % Work out the aspect ratio
            pos = get( gui.Window, 'Position' );
            aspect = pos(4)/pos(3);

            YPath = interp1( [0,1], [data.Y,targetY], ratio );
            heightPath = aspect*xRange;
            minYPath = YPath - 0.5*heightPath;
            maxYPath = YPath + 0.5*heightPath;
            for ii=1:numel(ratio)
                % Setting the limits will cause a redraw
                set( gui.MainAxes, ...
                    'XLim', [minXPath(ii),maxXPath(ii)], ...
                    'YLim', [minYPath(ii),maxYPath(ii)] );
                if data.IsAnimating == false
                    break;
                end
            end
        end
        data.IsAnimating = false;
        % Do a final redraw at full res
        redraw();
    end % animatedMove

    function kernel = loadKernel()
        thisDir = fileparts( mfilename( 'fullpath' ) );
        baseName = 'mandelbrotViewerProcessElement';
        data.CUDAFile = fullfile( thisDir, [baseName,'.cu'] );
        ptxname = [baseName,'.',parallel.gpu.ptxext];
        data.PTXFile = fullfile( thisDir, ptxname );
        if exist( data.PTXFile, 'file' ) ~= 2
            close( gui.Window );
            error( 'mandelbrotViewer:MissingPTX', 'Could not find ''%s''. Please use NVCC to compile it', ptxname );
        end
        kernel = parallel.gpu.CUDAKernel( data.PTXFile, data.CUDAFile );
    end % loadKernel

    function redraw()
        % Protect against the window closing
        if ~ishandle(gui.MainAxes)
            return;
        end
        % To work out what to draw and at what resolution we need the axis
        % limits and pixel counts.
        xlim = get(gui.MainAxes,'XLim');
        ylim = get(gui.MainAxes,'YLim');
        data.XLim = xlim;
        data.Y = mean( ylim );
        imWidth = data.WindowPixelSize(1);
        imHeight = data.WindowPixelSize(2);
        if data.IsAnimating && (imWidth*imHeight>600000)
            % To speed up animations with large windows, subsample by 2
            imWidth = round(imWidth/2);
            imHeight = round(imHeight/2);
        end

        zoomLevel = imWidth / diff( xlim );
        maxIterations = min( data.MaxIterations, 200 + 0.1*sqrt(zoomLevel) );

        % Call the computation
        t = tic;
        switch( data.SelectedCalculationMethod )
            case 'CUDAKernel'
                logCount = computeMandelbrotCUDAKernel( xlim, imWidth, ...
                    ylim, imHeight, ...
                    maxIterations );

            case 'CPU'
                logCount = computeMandelbrotCPU( xlim, imWidth, ...
                    ylim, imHeight, ...
                    maxIterations );

            case 'GPU (simple)'
                logCount = computeMandelbrotGPU( xlim, imWidth, ...
                    ylim, imHeight, ...
                    maxIterations );

            case 'GPU ArrayFun'
                logCount = computeMandelbrotArrayFun( xlim, imWidth, ...
                    ylim, imHeight, ...
                    maxIterations );

            otherwise
                error( 'mandelbrotViewer:BadMethod', 'Unrecognised calculation method ''%s''', data.SelectedCalculationMethod );
        end

        computeTime = toc(t);

        minCount = min( logCount(:) );
        logCount = (logCount - minCount) ./ (log(maxIterations+1)-minCount);
        % Guard against a closed window
        if ~ishandle( gui.Image )
            return;
        end
        set( gui.Image, ...
            'XData', xlim, ...
            'YData', ylim, ...
            'CData', logCount );
        if data.ControlsVisible
            set( gui.ComputeText, 'String', sprintf( 'Computed in %dms', round(1000*computeTime) ) )

            % Capture the current time for frame-rate calculations
            thisFrameTime = now();
            framerate = 1 / (86400*(thisFrameTime - data.LastFrameTime)); % convert days to seconds
            set( gui.FrameRateText, 'String', sprintf( 'Displaying at %dfps', round(framerate) ) )
            data.LastFrameTime = thisFrameTime;

            % Force a redraw
            drawnow();
        end

        % Capture!
        if data.WriteVideo
            t0 = now();
            currFrame = getframe( gui.Window );
            writeVideo( data.VideoWriter, currFrame );
            % Also reset the frame time to exclude the video writing
            delta_t = now() - t0;
            data.LastFrameTime = data.LastFrameTime + delta_t;
        end

    end % redraw

    function locations = readLocationList()
        fid = fopen( 'locations.csv', 'rt' );
        if fid<0
            close( gui.Window );
            error( 'mandelbrotViewer:BadLocationRead', 'Could not open location list for reading: ''locations.csv''' );
        end

        locData = textscan( fid, '%f,%f,%f' );
        N = size( locData{1}, 1 );
        if N<1
            close( gui.Window );
            error( 'mandelbrotViewer:EmptyLocationFile', 'No locations found in: ''locations.csv''' );
        end
        locations = struct( ...
            'XLim', cell( N, 1 ), ...
            'Y', cell( N, 1 ) );
        for ii=1:N
            locations(ii).XLim = [locData{1}(ii), locData{2}(ii)];
            locations(ii).Y = locData{3}(ii);
        end

        fclose( fid );
    end % readLocationList

    function writeLocationList( locations )
        fid = fopen( 'locations.csv', 'wt' );
        if fid<0
            error( 'mandelbrotViewer:BadLocationWrite', 'Could not open location list for writing: ''locations.csv''' );
        end
        N = numel( locations );
        for ii=1:N
            fprintf( fid, '%1.15f,%1.15f,%1.15f\n', ...
                locations(ii).XLim(1), ...
                locations(ii).XLim(2), ...
                locations(ii).Y );
        end

        fclose( fid );
    end % writeLocationList

    function cdata = readIcon( filename )
        [cdata,~,alpha] = imread( filename );
        idx = find( ~alpha );
        page = size(cdata,1)*size(cdata,2);
        cdata = double( cdata ) / 255;
        cdata(idx) = nan;
        cdata(idx+page) = nan;
        cdata(idx+2*page) = nan;
    end % readIcon

    function matlabVersionCheck()
        % R2011a is v7.12
        majorMinor = sscanf( version, '%d.%d' );
        if (majorMinor(1)<7) || (majorMinor(1)==7 && majorMinor(2)<13)
            error( 'mandelbrotViewer:MATLABTooOld', 'mandelbrotViewer requires MATLAB R2011b or above.' );
        end
    end % matlabVersionCheck

    function gpuCheck()
        try
            d = gpuDevice();
        catch err
            error( 'mandelbrotViewer:NoGPU', 'mandelbrotViewer requires a GPU and none appear to be availble. Type "gpuDevice" for more information.' );
        end
        if ~d.DeviceSupported
            error( 'mandelbrotViewer:GPUNotSupported', 'The selected GPU is not supported. Type "gpuDevice" for more information.' );
        end
    end % matlabVersionCheck

end % mandelbrotViewer


function newSteps = dtiUnwarpStep(origSteps,param)
% Unwarp (shift and scale) the steps into a new coord system
%
%    newSteps = dtiUnwarpStep(origSteps,param)
%
%    Shifts and scales according to: f(x,p) = ( x/p(1) ) - p(2)
%
%    origSteps - nx1 vector
%    param     - 2x1 vector [scale shift]
%
%    newSteps  - nx1 vector
%
% Example:
%    % original x's
%    x = 1:10;
%    % stretch by 2 times after shifting 5 units to the right
%    nx = dtiWarpStep(x,[2 5]);
%    % go back to the original
%    ox = dtiUnwarpStep(nx,[2 5]);
%
% See also: dtiWarpStep
%
% History:
%    2007/01/17 shc wrote it.
%

if ieNotDefined('origSteps'), error('Require input data!');         end
if ieNotDefined('param'),     error('Require warping parameters!'); end

newSteps = (origSteps/param(1))-param(2);

return


% [INPUT]
% y = A vector of floats (-Inf,Inf) of length t representing the residuals.
% a = A float (0,1) representing the significance level (optional, default=0.05).
%
% [OUTPUT]
% h0 = A boolean representing the null hypothesis (residuals are normally distributed) rejection outcome.
% pval = A float [0,1] representing the test p-value.
% stat = A float (-Inf,Inf) representing the normalized test statistic.
%
% [NOTES]
% When residuals are leptokurtic, the Shapiro-Francia test is performed.
% When residuals are platykurtic, the Shapiro-Wilk test is performed.

function [h0,pval,stat] = shapiro_test(varargin)

    persistent ip;

    if (isempty(ip))
        ip = inputParser();
        ip.addRequired('y',@(x)validateattributes(x,{'double'},{'real' 'finite' 'vector' 'nonempty'}));
        ip.addOptional('a',0.05,@(x)validateattributes(x,{'double'},{'real' 'finite' '>' 0 '<' 1 'scalar'}));
    end

    ip.parse(varargin{:});

    ipr = ip.Results;
    y = validate_input(ipr.y);
    a = ipr.a;

    nargoutchk(2,3);

    [h0,pval,stat] = shapiro_test_internal(y,a);

end

function [h0,pval,stat] = shapiro_test_internal(x,alpha)

    x = sort(x);

    if (kurtosis(x) > 3)
        [pval,stat] = shapiro_francia(x);
    else
        [pval,stat] = shapiro_wilks(x);
    end

    h0  = (alpha >= pval);

end

function [pval,stat] = shapiro_francia(x)

    n = numel(x);

    x0 = x - mean(x);
    m = norminv(((1:n).' - 0.375) ./ (n + 0.25));
    w = (1 / sqrt(m.' * m)) .* m;
    k = (w.' * x)^2 / (x0.' * x0);

    nu = log(n);
    u1 = log(nu) - nu;
    u2 = log(nu) + (2 / nu);

    mu = -1.27250 + (1.05210 * u1);
    sigma = 1.03080 - (0.26758 * u2);

    stat = (log(1 - k) - mu) / sigma;
    pval = 1 - normcdf(stat,0,1);

end

function [pval,stat] = shapiro_wilks(x)

    n = numel(x);

    x0 = x - mean(x);
    m = norminv(((1:n).' - 0.375) ./ (n + 0.25));
    c = (1 / sqrt(m.' * m)) .* m;
    u = 1 / sqrt(n);

    pc_1 = [-2.7060560  4.434685 -2.071190 -0.147981 0.221157 c(n)];
    pc_2 = [-3.5826330  5.682633 -1.752461 -0.293762 0.042981 c(n - 1)];
    pc_3 = [-0.0006714  0.025054 -0.399780  0.544000];
    pc_4 = [-0.0020322  0.062767 -0.778570  1.382200];
    pc_5 = [ 0.0038915 -0.083751 -0.310820 -1.586100];
    pc_6 = [ 0.0030302 -0.082676 -0.480300];
    pc_7 = [ 0.459 -2.273];

    w = zeros(n,1);
    w(n) = polyval(pc_1,u);
    w(1) = -w(n);

    if (n >= 6)
        off = 3:n-3+1;

        w(n-1) = polyval(pc_2,u);
        w(2) = -w(n-1);

        phi = ((m.' * m) - (2 * m(n)^2) - (2 * m(n-1)^2)) /  (1 - (2 * w(n)^2) - (2 * w(n-1)^2));
    else
        off = 2:n-2+1;

        if (n == 3)
            w(1) = 1 / sqrt(2);
            w(n) = -w(1);

            phi = 1;
        else
            phi = ((m' * m) - (2 * m(n)^2)) / (1 - (2 * w(n)^2));
        end
    end

    w(off) = m(off) ./ sqrt(phi);

    k = (w.' * x)^2 / (x0.' * x0);

    if (n == 3)
        mu = 0;
        sigma = 1;
        kn = 0;
    elseif ((n >= 4) && (n <= 11))
        mu = polyval(pc_3,n);
        sigma = exp(polyval(pc_4,n));
        gamma = polyval(pc_7,n);
        kn = -log(gamma - log(1 - k));
    else
        ln = log(n);
        mu = polyval(pc_5,ln);
        sigma = exp(polyval(pc_6,ln));
        kn = log(1 - k);
    end

    stat = (kn - mu) / sigma;

    if (n == 3)
        pval = (6 / pi()) * (asin(sqrt(k)) - asin(sqrt(0.75)));
    else
        pval = 1 - normcdf(stat,0,1);
    end

end

function y = validate_input(y)

    y = y(:);
    t = numel(y);

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

end


function a = gk324_inverse ( n, x )

%*****************************************************************************80
%
%% GK324_INVERSE returns the inverse of the GK324 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%   18 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Robert Gregory, David Karney,
%    A Collection of Matrices for Testing Computational Algorithms,
%    Wiley, 1969, page 51,
%    LC: QA263.G68.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real X(N-1), the first N-1 entries of the last row.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  for i = 1 : n
    for j = 1 : n

      if ( i < n )

        if ( j < i )
          a(i,j) = 0.0;
        elseif ( j == i )
          a(i,j) = 1.0 / ( 1.0 - x(i) );
        elseif ( j == i + 1 )
          a(i,j) = - 1.0 / ( 1.0 - x(i) );
        elseif ( i + 1 < j )
          a(i,j) = 0.0;
        end

      elseif ( i == n )

        if ( j == 1 )

          a(i,j) = - x(1) / ( 1.0 - x(1) );

        elseif ( j < n )

          a(i,j) = ( x(j-1) - x(j) ) / ( 1.0 - x(j) ) / ( 1.0 - x(j-1) );

        elseif ( j == n )

          a(i,j) = 1.0 / ( 1.0 - x(n-1) );

        end

      end

    end
  end

  return
end


classdef MatrixOperator < spx.dict.Operator

    properties(SetAccess=private)
        % The matrix as an operator
        A
        % The Hermitian of matrix
        AH
    end

    methods
        function self = MatrixOperator(A)
            self.A = A;
            self.AH = A';
        end

        function [mm, nn]  = get_size(self)
            [mm, nn] = size(self.A);
        end

        function result = apply(self, vectors)
            result = self.A * vectors;
        end

        function result = apply_transpose(self, vectors)
            result = transpose(self.A) * vectors;
        end

        function result = apply_ctranspose(self, vectors)
            result = self.AH * vectors;
        end

        function result = norm(self)
            result = norm(self.A);
        end

    end

    methods
        % We override some of the implementations here
        function result = apply_columns(self, vectors, columns)
            result = self.A(:, columns) * vectors;
        end

        function result = columns(self, columns)
            result = self.A(:, columns);
        end

        function result = columns_operator(self, columns)
            b = self.A(:, columns);
            result = spx.dict.MatrixOperator(b);
        end

        function result = double(self)
            result = self.A;
        end

        function result = subsref2(self, s)
            result = subsref(self.A, s);
        end
    end

end


 % Copyright 2001, Brown University, Providence, Rhode Island.
 %
 % All Rights Reserved
 %
 % Permission to use this software for noncommercial research and
 % educational purposes is hereby granted without fee.
 % Redistribution, sale, or incorporation of this software into a
 % commercial product is prohibited.
 %
 % BROWN UNIVERSITY DISCLAIMS ANY AND ALL WARRANTIES WITH REGARD TO
 % THIS SOFTWARE,INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY
 % AND FITNESS FOR ANY PARTICULAR PURPOSE.  IN NO EVENT SHALL BROWN
 % UNIVERSITY BE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL
 % DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE,
 % DATA OR PROFITS.

% ----------------------------------------------------------------
%  jacobd() - derivative of jacobi polynomials - vector version
%
%  Get a vector 'poly' of values of the derivative of the n_th order
%  Jacobi polynomial P^(alpha,beta)_N(z) at the np points z.
%
%  To do this we have used the relation
%
%  d   alpha,beta   1                  alpha+1,beta+1
%  -- P (z)       = -(alpha+beta+n+1) P  (z)
%  dz  n            2                  n-1
%  ----------------------------------------------------------------*/

function jd = jacobd(z, n, alpha, beta)

 jd = zeros(size(z));

  one = 1.0;
  if(n == 0)
    jd(:,:) = 0.0;
  else
    jd = am282jacobi1d(z,n-1,alpha+one,beta+one);
    jd = jd*0.5*(alpha + beta + n + one);
  end


function [c,Ls]=dgt2(f,g1,p3,p4,p5,p6)
%DGT2  2-D Discrete Gabor transform
%   Usage: c=dgt2(f,g,a,M);
%          c=dgt2(f,g1,g2,[a1,a2],[M1,M2]);
%          c=dgt2(f,g1,g2,[a1,a2],[M1,M2],[L1,L2]);
%          [c,Ls]=dgt2(f,g1,g2,[a1,a2],[M1,M2]);
%          [c,Ls]=dgt2(f,g1,g2,[a1,a2],[M1,M2],[L1,L2]);
%
%   Input parameters:
%         f       : Input data, matrix.
%         g,g1,g2 : Window functions.
%         a,a1,a2 : Length of time shifts.
%         M,M1,M2 : Number of modulations.
%         L1,L2   : Length of transform to do
%
%   Output parameters:
%         c       : array of coefficients.
%         Ls      : Original size of input matrix.
%
%   `dgt2(f,g,a,M)` will calculate a separable two-dimensional discrete
%   Gabor transformation of the input signal *f* with respect to the window
%   *g* and parameters *a* and *M*.
%
%   For each dimension, the length of the transform will be the smallest
%   possible that is larger than the length of the signal along that dimension.
%   f will be appropriately zero-extended.
%
%   `dgt2(f,g,a,M,L)` computes a Gabor transform as above, but does
%   a transform of length *L* along each dimension. *f* will be cut or
%   zero-extended to length *L* before the transform is done.
%
%   `[c,Ls]=dgt2(f,g,a,M)` or `[c,Ls]=dgt2(f,g,a,M,L)` additionally returns
%   the length of the input signal *f*. This is handy for reconstruction::
%
%                [c,Ls]=dgt2(f,g,a,M);
%                fr=idgt2(c,gd,a,Ls);
%
%   will reconstruct the signal *f* no matter what the size of *f* is, provided
%   that *gd* is a dual window of *g*.
%
%   `dgt2(f,g1,g2,a,M)` makes it possible to use a different window along the
%   two dimensions.
%
%   The parameters *a*, *M*, *L* and *Ls* can also be vectors of length 2.
%   In this case the first element will be used for the first dimension
%   and the second element will be used for the second dimension.
%
%   The output *c* has *4* or *5* dimensions. The dimensions index the
%   following properties:
%
%   1. Number of translation along 1st dimension of input.
%
%   2. Number of channel along 1st dimension  of input
%
%   3. Number of translation along 2nd dimension of input.
%
%   4. Number of channel along 2nd dimension  of input
%
%   5. Plane number, corresponds to 3rd dimension of input.
%
%   See also:  dgt, idgt2, gabdual

complainif_argnonotinrange(nargin,4,6,mfilename);

L=[];

if prod(size(p3))>2
  % Two windows was specified.
  g2=p3;
  a=p4;
  M=p5;
  if nargin==6
    L=p6;
  end;
else
  g2=g1;
  a=p3;
  M=p4;
  if nargin==5
    L=p5;
  end;
end;

if isempty(L)
  L1=[];
  L2=[];
else
  L1=L(1);
  L2=L(2);
end;

% Expand 'a' and M if necessary to two elements
a=bsxfun(@times,a,[1 1]);
M=bsxfun(@times,M,[1 1]);

Ls=size(f);
Ls=Ls(1:2);

c=dgt(f,g1,a(1),M(1),L1);
c=dgt(c,g2,a(2),M(2),L2,'dim',3);


function discon_meas = localized_discontinuity_measure(topographies, channel_locations, n_ics)
% localized_discontinuity_measure calculates a measure of how discontinuous
% an IC scalp map is. A high value indicates that the IC might represent an
% artifact. A very similar measure was used for the ADJUST toolbox,
% http://www.unicog.org/pm/pmwiki.php/MEG/RemovingArtifactsWithADJUST
% Our weighting scheme for electrodes surrounding the electrode for which
% the measure is being calculated is a little different.
%
% Input:
% topographies: Matrix of topographies (scalp maps) of ICs. Each row
% corresponds to an IC, and each column to an electrode, so it is the
% transpose of the field icawinv in EEGLab data structures.
%
% channel_locations: Channel locations of the electrodes whose activations
% are given in topographies.
%
% n_ics: The number of ICs represented in topographies.
%
% Output:
% discon_meas: A vector of discontinuity measures for each IC. For each IC,
% the discontinuity measure is found by first calculating how peaked the
% spatial map is for each electrode. The discontinuity measure is then the
% highest value found.

% This code is based heavily on the code from the ADJUST toolbox. It was
% modified by Laura Froelich 24/4/2013.
%
% Copyright (C) 2009 Andrea Mognon and Marco Buiatti,
% Center for Mind/Brain Sciences, University of Trento, Italy
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

xpos=[channel_locations.X];ypos=[channel_locations.Y];zpos=[channel_locations.Z];
pos=[xpos',ypos',zpos'];
discon_meas=zeros(1,n_ics);

for ic=1:n_ics
    aux=zeros(1,length(channel_locations));
    for el=1:length(channel_locations)-1

        P=pos(el,:); %position of current electrode
        d=pos-repmat(P,length(channel_locations),1);
        dist=sqrt(sum((d.*d),2));

        [y,I]=sort(dist, 'ascend'); % sort electrodes in ascending order of distance, so closest are first in list
        repchas=I(2:end);
        weightdist=exp(-y(2:end)); % weights computed wrt distance
        weightdist=weightdist/sum(weightdist); % ensure that weights sum to one

        aux(el)=abs(topographies(ic,el)-sum(weightdist.*topographies(ic,repchas)')); % Since the sum
        % of weights is one, the quantity
        % sum(weightdist.*topographies(ic,repchas)'), is a weighted
        % average. Hence this calculation finds the difference between the
        % activation in the current electrode and the average activation
        % surrounding it.
    end

    discon_meas(ic)=max(aux);
end


function geometry_test043 ( )

%*****************************************************************************80
%
%% TEST043 tests SEGMENTS_DIST_3D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST043\n' );
  fprintf ( 1, '  SEGMENTS_DIST_3D computes the distance between\n' );
  fprintf ( 1, '    line segments in 3D.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Case   Computed    True\n' );
  fprintf ( 1, '\n' );
%
%  Case 1, parallel, not coincident.
%
%  LS1: (2,3,0) + t * (2,1,0) for t = 0 to 3.
%  LS2: (11,6,4) + t * (2,1,0) for t = 0 to 3.
%  Distance is 5.
%
  p1(1:3,1) = [  2.0; 3.0; 0.0 ];
  p2(1:3,1) = [  8.0; 6.0; 0.0 ];
  q1(1:3,1) = [ 11.0; 6.0; 4.0 ];
  q2(1:3,1) = [ 17.0; 9.0; 4.0 ];

  dist = segments_dist_3d ( p1, p2, q1, q2 );

  fprintf ( 1, '  1  %8f  5.0\n', dist );
%
%  Case 2, parallel, coincident, overlapping.
%
%  (1,2,3) + t * ( 1,-1,2)
%  LS1: t = 0 to t = 3.
%  Distance is 0.
%
  p1(1:3,1) = [  1.0;  2.0;  3.0 ];
  p2(1:3,1) = [  4.0; -1.0;  9.0 ];
  q1(1:3,1) = [  3.0;  0.0;  7.0 ];
  q2(1:3,1) = [  6.0; -3.0; 13.0 ];

  dist = segments_dist_3d ( p1, p2, q1, q2 );

  fprintf ( 2, '  1  %8f  0.0\n', dist );
%
%  Case 3, parallel, coincident, disjoint.
%
%  LS1: (3,4,5) + t * ( 2,2,1) for 0 <= t <= 2.
%  LS2: (3,4,5) + t * ( 2,2,1) for 3 <= t <= 5.
%  Distance = 3.
%
  p1(1:3,1) = [  3.0;  4.0;  5.0 ];
  p2(1:3,1) = [  7.0;  8.0;  7.0 ];
  q1(1:3,1) = [  9.0; 10.0;  8.0 ];
  q2(1:3,1) = [ 13.0; 14.0; 10.0 ];

  dist = segments_dist_3d ( p1, p2, q1, q2 );

  fprintf ( 1, '  3  %8f  3.0\n', dist );
%
%  Case 4, nonparallel, could intersect, and does intersect.
%
%  L1: (1,1,1) + t * (0,1,2)
%  L2: (0,2,3) + t * (1,0,0)
%  intersect at (1,2,3)
%  Distance is 0.
%
  p1(1:3,1) = [  1.0;  1.0;  1.0 ];
  p2(1:3,1) = [  1.0;  4.0;  7.0 ];
  q1(1:3,1) = [  0.0;  2.0;  3.0 ];
  q2(1:3,1) = [  5.0;  2.0;  3.0 ];

  dist = segments_dist_3d ( p1, p2, q1, q2 );

  fprintf ( 1, '  4  %8f  0.0\n', dist );
%
%  Case 5, nonparallel, could intersect, and does not intersect.
%
%  L1: (1,1,1) + t * (0,1,2)
%  L2: (0,2,3) + t * (1,0,0)
%  lines intersect at (1,2,3), line segments do not.
%  Distance is 1.0
%
  p1(1:3,1) = [  1.0;  1.0;  1.0 ];
  p2(1:3,1) = [  1.0;  4.0;  7.0 ];
  q1(1:3,1) = [  0.0;  2.0;  3.0 ];
  q2(1:3,1) = [ -5.0;  2.0;  3.0 ];

  dist = segments_dist_3d ( p1, p2, q1, q2 );

  fprintf ( 1, '  5  %8f  1.0\n', dist );
%
%  Case 6, nonparallel, can not intersect, "end-to-end".
%
%  L1: (2,2,1) + t * (0,1,2)  0 <= t <= 5
%  L2: (0,0,0) + t * (-1,-1,-1) 0 <= t <= 5
%  Distance is 3.
%
  p1(1:3,1) = [  2.0;  2.0;  1.0 ];
  p2(1:3,1) = [  2.0;  7.0;  11.0 ];
  q1(1:3,1) = [  0.0;  0.0;  0.0 ];
  q2(1:3,1) = [ -5.0; -5.0; -5.0 ];

  dist = segments_dist_3d ( p1, p2, q1, q2 );

  fprintf ( 1, '  6  %8f  3.0\n', dist );
%
%  Case 7, nonparallel, can not intersect, "end-to-mid".
%
%  L1: (1,1,1) + t * (0,1,2) 0 <= t <= 5
%  L2: (0,4,7) + t * (-1,0,0) 0 <= t <= 5
%  Distance is 1.
%
  p1(1:3,1) = [  1.0;  1.0;  1.0 ];
  p2(1:3,1) = [  1.0;  6.0; 11.0 ];
  q1(1:3,1) = [  0.0;  4.0;  7.0 ];
  q2(1:3,1) = [ -5.0;  4.0;  7.0 ];

  dist = segments_dist_3d ( p1, p2, q1, q2 );

  fprintf ( 1, '  7  %8f  1.0\n', dist );
%
%  Case 8, nonparallel, can not intersect, "mid-to-mid".
%
%  L1: (0,5,10) + t * (1,-1,0) 0 <= t <= 5
%  L2: (0,0,0) + t * (1,1,0) 0 <= t <= 6
%  Distance = 10.
%
  p1(1:3,1) = [  0.0;  5.0; 10.0 ];
  p2(1:3,1) = [  5.0;  0.0; 10.0 ];
  q1(1:3,1) = [  0.0;  0.0;  0.0 ];
  q2(1:3,1) = [  6.0;  6.0;  0.0 ];

  dist = segments_dist_3d ( p1, p2, q1, q2 );

  fprintf ( 1, '  8  %8f 10.0\n', dist );
%
%  Case 9, nonparallel, can not intersect, "mid-to-end".
%
%  L1: (-2,0,0) + t * (1,0,0) 0 <= t <= 12
%  L2: (-2,8,1) + t * (9,-4,-1) 0 <= t <= 1
%  Distance = 4.
%
  p1(1:3,1) = [ -2.0;  0.0;  0.0 ];
  p2(1:3,1) = [ 10.0;  0.0;  0.0 ];
  q1(1:3,1) = [ -2.0;  8.0;  1.0 ];
  q2(1:3,1) = [  7.0;  4.0;  0.0 ];

  dist = segments_dist_3d ( p1, p2, q1, q2 );

  fprintf ( 1, '  9  %8f  4.0\n', dist );

  return
end


function value = r8_cosh ( x )

%*****************************************************************************80
%
%% R8_COSH evaluates the hyperbolic cosine of an R8 argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the hyperbolic cosine of X.
%
  persistent ymax

  if ( isempty ( ymax ) )
    ymax = 1.0 / sqrt ( r8_mach ( 3 ) );
  end

  y = exp ( abs ( x ) );

  if ( y < ymax )
    value = 0.5 * ( y + 1.0 / y );
  else
    value = 0.5 * y;
  end

  return
end


function err=geterror(dataname,traindataname)

load(dataname);
load (traindataname);

[numcases numdims numbatches]=size(batchdata);
N=numcases;
err=0;
for batch = 1:numbatches
  data = [batchdata(:,:,batch)];
  data = [data ones(N,1)];
  w1probs = 1./(1 + exp(-data*w1)); w1probs = [w1probs  ones(N,1)];
  w2probs = 1./(1 + exp(-w1probs*w2)); w2probs = [w2probs ones(N,1)];
  w3probs = 1./(1 + exp(-w2probs*w3)); w3probs = [w3probs  ones(N,1)];
  w4probs = w3probs*w4; w4probs = [w4probs  ones(N,1)];
  w5probs = 1./(1 + exp(-w4probs*w5)); w5probs = [w5probs  ones(N,1)];
  w6probs = 1./(1 + exp(-w5probs*w6)); w6probs = [w6probs  ones(N,1)];
  w7probs = 1./(1 + exp(-w6probs*w7)); w7probs = [w7probs  ones(N,1)];
  dataout = 1./(1 + exp(-w7probs*w8));
%   err=err+sum(sum(dataori.*dataout))/(sqrt(sum(sum(dataori.^2)))*sqrt(sum(sum(dataout.^2))));
  err = err +  1/N*sum(sum( (data(:,1:end-1)-dataout).^2 ));
end
 err=err/numbatches;
 fprintf(1,'squared error: %6.3f ,%s of %s \t \t \n',err, dataname, traindataname);


function [error_x,error_y] = ...
         navierpost_bc(viscosity,aez,fezx,fezy,elerrorx,elerrory,xy,ev,ebound);
%navierpost_q1p0_bc   postprocesses Poisson error estimator
%   [error_x,error_y] = ...
%   navierpost_q1p0_bc(viscosity,aez,fezx,fezy,elerrorx,elerrory,xy,ev,ebound);;
%   input
%          viscosity            viscosity parameter
%          aez                  elementwise Poisson problem matrices
%          fezx,fezy            elementwise rhs vectors
%          elerrorx, elerrory   elementwise error estimate (without BC imposition)
%          xy                   vertex coordinate vector
%          ev                   element mapping matrix
%          ebound               element edge boundary matrix
%   output
%          error_x, error_y     component elementwise error estimate
%
%   calls function localbc_xycd
%   IFISS function: DJS; 11 March 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
      x=xy(:,1); y=xy(:,2);
      nel=length(ev(:,1));
      lev=[ev,ev(:,1)];
	  error_x=elerrorx;  error_y=elerrory;
%
% recompute contributions from elements with Dirichlet boundaries
      nbde=length(ebound(:,1));
      ebdy = zeros(nel,1);
      edge = zeros(nel,1);
% isolate boundary elements
      for el = 1:nbde
      ee = ebound(el,1);
      ebdy(ee) = ebdy(ee)+1; edge(ee)=ebound(el,2);
      end
%
% two edge elements
      k2=find(ebdy==2);
      nel2b=length(k2);
% loop over two edge elements
      for el = 1:nel2b
      el2e=k2(el);
      kk=find(ebound(:,1) == el2e);
      edges=ebound(kk,2);
% set up original matrix and RHS vector
	  ae=squeeze(aez(el2e,1:5,1:5));
      fex=fezx(el2e,:)'; fey=fezy(el2e,:)';
% set up local coordinates and impose interpolated error as Dirichlet bc
      xl=x(lev(el2e,:)); yl=y(lev(el2e,:));
	  [bae,fex,fey] = localbc_xycd(viscosity,ae,fex,fey,edges,xl,yl);
% solve local problems
      errx=viscosity*bae\fex;  erry=viscosity*bae\fey;
	  error_x(el2e,1) = errx'*ae*errx; error_y(el2e,1) = erry'*ae*erry;
	  end
% end of element loop
%
% one edge elements
      k1=find(ebdy==1);
      nel1b=length(k1);
% loop over one edge elements
      for el = 1:nel1b
      el1e=k1(el);
      kk=find(ebound(:,1) == el1e);
      edges=ebound(kk,2);
% set up original matrix and RHS vector
      fex=fezx(el1e,:)'; fey=fezy(el1e,:)';
	  ae=squeeze(aez(el1e,1:5,1:5));
% set up local coordinates and impose interpolated error as Dirichlet bc
      xl=x(lev(el1e,:)); yl=y(lev(el1e,:));
	  [bae,fex,fey] = localbc_xycd(viscosity,ae,fex,fey,edges,xl,yl);
% solve local problems
      errx=viscosity*bae\fex;  erry=viscosity*bae\fey;
	  error_x(el1e,1) = errx'*ae*errx; error_y(el1e,1) = erry'*ae*erry;
      end
% end of element loop
%
      err_x = sqrt(sum(error_x)); error_x = sqrt(error_x);
	  err_y = sqrt(sum(error_y)); error_y = sqrt(error_y);
      fprintf('estimated velocity error (in energy):  (%10.6e,%10.6e) \n',err_x,err_y)
 return


function out = traj_cost_lgr(opt_vars,traj_par,baseQR)
% -------------------------------------------------------------------
% This function computes cost in terms of condition number for
% trajectory optimization needed for dynamic parameter identification
% The computation of regressor matrix is obtained using screw
% theory methods.
% -------------------------------------------------------------------

% Trajectory parameters
N = traj_par.N;
wf = traj_par.wf;
T = traj_par.T;
t = traj_par.t;

% As paramters of the trajectory are in a signle vector we reshape them as
% to feed the function that computes the trajectory
ab = reshape(opt_vars,[12,N]);
a = ab(1:6,:); % sin coeffs
b = ab(7:12,:); % cos coeffs

% To guarantee that positions, velocities and accelerations are zero in the
% beginning and at time T, we add fifth order polynomial to fourier
% series. The parameters of the polynomial depends on the parameters of
% fourier series. Here we compute them.
c_pol = getPolCoeffs(T, a, b, wf, N, traj_par.q0);

% Compute trajectory (Fouruer series + fifth order polynomail)
[q,qd,q2d] = mixed_traj(t, c_pol, a, b, wf, N);

% Obtain observation matrix by computing regressor for each sampling time
E1 = baseQR.permutationMatrix(:,1:baseQR.numberOfBaseParameters);
W = [];
for i = 1:length(t)
%     Y = base_regressor_UR10E(q(:,i),qd(:,i),q2d(:,i));
    if baseQR.motorDynamicsIncluded
        Y = [regressorWithMotorDynamics(q(:,i),qd(:,i),q2d(:,i))*E1, ...
             frictionRegressor(qd(:,i))];
    else
        Y = [full_regressor_UR10E(q(:,i),qd(:,i),q2d(:,i))*E1, ...
             frictionRegressor(qd(:,i))];
    end
    W = vertcat(W,Y);
end

out = cond(W);


function c = plus(a,b)
% function C=plus(A,B)
%
% DESCRIPTION
%   Add two polynomials and combine common terms.
%
% INPUTS
%   A,B: polynomials
%
% OUTPUTS
%   C: polynomial, the result of addition A+B
%
% SYNTAX
%   C= A+B
%     Adds A and B.  A and B must have the same dimensions unless one is
%     is a scalar.  A scalar can be added to anything.
%   C = plus(A,B)
%     Function-call form for addition.

% 6/7/2002: PJS  Initial Coding
% 6/8/2002: PJS  Allow matrices of polynomials

% Promote a to polynomial
a = polynomial(a);
sza = size(a);

% Promote b to polynomial
b = polynomial(b);
szb = size(b);

if isempty(a) || isempty(b)

    if isempty(a) && all(szb==[1 1])
        % empty+scalar = empty(sza)
        c=polynomial(zeros(sza));
        return;
    elseif isempty(b) && all(sza==[1 1])
        % scalar+empty = empty(szb)
        c=polynomial(zeros(szb));
        return;
    elseif all(sza==szb)
        c=polynomial(zeros(sza));
        return;
    else
        error('Matrix dimensions must agree.');
    end

elseif all(sza==szb)
    % Matrix + Matrix

    % Get Dimensions
    nta = size(a.degmat,1);
    nva = length(a.varname);
    ntb = size(b.degmat,1);
    nvb = length(b.varname);

    % Stack up coefficients
    acoef = a.coefficient;
    bcoef = b.coefficient;
    coefficient = [acoef; bcoef];

    if nva==0 && nvb==0
        degmat = zeros(nta+ntb,0);
        varname = {};
    else
        adeg = a.degmat;
        bdeg = b.degmat;
        degmat = blkdiag(adeg,bdeg);
        varname = [a.varname(:); b.varname(:)];
    end

    % Form polynomial and combine terms
    chkval = 0; % skip validity check
    c = polynomial(coefficient,degmat,varname,sza,chkval);
    c = combine(c);

elseif all(sza==[1 1]) || all(szb==[1 1])
    % Scalar + Matrix / Matrix + Scalar

    % Make first term of sum be the scalar.
    if ~all(sza==[1 1])
        temp = a;
        a = b;
        b = temp;
        szb = sza;
    end

    % Turn into matrix+matrix
    a.coefficient = repmat(a.coefficient,1,szb(1)*szb(2));
    a.matdim = szb;
    c = plus(a,b);
else
    error('Matrix dimensions must agree');
end


function need = inits ( os, nos, eta )

%*****************************************************************************80
%
%% INITS finds the number of Chebyshev terms needed to achieve a given accuracy.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 November 2009
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    FORTRAN90 version by John Burkardt.
%
%  Parameters:
%
%    Input, real OS(NOS), the array of coefficients.
%
%    Input, integer NOS, the number of coefficients.
%
%    Input, real ETA, the requested accuracy.
%    A typical value of ETA is ( EPSILON ( 1.0 ) ) / 10.0.
%
%    Output, integer NEED, the number of terms needed for
%    the given accuracy.
%
  if ( nos < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'INITS - Fatal error!\n' );
    fprintf ( 1, '  The number of coefficients is less than 1.\n' );
    fprintf ( 1, '  NOS = %d\n', nos );
    error ( 'INITS - Fatal error!' );
  end

  need = nos;

  err = 0.0;
  for i = nos : -1 : 1
    err = err + abs ( os(i) );
    if ( eta < err )
      need = i;
      return
    end
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, 'INITS - Warning!\n' );
  fprintf ( 1, '  The requested accuracy, ETA = %e\n', eta );
  fprintf ( 1, '  is smaller than the Chebyshev coefficients\n' );
  fprintf ( 1, '  can guarantee.\n' );

  return
end


function line2 = normalizeLine3d(line)
%NORMALIZELINE3D Normalizes the direction vector of a 3D line.
%
%   LINE2 = normalizeVector3d(LINE);
%   Returns the normalization of the direction vector of the line, such
%   that ||LINE2(4:6)|| = 1.
%
%   See also
%     normalizePlane, normalizeVector3d
%

% ------
% Author: oqilipo
% E-mail: N/A
% Created: 2020-03-13
% Copyright 2020-2022

isLine3d = @(x) validateattributes(x,{'numeric'},...
    {'nonempty','nonnan','real','finite','size',[nan,6]});

% Check if the line is valid
p=inputParser;
addRequired(p,'line',isLine3d)
parse(p,line)

line2 = line;
line2(:,4:6) = normalizeVector3d(line2(:,4:6));

end


function Y = multiherm(X)
% Returns the Hermitian parts of the matrices in a 3D array
%
% function Y = multiherm(X)
%
% Y is a 3D array the same size as X. Each slice Y(:, :, i) is the
% Hermitian part of the slice X(:, :, i), that is,
%
%   Y(:, :, i) = .5*(X(:, :, i) + X(:, :, i)')
%
% See also: multisym multiskew multiskewh multihconj multiprod multitransp multiscale

% This file is part of Manopt: www.manopt.org.
% Original author: Hiroyuki Sato, April 27, 2015.
% Contributors:
% Change log:

    Y = .5*(X + multihconj(X));

end


function [ind,t0,s0,t0close,s0close] = crossing(S,t,level,imeth)
% CROSSING find the crossings of a given level of a signal
%   ind = CROSSING(S) returns an index vector ind, the signal
%   S crosses zero at ind or at between ind and ind+1
%   [ind,t0] = CROSSING(S,t) additionally returns a time
%   vector t0 of the zero crossings of the signal S. The crossing
%   times are linearly interpolated between the given times t
%   [ind,t0] = CROSSING(S,t,level) returns the crossings of the
%   given level instead of the zero crossings
%   ind = CROSSING(S,[],level) as above but without time interpolation
%   [ind,t0] = CROSSING(S,t,level,par) allows additional parameters
%   par = {'none'|'linear'}.
%	With interpolation turned off (par = 'none') this function always
%	returns the value left of the zero (the data point thats nearest
%   to the zero AND smaller than the zero crossing).
%
%	[ind,t0,s0] = ... also returns the data vector corresponding to
%	the t0 values.
%
%	[ind,t0,s0,t0close,s0close] additionally returns the data points
%	closest to a zero crossing in the arrays t0close and s0close.
%
%	This version has been revised incorporating the good and valuable
%	bugfixes given by users on Matlabcentral. Special thanks to
%	Howard Fishman, Christian Rothleitner, Jonathan Kellogg, and
%	Zach Lewis for their input.

% Steffen Brueckner, 2002-09-25
% Steffen Brueckner, 2007-08-27		revised version

% Copyright (c) Steffen Brueckner, 2002-2007
% brueckner@sbrs.net

% check the number of input arguments
narginchk(1,4);

% check the time vector input for consistency
if nargin < 2 || isempty(t)
	% if no time vector is given, use the index vector as time
    t = 1:length(S);
elseif length(t) ~= length(S)
	% if S and t are not of the same length, throw an error
    error('t and S must be of identical length!');
end

% check the level input
if nargin < 3
	% set standard value 0, if level is not given
    level = 0;
end

% check interpolation method input
if nargin < 4
    imeth = 'linear';
end

% make row vectors
t = t(:)';
S = S(:)';

% always search for zeros. So if we want the crossing of
% any other threshold value "level", we subtract it from
% the values and search for zeros.
S   = S - level;

% first look for exact zeros
ind0 = find( S == 0 ); ind0(ind0==1)=[];

% then look for zero crossings between data points
S1 = S(1:end-1) .* S(2:end);
ind1 = find( S1 < 0 ); ind1(ind1==1)=[];

% bring exact zeros and "in-between" zeros together
ind = sort([ind0 ind1]);

% and pick the associated time values
t0 = t(ind);
s0 = S(ind);

if strcmp(imeth,'linear')
    % linear interpolation of crossing
    for ii=1:length(t0)
        if abs(S(ind(ii))) > eps(S(ind(ii)))
            % interpolate only when data point is not already zero
            NUM = (t(ind(ii)+1) - t(ind(ii)));
            DEN = (S(ind(ii)+1) - S(ind(ii)));
            DELTA =  NUM / DEN;
            t0(ii) = t0(ii) - S(ind(ii)) * DELTA;
            % I'm a bad person, so I simply set the value to zero
            % instead of calculating the perfect number ;)
            s0(ii) = 0;
        end
    end
end

% Addition:
% Some people like to get the data points closest to the zero crossing,
% so we return these as well
[CC,II] = min(abs([S(ind-1) ; S(ind) ; S(ind+1)]),[],1);
ind2 = ind + (II-2); %update indices

t0close = t(ind2);
s0close = S(ind2);


function [fig] = plot_acceleration_offline(accel_history, ...
    time_history, accel_norm_max, fontsize, color)

% plot_acceleration_offline - Plot the min/avg/max acceleration of the
% agents

x0 = 10;
y0 = 10;
width = 600;
height = 150;

t_steps = length(accel_history(:,1));
min_accel = zeros(t_steps,1);
max_accel = zeros(t_steps,1);
avg_accel = zeros(t_steps,1);

for k = 1:t_steps
    Accel_k = accel_history(k,:);
    Accel_k = reshape(Accel_k,3,[]);
    Accel_norm_k = sqrt(sum(Accel_k.^2,1));
    min_accel(k) = min(Accel_norm_k);
    max_accel(k) = max(Accel_norm_k);
    avg_accel(k) = mean(Accel_norm_k);
end

fig = figure('Name','Offline swarm acceleration','NumberTitle','off');
hold on;
grid on;
box on;

% Plot envelope
err_bar(1,:,:) = [max_accel-avg_accel, avg_accel-min_accel];
if ~isempty(color)
    line_props.col = {color};
    mseb(time_history(1:t_steps)',avg_accel',err_bar,line_props);
else
    mseb(time_history(1:t_steps)',avg_accel',err_bar);
end

% Plot reference value
reference = yline(0,'--','LineWidth',1.5);
reference.Color = [0.25 0.25 0.25];
if ~isempty(accel_norm_max)
    threshold = yline(accel_norm_max,'-.','LineWidth',1.5);
    threshold.Color = [0.25 0.25 0.25];
end

xlabel('Time [s]','fontsize',fontsize);
ylabel('Acceleration [m/s^2]','fontsize',fontsize);
if ~isempty(accel_norm_max)
    h = legend('Average', 'Reference', 'Threshold','fontsize',fontsize);
else
    h = legend('Average', 'Reference','fontsize',fontsize);
end
set(h, 'location', 'northeastoutside');

set(fig,'units','pixels','position',[x0,y0,width,height]);
set(fig,'PaperPositionMode','auto');

end


function C=multiDimPolyMatMultiply(A,B)
%%MULTIDIMPOLYMATMULTIPLY Given 2D matrices of multivariate polynomials,
%          multiply the matrices to get a resulting matrix containing
%          multivariate polynomials. The multivariate polynomials
%          themselves are represented as hypermatrices, so the "matrices"
%          of these polynomials are represented by cell arrays of
%          hypermatrices holding the polynomial coefficients.
%
%INPUTS:   A An mXr cell array where each element contains a hypermatrix
%            representing a multivariate polynomials. The format of these
%            hypermatrices is discussed below.
%          B An rXn cell array where each element contains a hypermatrix
%            representing a multivariate polynomial.
%
%OUTPUTS: C The mXn-sized product of the matrices of multivariate
%           polynomials, A and B. C is a cell array where each entry is a
%           hypermatrix of coefficients of the multivariate polynomial.
%
%The coefficients of a hypermatrix representing a multivariate polynomial
%coeffs are arranged such that coeffs(a1,a2,a3...an) corresponds to the
%coefficient of an x1^(a1-1)*x2^(a2-1)*x3^(a3-1)...xn^(an-1) term.  Thus,
%the number of indices coeffs takes is equal to the dimensionality of x
%(not counting singleton dimensions at the end of coeffs). Note that this
%ordering is the reverse that used in the 1D polyval function that is built
%into Matlab. The number of elements for each index in coeffs is the
%maximum order of that dimension +1. The indices of the multivariate
%polynomials in the different elements of the cell arrays must represent
%the same variable.
%
%This just implements the rules for basic matrix multiplication as in
%Chapter 1.11 of [1], using the appropriate functions for multiplying
%(convn) and summing (polySumMultiDim) multivariate polynomials.
%
%EXAMPLE:
%COnsider multiplying a 2X2 matrix of polynomials by a 2X1 vector of
%polynomials.
% A=cell(2,2);
% B=cell(2,1);
% %A multivariate linear polynomial with 3 variables.
% %-3+4*x1+5*x1*x2*x3
% A11=zeros(2,2,2);
% A11(0+1,0+1,0+1)=-3;
% A11(1+1,0+1,0+1)=4;
% A11(1+1,1+1,1+1)=5;
% A{1,1}=A11;
% %Both diagonals equal and just a single third-order term.
% %12*x1^3
% A12=zeros(4,1,1);
% A12(3+1,0+1,0+1)=12;
% A{1,2}=A12;
% A{2,1}=A12;
% %A second order polynomial only with x1 and x3.
% %6+x1^2-2*x3^2
% A22=zeros(3,1,3);
% A22(0+1,0+1,0+1)=6;
% A22(2+1,0+1,0+1)=1;
% A22(0+1,0+1,2+1)=-2;
% A{2,2}=A22;
% %Now the B vector.
% %-6+x1*x2+x1^2
% B1=zeros(2,3);
% B1(0+1,0+1)=-6;
% B1(1+1,1+1)=1;
% B1(2+1,0+1)=1;
% B{1}=B1;
% %-12-4*x1^2*x2^2
% B2=zeros(3,3);
% B2(0+1,0+1)=-12;
% B2(2+1,2+1)=-4;
% B{2}=B2;
% C=multiDimPolyMatMultiply(A,B)
% %One finds that C is a 2X1 cell array where the highest exponent in C{1} is
% %3 and the highest in C{2} is 4. We can evaluate the polynomial at a
% %point. Say
% x=[1;2;3];
% f=@(coeffs)polyValMultiDim(coeffs,x);
% val=cellfun(f,C)
%
%REFERENCES:
%[1] G. H. Golub and C. F. Van Loan, Matrix Computations, 4th ed.
%    Baltimore: Johns Hopkins University Press, 2013.
%
%December 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

m=size(A,1);
r=size(A,2);
n=size(B,2);

C=num2cell(zeros(m,n));

for j=1:n
    for k=1:r
        for i=1:m
           polyProd=convn(A{i,k},B{k,j});
           C{i,j}=polySumMultiDim(C{i,j},polyProd);
        end
    end
end
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function l = gaussian2PriorLogProb(prior, x)

% GAUSSIAN2PRIORLOGPROB Log probability of Gaussian prior.
% COPYRIGHT: Neil D. Lawrence 2004
% MODIFICATIONS: Andreas C. Damianou, 2013
% SHEFFIELDML

% Compute log prior
x_c = x - prior.mean;
l = length(prior.mean) * (-.5*sum(sum(prior.precision*x_c.*x_c + log(2*pi) - log(prior.precision))));


function [W_sorted, ind_sort, modularity, com_id, com_sizes] = gsp_sort_nodes(W, recursive, self_loops)
%GSP_SORT_NODES Sort nodes using the Louvain clustering method
%   Usage:  [Wout, ind, modularity, com_id, com_sizes] = gsp_sort_nodes(W)
%           [Wout, ind, modularity, com_id, com_sizes] = gsp_sort_nodes(W, recursive)
%           [Wout, ind, modularity, com_id, com_sizes] = gsp_sort_nodes(W, recursive, self_loops)
%
%   Inputs:
%         W         : Weigthed adjacency matrix. Can contain self-loops
%         recursive : Use recursive computation (default: 1)
%         self_loops: Take into account self loops (default: 0)
%
%   Outputs:
%         Wout      : Sorted weighted adjacency matrix
%         ind       : Indices used for sorting:       Wout = W(ind, ind)
%         modularity: Modularity of final clustering used for sorting
%         com_id    : Community ID of each node in the initial W
%         com_sizes : Number of nodes in each community
%
%   gsp_sort_nodes(W) sorts the nodes of the weighted adjacency matrix $W$
%   by clustering them according to the "Louvain" method.
%
%   By default the clustering is recursive and the level of recursion that
%   gives the maximum modularity is kept. The nodes of the graph are sorted
%   according to the clusters given.
%
%   The clustering method is using code given free online by Antoine
%   Scherrer available here:
%   https://perso.uclouvain.be/vincent.blondel/research/louvain.html
%
%   Example:::
%
%         G = gsp_nn_graph([[randn(100, 1)-1; (randn(100, 1)+7)], ...
%           [randn(50, 1); (randn(100,1)-4); (randn(50, 1)*2+8)]]);
%         [Wout, ind, modularity, com_id, com_sizes] = gsp_sort_nodes(G.W);
%         figure; imagesc(Wout); title('sorted adjacency matrix');
%         figure; gsp_plot_signal(G, ind);
%
%
%
%   See also: cluster_jl
%
%   References: blondel2008fast
%

% Author: Vassilis Kalofolias
% Date: June 2016

if nargin < 2
    % Use recursive clustering and pick the level with the highest modularity
    recursive = 1;
end
if nargin < 3
    % Don't use self loops by default
    self_loops = 0;
end
debug = 0;
verbose = 0;

COMTY = cluster_jl(W, recursive, self_loops, debug, verbose);

% Keep as best the splitting with highest modularity
ind_best_splitting = argmax(COMTY.MOD);

% Which node belongs to which community
com_id = COMTY.COM{ind_best_splitting};

% Sort according to the communities
[~, ind_sort] = sort(com_id);

modularity = COMTY.MOD(ind_best_splitting);
com_sizes = COMTY.SIZE{ind_best_splitting};

W_sorted = W(ind_sort, ind_sort);