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

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

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

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

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

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

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

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

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

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

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

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

    u = zeros(Ndof,1);

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

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

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

    end % end of getbdP2

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

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

% Solver
tic;

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

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


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

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

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

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

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

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

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

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


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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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


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

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

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

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

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

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

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

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

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

% Set up the small matrices

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

if p==1

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

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

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

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

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

% Place the result
if p==1

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

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

end;


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

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

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

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

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

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

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

Cbn_new=mx_b*Cbn*mx_a;

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

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

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


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

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

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

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

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

  for step_num = 1 : step_max

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

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

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

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

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

    fxc = p00_fx ( prob, xc );

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

  end

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

  return
end


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

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

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

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

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

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

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

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

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

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


function linpack_d_test25 ( )

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

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

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

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

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

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

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

  return
end


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

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

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

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

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

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

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

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


function g = diskfun(f, varargin)
% DISKFUN returns a diskfun representing the BALLFUN F evaluated at a
% planar slice.
%
%   G = DISKFUN(F) is the slice of F in the XY plane.
%
%   G = DISKFUN(F, 'x', C) is the slice of F in the plane X = C,
%   scaled to the unit disk; G is a diskfun.
%
%   G = DISKFUN(F, 'y', C) is the slice of F in the plane Y = C,
%   scaled to the unit disk; G is a diskfun.
%
%   G = DISKFUN(F, 'z', C) is the slice of F in the plane Z = C,
%   scaled to the unit disk; G is a diskfun.
%
%   G = DISKFUN(F, PHI, THETA, PSI, C) rotates F using Euler angles phi, theta,
%   and psi with the ZXZ convention and then evaluates it in the plane Z = C
%   (C = 0 by default)
%
% % See also BALLFUN/SPHEREFUN.

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

if isempty( f )
   g = diskfun();
   return
end

% Parse user inputs to get the Euler angles
[phi, theta, psi, c] = parseInputs(varargin{:});

% Throw an error if c>1 or c<1
if abs(c) > 1
    error('CHEBFUN:BALLFUN:diskfun:sliceNotInBall',...
        ['The specified slice does not lie in unit ball.']);
end

% Rotate f using Euler angles phi, theta and psi
f = rotate(f, phi, theta, psi);

% Get the size
[m,n,~] = size(f);

% If n is odd, make it even
m = m + 1-mod(m,2);
n = n + mod(n,2);

% Evaluation points in [c, 1]
rho = chebpts(m)*sqrt(1-c^2);
rho = rho(ceil(m/2):end);

% Evaluation points in [-pi,pi)
lambda = pi*trigpts(n);

% Build the grid and evaluate at the plane Z = C
r  = sqrt(rho.^2+c^2);

theta = atan2(rho,c);
G = zeros(length(r),n);
for i = 1:length(r)
   G(i,:) = fevalm(f, r(i), lambda, theta(i));
end

% Return the diskfun
g = diskfun(real(G));
end

function [phi, theta, psi, c] = parseInputs(varargin)
% Parse user inputs to DISKFUN.
c = 0;
if nargin == 0
    phi = 0;
    theta = 0;
    psi = 0;
elseif ischar(varargin{1})
    phi = 0;
    if strcmp(varargin{1},'x')
        theta = -pi/2;
        psi = pi/2;
        % Evaluate at Y-Z plane
    elseif strcmp(varargin{1},'y')
        % Evaluate at X-Z plane
        theta = -pi/2;
        psi = 0;
    elseif strcmp(varargin{1},'z')
        % Evaluate at X-Y plane
        theta = 0;
        psi = 0;
    end
    if nargin >= 2
       c = varargin{2};
    end
else
    phi = varargin{1};
    theta = 0;
    psi = 0;
    if nargin >= 2
        theta = varargin{2};
    end
    if nargin >= 3
        psi = varargin{3};
    end
    if nargin >= 4
        c = varargin{4};
    end
end
end


function [score] = EMRtest(x,model)
% [score] = EMRtest(x,model): Efficient Manifold Ranking for out-of-sample
%                             retrieval
% Input:
%       - x:  the query point, row vector.
%       - model:  the model learned by EMR
%
% Output:
%       - score: the ranking scores for each point in the database
%
% Usage:
%
%      See: http://www.zjucadcg.cn/dengcai/Data/Examples.html#EMR
%
%Reference:
%
%	 Bin Xu, Jiajun Bu, Chun Chen, Deng Cai, Xiaofei He, Wei Liu, Jiebo
%	 Luo, "Efficient Manifold Ranking for Image Retrieval",in Proceeding of
%	 the 34th International ACM SIGIR Conference on Research and
%	 Development in Information Retrieval (SIGIR), 2011, pp. 525-534.
%
%   version 2.0 --Feb./2012
%   version 1.0 --Sep./2010
%
%   Written by Bin Xu (binxu986 AT gmail.com)
%              Deng Cai (dengcai AT gmail.com)

r = model.r;
a = model.a;
p = size(model.landmarks,1);

% Z construction
D = EuDist2(x,model.landmarks);
[dump,idx] = sort(D);
dump = dump(1:r)/dump(r);
dump = 0.75 * (1 - dump.^2);
z = sparse(idx(1:r),1,dump,p,1);

Z = [model.Z z];
Z = Z';

nSmp =size(Z,1);

y0 = zeros(nSmp,1);
y0(end) = 1;

