function [u,p,edge,A,eqn,info] = StokesP2P1(node,elem,pde,bdFlag,option)
%% STOKESP2P1 Stokes equation: P2-P1 Taylor-Hood elements.
%
%   [u,p] = STOKESP2P1(node,elem,pde,bdFlag) use quadratic and piceswise
%   linear elements to approximate velocity u and pressure p, repectively.
%
%       -div(mu*grad u) + grad p = f in \Omega,
%                        - div u = 0  in \Omega,
%   with
%       Dirichlet boundary condition        u = g_D  on \Gamma_D,
%       Neumann boundary condition du/dn - np = g_N  on \Gamma_N.
%
%   It is a choice of option.fem in Stokes. Please read Stokes for more
%   information on the input and output.
%
% See also Stokes, Poisson, StokesP2P1
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if ~exist('option','var'), option = []; end

tic;
%% Construct Data Structure
[elem2dof,edge,bdDof] = dofP2(elem);
N = size(node,1);  NT = size(elem,1);  Nu = N+size(edge,1);   Np = N;

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

%% Assemble stiffness matrix for Laplace operator
% generate sparse pattern
ii = zeros(21*NT,1); jj = zeros(21*NT,1);
index = 0;
for i = 1:6
    for j = i:6
        ii(index+1:index+NT) = double(elem2dof(:,i));
        jj(index+1:index+NT) = double(elem2dof(:,j));
        index = index + NT;
    end
end
% quadrature points
if ~isfield(pde,'nu'), pde.nu = []; end
if ~isfield(option,'quadorder')
    % constant viscosity
    option.quadorder = 2;        % default order
    if ~isempty(pde.nu) && isnumeric(pde.nu) % numerical viscosity
        option.quadorder = 3;    % exact for linear diffusion coefficient
    end
end
[lambda, w] = quadpts(option.quadorder);
nQuad = size(lambda,1);
% compute non-zeros
sA = zeros(21*NT,nQuad);
for p = 1:nQuad
    % Dphi at quadrature points
    Dphip(:,:,6) = 4*(lambda(p,1)*Dlambda(:,:,2)+lambda(p,2)*Dlambda(:,:,1));
    Dphip(:,:,5) = 4*(lambda(p,3)*Dlambda(:,:,1)+lambda(p,1)*Dlambda(:,:,3));
    Dphip(:,:,4) = 4*(lambda(p,2)*Dlambda(:,:,3)+lambda(p,3)*Dlambda(:,:,2));
    Dphip(:,:,1) = (4*lambda(p,1)-1).*Dlambda(:,:,1);
    Dphip(:,:,2) = (4*lambda(p,2)-1).*Dlambda(:,:,2);
    Dphip(:,:,3) = (4*lambda(p,3)-1).*Dlambda(:,:,3);
    index = 0;
    for i = 1:6
        for j = i:6
            Aij = 0;
            if isempty(pde.nu) || isnumeric(pde.nu)
                Aij = Aij + w(p)*dot(Dphip(:,:,i),Dphip(:,:,j),2);
            else
                pxy = lambda(p,1)*node(elem(:,1),:) ...
                    + lambda(p,2)*node(elem(:,2),:) ...
                    + lambda(p,3)*node(elem(:,3),:);
                Aij = Aij + w(p)*dot(Dphip(:,:,i),Dphip(:,:,j),2).*pde.d(pxy);
            end
            if ~isempty(pde.nu) && (pde.nu~=1)
                Aij = pde.nu*Aij;
            end
            Aij = Aij.*area;
            sA(index+1:index+NT,p) = Aij;
            index = index + NT;
        end
    end
end
sA = sum(sA,2);
% assemble the matrix
diagIdx = (ii == jj);   upperIdx = ~diagIdx;
A = sparse(ii(diagIdx),jj(diagIdx),sA(diagIdx),Nu,Nu);
AU = sparse(ii(upperIdx),jj(upperIdx),sA(upperIdx),Nu,Nu);
A = A + AU + AU';
A = blkdiag(A,A);
clear Aij ii jj sA

%% Assemble the matrix for divergence operator
Dx = sparse(Np,Nu);
Dy = sparse(Np,Nu);
[lambda, w] = quadpts(2); % (div(P2), P1) is P2
nQuad = size(lambda,1);
for p = 1:nQuad
    % Dphi at quadrature points
    Dphip(:,:,1) = (4*lambda(p,1)-1).*Dlambda(:,:,1);
    Dphip(:,:,2) = (4*lambda(p,2)-1).*Dlambda(:,:,2);
    Dphip(:,:,3) = (4*lambda(p,3)-1).*Dlambda(:,:,3);
    Dphip(:,:,4) = 4*(lambda(p,2)*Dlambda(:,:,3)+lambda(p,3)*Dlambda(:,:,2));
    Dphip(:,:,5) = 4*(lambda(p,3)*Dlambda(:,:,1)+lambda(p,1)*Dlambda(:,:,3));
    Dphip(:,:,6) = 4*(lambda(p,1)*Dlambda(:,:,2)+lambda(p,2)*Dlambda(:,:,1));
    for i = 1:6
        for j = 1:3
            Dxij = 0;
            Dyij = 0;
            Dxij = Dxij + w(p)*Dphip(:,1,i).*lambda(p,j);
            Dyij = Dyij + w(p)*Dphip(:,2,i).*lambda(p,j);
            Dx = Dx + sparse(elem(:,j),double(elem2dof(:,i)),Dxij.*area,Np,Nu);
            Dy = Dy + sparse(elem(:,j),double(elem2dof(:,i)),Dyij.*area,Np,Nu);
        end
    end
end
B = -[Dx Dy];
clear Dxij Dyij Dx Dy Dphip

%% Assemble right hand side
f1 = zeros(Nu,1);
f2 = zeros(Nu,1);
if ~isfield(option,'fquadorder')
    option.fquadorder = 4;   % 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,weight] = quadpts(option.fquadorder);
    % basis values at quadrature points
    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);
    nQuad = size(lambda,1);
    ft1 = zeros(NT,6);
    ft2 = 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),:);
        % function values at quadrature points
        fp = pde.f(pxy);
        % evaluate fp outside.
        for j = 1:6
            ft1(:,j) = ft1(:,j) + fp(:,1).*phi(p,j)*weight(p);
            ft2(:,j) = ft2(:,j) + fp(:,2).*phi(p,j)*weight(p);
        end
    end
    ft1 = ft1.*repmat(area,1,6);
    ft2 = ft2.*repmat(area,1,6);
    f1 = accumarray(elem2dof(:),ft1(:),[Nu 1]);
    f2 = accumarray(elem2dof(:),ft2(:),[Nu 1]);
end
clear phi ft1 ft2

%% Boundary Conditions
[AD,BD,f,g,u,p,ufreeDof,pDof] = getbdStokesP2P1;

%% Record assembeling time
assembleTime = toc;
if ~isfield(option,'printlevel'), option.printlevel = 1; end
if option.printlevel >= 2
    fprintf('Time to assemble matrix equation %4.2g s\n',assembleTime);
end

%% Solve the system of linear equations
if isempty(ufreeDof), return; end
if isempty(option) || ~isfield(option,'solver')    % no option.solver
    if length(f)+length(g) <= 1e3  % Direct solver for small size systems
        option.solver = 'direct';
    else          % Multigrid-type  solver for large size systems
        option.solver = 'asmg';
    end
end
solver = option.solver;

% solve
switch solver
    case 'direct'
        tic
        bigA = [AD, B'; ...
                B, sparse(Np,Np)];
        bigF = [f; g];
        bigu = [u; p];
        bigFreeDof = [ufreeDof; 2*Nu+pDof];
        bigu(bigFreeDof) = bigA(bigFreeDof,bigFreeDof)\bigF(bigFreeDof);
        u = bigu(1:2*Nu);
        p = bigu(2*Nu+1:end);
        residual = norm(bigF - bigA*bigu);
        info = struct('solverTime',toc,'itStep',0,'err',residual,'flag',2,'stopErr',residual);
    case 'mg'
%         option.tol = Np^(-2);
        option.solver  = 'WCYCLE';
        [u(ufreeDof),p,info] = mgstokes(A(ufreeDof,ufreeDof),B(:,ufreeDof),f(ufreeDof),g,...
                                        u(ufreeDof),p,elem,ufreeDof,option);
    case 'asmg'
        [u(ufreeDof),p,info] = asmgstokes(A(ufreeDof,ufreeDof),B(:,ufreeDof),f(ufreeDof),g,...
                                          u,p,node,elem,bdFlag,ufreeDof,option);
end

%% Post-process
if length(pDof) ~= Np % p is unique up to a constant
    % impose the condition int(p)=0
    c = sum(mean(p(elem),2).*area)/sum(area);
    p = p - c;
end

%% Output information
eqn = struct('A',AD,'B',BD,'f',f,'g',g,'ufreeDof',ufreeDof,'pDof',pDof);
info.assembleTime = assembleTime;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunctions getbdStokesP2P1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function [AD,BD,f,g,u,p,ufreeDof,pDof] = getbdStokesP2P1
    %% Boundary condition of Stokes equation: P2-P0 elements

    %% Initial set up