% Efficient Ranking
feaSum = full(sum(Z,1));
D = Z*feaSum';
D = max(D, 1e-12);
D = D.^(-.5);
H = spdiags(D,0,nSmp,nSmp)*Z;

C = speye(p);
A = H'*H-(1/a)*C;

tmp = H'*y0;
tmp = A\tmp;
score = y0 - H*tmp;

score(end) = [];


function [P3, S_bar, V, Z, RO, Tr] = random_nr_motion(T, J, K, state)
% [P3, S_bar, V, Z, RO, Tr] = random_nr_motion(T, J, K, state)
%
%  INPUT:
%
%  T           - number of frames
%  J           - number of points
%  K           - number of deformation basis
%
%  OUTPUT:
%
%  P3          - (3*T) x J 3D-motion matrix:    P3([t t+T t+2*T],:) contains the 3D coordinates of the J points at time t
%  S_bar       - shape average:                 3 x J matrix
%  V           - deformation shapes:            (3*K) x J matrix     ( V((n-1)*3+[1:3],:) contains the n-th deformation basis )
%  Z           - deformation weights:           T x K matrix
%  RO          - rotation:                      cell array           ( RO{t} gives the rotation matrix at time t )
%  Tr          - translation:                   T x 2 matrix

rand('state', state);
randn('state', state);

S_bar = rand(3, J);

[q,r] = qr(rand(3*J));

V = zeros(3*K, J);
for kk=1:K,
   V(1+(kk-1)*3:3*kk, :) = reshape(q(:,kk), 3, J);
end

Z = randn(T,K);
Tr = randn(T,2);

a = (rand(T,1)-0.5)*2*pi;
b = (rand(T,1)-0.5)*2*pi;
c = (rand(T,1)-0.5)*2*pi;
P3 = zeros(3*T, J);
for t=1:T,
   R1 = [1 0 0; 0 cos(a(t)) -sin(a(t)); 0 sin(a(t)) cos(a(t))];
   R2 = [cos(b(t)) 0 sin(b(t)); 0 1 0; -sin(b(t)) 0 cos(b(t))];
   R3 = [cos(c(t)) -sin(c(t)) 0; sin(c(t)) cos(c(t)) 0; 0 0 1];

   RO{t} = R1*R2*R3;

   Sdef = S_bar;
   for kk = 1:K,
      Sdef = Sdef+Z(t,kk)*V((kk-1)*3+[1:3],:);
   end;
   Sdef = RO{t}*Sdef +[Tr(t,:)'; 0]*ones(1, J);

   P3([t t+T t+2*T], :) = Sdef;
end


function [texture structure] = structure_texture_decomposition_rof(im, theta, nIters, alp)
%
% Decompose the input IMAGE into structure and texture parts using the
% Rudin-Osher-Fatemi method. The final output is a linear combination
% of the decomposed texture and the structure parts.
%
% According to Wedel etal "An Improved Algorithm for TV-L1 optical flow"
%       equations (8)-(10)
%
% Test code
% [im1, im2, tu, tv] = read_image_flow_tune_para(4,0);
% [t1 s1] = structure_texture_decomposition_rof(im1);
% [t2 s2] = structure_texture_decomposition_rof(im2);
% indx = ~isnan(tu) | ~isnan(tv);
% uv = cat(3, tu, tv);
% uv(isnan(uv)) = 0;
% figure; imshow(-abs(partial_deriv(cat(3,im1, im2), uv)).*indx, []); title('original');
% figure; imshow(-abs(partial_deriv(cat(3,s1, s2), uv)).*indx, []);  title('structure');
% figure; imshow(-abs(partial_deriv(cat(3,t1, t2), uv)).*indx, []);  title('texture');
% tmp = partial_deriv(cat(3,t1, t2, uv);
% figure; imshow(t1, []); figure; imshow(s1, []);

%   Author: Deqing Sun, Department of Computer Science, Brown University
%   Contact: dqsun@cs.brown.edu
%   $Date: 2009 $
%
% Copyright 2009-2010, Brown University, Providence, RI. USA
%
%                          All Rights Reserved
%
% All commercial use of this software, whether direct or indirect, is
% strictly prohibited including, without limitation, incorporation into in
% a commercial product, use in a commercial service, or production of other
% artifacts for commercial purposes.
%
% Permission to use, copy, modify, and distribute this software and its
% documentation for research purposes is hereby granted without fee,
% provided that the above copyright notice appears in all copies and that
% both that copyright notice and this permission notice appear in
% supporting documentation, and that the name of the author and Brown
% University not be used in advertising or publicity pertaining to
% distribution of the software without specific, written prior permission.
%
% For commercial uses contact the Technology Venture Office of Brown University
%
% THE AUTHOR AND BROWN UNIVERSITY DISCLAIM ALL WARRANTIES WITH REGARD TO
% THIS SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND
% FITNESS FOR ANY PARTICULAR PURPOSE.  IN NO EVENT SHALL THE AUTHOR OR
% BROWN UNIVERSITY BE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL
% DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR
% PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS
% ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF
% THIS SOFTWARE.

if nargin == 1
    theta   = 1/8;
    nIters  = 100;
    alp     = 0.95;  % alp = 0.75 results in 4:1
end;

% Rescale the input image to [-1 1]
IM   = scale_image(im, -1,1);

% Backup orginal images
im   = IM;

% stepsize
delta = 1.0/(4.0*theta);

for iIm = 1:size(im,3)

    % Initialize dual variable p to be 0
    p = zeros([size(im,1) size(im,2) 2]);

    % Gradient descend
    I = squeeze(IM(:,:,iIm));

    for iter = 1:nIters

        % Compute divergence        eqn(8)
        div_p = imfilter(p(:,:,1), [-1 1 0], 'corr', 0)+ ...
                imfilter(p(:,:,2), [-1 1 0]', 'corr', 0);

        I_x = imfilter(I+theta*div_p, [-1 1], 'replicate');

        I_y = imfilter(I+theta*div_p, [-1 1]', 'replicate');

        % Update dual variable      eqn(9)
        p(:,:,1) = p(:,:,1) + delta*(I_x);
        p(:,:,2) = p(:,:,2) + delta*(I_y);

        % Reproject to |p| <= 1     eqn(10)
        reprojection = max(1.0, sqrt(p(:,:,1).^2 + p(:,:,2).^2));
        p(:,:,1) = p(:,:,1)./reprojection;
        p(:,:,2) = p(:,:,2)./reprojection;

    end

    % compute divergence
    div_p = imfilter(p(:,:,1), [-1 1 0], 'corr', 0)+ ...
            imfilter(p(:,:,2), [-1 1 0]', 'corr', 0);

    % compute structure component
    IM(:,:,iIm) = I + theta*div_p;

end;

texture   = squeeze(scale_image(im - alp*IM, 0, 255));

if nargout == 2
    structure = squeeze(scale_image(IM, 0, 255)); %(u-min(u(:)))/(max(u(:))-min(u(:))) - 1;
end;


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

function y = sprice_2(a, b, r, n, f, k, t,cp)
% sabr prices using an admissible region kl <= k <= ku where sabr is used
% for 0 <= k <= kl
% we use a put pricing function f(x) = K^mu exp(a + bx + cx^2)
% for ku < k < +infty
% we use a call pricing function f(x) = K^(-nu) exp(a + bx^(-1) + cx(-2)
eps = 1e-004;

%find index where density is positive
s = @(x) psabr(a, b, r, n, f, x, t);
index = find(s(k)<0,1,'last');

if isempty(index)
   kl = .25 *f;
else
   kl = max(.25 * f,k(index+1));% lower strike level (free parameter)
end

%kl = .5 * f;                               % lower strike level

s = @(x) sprice(a, b, r, n, f, x, t,-1);    % sabr price (-1) for puts

s1 = s(kl-eps);                             % for calc of derivatives
s2 = s(kl);                                 % for calc of derivatives
s3 = s(kl+eps);                             % for calc of derivatives

V1 = log(s2);                               % log put price

U2 = (s3-s1)/(2*eps);                           % derivative of the price
V2 = U2/s2;                                 % derivative of the log price

U3 = (s3-2*s2+s1)/eps^2;                    % second derivative of the price
V3 = U3/s2 - V2^2;                     % second derivative of the log price

% fix mu and solve
%       V1 = mu log kl + a + b kl + c kl^2
%       V2 = mu / kl + b + 2c kl
%       V3 = - mu / kl^2 + 2c

mu = -V3*kl^2; bl = V2 - mu / kl; al = V1 - mu *log(kl) - bl * kl;

ku = 15 * f;                               % upper strike level
s = @(x) sprice(a, b, r, n, f, x, t,1);     % sabr price (1) for calls
s1 = s(ku-eps);                             % for calc of derivatives
s2 = s(ku);                                 % for calc of derivatives
s3 = s(ku+eps);                             % for calc of derivatives

V1 = log(s2);                               % log call price

U2 = (s3-s1)/(2*eps);                           % derivative of the price
V2 = U2/s2;                                 % derivative of the log price

U3 = (s3-2*s2+s1)/eps^2;                    % second derivative of the price
V3 = U3/s2 - (U2/s2)^2;                     % second derivative f the price

% fix nu and solve
%       V1 = -nu log ku + a + b/ku + c/ku^2
%       V2 = -nu / ku - b / ku^2 - 2c/ku^3
%       V3 = nu / ku^2 + 2 b / ku^3 - 6 c / ku^4

nu = 2; cu = (-1.5*nu / ku + .5*V3 * ku - V2)*ku^3/5;
bu = -ku^2*(V2 + nu/ku +2*cu/ku^3);
au = V1 + nu * log(ku) - bu / ku - cu / ku^2;

% the sabr volatility for the admissible region kl <= k <= ku
sigma = svol(a,b,r,n,f,k((kl<=k)&(k<=ku)),t);

    d1= (log(f./k((kl<=k)&(k<=ku)))+(0.5*t*sigma.*sigma))./(sqrt(t)*sigma); % BS d1 with sabr vol
    d2= (log(f./k((kl<=k)&(k<=ku)))-(0.5*t*sigma.*sigma))./(sqrt(t)*sigma); % BS d2 with sabr vol

if cp==1
    yu = k(k>ku).^(-nu) .* exp(au+bu./k(k>ku)+cu./k(k>ku).^2);              % call price ku < k < +infty
    ym = f.*normcdf( d1,0,1)-k((kl<=k)&(k<=ku)).*normcdf( d2,0,1);          % admissible region kl <= k <= ku
    yl = k(k<kl).^mu .* exp(al + bl.*k(k<kl)) + f - k(k<kl); % call price using call-put parity 0<= k < kl
else
    yl = k(k<kl).^mu .* exp(al + bl .* k(k<kl));               % put price 0<= k < kl
    ym = k((kl<=k)&(k<=ku)).*normcdf(-d2,0,1)-f.*normcdf(-d1,0,1);          % admissible region kl <= k <= ku
    yu = k(k>ku).^(-nu) .* exp(au+bu./k(k>ku)+cu./k(k>ku).^2) - f + k(k>ku);% put prices using call-out parity ku < k < +infty
end

y = [yl ym yu];                                                             % output
end


function [transParameters,nu,lambda]=igarch_transform(parameters,p,q,errorType,constant)
% IGARCH(P,Q) parameter transformation.  Used to map parameters from a IGARCH
% process to the positive unit simplex. Used in the estimation of IGARCH.
%
% USAGE:
%   [TRANSPARAMETERS,NU,LAMBDA]=igarch_transform(PARAMETERS,P,Q,ERRORTYPE,CONSTANT)
%
% INPUTS:
%   PARAMETERS       - Column parameter vector
%   P                - Positive, scalar integer representing the number of
%                      symmetric innovations
%   Q                - Non-negative, scalar integer representing the number
%                      of lags of conditional variance (0 for ARCH)
%   ERRORTYPE       - One of:
%                        1 - Gaussian Innovations
%                        2 - T-distributed errors
%                        3 - Generalized Error Distribution
%                        4 - Skewed T distribution
%   CONSTANT         - 1 if model includes a constant, 0 otherwise
%
% OUTPUTS:
%   TRANSPARAMETERS - A CONSTANT+p+q-1 column vector of transformed parameters corresponding to
%                      [omega,alpha(1),...,alpha(p) beta1 ... beta(q-1)]'
%                      where the final beta has been excluded
%   NU               - Distribution kurtosis parameter, empty if not applicable
%   LAMBDA           - Distribution asymmetry parameter, empty if not applicable
%
% COMMENTS:
%   Input parameters must satisfy:
%    (1) omega > 0
%    (2) alpha(i) >= 0 for i = 1,2,...,p
%    (3) beta(i)  >= 0 for i = 1,2,...,q
%    (4) sum(alpha(i) + beta(j)) = 1 for i = 1,2,...p and j = 1,2,...q
%    (5) nu>2 of Students T and nu>1 for GED
%    (6) -.99<lambda<.99 for Skewed T
%
% See also IGARCH

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 3    Date: 9/1/2005

%Constraints are omega>0
%nu>2.01 or 1.01
%-.99<lambda<.99
% alpha>0
% gamma>-alpha(i)
% beta>0
% sum(alpha)+0.5sum(gamma)+sum(beta)<1

nu=[];
lambda=[];

%Handle nu, >2.01 in the case of a T, 1.01<nu<50 in the case of a GED
%T uses square, ged uses a logistic
if errorType==2 || errorType==4
    nu=parameters(p+q+constant);
    nu=sqrt(nu-2.01);
elseif errorType==3
    nu=parameters(p+q+constant);
    temp=(nu-1)/49;
    nu=log(temp/(1-temp));
end

%Lambda must be between -.99 and .99.  Use a logistic
if errorType==4
    lambda=parameters(p+q+constant+1);
    temp=(lambda+0.995)/1.99;
    lambda=log(temp/(1-temp));
end

%Use log transform for omega
if constant
    omega=parameters(1);
    tomega=log(omega);
else
    tomega = [];
end

%Parse the parameters
alpha=parameters(constant+1:p+constant);
beta=parameters(constant+p+1:constant+p+q-1);

%Upper bound to keep it a bit away from 1
UB=.999998;

%Check that the parameters satisfy the necessary constraints
if isempty(beta)
    sumbeta = 0;
else
    sumbeta = sum(beta);
end
if  any(alpha<0) || any(beta<0) || (sum(alpha)+sumbeta)>=UB
    error('These do not conform to the necessary set of restrictions to be transformed.')
end

%Finally, must be certain that none of the parameters are exactly zero, and
%that the alpha2+gamma2>0
alpha(alpha==0)=1e-8;
beta(beta==0)=1e-8;

%Finally, up the upper bound a tiny bit
UB=UB+1e-8*(p+q);

%Set the scale
scale=UB;
%Initialze the transformed alpha
talpha=alpha;
for i=1:p
    %Scale the alpha
    talpha(i)=alpha(i)./scale;
    %Use an inverse logistic
    talpha(i)=log(talpha(i)./(1-talpha(i)));
    %Update the scale
    scale=scale-alpha(i);
end

if q>1
    %Initialize the beta
    tbeta=beta;
    %Iterate over betas
    for i=1:(q-1)
        %Scale the betas
        tbeta(i)=tbeta(i)./scale;
        %Use an inverse logistic
        tbeta(i)=log(tbeta(i)./(1-tbeta(i)));
        %Update the scale
        scale=scale-beta(i);
    end
else
    tbeta = [];
end

%Regroup the parameters
transParameters=[tomega;talpha;tbeta];


function [eta Heta inner_it stop_tCG storedb] ...
                      = tCG(problem, x, grad, eta, Delta, options, storedb)
% tCG - Truncated (Steihaug-Toint) Conjugate-Gradient method
% minimize <eta,grad> + .5*<eta,Hess(eta)>
% subject to <eta,eta> <= Delta^2

% This file is part of Manopt: www.manopt.org.
% This code is an adaptation to Manopt of the original GenRTR code:
% RTR - Riemannian Trust-Region
% (c) 2004-2007, P.-A. Absil, C. G. Baker, K. A. Gallivan
% Florida State University
% School of Computational Science
% (http://www.math.fsu.edu/~cbaker/GenRTR/?page=download)
% See accompanying license file.
% The adaptation was executed by Nicolas Boumal.
% Change log:
%   NB Feb. 12, 2013:
%       We do not project r back to the tangent space anymore: it was not
%       necessary, and as of Manopt 1.0.1, the proj operator does not
%       coincide with this notion anymore.
%   NB April 3, 2013:
%       tCG now also returns Heta, the Hessian at x along eta. Additional
%       esthetic modifications.
%   NB Dec. 2, 2013:
%       If options.useRand is activated, we now make sure the preconditio-
%       ner is not used, as was originally intended in GenRTR. In time, we
%       may want to investigate whether useRand can be modifed to work well
%       with preconditioning too.
%   NB Jan. 9, 2014:
%       Now checking explicitly for model decrease at each iteration. The
%       first iteration is a Cauchy point, which necessarily realizes a
%       decrease of the model cost. If a model increase is witnessed
%       (which is theoretically impossible if a linear operator is used for
%       the Hessian approximation), then we return the previous eta. This
%       ensures we always achieve at least the Cauchy decrease, which
%       should be sufficient for convergence.

% All terms involving the trust-region radius will use an inner product
% w.r.t. the preconditioner; this is because the iterates grow in
% length w.r.t. the preconditioner, guaranteeing that we will not
% re-enter the trust-region.
%
% The following recurrences for Prec-based norms and inner
% products come from [CGT2000], pg. 205, first edition.
% Below, P is the preconditioner.
%
% <eta_k,P*delta_k> =
%          beta_k-1 * ( <eta_k-1,P*delta_k-1> + alpha_k-1 |delta_k-1|^2_P )
% |delta_k|^2_P = <r_k,z_k> + beta_k-1^2 |delta_k-1|^2_P
%
% therefore, we need to keep track of
% 1)   |delta_k|^2_P
% 2)   <eta_k,P*delta_k> = <eta_k,delta_k>_P
% 3)   |eta_k  |^2_P
%
% initial values are given by:
%    |delta_0|_P = <r,z>
%    |eta_0|_P   = 0
%    <eta_0,delta_0>_P = 0
% because we take eta_0 = 0 (if useRand = false).
%
% [CGT2000] Conn, Gould and Toint: Trust-region methods, 2000.

inner = problem.M.inner;

theta = options.theta;
kappa = options.kappa;

if ~options.useRand % and therefore, eta == 0
    Heta = problem.M.zerovec(x);
    r = grad;
    e_Pe = 0;
else % and therefore, no preconditioner
    % eta (presumably) ~= 0 was provided by the caller
    [Heta storedb] = getHessian(problem, x, eta, storedb);
    r = problem.M.lincomb(x, 1, grad, 1, Heta);
    e_Pe = inner(x, eta, eta);
end
r_r = inner(x, r, r);
norm_r = sqrt(r_r);
norm_r0 = norm_r;

% precondition the residual
if ~options.useRand
    [z storedb] = getPrecon(problem, x, r, storedb);
else
    z = r;
end

% compute z'*r
z_r = inner(x, z, r);
d_Pd = z_r;

% Initial search direction
delta  = problem.M.lincomb(x, -1, z);
if ~options.useRand % and therefore, eta == 0
    e_Pd = 0;
else % and therefore, no preconditioner
    e_Pd = inner(x, eta, delta);
end

% If the Hessian or a linear Hessian approximation is in use, it is
% theoretically guaranteed that the model value decreases monotonically
% with each iteration of tCG. Hence, there is no need to monitor the model
% value. But, when a nonlinear Hessian approximation is used (such as the
% built-in finite-difference approximation for example), the model may
% increase. It is then important to terminate the tCG iterations and return
% the previous (the best-so-far) iterate. The variable below will hold the
% model value.
model_fun = @(eta, Heta) inner(x, grad, eta) + .5*inner(x, eta, Heta);
if ~options.useRand
    model_value = 0;
else
    model_value = model_fun(eta, Heta);
end

% Pre-assume termination b/c j == end
stop_tCG = 5;

% Begin inner/tCG loop
j = 0;
for j = 1 : options.maxinner

    [Hdelta storedb] = getHessian(problem, x, delta, storedb);

    % Compute curvature
    d_Hd = inner(x, delta, Hdelta);

    alpha = z_r/d_Hd;
    % <neweta,neweta>_P =
    % <eta,eta>_P + 2*alpha*<eta,delta>_P + alpha*alpha*<delta,delta>_P
    e_Pe_new = e_Pe + 2.0*alpha*e_Pd + alpha*alpha*d_Pd;

    if options.debug > 2,
        fprintf('DBG:   (r,r)  : %e\n',r_r);
        fprintf('DBG:   (d,Hd) : %e\n',d_Hd);
        fprintf('DBG:   alpha  : %e\n',alpha);
    end

    % Check against negative curvature and trust-region radius violation.
    % If either condition triggers, we bail out.
    if d_Hd <= 0 || e_Pe_new >= Delta^2,
        % want
        %  ee = <eta,eta>_prec,x
        %  ed = <eta,delta>_prec,x
        %  dd = <delta,delta>_prec,x
        tau = (-e_Pd + sqrt(e_Pd*e_Pd + d_Pd*(Delta^2-e_Pe))) / d_Pd;
        if options.debug > 2,
            fprintf('DBG:     tau  : %e\n', tau);
        end
        eta  = problem.M.lincomb(x, 1,  eta, tau,  delta);
        Heta = problem.M.lincomb(x, 1, Heta, tau, Hdelta);
        if d_Hd <= 0,
            stop_tCG = 1;     % negative curvature
        else
            stop_tCG = 2;     % exceeded trust region
        end
        break;
    end

    % No negative curvature and eta_prop inside TR: accept it
    e_Pe = e_Pe_new;
    new_eta  = problem.M.lincomb(x, 1,  eta, alpha,  delta);
    new_Heta = problem.M.lincomb(x, 1, Heta, alpha, Hdelta);

    % Verify that the model cost decreased in going from eta to new_eta. If
    % the cost increased (which can only occur if the Hessian approximation
    % is nonlinear), then we return the previous eta (which necessarily is
    % the best reached so far, according to the model cost). Otherwise, we
    % accept the new eta and go on.
    new_model_value = model_fun(new_eta, new_Heta);
    if new_model_value > model_value
        stop_tCG = 6;
        break;
    end

    eta = new_eta;
    Heta = new_Heta;

    % Update the residual
    r = problem.M.lincomb(x, 1, r, alpha, Hdelta);

    % Compute new norm of r
    r_r = inner(x, r, r);
    norm_r = sqrt(r_r);

    % Check kappa/theta stopping criterion
    if j >= options.mininner && norm_r <= norm_r0*min(norm_r0^theta, kappa)
        % Residual is small enough to quit
        if kappa < norm_r0^theta,
            stop_tCG = 3;  % linear convergence
        else
            stop_tCG = 4;  % superlinear convergence
        end
        break;
    end

    % Precondition the residual
    if ~options.useRand
        [z storedb] = getPrecon(problem, x, r, storedb);
    else
        z = r;
    end

    % Save the old z'*r
    zold_rold = z_r;
    % Compute new z'*r
    z_r = inner(x, z, r);

    % Compute new search direction
    beta = z_r/zold_rold;
    delta = problem.M.lincomb(x, -1, z, beta, delta);

    % Update new P-norms and P-dots [CGT2000, eq. 7.5.6 & 7.5.7]
    e_Pd = beta*(e_Pd + alpha*d_Pd);
    d_Pd = z_r + beta*beta*d_Pd;

end  % of tCG loop
inner_it = j;

end


function g =  getGroupOverlap(row, col)

    N = row*col;

    g = sparse(zeros(N,1));
    g = diag(g);

    %% build overlapping group

    % top let corner group

    g(1, 1 ) = 1;
    g(1, 2 ) = 1;
    g(1, 1+row ) = 1;
    g(1, 2+row ) = 1;

     % bottom let corner group

    g(row, row-1 ) = 1;
    g(row, row ) = 1;
    g(row, row-1+row ) = 1;
    g(row, row+row ) = 1;

    % top right corner group

    g((col-1)*row+1, (col-1)*row+1 ) = 1;
    g((col-1)*row+1, (col-2)*row+1 ) = 1;
    g((col-1)*row+1, (col-1)*row+2 ) = 1;
    g((col-1)*row+1, (col-2)*row+2 ) = 1;

    % bottom right corner group

    g((col-1)*row+row , (col-1)*row+row-1 ) = 1;
    g((col-1)*row+row , (col-2)*row+row-1 ) = 1;
    g((col-1)*row+row , (col-1)*row+row ) = 1;
    g((col-1)*row+row , (col-2)*row+row ) = 1;

    % boundary group

    for i=2:col-1
        % top row rgoup

        j = 1;

        g( (i-1)*row+j  , (i-2)*row+j   ) = 1;
        g( (i-1)*row+j  , (i-2)*row+j+1 ) = 1;

        g( (i-1)*row+j  , (i-1)*row+j   ) = 1;
        g( (i-1)*row+j  , (i-1)*row+j+1 ) = 1;

        g( (i-1)*row+j  , i*row+j   ) = 1;
        g( (i-1)*row+j  , i*row+j+1 ) = 1;

        % bottom row group

        j = row;

        g( (i-1)*row+j  , (i-2)*row+j-1 ) = 1;
        g( (i-1)*row+j  , (i-2)*row+j   ) = 1;

        g( (i-1)*row+j  , (i-1)*row+j-1 ) = 1;
        g( (i-1)*row+j  , (i-1)*row+j   ) = 1;

        g( (i-1)*row+j  , i*row+j-1 ) = 1;
        g( (i-1)*row+j  , i*row+j   ) = 1;

    end

    for j=2:row-1
          % left column rgoup

        i=1;

        g( (i-1)*row+j  , (i-1)*row+j-1 ) = 1;
        g( (i-1)*row+j  , (i-1)*row+j   ) = 1;
        g( (i-1)*row+j  , (i-1)*row+j+1 ) = 1;
        g( (i-1)*row+j  , i*row+j-1 ) = 1;
        g( (i-1)*row+j  , i*row+j   ) = 1;
        g( (i-1)*row+j  , i*row+j+1 ) = 1;

        % right  column group

        i=col;

        g( (i-1)*row+j  , (i-2)*row+j-1 ) = 1;
        g( (i-1)*row+j  , (i-2)*row+j   ) = 1;
        g( (i-1)*row+j  , (i-2)*row+j+1 ) = 1;
        g( (i-1)*row+j  , (i-1)*row+j-1 ) = 1;
        g( (i-1)*row+j  , (i-1)*row+j   ) = 1;
        g( (i-1)*row+j  , (i-1)*row+j+1 ) = 1;

    end

    for i=2:col-1
        for j=2:row-1

            g( (i-1)*row+j  , (i-2)*row+j-1 ) = 1;
            g( (i-1)*row+j  , (i-2)*row+j   ) = 1;
            g( (i-1)*row+j  , (i-2)*row+j+1 ) = 1;
            g( (i-1)*row+j  , (i-1)*row+j-1 ) = 1;
            g( (i-1)*row+j  , (i-1)*row+j   ) = 1;
            g( (i-1)*row+j  , (i-1)*row+j+1 ) = 1;
            g( (i-1)*row+j  , i*row+j-1 ) = 1;
            g( (i-1)*row+j  , i*row+j   ) = 1;
            g( (i-1)*row+j  , i*row+j+1 ) = 1;

        end
    end

end


function Y = symmetrize(X,grps)
%SYMMETRIZE Symmetrize a tensor X in specified modes.
%
%   Y = symmetrize(X) will symmetrize a tensor X with respect to all
%   modes so that Y is symmetric with respect to any permutation of
%   indices.
%
%   Y = symmetrize(X,MODES) will symmetrize a tensor X with respect to the
%   modes specified by the vector MODES of mode indices. The second
%   argument may alternatively be a cell array of vectors of modes to,
%   e.g., specify that it should be symmetric with respect to mode [1 3] as
%   well as [2 4].
%
%   See also TENSOR, TENSOR/ISSYMMETRIC.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

%T. Kolda, April 2011
n = ndims(X);
sz = size(X);

% Check that grps exists; if not, create it.
if ~exist('grps','var')
    grps = {1:n};
end

% Check that grps is a cell array.
if ~iscell(grps)
    grps = {grps};
end

if ~isnumeric(grps{1})
    error('MODES must be numeric');
end

% Check tensor dimensions for compatibility with symmetrization
ngrps = length(grps);
for i = 1:ngrps
    dims = grps{i};
    for j = dims(2:end)
        if sz(j) ~= sz(dims(1))
            error('Dimension mismatch for symmetrization');
        end
    end
end

% Check for no overlap in the sets
for i = 1:ngrps
    for j = i+1:ngrps
        if ~isempty(intersect(grps{i},grps{j}))
            error('Cannot haver overlapping symmetries');
        end
    end
end

% Create the combinations for each symmetrized subset
combos = cell(ngrps,1);
for i = 1:ngrps
    combos{i} = perms(grps{i});
end

% Create all the permuations to be averaged
total_perms = prod(cellfun(@length,combos));
sym_perms = repmat(1:n, total_perms, 1);
for i = 1:ngrps
    ntimes = prod(cellfun(@length,combos(1:i-1)));
    ncopies = prod(cellfun(@length,combos(i+1:end)));
    nelems = length(combos{i});

    idx = 1;
    for j = 1:ntimes
        for k = 1:nelems
            for l = 1:ncopies
                sym_perms(idx,grps{i}) = combos{i}(k,:);
                idx = idx + 1;
            end
        end
    end
end

% Create an average tensor
Y = tenzeros(size(X));
for i = 1:total_perms
    Y = Y + permute(X,sym_perms(i,:));
end
Y = Y / total_perms;

% It's not *exactly* symmetric due to oddities in differently ordered
% summations and so on, so let's fix that.
% Idea borrowed from Gergana Bounova:
% http://www.mit.edu/~gerganaa/downloads/matlab/symmetrize.m
for i = 1:total_perms
    Z = permute(Y,sym_perms(i,:));
    Y.data(:) = max(Y.data(:),Z.data(:));
end


% RANSACFITFUNDMATRIX - fits fundamental matrix using RANSAC
%
% Usage:   [F, inliers] = ransacfitfundmatrix(x1, x2, t)
%
% Arguments:
%          x1  - 2xN or 3xN set of homogeneous points.  If the data is
%                2xN it is assumed the homogeneous scale factor is 1.
%          x2  - 2xN or 3xN set of homogeneous points such that x1<->x2.
%          t   - The distance threshold between data point and the model
%                used to decide whether a point is an inlier or not.
%                Note that point coordinates are normalised to that their
%                mean distance from the origin is sqrt(2).  The value of
%                t should be set relative to this, say in the range
%                0.001 - 0.01
%
% Note that it is assumed that the matching of x1 and x2 are putative and it
% is expected that a percentage of matches will be wrong.
%
% Returns:
%          F       - The 3x3 fundamental matrix such that x2'Fx1 = 0.
%          inliers - An array of indices of the elements of x1, x2 that were
%                    the inliers for the best model.
%
% See Also: RANSAC, FUNDMATRIX

% Copyright (c) 2004-2005 Peter Kovesi
% 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.

% February 2004  Original version
% August   2005  Distance error function changed to match changes in RANSAC

function [F, inliers] = ransacfitfundmatrix(x1, x2, t, feedback)

    if ~all(size(x1)==size(x2))
        error('Data sets x1 and x2 must have the same dimension');
    end

    if nargin == 3
	feedback = 0;
    end

    [rows,npts] = size(x1);
    if rows~=2 & rows~=3
        error('x1 and x2 must have 2 or 3 rows');
    end

    if rows == 2    % Pad data with homogeneous scale factor of 1
        x1 = [x1; ones(1,npts)];
        x2 = [x2; ones(1,npts)];
    end

    % Normalise each set of points so that the origin is at centroid and
    % mean distance from origin is sqrt(2).  normalise2dpts also ensures the
    % scale parameter is 1.  Note that 'fundmatrix' will also call
    % 'normalise2dpts' but the code in 'ransac' that calls the distance
    % function will not - so it is best that we normalise beforehand.
    [x1, T1] = normalise2dpts(x1);
    [x2, T2] = normalise2dpts(x2);

    s = 8;  % Number of points needed to fit a fundamental matrix. Note that
            % only 7 are needed but the function 'fundmatrix' only
            % implements the 8-point solution.

    fittingfn = @fundmatrix;
    distfn    = @funddist;
    degenfn   = @isdegenerate;
    % x1 and x2 are 'stacked' to create a 6xN array for ransac
    [F, inliers] = ransac([x1; x2], fittingfn, distfn, degenfn, s, t, feedback);

    % Now do a final least squares fit on the data points considered to
    % be inliers.
    F = fundmatrix(x1(:,inliers), x2(:,inliers));

    % Denormalise
    F = T2'*F*T1;

%--------------------------------------------------------------------------
% Function to evaluate the first order approximation of the geometric error
% (Sampson distance) of the fit of a fundamental matrix with respect to a
% set of matched points as needed by RANSAC.  See: Hartley and Zisserman,
% 'Multiple View Geometry in Computer Vision', page 270.
%
% Note that this code allows for F being a cell array of fundamental matrices of
% which we have to pick the best one. (A 7 point solution can return up to 3
% solutions)

function [bestInliers, bestF] = funddist(F, x, t);

    x1 = x(1:3,:);    % Extract x1 and x2 from x
    x2 = x(4:6,:);

    if iscell(F)  % We have several solutions each of which must be tested

	nF = length(F);   % Number of solutions to test
	bestF = F{1};     % Initial allocation of best solution
	ninliers = 0;     % Number of inliers

	for k = 1:nF
	    x2tFx1 = zeros(1,length(x1));
	    for n = 1:length(x1)
		x2tFx1(n) = x2(:,n)'*F{k}*x1(:,n);
	    end

	    Fx1 = F{k}*x1;
	    Ftx2 = F{k}'*x2;

	    % Evaluate distances
	    d =  x2tFx1.^2 ./ ...
		 (Fx1(1,:).^2 + Fx1(2,:).^2 + Ftx2(1,:).^2 + Ftx2(2,:).^2);

	    inliers = find(abs(d) < t);     % Indices of inlying points

	    if length(inliers) > ninliers   % Record best solution
		ninliers = length(inliers);
		bestF = F{k};
		bestInliers = inliers;
	    end
	end

    else     % We just have one solution
	x2tFx1 = zeros(1,length(x1));
	for n = 1:length(x1)
	    x2tFx1(n) = x2(:,n)'*F*x1(:,n);
	end

	Fx1 = F*x1;
	Ftx2 = F'*x2;

	% Evaluate distances
	d =  x2tFx1.^2 ./ ...
	     (Fx1(1,:).^2 + Fx1(2,:).^2 + Ftx2(1,:).^2 + Ftx2(2,:).^2);

	bestInliers = find(abs(d) < t);     % Indices of inlying points
	bestF = F;                          % Copy F directly to bestF

    end

%----------------------------------------------------------------------
% (Degenerate!) function to determine if a set of matched points will result
% in a degeneracy in the calculation of a fundamental matrix as needed by
% RANSAC.  This function assumes this cannot happen...

function r = isdegenerate(x)
    r = 0;


function q = times(q1,q2,takeRight)
% quaternion .* quaternion and quaternion .* vector3d
%
% Syntax
%   q = q1 .* q2
%
%   q = times(q1,q2)
%   q = times(q1,q2,takeRight)
%
% Input
%  q1 - @quaternion
%  q2 - @quaternion
%  takeRight - logical, use as output left or right input
%
% Output
%  q  - @quaternion

if isa(q1,'quaternion') && isa(q2,'quaternion')

  % which input will become the output?
  if nargin == 3
    if takeRight, q = q2; else, q = q1; end
  elseif isa(q1,'rotation')
    q = q1;
  else
    q = q2;
  end

  a1 = q1.a; b1 = q1.b; c1 = q1.c; d1 = q1.d;
  a2 = q2.a; b2 = q2.b; c2 = q2.c; d2 = q2.d;

  %standard algorithm
  q.a = a1 .* a2 - b1 .* b2 - c1 .* c2 - d1 .* d2;
  q.b = b1 .* a2 + a1 .* b2 - d1 .* c2 + c1 .* d2;
  q.c = c1 .* a2 + d1 .* b2 + a1 .* c2 - b1 .* d2;
  q.d = d1 .* a2 - c1 .* b2 + b1 .* c2 + a1 .* d2;

elseif isa(q1,'quaternion') && isa(q2,'double')

  q1.a = q1.a .* q2;
  q1.b = q1.b .* q2;
  q1.c = q1.c .* q2;
  q1.d = q1.d .* q2;
  q = q1;

elseif isa(q2,'quaternion') && isa(q1,'double')

  q2.a = q1 .* q2.a;
  q2.b = q1 .* q2.b;
  q2.c = q1 .* q2.c;
  q2.d = q1 .* q2.d;
  q = q2;

else

  q = rotate(q2,q1);

end