%     f = [f1; f2];    % set in Neumann boundary condition
    g = zeros(Np,1);
    u = zeros(2*Nu,1);
    p = zeros(Np,1);
    ufreeDof = (1:Nu)';
    pDof = (1:Np)';

    if ~exist('bdFlag','var'), bdFlag = []; end
    if ~isfield(pde,'g_D'), pde.g_D = []; end
    if ~isfield(pde,'g_N'), pde.g_N = []; end
    if ~isfield(pde,'g_R'), pde.g_R = []; end

    %% Part 1: Find Dirichlet dof and modify the matrix
    % Find Dirichlet boundary dof: fixedDof and pDof
    isFixedDof = false(Nu,1);
    if ~isempty(bdFlag)       % case: bdFlag is not empty
        elem2edge = elem2dof(:,4:6)-N;
        isDirichlet(elem2edge(bdFlag(:)==1)) = true;
        isFixedDof(edge(isDirichlet,:)) = true;   % nodes of all D-edges
        isFixedDof(N + find(isDirichlet')) = true;% dof on D-edges
        fixedDof = find(isFixedDof);
        ufreeDof = find(~isFixedDof);
    end
    if isempty(bdFlag) && ~isempty(pde.g_D) && isempty(pde.g_N) && isempty(pde.g_R)
        fixedDof = bdDof;
        isFixedDof(fixedDof) = true;
        ufreeDof = find(~isFixedDof);
    end
    if isempty(fixedDof) % pure Neumann boundary condition
        % pde.g_N could be empty which is homogenous Neumann boundary condition
        fixedDof = 1;
        ufreeDof = (2:Nu)';    % eliminate the kernel by enforcing u(1) = 0;
    end

    % Modify the matrix
    % Build Dirichlet boundary condition into the matrix AD by enforcing
    % AD(fixedDof,fixedDof)=I, AD(fixedDof,ufreeDof)=0, AD(ufreeDof,fixedDof)=0.
    % BD(:,fixedDof) = 0 and thus BD'(fixedDof,:) = 0.
    bdidx = zeros(2*Nu,1);
    bdidx([fixedDof; Nu+fixedDof]) = 1;
    Tbd = spdiags(bdidx,0,2*Nu,2*Nu);
    T = spdiags(1-bdidx,0,2*Nu,2*Nu);
    AD = T*A*T + Tbd;
    BD = B*T;

    %% Part 2: Find boundary edges and modify the right hand side f and g
    % Find boundary edges: Neumann and Robin
    Neumann = []; Robin = []; %#ok<*NASGU>
    if ~isempty(bdFlag)
        isNeumann(elem2edge((bdFlag(:)==2)|(bdFlag(:) == 3))) = true;
        isRobin(elem2edge(bdFlag(:)==3)) = true;
        Neumannidx = find(isNeumann);
        Neumann   = edge(isNeumann,:);
        Robin     = edge(isRobin,:);
    end
    if isempty(bdFlag) && (~isempty(pde.g_N) || ~isempty(pde.g_R))
        % no bdFlag, only pde.g_N or pde.g_R is given in the input
        Neumann = edge(bdDof>N,:);
        if ~isempty(pde.g_R)
            Robin = Neumann;
        end
    end

    % Neumann boundary condition
    if ~isempty(pde.g_N) && ~isempty(Neumann) && ~(isnumeric(pde.g_N) && (pde.g_N == 0))
        [lambda,w] = quadpts1(3);
        nQuad = size(lambda,1);
        % quadratic bases (1---3---2)
        bdphi(:,1) = (2*lambda(:,1)-1).*lambda(:,1);
        bdphi(:,2) = (2*lambda(:,2)-1).*lambda(:,2);
        bdphi(:,3) = 4*lambda(:,1).*lambda(:,2);
        % length of edge
        ve = node(Neumann(:,1),:) - node(Neumann(:,2),:);
        edgeLength = sqrt(sum(ve.^2,2));
        % update RHS
        gex = zeros(size(Neumann,1),2);   % x-component
        gey = zeros(size(Neumann,1),2);   % y-component
        for pp = 1:nQuad
            pxy = lambda(pp,1)*node(Neumann(:,1),:)+lambda(pp,2)*node(Neumann(:,2),:);
            gp = pde.g_N(pxy);
            gex(:,1) = gex(:,1) + w(pp)*edgeLength.*gp(:,1)*bdphi(pp,1);
            gex(:,2) = gex(:,2) + w(pp)*edgeLength.*gp(:,1)*bdphi(pp,2);
            gey(:,1) = gey(:,1) + w(pp)*edgeLength.*gp(:,2)*bdphi(pp,1);
            gey(:,2) = gey(:,2) + w(pp)*edgeLength.*gp(:,2)*bdphi(pp,2);
            f1(N+Neumannidx) = f1(N+Neumannidx) + w(pp)*edgeLength.*gp(:,1)*bdphi(pp,3); % interior bubble
            f2(N+Neumannidx) = f2(N+Neumannidx) + w(pp)*edgeLength.*gp(:,2)*bdphi(pp,3); % interior bubble
        end
        f1(1:N) = f1(1:N) + accumarray(Neumann(:), gex(:),[N,1]);
        f2(1:N) = f2(1:N) + accumarray(Neumann(:), gey(:),[N,1]);
    end
    f = [f1; f2];
    % The case non-empty Neumann but g_N=[] corresponds to the zero flux
    % boundary condition on Neumann edges and no modification is needed.

    % Dirichlet boundary conditions
    if ~isempty(fixedDof) && ~isempty(pde.g_D) && ~(isnumeric(pde.g_D) && (pde.g_D == 0))
        u1 = zeros(Nu,1);
        u2 = zeros(Nu,1);
        idx = (fixedDof > N);              % index of edge dof
        uD = pde.g_D(node(fixedDof(~idx),:));  % bd value at vertex dofs
        u1(fixedDof(~idx)) = uD(:,1);
        u2(fixedDof(~idx)) = uD(:,2);
        bdEdgeIdx = fixedDof(idx)-N;
        bdEdgeMid = (node(edge(bdEdgeIdx,1),:)+node(edge(bdEdgeIdx,2),:))/2;
        uD = pde.g_D(bdEdgeMid);         % bd values at middle points of edges
        u1(fixedDof(idx)) = uD(:,1);
        u2(fixedDof(idx)) = uD(:,2);
        u = [u1; u2]; % Dirichlet bd condition is built into u
        f = f - A*u;  % bring affect of nonhomgenous Dirichlet bd condition to
        g = g - B*u;  % the right hand side
        g = g - mean(g);
        f(fixedDof) = u1(fixedDof);
        f(fixedDof+Nu) = u2(fixedDof);
    end
    % The case non-empty Dirichlet but g_D=[] corresponds to the zero Dirichlet
    % boundary condition and no modification is needed.

    % modfiy pressure dof for pure Dirichlet
    if isempty(Neumann)
        pDof = (1:Np-1)';
    end

    ufreeDof = [ufreeDof; Nu+ufreeDof];
    end % end of function getbdStokesP2P1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end


function pred = ml_predicthdca(trials, model)
% Prediction function for Hierarchical Discriminant Component Analysis
% Prediction = ml_predicthdca(Trials, Model)
%
% In:
%   Trials  : the data a matrix, as in ml_predict
%
%   Model   : predictive model as produced by ml_trainhdca
%
% Out:
%   Prediction  : discrete probability distribution, formatted as
%                 {'disc' [NxC] [Cx1]}, with element #2 being the per-class probability and
%                 element #3 the original target values per class
%                 thus, the expected target values are Prediction{2}*Prediction{3}
%
% Examples:
%   targets might look like this: [-1 -1 1 -1 1 -1 -1 1 -1 -1 1 -1 -1 1 ...]'
%
%   model = ml_trainhdca(data,targets)
%   p = ml_predicthdca(data, model); expectation = p{2}*p{3};
%   now expectation might look like this: [-0.6 -0.9 +0.4 -0.7 +0.8 -0.1 +0.5 +1.0 -0.9 +1.0 -1.0 -1.0 +1.0 ...]'
%
% See also:
%   ml_trainhdca
%
%                           Christian Kothe, Swartz Center for Computational Neuroscience, UCSD
%                           2010-04-03

if isfield(model,'voted')
    % dispatch to the voter for multi-class classification
    pred = ml_predictvote(trials,model);
else
    % for each modality range...
    for m=1:length(model.modality_ranges)
        range = model.modality_ranges{m};

        % for each sub-block...
        for b=size(trials,2):-1:1
            % extract per-block features
            blocktrials = reshape(trials(range,b,:),[],size(trials,3))';
            blockpredictions{b} = blocktrials*model.blockmodels{m}{b}.w' - model.blockmodels{m}{b}.b';
        end
        % extract per-modality features
        layertrials = cat(2,blockpredictions{:});
        rangepredictions{m} = layertrials*model.rangemodels{m}.w' - model.rangemodels{m}.b'; %#ok<AGROW>
    end
    % perform top-level prediction
    toptrials = cat(2,rangepredictions{:});
    pred = ml_predictlda(toptrials,model.topmodel);
end


% load textbook.mat
S=[-1  0  1 1 0;
    1 -1  0 0 0;
    0  1 -1 0 -1];

model.S=S;
model.lb=[0 0 -10 5 5]';
model.ub=[5 5 0 0 0]';
model.c=zeros(5,1);
model.c(5,1)=1;

v = checkThermodynamicConsistency(model);

disp(norm(v))


function X = mrdivide(A, B)
%/   Right matrix divide for a BNDFUN.
%   A/B divides the BNDFUN A by a scalar B. More generally, it gives the
%   least-squares solution (with respect to the continuous L^2 norm) to X*B = A
%   when either A or B is a BNDFUN.  Note that in the latter case, formally it
%   is X.' that is returned, as BNDFUN objects are always columns.
%
% See also QR, RDIVIDE, MLDIVIDE.

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Case B is a matrix: X*B = A  ==> X = A/B = (Q*R)/B = Q*(R/B)
%
% Case A is a matrix: X*B = X*(Q*R) = A ==> X = (A/R)*Q' ==> X' = Q*(A/R)'
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if ( (size(B, 2) ~= size(A, 2)) && ~(isa(B, 'double') && isscalar(B)) )
    error('CHEBFUN:BNDFUN:mrdivide:size', 'Matrix dimensions must agree.');

elseif ( isa(B, 'double') )  % BNDFUN / double

    if ( isscalar(B) )
        % Scalar case is easy:
        X = A;                              % Copy A to X
        X.onefun = X.onefun/B;              % mrdivide of the onefun
    else
        % Call MRDIVIDE at the ONEFUN level:
        X = A;
        X.onefun = (A.onefun/B);

%         % Alternatively, we could call QR() at the BNDFUN level
%         % For matrix case, we do least squares via QR:
%         [Q, R] = qr(A, 0);
%         X = Q*(R/B);
    end

elseif ( isa(A, 'double') )  % double / BNDFUN

    % Call MRDIVIDE at the ONEFUN level:
    X = B;
    X.onefun = (A/B.onefun);
    X = X/(.5*diff(B.domain));

%     % Alternatively we could call QR() at the BNDFUN level:
%     % Do least squares via QR:
%     [Q, R] = qr(B, 0);
%     % Return the transpose for the output.
%     X = Q*(A/R).';

elseif ( isa(B, 'bndfun') && isa(A, 'bndfun') )
    error('CHEBFUN:BNDFUN:mrdivide:bndfunDivBndfun', ...
        'Use ./ to divide BNDFUN by a BNDFUN.');

else
    error('CHEBFUN:BNDFUN:mrdivide:badArg', '%s/%s is not well-defined.', ...
        class(A), class(B));

end

end


%%
% Perform softmax prediction.
%
% Thang Luong @ 2015, <lmthang@stanford.edu>
%
%%
function [cost, probs, scores, scoreIndices] = softmaxLayerForward(W, inVec, predLabels, curMask)
  mask = curMask.mask;
  unmaskedIds = curMask.unmaskedIds;

  % softmax_h -> predictions
  [probs, scores, norms] = softmax(W*inVec, mask);

  % cost
  predLabels = predLabels(unmaskedIds);
  scoreIndices = sub2ind(size(scores), predLabels, unmaskedIds); % 1 * length(tgtPredictedWords)
  cost = - sum(scores(scoreIndices)) + sum(log(norms).*mask);
end


function out = Rstar(d, n, e, fun)
%finds rate that achieves a given excess distortion at a given blocklength for
%Gaussian source with unit variance
%d - excess distortion
%n - block length (scalar)
%e - excess probability
%fun - which function to use for calculation

%
%   Created in 2012 by Victoria Kostina (vkostina@caltech.edu)
%

%starting points - 'persistent' to make optimization faster
persistent x0;
%rate distortion
Rd = -1/2*log2(d);
%precision options
tol = 1e-10;
options = optimset('TolX',tol, 'MaxFunEvals', 500, 'Algorithm', 'active-set', 'Display', 'off');

switch lower(fun)
    case {'shannon', 'spherecoveringa', 'generalc', 'generalcopt'}
        if isempty(x0)
            x0 = [Rd 3*Rd]; %rate
        end
        out = Generic();
    case 'normal'
        %normal approximation:
        out = Normal();
    case 'spherecoveringc'
        %converse via sphere covering
        out = SphereCoveringC();
    case 'rogersa'
        out = RogersA();
    otherwise
        disp('Unknown type.')
end

%--------------------------------------------------------------------------
    function out = Generic()
        out = fzero(@(x)goal(x) - e, x0, options);
        function out = goal(x)
            out = Pexcess(x, d, n, fun);
        end
    end

%--------------------------------------------------------------------------
    function out = Normal()
        %Normal approximation
        out = - .5*log2(d) + 1/sqrt(2*n)*Qinv(e)*log2(exp(1));
    end

%--------------------------------------------------------------------------
    function out = SphereCoveringC()
        %converse via sphere covering

        %find rstar:
        factor = 3;
        rstar = fzero(@(x)chi2cdf(x^2*n, n) - chi2cdf(factor*n, n) + e, 1);
        out = rate(d);

        function out = rate(x)
            out = 1/2*log2(rstar^2/x);
        end
    end

%--------------------------------------------------------------------------
    function out = RogersA()
        rstar = fzero(@(x)1 - chi2cdf(x^2*n, n) - e, 1);
        out = rogers(rstar/sqrt(d));
        function out = rogers(r)
            if r >= n
                out = log2(exp(1))+ log2((n*log(n)+n*log(log(n)) + 5*n)) + n*log2(r);
            elseif r >= n/log(n)
                out = log2(n*(n*log(n)+n*log(log(n)) + 5*n)) + n*log2(r);
            elseif r >2
                out = log2(7^(4*log(7)/7)/4*sqrt(2*pi)) + log2(n^(3/2)*((n-1)*log(r*n)+(n-1)*log(log(n)+log(n)/2+log((pi*sqrt(2*n))/(sqrt(pi*n)-2))))) - log2(r*(1-2/log(n))*(1 - 2/sqrt(pi*n))*(log(n))^2) + n*log2(r);
            else
                out = log2(sqrt(2*pi)) + log2(sqrt(n)*((n-1)*log(r*n)+(n-1)*log(log(n)+log(n)/2+log((pi*sqrt(2*n))/(sqrt(pi*n)-2))))) - log2(r*(1-2/log(n))*(1 - 2/sqrt(pi*n))) + n*log2(r);
            end
            out = out/n;
        end
    end

end


% Test file for @classicfun/max.m

function pass = test_max(pref)

% Get preferences.
if ( nargin < 1 )
    pref = chebfunpref();
end

% Set a domain for BNDFUN.
data.domain = [-2 7];

%%
% Spot-check the extrema for a few BNDFUN.
pass(1) = test_spotcheck_max(@(x) sin(10*x), data, 1, pref);
pass(2) = test_spotcheck_max(@airy, data, 0.535656656015700, pref);
pass(3) = test_spotcheck_max(@(x) -1./(1 + x.^2), data, -.02, pref);
pass(4) = test_spotcheck_max(@(x) (x/10).^3.*cosh(x/10), ...
    data, 0.7^3*cosh(0.7), pref);

%%
% Check operation for array-valued BNDFUN inputs.
fun_op = @(x) [sin(10*x) airy(x) (x/10).^3.*cosh(x/10)];
f = bndfun(fun_op, data, pref);
[y, x] = max(f);
exact_max = [1 0.535656656015700 0.7^3*cosh(0.7)];
fx = [sin(10*x(1)) airy(x(2)) (x(3)/10).^3.*cosh(x(3)/10)];
tol = 10*get(f, 'vscale')*eps;
pass(5) = (all(abs(y - exact_max) < 10*tol) && ...
    all(abs(fx - exact_max) < tol));

%%
% Test for complex-valued BNDFUN.
pass(6) = test_spotcheck_max(@(x) (x/2).*(exp(1i*(x/2))+1i*sin(x/2)), ...
    data, -3.277598405517787 - 2.455482593827339i, pref);

fun_op = @(x) [((x-2).^2/4+1).*exp(1i*(x/2)) ...
    -((x+1).^2/4+1).*exp(1i*(x/2))];
f = bndfun(fun_op, data, pref);
[y, x] = max(f);
exact_max = [-6.789310982858273-2.543178400749744i 15.919763683943538+5.963314870723537i];
fx = [((x(1)-2).^2/4+1).*exp(1i*(x(1)/2)) ...
    -((x(2)+1).^2/4+1).*exp(1i*(x(2)/2))];
tol = get(f, 'vscale')*eps;
pass(7) = (all(abs(y - exact_max) < 10*tol) && ...
    all(abs(fx - exact_max) < tol));

%% Test for UNBNDFUN:

% Functions on [a inf]:

% Set the domain:
data.domain = [1 Inf];

op = @(x) x.*exp(-x);
f = unbndfun(op, data);
[y, x] = max(f);
yExact = exp(-1);
xExact = 1;
errY = y - yExact;
errX = x - xExact;
pass(8) = norm([errY errX], inf) < 1e3*eps*get(f,'vscale');

end

%%
% Spot-check the results for a given BNDFUN.
function result = test_spotcheck_max(fun_op, data, exact_max, pref)

f = bndfun(fun_op, data, pref);
[y, x] = max(f);
fx = fun_op(x);
tol = 100*get(f, 'vscale')*eps;

result = (all(abs(y - exact_max) < tol) && ...
          all(abs(fx - exact_max) < tol));

end


function gX = rbfperiodic2KernDiagGradX(kern, X)

% RBFPERIODIC2KERNDIAGGRADX Gradient of RBFPERIODIC2 kernel's diagonal with respect to X.
% FORMAT
% DESC computes the gradient of the diagonal of the RBF periodic covariance with variying period kernel matrix with
% respect to the elements of the design matrix given in X.
% ARG kern : the kernel structure for which gradients are being computed.
% ARG X : the input data in the form of a design matrix.
% RETURN gX : the gradients of the diagonal with respect to each element
% of X. The returned matrix has the same dimensions as X.
%
% SEEALSO : rbfperiodic2KernParamInit, kernDiagGradX, rbfperiodic2kernGradX
%
% COPYRIGHT : Neil D. Lawrence, 2007, 2009
%
% MODIFICATIONS : Andreas C. Damianou, 2011
%
% MODIFICATIONS : Michalis K. Titsias, 2011

% KERN

gX = zeros(size(X));


%% GEOMETRIC COLLISION AVOIDANCE AGENT (agent_2D_vectorSharing.m) %%%%%%%%%
% This programs contains a generic agent object with the 2017 3D geometric
% avoidance alogorithm applied to conduct course corrections.

% Author: James A. Douthwaite

classdef agent_2D_vectorSharing < agent_2D & agent_vectorSharing
    %% ////////////////////// MAIN CLASS METHODS //////////////////////////
    methods
        % Constructor
        function [this] = agent_2D_vectorSharing(varargin)

            % Call the super class
            this@agent_2D(varargin);

            % //////////////////// SENSOR PARAMETERS //////////////////////
            [this.SENSORS] = this.GetDefaultSensorParameters();     % Default sensing
%             [this.SENSORS] = this.GetCustomSensorParameters();       % Experimental sensing
            % /////////////////////////////////////////////////////////////

            % //////////////// Check for user overrides ///////////////////
            this = this.ApplyUserOverrides(varargin); % Recursive overrides
            % /////////////////////////////////////////////////////////////
        end
        % Setup - X = [x y psi dx dy dpsi]
        function [this] = setup(this,localXYZVelocity,localXYZrotations)
            % This function calculates the intial state for a generic
            % object.
            % The default state vector:
            % [x y psi dx dy dpsi]
            % INITIALISE THE 2D STATE VECTOR WITH CONCANTINATED VELOCITIES
            [this] = this.setup_2DVelocities(localXYZVelocity,localXYZrotations);
        end
        % Main
        function [this] = main(this,ENV,varargin)
            % This function is designed to house a generic agent process
            % cycle that results in an acceleration vector in the global axis.
            % INPUTS:
            % varargin - Cell array of inputs
            % >dt      - The timestep
            % >objects - The detectable objects cell array of structures
            % OUTPUTS:
            % obj      - The updated project

            % INPUT HANDLING
            dt = ENV.dt;

            % PLOT AGENT FIGURE
            visualiseProblem = 0;
            visualiseAgent = 1;
            if this.objectID == visualiseAgent && visualiseProblem == 1
                overHandle = figure(100+this.objectID);
                hold on; grid on;
                axis equal;
                xlabel('x_{m}'); ylabel('y_{m}'); zlabel('z_{m}');
            end

            % //////////// CHECK FOR NEW INFORMATION UPDATE ///////////////
            [this,obstacleSet,agentSet] = this.GetAgentUpdate(ENV,varargin{1});       % IDEAL INFORMATION UPDATE
            % /////////////////////////////////////////////////////////////

            % /////////////////// WAYPOINT TRACKING ///////////////////////
            % Design the current desired trajectory from the waypoint.
            desiredHeadingVector = this.GetTargetHeading();
            desiredVelocity = desiredHeadingVector*this.v_nominal;

            % ////////////////// OBSTACLE AVOIDANCE ///////////////////////
            % Modify the desired velocity with the augmented avoidance velocity.
            avoidanceSet = [obstacleSet,agentSet];
            algorithm_start = tic; algorithm_indicator = 0;
            if ~isempty(avoidanceSet)
                algorithm_indicator = 1;
                % GET THE UPDATED DESIRED VELOCITY
                 [desiredHeadingVector,desiredSpeed] = this.GetAvoidanceCorrection(desiredVelocity,visualiseProblem);
                 desiredVelocity = desiredHeadingVector*desiredSpeed;
            end
            algorithm_dt = toc(algorithm_start);                           % Stop timing the algorithm

            desiredVelocity

            % ///////////////////// CONTROLLER ////////////////////////////
            this = this.Controller(dt,desiredVelocity);

            % ////////////// RECORD THE AGENT-SIDE DATA ///////////////////
            this = this.writeAgentData(ENV,algorithm_indicator,algorithm_dt);      % Record when the algorithm is ran
            this.DATA.inputNames = {'$v_x$ (m/s)','$v_y$ (m/s)','$\dot{\psi}$ (rad/s)'};
            this.DATA.inputs(1:length(this.DATA.inputNames),ENV.currentStep) = this.localState(4:6);         % Record the control inputs

            % // DISPLAY CONFLICT RESOLUTION
            if this.objectID == visualiseAgent && visualiseProblem == 1
                this = this.GetAnimationFrame(overHandle,ENV,'resolutionZone.gif');
                close(overHandle);
            end
        end
    end
    %% /////////////////////// AUXILLARY METHODS //////////////////////////
    methods (Access = public)
        % GET THE AVOIDANCE CORRECTION
        function [heading,speed] = GetAvoidanceCorrection(this,desiredVelocity,visualiseProblem)
            % This function calculates the collision avoidance velocity in
            % light of the current obstacles

            % Check we aren't stopping
            [heading,speed] = this.nullVelocityCheck(desiredVelocity);
            if speed == 0
                return
            end

            % AGENT KNOWLEDGE
            [p_i,v_i,r_i] = this.GetAgentMeasurements(); % Its own position, velocity and radius

            % Define the obstacle list
            obstacleIDs = [this.MEMORY([this.MEMORY.type] ~= OMAS_objectType.waypoint).objectID];

            % MOVE THROUGH THE PRIORITISED OBSTACLE SET
            optimalSet = [];
            for j = 1:numel(obstacleIDs)
                % Fetch the obstacle trajectories
                p_j = this.GetLastMeasurementByID(obstacleIDs(j),'position');
                % Neighbour conditions
                neighbourConditionA = j < this.maxNeighbours;            % Maximum number of neighbours
                neighbourConditionB = norm(p_j) < this.neighbourDist;       % [CONFIRMED]
                if ~neighbourConditionA || ~neighbourConditionB
                    continue
                end

                % Get further obstacle information
                v_j = this.GetLastMeasurementByID(obstacleIDs(j),'velocity');
                r_j = this.GetLastMeasurementByID(obstacleIDs(j),'radius');

                % OBSTACLE KNOWLEDGE
                p_j = p_j + p_i;
                v_j = v_j + v_i;                                           % Convert relative parameters to absolute

                % COMPUTE THE VECTOR SHARING PROBLEM
                [Voptimal] = this.Define2DVectorSharingVelocity(...
                    desiredVelocity,...
                    p_i,v_i,r_i,...
                    p_j,v_j,r_j,...
                    visualiseProblem);
                optimalSet = horzcat(optimalSet,[Voptimal;abs(norm(desiredVelocity - Voptimal))]);
            end

            % INTERPRETING THE AVOIDANCE VELOCITY SET
            if ~isempty(optimalSet)
                inputDim = 2;
                % THE CLOSEST OBSTACLE
                %                 avoidanceVelocity = optimalSet(1:inputDim,1);
                % FIND THE MINIMUM MAGNITUDE DEVIATION FROM THE DESIRED
                [~,minIndex] = min(optimalSet((inputDim+1),:),[],2);       % Return the index of the smallest vector
                avoidanceVelocity = optimalSet(1:inputDim,minIndex);
            else
                % NOTHING TO AVOID, CHOOSE OPTIMAL VELOCITY
                avoidanceVelocity = desiredVelocity;
            end

            % CHECK VELOCITY IS PERMISSIBLE
            if any(isnan(avoidanceVelocity))
%                 heading = [1;0];
%                 speed = 0;
                speed   = norm(desiredVelocity);
                heading = desiredVelocity/speed;    % Retain forward direction
            else
                [heading,speed] = this.nullVelocityCheck(avoidanceVelocity);
            end
        end
        % DEFINE THE VECTOR SHARING AVOIDANCE PROBLEM
        function [U_a] = Define2DVectorSharingVelocity(obj,desiredVelocity,p_a,v_a,r_a,p_b,v_b,r_b,visualiseProblem)
            % This function calculates the avoidance vectors based on the
            % principle of vector sharing.
            % INPUTS:
            % desiredVelocity - The true optimal vector
            % p_a - The agent's absolute position
            % v_a - The agent's absolute velocity
            % r_a - The agent's radius
            % p_b - The obstacle's absolute position
            % v_b - The obstacle's absolute velocity
            % r_b - The obstacle's radius
            % visualiseProblem - Plot flag
            % OUTPUTS:
            % U_a - The optimal vector heading, scaled by the desired
            %       velocity.

            % Generate the 3D inputs
            p_a = [p_a;0]; v_a = [v_a;0];
            p_b = [p_b;0]; v_b = [v_b;0];
            % Pass to the 3D function
            [U_a] = obj.Define3DVectorSharingVelocity(...
                desiredVelocity,...
                p_a,v_a,r_a,...
                p_b,v_b,r_b,...
                visualiseProblem);
            % Reform the inputs for 2D application
            U_a = U_a(1:2,1);
        end
    end
    % The function uses the same sensor characteristics from the parent
    % class "agent_vectorSharing.m".
end
% AGENT STATE VECTOR [x;y;phi;xdot;ydot;phidot]


function [yCouch, lines] =  getCouchLocationHough(scan3M,minLengthOpt,retryOpt)

% Function: getCouchLocationHough
% Description: Returns anterior coordinate of patient couch surface
%
% Usage
% inputStack is a 3-dim array of CT scan image with array axes [Y,X,Z]. [Note: "Image" notation permutes Y <-> X]
% yCouch, the Y-coordinate of the scanning table/couch
%
% EML 2020-04-13
%

if ~exist('minLengthOpt','var')
    minLengthOpt = [];
end

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

midptS = floor(size(scan3M,1)/2);

maxM = max(scan3M, [], 3);
histeqM = histeq(maxM);
edgeM1 = edge(histeqM,'sobel',[],'horizontal');
edgeM2 = bwmorph(edgeM1,'thicken');

[H,T,R] = hough(edgeM2);
P = houghpeaks(H,20);

if isempty(minLengthOpt)
    minLength = floor(size(edgeM2,2)/8); % couch covers 1/8th of image
else
    minLength = minLengthOpt;
end

% lines = houghlines(edgeM2,T,R,P,'FillGap',5,'MinLength',minLength);
lines = houghlines(edgeM2,T,R,P);
overlapFraction = zeros(1,numel(lines));
midV = [floor(0.5*midptS):floor(0.5*midptS) + midptS];
% Require couch lines to have same starting & ending point2
yi = zeros(1,numel(lines));
% figure; imagesc(maxM); axis equal; hold on
for i = 1:numel(lines)
    len = norm(lines(i).point1 - lines(i).point2);
    if lines(i).point1(2) == lines(i).point2(2) && len > minLength
%         xy = [lines(i).point1; lines(i).point2];
%         plot(xy(:,1),xy(:,2),'LineWidth',2,'Color','green');
%         % Plot beginnings and ends of lines
%         plot(xy(1,1),xy(1,2),'x','LineWidth',2,'Color','yellow');
%         plot(xy(2,1),xy(2,2),'x','LineWidth',2,'Color','red');
        lineV = [lines(i).point1(1):lines(i).point2(1)];
        if lines(i).point1(2) > midptS && ~isempty(intersect(lineV,midV))
            yi(i) = lines(i).point2(2);
            overlapFraction(i) = numel(intersect(lineV,midV));
        end
    end
end

if any(overlapFraction)
    [~,I] = max(overlapFraction);
    yCouch = yi(I);
else
    yCouch = min(yi(find(yi > 0)));
end

if retryOpt && isempty(yCouch)
    [yCouch, lines] =  getCouchLocationHough(scan3M,minLength/2);
end


function dz = dynamics(z,u,param)
% dz = dynamics(z,u,param)
%
% Computes the first-order form of the dynamics for the combined chain
% integrator and pendulum system
%
% INPUTS:
%   z = [x;v1;v2;a2;j2];
%   u = [u1;u2];
%
% OUTPUTS:
%   dz = dz/dt
%

x = z(1,:);
v1 = z(2,:);
% v2 = z(3,:);   %Unused
a2 = z(4,:);
j2 = z(5,:);
u1 = u(1,:);
u2 = u(2,:);

% Pendulum physics
dv1 = pendulum(x,v1,u1,param);

% Integrator chain physics:
dx = v1;
dv2 = a2;
da2 = j2;
dj2 = u2;

% Combine:
dz = [dx;dv1;dv2;da2;dj2];

end


%QUATERNION	constructor for quaternion objects
%
% 	QUATERNION([s v1 v2 v3])	from 4 elements
% 	QUATERNION(v, theta)		from vector plus angle
% 	QUATERNION(R)			from a 3x3 or 4x4 matrix
% 	QUATERNION(q)			from another quaternion

% Ryan Steindl based on Robotics Toolbox for MATLAB (v6 and v9)
%
% Copyright (C) 1993-2011, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

function q = Quaternion(a1, a2)

	if nargin == 0,
		q.s = 1;
		q.v = [0 0 0];
		q = class (q, 'Quaternion');

	elseif nargin == 1
		if isa(a1, 'Quaternion')
			q = a1;
			q = class(q, 'Quaternion');
		elseif isreal (a1) && size(a1) == 1
			q.s = a1(1);
			q.v = [0,0,0];
			q = class(q, 'Quaternion');
		elseif isreal (a1) && all (size (a1) == [1 3]) # Quaternion (vector part)
			q.s = 0;
			q.v = a1(1:3);
			q = class(q, 'Quaternion');
		elseif all(size(a1) == [3 3])
			q = Quaternion( tr2q(a1) );
		elseif all(size(a1) == [4 4])
			q = Quaternion( tr2q(a1(1:3,1:3)) );
		elseif all(size(a1) == [1 4])
			q.s = a1(1);
			q.v = a1(2:4);
			q = class(q, 'Quaternion');
		else
			error('unknown dimension of input');
		end
	elseif nargin == 2
		if  isscalar(a1) && isvector(a2)
			q.s = cos(a1/2);
			q.v = (sin(a1/2)*unit(a2(:)'));
			q = class(q, 'Quaternion');
		end
	end
endfunction
%TR2Q	Convert homogeneous transform to a unit-quaternion
%
%	Q = tr2q(T)
%
%	Return a unit quaternion corresponding to the rotational part of the
%	homogeneous transform T.
%
%	See also Q2TR

% Ryan Steidnl based on Robotics Toolbox for MATLAB (v6 and v9)
%

% Ryan Steindl based on Robotics Toolbox for MATLAB (v6 and v9)
%
% Copyright (C) 1993-2011, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com
function q = tr2q(t)
	qs = sqrt(trace(t)+1)/2.0;
	kx = t(3,2) - t(2,3);	% Oz - Ay
	ky = t(1,3) - t(3,1);	% Ax - Nz
	kz = t(2,1) - t(1,2);	% Ny - Ox

	if (t(1,1) >= t(2,2)) & (t(1,1) >= t(3,3))
		kx1 = t(1,1) - t(2,2) - t(3,3) + 1;	% Nx - Oy - Az + 1
		ky1 = t(2,1) + t(1,2);			% Ny + Ox
		kz1 = t(3,1) + t(1,3);			% Nz + Ax
		add = (kx >= 0);
	elseif (t(2,2) >= t(3,3))
		kx1 = t(2,1) + t(1,2);			% Ny + Ox
		ky1 = t(2,2) - t(1,1) - t(3,3) + 1;	% Oy - Nx - Az + 1
		kz1 = t(3,2) + t(2,3);			% Oz + Ay
		add = (ky >= 0);
	else
		kx1 = t(3,1) + t(1,3);			% Nz + Ax
		ky1 = t(3,2) + t(2,3);			% Oz + Ay
		kz1 = t(3,3) - t(1,1) - t(2,2) + 1;	% Az - Nx - Oy + 1
		add = (kz >= 0);
	end

	if add
		kx = kx + kx1;
		ky = ky + ky1;
		kz = kz + kz1;
	else
		kx = kx - kx1;
		ky = ky - ky1;
		kz = kz - kz1;
	end
	nm = norm([kx ky kz]);
	if nm == 0,
		q = Quaternion([1 0 0 0]);
	else
		s = sqrt(1 - qs^2) / nm;
		qv = s*[kx ky kz];

		q = Quaternion([qs qv]);

	end
endfunction


function [ahm,AHM_f,AHM_ahmf] = fromFrameAhm(F,ahmf)

% FROMFRAMEAHM  Transforms AHM from local frame to global frame.
%   AHM = FROMFRAMEAHM(F,AHMF) transforms the Inverse Depth point IF from the
%   local frame F to the global frame. The frame F can be specified either
%   with a 7-vector F=[T;Q], where T is the translation vector and Q the
%   orientation quaternion, of via a structure containing at least the
%   fields F.t, F.q, F.R and F.Rt (translation, quaternion, rotation matrix
%   and its transpose).
%
%   [AHM,AHM_f,AHM_if] = FROMFRAMEAHM(...) returns the Jacobians wrt F and AHMF.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

xf  = ahmf(1:3,:);
mf  = ahmf(4:6,:);
sf  = ahmf(7,:);

[t,q,R] = splitFrame(F);

if nargout == 1

    x  = fromFrame(F,xf);
    m  = R*mf;

    ahm  = [x;m;sf];

else

    if size(ahmf,2) > 1
        error('Jacobians not available for multiple ahms')
    else

        [x, X_f, X_xf] = fromFrame(F,xf);
        [m,M_q,M_mf]   = Rp(q,mf);

        ahm    = [x;m;sf];

        AHM_f = [...
            X_f
            zeros(3,3) M_q
            zeros(1,7)];

        AHM_ahmf = [...
            X_xf          zeros(3,4)
            zeros(3,3) M_mf zeros(3,1)
            zeros(1,6)           1];

    end
end

return

%% jac

syms x y z a b c d X Y Z U V W R real
F   = [x;y;z;a;b;c;d];
ahmf = [X;Y;Z;U;V;W;R];

[ahm,AHM_f,AHM_ahmf] = fromFrameAhm(F,ahmf);

simplify(AHM_f  - jacobian(ahm,F))
simplify(AHM_ahmf - jacobian(ahm,ahmf))

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

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

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function [cout,hout] = tricontour(p,t,Hn,N)
% Contouring for functions defined on triangular meshes
%
%   TRICONTOUR(p,t,F,N)
%
% Draws contours of the surface F, where F is defined on the triangulation
% [p,t]. These inputs define the xy co-ordinates of the points and their
% connectivity:
%
%   P = [x1,y1; x2,y2; etc],            - xy co-ordinates of nodes in the
%                                         triangulation
%   T = [n11,n12,n13; n21,n23,n23; etc] - node numbers in each triangle
%
% The last input N defines the contouring levels. There are several
% options:
%
%   N scalar - N number of equally spaced contours will be drawn
%   N vector - Draws contours at the levels specified in N
%
% A special call with a two element N where both elements are equal draws a
% single contour at that level.
%
%   [C,H] = TRICONTOUR(...)
%
% This syntax can be used to pass the contour matrix C and the contour
% handels H to clabel by adding clabel(c,h) or clabel(c) after the call to
% TRICONTOUR.
%
% TRICONTOUR can also return 3D contours similar to CONTOUR3 by adding
% view(3) after the call to TRICONTOUR.
%
% Type "contourdemo" for some examples.
%
% See also, CONTOUR, CLABEL
% This function does NOT interpolate back onto a Cartesian grid, but
% instead uses the triangulation directly.
%
% If your going to use this inside a loop with the same [p,t] a good
% modification is to make the connectivity "mkcon" once outside the loop
% because usually about 50% of the time is spent in "mkcon".
%
% Darren Engwirda - 2005 (d_engwirda@hotmail.com)
% Updated 15/05/2006
% I/O checking
if nargin~=4
    error('Incorrect number of inputs')
end
if nargout>2
    error('Incorrect number of outputs')
end
% Error checking
if (size(p,2)~=2) || (size(t,2)~=3) || (size(Hn,2)~=1)
    error('Incorrect input dimensions')
end
if size(p,1)~=size(Hn,1)
    error('F and p must be the same length')
end
if (max(t(:))>size(p,1)) || (min(t(:))<=0)
    error('t is not a valid triangulation of p')
end
if (size(N,1)>1) && (size(N,2)>1)
    error('N cannot be a matrix')
end
% Make mesh connectivity data structures (edge based pointers)
[e,eINt,e2t] = mkcon(p,t);
numt = size(t,1);       % Num triangles
nume = size(e,1);       % Num edges
%==========================================================================
%                Quadratic interpolation to centroids
%==========================================================================
% Nodes
t1 = t(:,1); t2 = t(:,2); t3 = t(:,3);
% FORM FEM GRADIENTS
% Evaluate centroidal gradients (piecewise-linear interpolants)
x23 = p(t2,1)-p(t3,1);  y23 = p(t2,2)-p(t3,2);
x21 = p(t2,1)-p(t1,1);  y21 = p(t2,2)-p(t1,2);
% Centroidal values
Htx = (y23.*Hn(t1) + (y21-y23).*Hn(t2) - y21.*Hn(t3)) ./ (x23.*y21-x21.*y23);
Hty = (x23.*Hn(t1) + (x21-x23).*Hn(t2) - x21.*Hn(t3)) ./ (y23.*x21-y21.*x23);
% Form nodal gradients.
% Take the average of the neighbouring centroidal values
Hnx = 0*Hn; Hny = Hnx; count = Hnx;
for k = 1:numt
    % Nodes
    n1 = t1(k); n2 = t2(k); n3 = t3(k);
    % Current values
    Hx = Htx(k); Hy = Hty(k);
    % Average to n1
    Hnx(n1)   = Hnx(n1)+Hx;
    Hny(n1)   = Hny(n1)+Hy;
    count(n1) = count(n1)+1;
    % Average to n2
    Hnx(n2)   = Hnx(n2)+Hx;
    Hny(n2)   = Hny(n2)+Hy;
    count(n2) = count(n2)+1;
    % Average to n3
    Hnx(n3)   = Hnx(n3)+Hx;
    Hny(n3)   = Hny(n3)+Hy;
    count(n3) = count(n3)+1;
end
Hnx = Hnx./count;
Hny = Hny./count;
% Centroids [x,y]
pt = (p(t1,:)+p(t2,:)+p(t3,:))/3;
% Take unweighted average of the linear extrapolation from nodes to centroids
Ht = ( Hn(t1) + (pt(:,1)-p(t1,1)).*Hnx(t1) + (pt(:,2)-p(t1,2)).*Hny(t1) + ...
       Hn(t2) + (pt(:,1)-p(t2,1)).*Hnx(t2) + (pt(:,2)-p(t2,2)).*Hny(t2) + ...
       Hn(t3) + (pt(:,1)-p(t3,1)).*Hnx(t3) + (pt(:,2)-p(t3,2)).*Hny(t3) )/3;
% DEAL WITH CONTOURING LEVELS
if length(N)==1
    lev = linspace(max(Ht),min(Ht),N+1);
    num = N;
else
    if (length(N)==2) && (N(1)==N(2))
        lev = N(1);
        num = 1;
    else
        lev = sort(N);
        num = length(N);
        lev = lev(num:-1:1);
    end
end
% MAIN LOOP
c   = [];
h   = [];
in  = false(numt,1);
vec = 1:numt;
old = in;
for v = 1:num       % Loop over contouring levels

    % Find centroid values >= current level
    i     = vec(Ht>=lev(v));
    i     = i(~old(i));         % Don't need to check triangles from higher levels
    in(i) = true;

    % Locate boundary edges in group
    bnd  = [i; i; i];       % Just to alloc
    next = 1;
    for k = 1:length(i)
        ct    = i(k);
        count = 0;
        for q = 1:3     % Loop through edges in ct
            ce = eINt(ct,q);
            if ~in(e2t(ce,1)) || ((e2t(ce,2)>0)&&~in(e2t(ce,2)))
                bnd(next) = ce;     % Found bnd edge
                next      = next+1;
            else
                count = count+1;    % Count number of non-bnd edges in ct
            end
        end
        if count==3                 % If 3 non-bnd edges ct must be in middle of group
            old(ct) = true;         % & doesn't need to be checked for the next level
        end
    end
    numb = next-1; bnd(next:end) = [];

    % Skip to next lev if empty
    if numb==0
        continue
    end

    % Place nodes approximately on contours by interpolating across bnd
    % edges
    t1  = e2t(bnd,1);
    t2  = e2t(bnd,2);
    ok  = t2>0;

    % Get two points for interpolation. Always use t1 centroid and
    % use t2 centroid for internal edges and bnd midpoint for boundary
    % edges

    % 1st point is always t1 centroid
    H1 = Ht(t1);                                                % Centroid value
    p1 = ( p(t(t1,1),:)+p(t(t1,2),:)+p(t(t1,3),:) )/3;          % Centroid [x,y]

    % 2nd point is either t2 centroid or bnd edge midpoint
    i1        = t2(ok);                                         % Temp indexing
    i2        = bnd(~ok);
    H2        = H1;
    H2(ok)    = Ht(i1);                                         % Centroid values internally
    H2(~ok)   = ( Hn(e(i2,1))+Hn(e(i2,2)) )/2;                  % Edge values at boundary
    p2        = p1;
    p2(ok,:)  = ( p(t(i1,1),:)+p(t(i1,2),:)+p(t(i1,3),:) )/3;   % Centroid [x,y] internally
    p2(~ok,:) = ( p(e(i2,1),:)+p(e(i2,2),:) )/2;                % Edge [x,y] at boundary

    % Linear interpolation
    r     = (lev(v)-H1)./(H2-H1);
    penew = p1 + [r,r].*(p2-p1);

    % Do a temp connection between adjusted node & endpoint nodes in
    % ce so that the connectivity between neighbouring adjusted nodes
    % can be determined
    vecb    = (1:numb)';
    m       = 2*vecb-1;
    c1      = 0*m;
    c2      = 0*m;
    c1(m)   = e(bnd,1);
    c1(m+1) = e(bnd,2);
    c2(m)   = vecb;
    c2(m+1) = vecb;

    % Sort connectivity to place connected edges in sucessive rows
    [c1,i] = sort(c1); c2 = c2(i);

    % Connect adjacent adjusted nodes
    k    = 1;
    next = 1;
    while k<(2*numb)
        if c1(k)==c1(k+1)
            c1(next) = c2(k);
            c2(next) = c2(k+1);
            next     = next+1;
            k        = k+2;         % Skip over connected edge
        else
            k = k+1;                % Node has only 1 connection - will be picked up above
        end
    end
    ncc          = next-1;
    c1(next:end) = [];
    c2(next:end) = [];

    % Plot the contours
    % If an output is required, extra sorting of the
    % contours is necessary for CLABEL to work.
    if nargout>0

        % Form connectivity for the contour, connecting
        % its edges (rows in cc) with its vertices.
        ndx = repmat(1,nume,1);
        n2e = 0*penew;
        for k = 1:ncc
            % Vertices
            n1 = c1(k); n2 = c2(k);
            % Connectivity
            n2e(n1,ndx(n1)) = k; ndx(n1) = ndx(n1)+1;
            n2e(n2,ndx(n2)) = k; ndx(n2) = ndx(n2)+1;
        end
        bndn = n2e(:,2)==0;         % Boundary nodes
        bnde = bndn(c1)|bndn(c2);   % Boundary edges

        % Alloc some space
        tmpv = repmat(0,1,ncc);

        % Loop through the points at the current contour level (lev(v))
        % Try to assemble the CS data structure introduced in "contours.m"
        % so that clabel will work. Assemble CS by "walking" around each
        % subcontour segment contiguously.
        ce    = 1;
        start = ce;
        next  = 2;
        cn    = c2(1);
        flag  = false(ncc,1);
        x     = tmpv; x(1) = penew(c1(ce),1);
        y     = tmpv; y(1) = penew(c1(ce),2);
        for k = 1:ncc

            % Checked this edge
            flag(ce) = true;

            % Add vertices to patch data
            x(next) = penew(cn,1);
            y(next) = penew(cn,2);
            next    = next+1;

            % Find edge (that is not ce) joined to cn
            if ce==n2e(cn,1)
                ce = n2e(cn,2);
            else
                ce = n2e(cn,1);
            end

            % Check the new edge
            if (ce==0)||(ce==start)||(flag(ce))

                % Plot current subcontour as a patch and save handles
                x   = x(1:next-1);
                y   = y(1:next-1);
                z   = repmat(lev(v),1,next);
                h   = [h; patch('Xdata',[x,NaN],'Ydata',[y,NaN],'Zdata',z, ...
                                'Cdata',z,'facecolor','none','edgecolor','flat')]; hold on

                % Update the CS data structure as per "contours.m"
                % so that clabel works
                c = horzcat(c,[lev(v), x; next-1, y]);

                if all(flag)    % No more points at lev(v)
                    break
                else            % More points, but need to start a new subcontour

                    % Find the unflagged edges
                    edges = find(~flag);
                    ce    = edges(1);
                    % Try to select a boundary edge so that we are
                    % not repeatedly running into the boundary
                    for i = 1:length(edges)
                        if bnde(edges(i))
                            ce = edges(i); break
                        end
                    end
                    % Reset counters
                    start = ce;
                    next  = 2;
                    % Get the non bnd node in ce
                    if bndn(c2(ce))
                        cn = c1(ce);
                        % New patch vectors
                        x = tmpv; x(1) = penew(c2(ce),1);
                        y = tmpv; y(1) = penew(c2(ce),2);
                    else
                        cn = c2(ce);
                        % New patch vectors
                        x = tmpv; x(1) = penew(c1(ce),1);
                        y = tmpv; y(1) = penew(c1(ce),2);
                    end

                end

            else
                % Find node (that is not cn) in ce
                if cn==c1(ce)
                    cn = c2(ce);
                else
                    cn = c1(ce);
                end
            end

        end

    else        % Just plot the contours as is, this is faster...

        z = repmat(lev(v),2,ncc);

        patch('Xdata',[penew(c1,1),penew(c2,1)]', ...
              'Ydata',[penew(c1,2),penew(c2,2)]', ...
              'Zdata',z,'Cdata',z,'facecolor','none','edgecolor','flat');

        hold on

    end

end
% Assign outputs if needed
if nargout>0
    cout = c;
    hout = h;
end
return
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [e,eINt,e2t] = mkcon(p,t)
numt = size(t,1);
vect = 1:numt;
% DETERMINE UNIQUE EDGES IN MESH

e       = [t(:,[1,2]); t(:,[2,3]); t(:,[3,1])];             % Edges - not unique
vec     = (1:size(e,1))';                                   % List of edge numbers
[e,j,j] = unique(sort(e,2),'rows');                         % Unique edges
vec     = vec(j);                                           % Unique edge numbers
eINt    = [vec(vect), vec(vect+numt), vec(vect+2*numt)];    % Unique edges in each triangle
% DETERMINE EDGE TO TRIANGLE CONNECTIVITY
% Each row has two entries corresponding to the triangle numbers
% associated with each edge. Boundary edges have one entry = 0.
nume = size(e,1);
e2t  = repmat(0,nume,2);
ndx  = repmat(1,nume,1);
for k = 1:numt
    % Edge in kth triangle
    e1 = eINt(k,1); e2 = eINt(k,2); e3 = eINt(k,3);
    % Edge 1
    e2t(e1,ndx(e1)) = k; ndx(e1) = ndx(e1)+1;
    % Edge 2
    e2t(e2,ndx(e2)) = k; ndx(e2) = ndx(e2)+1;
    % Edge 3
    e2t(e3,ndx(e3)) = k; ndx(e3) = ndx(e3)+1;
end
return


function err = RotateMatrixCriterion(R,U)

dx = R*U(:,1)-[1;0;0;0];
dy = R*U(:,2)-[0;1;0;0];
err = dx'*dx + dy'*dy;


function RDr=scd_model_GPD_RDr(d,Delta,delta,Dr)
% RDr=scd_model_GPD_RDr(d,Delta,delta,Dr)

alpha=4*delta*Dr/d^2; beta=4*Delta*Dr/d^2;
a=[1.84118378134065	5.33144277352503	8.53631636634628	11.7060049025920	14.8635886339090].^2;% roots of bessel J1'
td=Delta-delta/3;

k=zeros(length(delta),5);
for m=1:5;
    numerator=2*alpha*a(m) - 2 + 2*exp(-alpha*a(m)) + (2-exp(alpha*a(m))-exp(-alpha*a(m))).*exp(-beta*a(m));
    denominator=alpha.^2*a(m).^3.*(a(m)-1);
    k(:,m)=numerator(:)./denominator(:);
end
k(isnan(k))=0;
RDr=sum(k,2).*d.^2./(2*td);


function H = hurst_estimate(sequence,method,isplot,opt)
%
% 'hurst_estimate' estimate the hurst parameter of a given sequence with
%     an appointed method. The algorithms of the methods can be found in
%     Murad's Taqqu, Vadim Teverovsky and Walter Willinger's paper
%     "Estimators for long-range dependence: an empirical study" or other
%     related papers.
% Inputs:
%     sequence: the input sequence for estimate
%     method: the name of a function which used to estimate the hurst
%             parameter of the sequence,e.g.
%               'aggvar': use aggvar function to estimate.
%               'RS': use RS function to estimate.
%               'per': use per function to estimate.
%     isplot: whether display the plot. without a plot if isplot equal to 0
%     opt: a optional parameter for some methods
% Outputs:
%     H: the estimated hurst coeffeient of the input sequence
% Examples:
%     H = hurst_estimate('peng',sequence,1);
%

%  Author: Chu Chen
%  Version 1.0,  03/10/2008
%  chen-chu@163.com
%

if nargin == 2
    isplot = 0;
    H = feval(method,sequence,isplot);
elseif nargin == 3
    H = feval(method,sequence,isplot);
elseif nargin == 4
    H = feval(method,sequence,isplot,opt);
end


function writePolygonSet(polys, filename)
%WRITEPOLYGONSET Write a set of simple polygons into a file.
%
%   writePolygonSet(POLYS, FILENAME);
%   Writes the set of polygons in the file FILENAME.
%   Following format is used:
%     X11 X12 X13 ... X1N
%     Y11 Y12 Y13 ... Y1N
%     X21 X22 X23 ... X2N
%     Y21 Y22 Y23 ... Y2N
%   Each polygon may have a different number of vertices.
%
%   See also
%   polygons2d, readPolygonSet

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2013-01-14
% Copyright 2013-2022 INRA - TPV URPOI - BIA IMASTE

% open file for reading
fid = fopen(filename, 'wt');

for i = 1:length(polys)
    poly = polys{i};
    n = size(poly, 1);

    % precompute format
    format = [repmat('%g ', 1, n) '\n'];

    % write one line for x, then one line for y
    fprintf(fid, format, poly(:,1)');
    fprintf(fid, format, poly(:,2)');
end

% close file
fclose(fid);


function dice = dice_labels(lname1, lname2)
% d = dice_labels(lname1, lname2)
%
% computes Dice coefficient of the two given label files.
%

%
% dice_labels.m
%
% Original Author: Nick Schmansky
% CVS Revision Info:
%    $Author: nicks $
%    $Date: 2011/03/02 00:04:12 $
%    $Revision: 1.2 $
%
% Copyright © 2011 The General Hospital Corporation (Boston, MA) "MGH"
%
% Terms and conditions for use, reproduction, distribution and contribution
% are found in the 'FreeSurfer Software License Agreement' contained
% in the file 'LICENSE' found in the FreeSurfer distribution, and here:
%
% https://surfer.nmr.mgh.harvard.edu/fswiki/FreeSurferSoftwareLicense
%
% Reporting: freesurfer@nmr.mgh.harvard.edu
%

label1 = read_label('',lname1);
label2 = read_label('',lname2);

label1Size = size(label1,1);
label2Size = size(label2,1);

hits = 0;
for i=1:label1Size
   x1 = label1(i,2);
   y1 = label1(i,3);
   z1 = label1(i,4);
   for j=1:label2Size
     x2 = label2(j,2);
     y2 = label2(j,3);
     z2 = label2(j,4);
     if ((x1==x2) && (y1==y2) && (z1==z2))
       hits = hits+1;
       break;
     end
   end
end

dice = (2 * hits) / (label1Size + label2Size);


function a = r8cc_inc ( m, n, nz_num, colptr, rowind, a, i, j, aij )

%*****************************************************************************80
%
%% R8CC_INC increments a value of a R8CC matrix.
%
%  Discussion:
%
%    The R8CC format is the double precision sparse compressed column
%    format.  Associated with this format, we have an M by N matrix
%    with NZ_NUM nonzero entries.  We construct the column pointer
%    vector COL of length N+1, such that entries of column J will be
%    stored in positions COL(J) through COL(J+1)-1.  This indexing
%    refers to both the ROW and A vectors, which store the row indices
%    and the values of the nonzero entries.  The entries of the
%    ROW vector corresponding to each column are assumed to be
%    ascending sorted.
%
%    The R8CC format is equivalent to the MATLAB "sparse" format,
%    and the Harwell Boeing "real unsymmetric assembled" (RUA) format.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Iain Duff, Roger Grimes, John Lewis,
%    User's Guide for the Harwell-Boeing Sparse Matrix Collection,
%    October 1992
%
%  Parameters:
%
%    Input, integer M, the number of rows of the matrix.
%
%    Input, integer N, the number of columns of the matrix.
%
%    Input, integer NZ_NUM, the number of nonzero entries.
%
%    Input, integer COLPTR(N+1), indicate where each column's data begins.
%
%    Input, integer ROWIND(NZ_NUM), the row indices.
%
%    Input, real A(NZ_NUM), the nonzero entries.
%
%    Input, integer I, J, the indices of the value to retrieve.
%
%    Input, real AIJ, the value to be added to A(I,J).
%
%    Output, real A(NZ_NUM), entry (I,J) has been incremented.
%

%
%  Seek sparse index K corresponding to full index (I,J).
%
  k = r8cc_ijk ( m, n, nz_num, colptr, rowind, i, j );
%
%  If no K was found, we fail.
%
  if ( k == -1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8CC_INC - Fatal error!\n' );
    fprintf ( 1, '  R8CC_IJK could not find the entry.\n' );
    fprintf ( 1, '  Row I = %d\n', i );
    fprintf ( 1, '  Col J = %d\n', j );
    error ( 'R8CC_INC - Fatal error!' );
  end

  a(k) = a(k) + aij;

  return
end


function epoch= getBestEpoch(recalls, recallNs, N)
    if nargin<3, N= 5; end

    % This command is generally OK
    % [~, epoch]= max(recalls(recallNs==N,:));
    % but the following does tie-breaking

    nRecalls= length(recallNs);
    nEpochs= size(recalls, 2);
    assert( nRecalls==size(recalls, 1) );

    posN= find( recallNs==N, 1);
    assert(~isempty(posN));
    posNs= posN:-1:1;
    if posN<nRecalls
        posNs= [posNs, (posN+1):nRecalls];
    end

    potential= true(1, nEpochs);
    for posN= posNs
        maxVal= max(recalls(posN, potential));
        isMax= abs(recalls(posN,:)-maxVal)<1e-6;
        potential= potential & isMax;
        if sum(potential)<=1
            break;
        end
    end

    epoch= find( potential, 1 );
    assert(~isempty(epoch));
end


function est = refine_grid_estimates(f_obj, grid, est_idx)
%REFINE_GRID_ESTIMATES Refines DOA estimates obtained from a grid.
%Inputs:
%   f_obj - Objective function. Its local minimums identify the DOAs.
%   grid - Grid used for the original estimation.
%   est_idx - Indices (corresponding to the grid) of the original
%             estimates.
%Output:
%   est - Refined estimates.
est = zeros(size(est_idx));
if size(grid, 1) == 1
    % 1d
    n_iter = 10;
    subgrid_size = 10;
    for kk = 1:length(est_idx)
        % k-th DOA
        % init bounds
        if est_idx(kk) > 1
            lb = grid(est_idx(kk) - 1);
        else
            lb = grid(1);
        end
        if est_idx(kk) < length(grid)
            ub = grid(est_idx(kk) + 1);
        else
            ub = grid(end);
        end
        % refine
        for ii = 1:n_iter
            % find minimum over the subgrid
            subgrid = linspace(lb, ub, subgrid_size);
            obj_vals = f_obj(subgrid);
            [~, min_idx] = min(obj_vals);
            % update bounds
            if min_idx > 1
                lb = subgrid(min_idx - 1);
            else
                lb = subgrid(1);
            end
            if min_idx < subgrid_size
                ub = subgrid(min_idx + 1);
            else
                ub = subgrid(end);
            end
        end
        est(kk) = subgrid(min_idx);
    end
else
    % 2d
    error('Not implemented.');
end
end


function [h,ax] = plot3d(v,data,varargin)
% plot spherical data
%
% Syntax
%   plot3d(v,data)
%
% Input
%
% See also
% savefigure

% -------------------- GET OPTIONS ----------------------------------------

% where to plot
if check_option(varargin,'parent')
  ax = get_option(varargin,'parent');
else
  ax = gca;
end

% scale and shift if required
v = v .* get_option(varargin,'scale',1);
v = v + get_option(varargin,'shift',0);

% plot
%v = v .* reshape(data,size(v));
h = surf(v.x,v.y,v.z,reshape(double(data),size(v,1),size(v,2),[]),'parent',ax,...
  'edgeColor','none');

% colormap
if numel(v) == numel(data), mtexColorMap(ax,getMTEXpref('defaultColorMap')); end

if ~ishold

  axis(ax,'equal','vis3d','off');

  set(ax,'XDir','rev','YDir','rev',...
    'XLim',[-1,1],'YLim',[-1,1],'ZLim',[-1,1]);
end


% The COBRAToolbox: testFBA.m
%
% Purpose:
%     - tests the basic functionality of FBA
%       Tests four basic solution: Optimal minimum 1-norm solution, Optimal
%       solution on fructose, Optimal anaerobic solution, Optimal ethanol
%       secretion rate solution returns 1 if all tests were completed succesfully, 0 if not
%
% Authors:
%     - Original file: Joseph Kang 04/27/09
%     - CI integration: Laurent Heirendt January 2017
%

% save the current path
currentDir = pwd;

% initialize the test
fileDir = fileparts(which('testFBA'));
cd(fileDir);

% set the tolerance
tol = 1e-8;

% define the solver packages to be used to run this test
solverPkgs = {'ibm_cplex', 'mosek', 'gurobi', 'tomlab_cplex', 'glpk'};

% load the model
load('testFBAData.mat');
model = readCbModel('testFBAData.mat','modelName','model');
for k = 1:length(solverPkgs)

    % change the COBRA solver (LP)
    solverOK = changeCobraSolver(solverPkgs{k}, 'LP', 0);

    if solverOK == 1
        fprintf('   Testing flux balance analysis using %s ... ', solverPkgs{k});

        % check the optimal solution - BiomassEcoli
        fprintf('\n>> Optimal minimum 1-norm solution\n');
        model = changeObjective(model, {'BiomassEcoli'}, 1);
        solution = optimizeCbModel(model);

        % testing if f values are within range
        assert(abs(solution.f - solutionStd.f) < tol);

        % testing if c*x == f
        assert(abs(model.c' * solution.x - solution.f) < tol);

        % print the flux vector
        printFluxVector(model, solution.x, true, true);

        % check the optimal solution - fructose
        fprintf('\n>> Optimal solution on fructose\n');
        model2 = changeRxnBounds(model, {'EX_glc(e)', 'EX_fru(e)'}, [0 -9], 'l');
        solution2 = optimizeCbModel(model2);

        % testing if f values are within range
        assert(abs(solution2.f - solution2Std.f) < tol);

        % testing if c*x == f
        assert(abs(model2.c' * solution2.x - solution2.f) < tol);

        % print the flux vector
        printFluxVector(model2, solution.x, true, true);

        % check the optimal anaerobic solution
        fprintf('\n>> Optimal anaerobic solution\n');
        model3 = changeRxnBounds(model, 'EX_o2(e)', 0, 'l');
        solution3 = optimizeCbModel(model3);

        % testing if f values are within range
        assert(abs(solution3.f - solution3Std.f) < tol);

        % testing if c*x == f
        assert(abs(model3.c' * solution3.x - solution3.f) < tol);

        % check the optimal ethanol secretion rate solution
        fprintf('\n>> Optimal ethanol secretion rate solution \n');
        model4 = changeObjective(model, 'EX_etoh(e)', 1);
        solution4 = optimizeCbModel(model4);

        % testing if f values are within range
        assert(abs(solution4.f - solution4Std.f) < tol);

        % testing if c*x == f
        assert(abs(model4.c' * solution4.x - solution4.f) < tol);

        % output a success message
        fprintf('Done.\n');
    end
end

% change the directory
cd(currentDir)


% Check that online inference gives same results as filtering for various algorithms

N = 3;
Q = 2;
ss = N*2;

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

obs_size = 1;
discrete_obs = 0;
bnet = mk_chmm(N, Q, obs_size, discrete_obs);
ns = bnet.node_sizes_slice;

engine = {};
engine{end+1} = hmm_inf_engine(bnet);
E = length(engine);

onodes = (1:N)+N;

T = 4;
ev = cell(ss,T);
ev(onodes,:) = num2cell(randn(N, T));

filter = 1;
loglik2 = zeros(1,E);
for e=1:E
  [engine2{e}, loglik2(e)] = enter_evidence(engine{e}, ev, 'filter', filter);
end

loglik = zeros(1,E);
marg1 = cell(E,N,T);
for e=1:E
  ll = zeros(1,T);
  engine{e} = dbn_init_bel(engine{e});
  for t=1:T
    [engine{e}, ll(t)] = dbn_update_bel(engine{e}, ev(:,t), t);
    for i=1:N
      marg1{e,i,t} = dbn_marginal_from_bel(engine{e}, i);
    end
  end
  loglik1(e) = sum(ll);
end

assert(approxeq(loglik1, loglik2))

a = zeros(E,N,T);
for e=1:E
  for t=1:T
    for i=1:N
      marg2{e,i,t} = marginal_nodes(engine2{e}, i, t);
      a(e,i,t) = (approxeq(marg2{e,i,t}.T(:), marg1{e,i,t}.T(:)));
    end
  end
end

assert(all(a(:)==1))


function [correct_class_decision_values normalized_rank_results rank_confusion_matrix] = get_rank_and_decision_value_results(cv, YTe, classifier_labels, decision_values, create_rank_confusion_matrix)
% This helper method calculates the normalized rank results, the decision values for the correct class, and the rank confusion matrix information.
%  This method should be run every time the classifier is tested.
%
%  Input parameters:
%    YTe:  the labels for the current test points
%    decision_values:  The decision values for the current test point
%    create_rank_confusion_matrix:  whether a confusion matrix should be created
%
%  The returned values are in the structure NORMALIZED_RANK_AND_DECISION_VALUE_RESULTS and have the fields
%
%   correct_class_decision_values: a [num_test_points x 1] vector containing the decision value for the ith test point (for the correct class of the ith point)
%
%   normalized_rank_results:  a [num_test_points x 1] vector containing the normalized rank value for the ith test point
%
%   rank_confusion_matrix: [num_predicted_classes x num_actual classes] current rank confusion matrix.
%      the i, j entry of this matrix tells how high up on the rank of predictions for the jth class
%      was entry i.
%

%==========================================================================

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

%==========================================================================

rank_confusion_matrix = [];

YTe_unique_values = unique(YTe);

% % %[vals sorted_inds] = sort(decision_values', 'descend');   % NEED TO BE CAREFUL THERE IS NOT TIE IN THE MAX DECISION VALUE (could create a bias in results)
[vals sorted_inds] = sort((decision_values + eps .* rand(size(decision_values)))', 'descend');   % adding a small amount of noise so that there is not a tie in the decision values
the_ranks_all_test_points = sorted_inds';   % each row has all the ranked order results for a given test point (i.e., all ranks for YTe(i))

% go through all test points and find the ranking and the decision value of the real label
for iTestPoint = 1:length(YTe)
    curr_rank_results(iTestPoint) = find(YTe_unique_values(the_ranks_all_test_points(iTestPoint, :)) == YTe(iTestPoint));  % find rank of the real label YTe
    correct_class_decision_values(iTestPoint) = decision_values(iTestPoint, (classifier_labels == YTe(iTestPoint)));   % find the decision value for real label
end

normalized_rank_results = 1 - (((curr_rank_results) - 1)./(length(YTe_unique_values) - 1));

% get information to create the rank confusion matrix
if create_rank_confusion_matrix == 1
    for iUniqueYTe = 1:length(YTe_unique_values)   % assuming that each time unique(YTe) has all possible label values (if it doesn't then this code will not work)

            curr_rank_inds = find(YTe == YTe_unique_values(iUniqueYTe));
            [rank_cm_vals rank_cm_inds] = sort(the_ranks_all_test_points(curr_rank_inds, :)');   % rank_cm_inds contains predicted classes from the most likely (first index value), to least likely (last index value)

            rank_confusion_matrix(:, iUniqueYTe) = sum(1 -((rank_cm_inds -1)./(length(YTe_unique_values) - 1)), 2);  % add all test points together for a total rank confusion matrix

    end
end


function newimg = move3dimage(img,Vy,Vx,Vz,method,offsets,jacobian_modulation)
%
% Calculate the moved image:
%    newimg = move3dimage3(img,Vy,Vx,Vz,method,offsets)
%
% Input:
%	Vy, Vx, Vz	- the motion field
%	method		- interpolation method, default value is 'linear'
%   zoffset		- Offset of Z for the motion field dimension respective to
%				  the img dimension
%
% In version 3, the input method is allowed to be larger than the dimension
% of motion fields. This actually allows better recostruction of the moved
% images because the motion fields could extend larger than the original
% dimension. An additional parameter has been added, the 'offsets'
% parameter.
%
%
if ~exist('method','var') || isempty(method)
	method = 'linear';
end

if ~exist('offsets','var') || isempty(offsets)
	offsets = [0 0 0];
elseif length(offsets) == 1
	offsets = [0 0 offsets];
end

if ~exist('jacobian_modulation','var') || isempty(jacobian_modulation)
	jacobian_modulation = 0;
end

% Computer Jacobian
if jacobian_modulation ~= 0
	jac = compute_jacobian(Vy,Vx,Vz);
end

dimimg = mysize(img);
dimmotion = mysize(Vy);
x0 = single([1:dimmotion(2)])+offsets(2);
y0 = single([1:dimmotion(1)])+offsets(1);
z0 = single([1:dimmotion(3)])+offsets(3);
[xx,yy,zz] = meshgrid(x0,y0,z0);	% xx, yy and zz are the original coordinates of image pixels

Vy = max((yy-Vy),1); clear yy; Vy = min(Vy,dimimg(1));
Vx = max((xx-Vx),1); clear xx; Vx = min(Vx,dimimg(2));
Vz = max((zz-Vz),1); clear zz; Vz = min(Vz,dimimg(3));

if dimimg(3) > 1
	newimg = zeros(dimmotion,'single');
	if offsets(3) == 0 && size(img,3) == size(Vy,3)
		spacing = 20;
		if mod(dimimg(3),spacing) == 1
			spacing = 19;
		end

		N = ceil(dimimg(3)/spacing);
		fprintf('Moving image');
		for k = 1:N
			fprintf('.');
			zmin = (k-1)*spacing+1;
			zmax = min(dimimg(3),k*spacing);
			zmin2 = floor(min(min(min(Vz(:,:,zmin:zmax)))));
			zmax2 = ceil(max(max(max(Vz(:,:,zmin:zmax)))));
			%newimg(:,:,zmin:zmax) = interp3(xx(:,:,zmin2:zmax2),yy(:,:,zmin2:zmax2),zz(:,:,zmin2:zmax2),img(:,:,zmin2:zmax2),Vx(:,:,zmin:zmax),Vy(:,:,zmin:zmax),Vz(:,:,zmin:zmax),method);
			newimg(:,:,zmin:zmax) = interp3(x0,y0,z0(zmin2:zmax2),img(:,:,zmin2:zmax2),Vx(:,:,zmin:zmax),Vy(:,:,zmin:zmax),Vz(:,:,zmin:zmax),method);
		end
		fprintf('\n');
	else
		newimg = interp3(img,Vx,Vy,Vz,method,0);
	end
	%newimg = interp3(img,Vx,Vy,Vz,method,0);
else
	newimg = interp2(img,Vx,Vy,method,0);
end

%newimg = interpn(img,yy-Vy,xx-Vx,zz-Vz,method,0);

if jacobian_modulation ~= 0
	% Apply Jacobin intensity modulation
	jac(isinf(jac)) = 1;
	jac(jac<0) = 1;
	newimg = newimg .* jac;
end


function jed = ymdf_to_jed_zoroastrian ( y, m, d, f )

%*****************************************************************************80
%
%% YMDF_TO_JED_ZOROASTRIAN converts a Zoroastrian YMDF date to a JED.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, M, D, real F, the YMDF date.
%
%    Output, real JED, the corresponding Julian Ephemeris Date.
%
  jed_epoch = epoch_to_jed_zoroastrian ( );

  jed = jed_epoch + ( d - 1 ) + 30 * ( m - 1 ) + 365 * ( y - 1 ) + f;

  return
end


function [y_splined, x_spline, s_weights, y_splined_ext] = splinerMat(x, y, dxs, reg_factor, x_ext)
% SYNTAX:
%   [y_splined, x_spline, s_weights, y_splined_ext] = splinerMat(x, y, dxs, reg_factor, <x_ext>)
%
% EXAMPLE:
%   [y_splined, x_spline, s_weights, y_splined_ext] = splinerMat(x, y, 4, 0, x_ext);
%   [y_splined, x_spline, s_weights, y_splined_ext] = splinerMat(x, [y y_var], 4, 0, x_ext);
%
% INPUT:
%   x            [n x 1] observation time
%   y            [n x 1] observation value
%                [n x 2] observation value and variances (in this case the variances of the data are taken into account)
%   dxs          [1 x 1] spline base size
%   reg_factor   [1 x 1] regularization factor on the first derivative
%   x_ext        [m x 1] points in which to compute the interpolation
%
% OUTPUT:
%   y_splined     [n x 1] interpolated observation (on the observation epochs)
%   x_spline      [o x 1] center of the (o) splines
%   s_weights     [o x 1] weights of the splines
%   y_splined_ext [m x 1] spline interpolated in x_ext positions
%
% DESCRIPTION:
%   Interpolate with cubic splines a given dataset
%

%--- * --. --- --. .--. ... * ---------------------------------------------
%               ___ ___ ___
%     __ _ ___ / __| _ | __|
%    / _` / _ \ (_ |  _|__ \
%    \__, \___/\___|_| |___/
%    |___/                    v 1.0RC1
%
%--------------------------------------------------------------------------
%  Copyright (C) 2021 Geomatics Research & Development srl (GReD)
%  Written by:        Andrea Gatti
%  Contributors:      Andrea Gatti, Giulio Tagliaferro
%  A list of all the historical goGPS contributors is in CREDITS.nfo
%--------------------------------------------------------------------------
%
%   This program is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%--------------------------------------------------------------------------
% 01100111 01101111 01000111 01010000 01010011
%--------------------------------------------------------------------------

    if (nargin < 4)
        reg_factor = 0;
    end
    if isempty(x)
        x = 1:size(y, 1);
    end

    inan = isnan(y);
    if (size(y,2) == 2)
        inan = inan(:,1) | inan(:,2);
    end

    x(inan) = [];
    y(inan) = [];

    [x, id] = sort(x);
    y = y(id,:);
    if ~isempty(y)
        if ((nargin == 3) || (reg_factor == 0))
            if (size(y,2) == 2)
                [y_splined, x_spline, s_weights] = spliner_v51(x,y(:,1),y(:,2),dxs);
            else
                [y_splined, x_spline, s_weights] = spliner_v5(x,y,dxs);
            end
        else
            if (size(y,2) == 2)
                [y_splined, x_spline, s_weights] = spliner_v51R(x,y(:,1),y(:,2),dxs, reg_factor);
            else
                [y_splined, x_spline, s_weights] = spliner_v5R(x,y,dxs, reg_factor);
            end
        end
    else
        y_splined = y;
        x_spline = [];
        s_weights = [];
    end

    % Interpolation => using spline to predict in different coordinates
    % (not present in the C version)
    if (nargin == 5)
        if ~isempty(x_spline)
            mask = (isnan(s_weights));
            if (length(mask) > 2)
                mask = mask | [mask(2:end); 0] | [0; mask(1:end-1)];
            end
            s_weights = interp1(x_spline(~mask),s_weights(~mask),x_spline);
            if (size(x_ext,1)==1)
                x_ext = x_ext';
            end
            y_splined_ext = zeros(size(x_ext,1),1);
            for s = 1:length(x_spline)
                tau = round((x_ext-repmat(x_spline(s),length(x_ext),1))/dxs *1e13)/1e13; % 1e13 rounding necessary to avoid numerical problems
                y_splined_ext = y_splined_ext + s_weights(s)*cubicSpline(tau);
            end
        else
            y_splined_ext = nan(numel(x_ext),1);
        end
    else
        y_splined_ext = y_splined;
    end
    tmp = nan(numel(inan),1);
    tmp(~inan) = y_splined;
    y_splined = tmp;
end

% No Regularization + variances
function [y_splined, x_spline, s_weights] = spliner_v51(x, y, y_var, dxs)
    nObs = length(x);

    % size of the intervall to interpolate
    x_span = x(nObs) - x(1);
    x0 = x(1);
    x = x - x0;

    % compute the number of splines needed for the interpolation
    n_splines = ceil(x_span/dxs) + 3;

    % compute spline centers
    x_spline = zeros(n_splines,1);
    s_weights = [];
    s_center = x(1) - (((n_splines-3)*dxs-x_span)/2) - dxs;
    for i = 1:n_splines
        x_spline(i) = s_center+(i-1)*dxs;
    end

    % init A matrix
    A = zeros(nObs, 4);
    skips = zeros(n_splines,1);
    N = sparse(n_splines,n_splines);
    TN = zeros(n_splines, 1);

    cur_spline = 1;          % first spline whose domain intersect the observation
    %tau = 0;                % normalized distance between the observation and the center of the cur_spline
    i = 1;                  % index of the first observation
    first_obs = i;          % first observation used in the current A matrix
    first_spline = 1;       % first spline used in the current A matrix
    n_skip = 0;              % number of spline to "skip" because ain't intersecting an observation
    used_obs = 0;            % number of observation used in building the N matrix
    y_splined = zeros(length(y),1);           % output
    skips(1) = 0;
    while (i <= nObs)
        % Compute the distance between the current observation and the current spline
        tau = round((x(i)-x_spline(cur_spline))/dxs *1e13)/1e13; % 1e13 rounding necessary to avoid numerical problems
        if (tau <= 2)
            % fill the design matrix
            A(i-first_obs+1,:) = cubicSpline([tau (tau-1) (tau-2) (tau-3)]);
            used_obs = used_obs+1;
            n_skip = 0;
            i = i+1;
        else
            cur_local_spline = cur_spline-first_spline+1;
            skips(cur_spline+1) = i-first_obs;
            % This block of the A matrix is completed
            % Computing N
            n_skip = n_skip +1;
            if (n_skip < 4)
                if (n_skip == 1)
                    A2 = A((skips(cur_spline)+1):i-first_obs,:);
                    iQ = sparse(diag(1./y_var(first_obs+skips(cur_spline):i-1)));
                    N(cur_local_spline:cur_local_spline+3,cur_local_spline:cur_local_spline+3) = sparse(N(cur_local_spline:cur_local_spline+3,cur_local_spline:cur_local_spline+3) + A2'*iQ*A2);

                    % Computing TN
                    TN(cur_local_spline:cur_local_spline+3) = TN(cur_local_spline:cur_local_spline+3) + A2' * iQ * y(first_obs+skips(cur_spline):i-1);
                end
                cur_spline = cur_spline +1;
            else
                % If I skip more than 3 times the spline solutions are independent,
                % I can start solving my filtering for the first i points

                s_par = [];
                if (used_obs < size(N,2))
                    fprintf('WARNING: Regularization is needed observations are less than splines.\n         Adding 1e-9 on the normal matrix diagonal\n');
                    R = sparse(eye(cur_local_spline)*1e-9);
                    s_par = (N(1:cur_local_spline,1:cur_local_spline)+R)\TN(1:cur_local_spline);
                else
                    s_par = (N(1:cur_local_spline,1:cur_local_spline))\TN(1:cur_local_spline);
                end
                s_weights = [s_weights; s_par];

                for s = first_spline:cur_spline-3
                    y_splined(skips(s)+first_obs:skips(s+1)+first_obs-1) = y_splined(skips(s)+first_obs:skips(s+1)+first_obs-1) + A((skips(s):skips(s+1)-1)+1,:) * s_par(s-first_spline+1:s+3-first_spline+1);
                end

                % find the next spline whose domain intersect the next observation
                tau = (x(i)-x_spline(cur_spline))/dxs;
                while (tau > 2)
                    cur_spline = cur_spline+1;
                    tau = (x(i)-x_spline(cur_spline))/dxs;
                    if (tau > 2),
                        s_weights(cur_spline) = nan;
                    end
                end
                first_spline = cur_spline;
                first_obs = i;
                skips(cur_spline) = 0;

                A = zeros(nObs-i, 4);
                N = sparse(n_splines-cur_spline+1, n_splines-cur_spline+1);
                TN = zeros(n_splines-cur_spline+1,1);
                used_obs = 0;
            end
        end
    end

    skips(cur_spline+1) = i-first_obs;
    if (n_skip == 0)
        A2 = A(skips(cur_spline)+1:i-first_obs,:);
        iQ = sparse(diag(1./y_var(first_obs+skips(cur_spline):i-1)));
        N(cur_spline-first_spline+1:cur_spline+3-first_spline+1,cur_spline-first_spline+1:cur_spline+3-first_spline+1) = sparse(N(cur_spline-first_spline+1:cur_spline+3-first_spline+1,cur_spline-first_spline+1:cur_spline+3-first_spline+1) + A2'*iQ*A2);

        % Computing TN
        TN(cur_spline-first_spline+1:cur_spline+3-first_spline+1) = TN(cur_spline-first_spline+1:cur_spline+3-first_spline+1) + A2' * iQ * y(first_obs+skips(cur_spline):i-1);
    end

    % find the interpolation for the last subset of observations
    s_par = [];
    if (used_obs < size(N,2))
        fprintf('WARNING: Regularization is needed observations are less than splines.\n         Adding 1e-9 on the normal matrix diagonal\n');
        R = speye(size(N,2), size(N,2))*1e-9;
        s_par = (N+R)\TN;
    else
        s_par = N\TN;
    end
    s_weights = [s_weights; s_par];

    for s = first_spline:cur_spline
        if ((skips(s)<skips(s+1)))
            y_splined(skips(s)+first_obs:skips(s+1)+first_obs-1) = y_splined(skips(s)+first_obs:skips(s+1)+first_obs-1) + A((skips(s):skips(s+1)-1)+1,:) * s_par(s-first_spline+1:s+3-first_spline+1);
        end
    end

    x_spline = x_spline + x0;
end

% Regularization + variances
function [y_splined, x_spline, s_weights] = spliner_v51R(x, y, y_var, dxs, reg_factor)
    nObs = length(x);

    y_var(y_var < reg_factor/2) = reg_factor/2;
    % size of the intervall to interpolate
    x_span = x(nObs) - x(1);
    x0 = x(1);
    x = x - x0;

    % compute the number of splines needed for the interpolation
    n_splines = ceil(x_span/dxs) + 3;

    % compute spline centers
    x_spline = zeros(n_splines,1);
    s_weights = [];
    s_center = x(1) - (((n_splines-3)*dxs-x_span)/2) - dxs;
    for i = 1:n_splines
        x_spline(i) = s_center+(i-1)*dxs;
    end

    % init A matrix
    A = zeros(nObs, 4);
    skips = zeros(n_splines,1);
    N = sparse(n_splines,n_splines);
    TN = zeros(n_splines, 1);

    cur_spline = 1;          % first spline whose domain intersect the observation
    tau = 0;                % normalized distance between the observation and the center of the cur_spline
    i = 1;                  % index of the first observation
    first_obs = i;          % first observation used in the current A matrix
    first_spline = 1;       % first spline used in the current A matrix
    n_skip = 0;              % number of spline to "skip" because ain't intersecting an observation
    used_obs = 0;            % number of observation used in building the N matrix
    y_splined = zeros(length(y),1);           % output
    skips(1) = 0;
    while (i <= nObs)
        % Compute the distance between the current observation and the current spline
        tau = round((x(i)-x_spline(cur_spline))/dxs *1e13)/1e13; % 1e13 rounding necessary to avoid numerical problems
        if (tau <= 2)
            % fill the design matrix
            A(i-first_obs+1,:) = cubicSpline([tau (tau-1) (tau-2) (tau-3)]);
            used_obs = used_obs+1;
            n_skip = 0;
            i = i+1;
        else
            cur_local_spline = cur_spline-first_spline+1;
            skips(cur_spline+1) = i-first_obs;
            % This block of the A matrix is completed
            % Computing N
            n_skip = n_skip +1;
            if (n_skip < 4)
                if (n_skip == 1)
                    A2 = A((skips(cur_spline)+1):i-first_obs,:);
                    iQ = sparse(diag(1./y_var(first_obs + skips(cur_spline):i-1)));
                    N(cur_local_spline:cur_local_spline+3,cur_local_spline:cur_local_spline+3) =   sparse(N(cur_local_spline:cur_local_spline+3,cur_local_spline:cur_local_spline+3) + A2'*iQ*A2);

                    % Computing TN
                    TN(cur_local_spline:cur_local_spline+3) = TN(cur_local_spline:cur_local_spline+3) + A2' * iQ * y(first_obs+skips(cur_spline):i-1);
                end
                cur_spline = cur_spline +1;
            else
                % find the next spline whose domain intersect the next observation
                tau = (x(i)-x_spline(cur_spline))/dxs;
                while (tau > 2)
                    cur_spline = cur_spline+1;
                    tau = (x(i)-x_spline(cur_spline))/dxs;
                end
                skips(cur_spline) = skips(cur_spline-1);
                used_obs = -1e10;
            end
        end
    end

    skips(cur_spline+1) = i-first_obs;
    if (n_skip == 0)
        A2 = A(skips(cur_spline)+1:i-first_obs,:);
        iQ = sparse(diag(1./y_var(first_obs+skips(cur_spline):i-1)));
        N(cur_spline-first_spline+1:cur_spline+3-first_spline+1,cur_spline-first_spline+1:cur_spline+3-first_spline+1) = sparse(N(cur_spline-first_spline+1:cur_spline+3-first_spline+1,cur_spline-first_spline+1:cur_spline+3-first_spline+1) + A2'*iQ*A2);

        % Computing TN
        TN(cur_spline-first_spline+1:cur_spline+3-first_spline+1) = TN(cur_spline-first_spline+1:cur_spline+3-first_spline+1) + A2' * iQ * y(first_obs+skips(cur_spline):i-1);
    end

    % find the interpolation for the last subset of observations
    s_par = [];
    if (size(N,2)>2)
        if reg_factor == 0
            fprintf('WARNING: Regularization is needed observations are less than splines.\n         Adding 1e-9 on the normal matrix diagonal\n');
            reg_factor = 1e-9;
        end
        R = sparse(eye(size(N,2))-diag(ones(size(N,2)-1,1),1)-diag(ones(size(N,2)-1,1),-1) + diag([0; ones(size(N,2)-2,1); 0]));
        R = R*reg_factor;
        s_par = (N+R)\TN;
    else
        if (used_obs < size(N,2))
            if reg_factor == 0
                fprintf('WARNING: Regularization is needed observations are less than splines.\n         Adding 1e-9 on the normal matrix diagonal\n');
                reg_factor = 1e-9;
            end
            R = sparse(eye(size(N,2))*reg_factor);
            s_par = (N+R)\TN;
        else
            s_par = N\TN;
        end
    end
    s_weights = [s_weights; s_par];

    for s = first_spline:cur_spline
        if ((skips(s)<skips(s+1)))
            y_splined(skips(s)+first_obs:skips(s+1)+first_obs-1) = y_splined(skips(s)+first_obs:skips(s+1)+first_obs-1) + A((skips(s):skips(s+1)-1)+1,:) * s_par(s-first_spline+1:s+3-first_spline+1);
        end
    end

    x_spline = x_spline + x0;
end

% No Regularization - no variances
function [y_splined, x_spline, s_weights] = spliner_v5(x, y, dxs)
    nObs = length(x);

    % size of the intervall to interpolate
    x_span = x(nObs) - x(1);
    x0 = x(1);
    x = x - x0;

    % compute the number of splines needed for the interpolation
    n_splines = ceil(x_span/dxs) + 3;

    % compute spline centers
    x_spline = zeros(n_splines,1);
    s_center = x(1) - (((n_splines-3)*dxs-x_span)/2) - dxs;
    for i = 1:n_splines
        x_spline(i) = s_center+(i-1)*dxs;
    end

    % init A matrix
    A = zeros(nObs, 4);
    A_idx = zeros(nObs, 4);

    tau   = round(rem(x',dxs)/dxs*1e13)/1e13;  % 1e13 rounding necessary to avoid numerical problems
    idx   = floor((x')/dxs)+1;
    A     = cubicSpline4Col(tau);
    A_idx = [idx(:) idx(:)+1 idx(:)+2 idx(:)+3];
    n_par = max(A_idx(:,4));
    n_obs = numel(x);
    rows  = repmat((1:n_obs)',1,4);

    A = sparse(rows, A_idx, A, n_obs, n_par);

    idx_null = sum(A~=0) == 0;
    A(:,idx_null) = [];

    N = A'*A;
    B = A'*y;

    x = N\B;

    s_weights = nan(numel(idx_null),1);
    s_weights(~idx_null) = x;
    y_splined = A*x;

    x_spline = x_spline + x0;
end

% Regularization - no variances
function [y_splined, x_spline, s_weights] = spliner_v5R(x, y, dxs, reg_factor)
    nObs = length(x);

    % size of the intervall to interpolate
    x_span = x(nObs) - x(1);
    x0 = x(1);
    x = x - x0;

    % compute the number of splines needed for the interpolation
    n_splines = ceil((x_span+eps(x_span))/dxs) + 3;

    % compute spline centers
    x_spline = zeros(n_splines,1);
    s_weights = [];
    s_center = x(1) - (((n_splines-3)*dxs-x_span)/2) - dxs;
    for i = 1:n_splines
        x_spline(i) = s_center+(i-1)*dxs;
    end

    % init A matrix
    A = zeros(nObs, 4);
    skips = zeros(n_splines,1);
    N = sparse(n_splines,n_splines);
    TN = zeros(n_splines, 1);

    cur_spline = 1;          % first spline whose domain intersect the observation
    tau = 0;                % normalized distance between the observation and the center of the cur_spline
    i = 1;                  % index of the first observation
    first_obs = i;          % first observation used in the current A matrix
    first_spline = 1;       % first spline used in the current A matrix
    n_skip = 0;              % number of spline to "skip" because ain't intersecting an observation
    used_obs = 0;            % number of observation used in building the N matrix
    y_splined = zeros(length(y),1);           % output
    skips(1) = 0;
    while (i <= nObs)
        % Compute the distance between the current observation and the current spline
        tau = round((x(i) - x_spline(cur_spline))/dxs * 1e13) / 1e13; % 1e13 rounding necessary to avoid numerical problems
        if (tau <= 2)
            % fill the design matrix
            A(i-first_obs+1,:) = cubicSpline([tau (tau-1) (tau-2) (tau-3)]);
            used_obs = used_obs+1;
            n_skip = 0;
            i = i+1;
        else
            cur_local_spline = cur_spline-first_spline+1;
            skips(cur_spline+1) = i-first_obs;
            % This block of the A matrix is completed
            % Computing N
            n_skip = n_skip +1;
            if (n_skip < 4)
                if (n_skip == 1)
                    A2 = A((skips(cur_spline)+1):i-first_obs,:);
                    N(cur_local_spline:cur_local_spline+3,cur_local_spline:cur_local_spline+3) =   sparse(N(cur_local_spline:cur_local_spline+3,cur_local_spline:cur_local_spline+3) + A2'*A2);

                    % Computing TN
                    TN(cur_local_spline:cur_local_spline+3) = TN(cur_local_spline:cur_local_spline+3) + A2' * y(first_obs+skips(cur_spline):i-1);
                end
                cur_spline = cur_spline +1;
            else
                % find the next spline whose domain intersect the next observation
                tau = (x(i)-x_spline(cur_spline))/dxs;
                while (tau > 2)
                    cur_spline = cur_spline+1;
                    tau = (x(i)-x_spline(cur_spline))/dxs;
                end
                skips(cur_spline) = skips(cur_spline-1);
                used_obs = -1e10;
            end
        end
    end

    skips(cur_spline+1) = i-first_obs;
    if (n_skip == 0)
        A2 = A(skips(cur_spline)+1:i-first_obs,:);
        N(cur_spline-first_spline+1:cur_spline+3-first_spline+1,cur_spline-first_spline+1:cur_spline+3-first_spline+1) = N(cur_spline-first_spline+1:cur_spline+3-first_spline+1,cur_spline-first_spline+1:cur_spline+3-first_spline+1) + A2'*A2;

        % Computing TN
        TN(cur_spline-first_spline+1:cur_spline+3-first_spline+1) = TN(cur_spline-first_spline+1:cur_spline+3-first_spline+1) + A2' * y(first_obs+skips(cur_spline):i-1);
    end

    % find the interpolation for the last subset of observations
    s_par = [];
    if (size(N,2)>2)
        %fprintf('WARNING: Regularization is needed observations are less than splines.\n         Adding 1e-9 on the normal matrix diagonal\n');
        R = (speye(size(N,2), size(N,2)) - spdiags(ones(size(N,2), 1), 1, size(N,2), size(N,2)) - spdiags(ones(size(N,2), 1), -1, size(N,2), size(N,2)) + spdiags([0; ones(size(N,2) - 2, 1); 0], 0, size(N,2), size(N,2))) * reg_factor;
        s_par = (N+R)\TN;
    else
        if (used_obs < size(N,2))
            %fprintf('WARNING: Regularization is needed observations are less than splines.\n         Adding 1e-9 on the normal matrix diagonal\n');
            R = speye(size(N,2)) * reg_factor;
            s_par = (N+R)\TN;
        else
            s_par = N\TN;
        end
    end
    s_weights = [s_weights; s_par];

    for s = first_spline:cur_spline
        if (skips(s) == 0 && s > 1)
            skips(s) = skips(s-1);
        end
        if ((skips(s)<skips(s+1)))
            y_splined(skips(s)+first_obs:skips(s+1)+first_obs-1) = y_splined(skips(s)+first_obs:skips(s+1)+first_obs-1) + A((skips(s):skips(s+1)-1)+1,:) * s_par(s-first_spline+1:s+3-first_spline+1);
        end
    end

    x_spline = x_spline + x0;
end

% SYNTAX:
%   [y] = cubicSpline(t)
%
% EXAMPLE:
%   [y] = cubicSpline(1)
%
% INPUT:
%   t = normalized value from [-2 to 2] of the distance between the
%       observation and the center of the cubic spline
%
% OUTPUT:
%   y = value of the spline in the given point
%
% DESCRIPTION:
%   Get the value of the cubic spline with a base of 4 at the given t normalized
%   value
%
% USEFUL VALUES:
%   2/3 = cubicSpline(0);
%   1/6 = cubicSpline(-1);
%   1/6 = cubicSpline(1);
%
% by Andrea Gatti
%
function [y] = cubicSpline(t)

% FAST VECTORIAL IMPLEMENTATION
y = zeros(size(t));
pos = (t > -2) + (t > -1) - (t > 1) - (t > 2);

% pos = 0    => spline = 0;
% pos = 1    => spline = (-abs(t)+2)^3/6;
% pos = 2    => spline = ((-abs(t)+2)^3 - 4*(-abs(t)+1)^3)/6;

p1 = pos==1;
p2 = pos==2;
t = abs(t);
y(p1) = (2-t(p1)).^3/6;
y(p2) = ((2-t(p2)).^3 - 4*(1-t(p2)).^3)/6;

%  PLAIN IMPLEMENTATION
%
% 	if ((t < -2) || (t > 2))
% 		y = 0;
%     else
%         if (t < 0)
%             if (t <= -1)
%                 tmp2=(t+2);
%                 y = (tmp2*tmp2*tmp2 / 6);
%             else
%                 tmp1=(t+1);
%                 tmp2=(t+2);
%                 y = ((tmp2*tmp2*tmp2 - 4*tmp1*tmp1*tmp1) / 6);
%             end
%         else
%             if (t < 1)
%                 tmp1=(1-t);
%                 tmp2=(2-t);
%                 y = ((tmp2*tmp2*tmp2 - 4*tmp1*tmp1*tmp1) / 6);
%             else
%                 tmp2=(2-t);
%                 y = (tmp2*tmp2*tmp2 / 6);
%             end
%         end
%     end
end

function [val] = cubicSpline4Col(t)
    % Compute matrix entry for cubic spline
    %
    % INPUT
    %   t -> 0 : 1
    %   order -> 1,3
    %
    % SYNTAX:
    %  Core_Utils.cubicSplic(t)
    val = zeros(numel(t),4);
    val(:,1) = (1 - t).^3/6;
    val(:,2) = ((2-t).^3 - 4*(1-t).^3)/6;
    val(:,3) = ((1+t).^3 - 4*(t).^3)/6;
    val(:,4) = (t).^3/6;
end