function D = computeDistMatrix_AVFC (data,T,options)
%
% It uses KL-divergence to compute a subject-by-subject distance matrix
% of average FC matrices (on fMRI).
%
% INPUT
% data          observations; in this case it has to be a cell, each with
%               the data for one subject
% T             length of series, also a cell.
%
% OUTPUT
% D             (N by N) distance matrix, with the distance between each
%               pair of subjects in "HMM space"
%
% Author: Diego Vidaurre, OHBA, University of Oxford (2020)

if ~iscell(data) || ~iscell(T), error('X and T must both be cells'); end

if nargin<3, options = struct(); end

N = length(data);

for n = 1:N
    if ischar(data{n})
        fsub = data{n};
        loadfile_sub;
    else
        X = data{n};
    end
    X = preprocdata(X,T{n},options);
    if n == 1
        ndim = size(X,2);
        V = zeros(ndim,ndim,N);
    end
    V(:,:,n) = X' * X;
end

D = NaN(N);
try
    for n1 = 1:N-1
        for n2 = n1+1:N
            D(n1,n2) =  ( wishart_kl(V(:,:,n1),V(:,:,n2),sum(T{n1}),sum(T{n2})) + ...
                wishart_kl(V(:,:,n2),V(:,:,n1),sum(T{n2}),sum(T{n1})) ) /2;
            D(n2,n1) = D(n1,n2);
        end
    end
catch
    for n = 1:N
        V(:,:,n) = V(:,:,n) + 0.0001*eye(ndim);
    end
    for n1 = 1:N-1
        for n2 = n1+1:N
            D(n1,n2) =  ( wishart_kl(V(:,:,n1),V(:,:,n2),sum(T{n1}),sum(T{n2})) + ...
                wishart_kl(V(:,:,n2),V(:,:,n1),sum(T{n2}),sum(T{n1})) ) /2;
            D(n2,n1) = D(n1,n2);
        end
    end
end

end

function X = preprocdata(X,T,options)

% Filtering
if isfield(options,'filter') && ~isempty(options.filter)
    X = filterX(X,T,options.Fs,options.filter);
end
% Detrend X
if isfield(options,'detrend') && options.detrend
    X = detrendX(X,T);
end
% Hilbert envelope
if isfield(options,'onpower') && options.onpower
    X = rawsignal2power(X,T);
end
% Embedding
if isfield(options,'embeddedlags') && length(options.embeddedlags) > 1
    X = embeddata(X,T,options.embeddedlags);
end
X = zscore(X);

end


function [u,w,AE,AI,isExteriorTElem,isExteriorSElem] = interfacefittedPoisson(node,telem,selem,pde,interfaceEdge,bdEdge,option)
%% INTERFACEFITTEDPOISSON Poisson equation: P1 linear element.
%
%   u = INTERFACEPOISSON(node,telem,selem,pde,E) produces the linear finite element
%   approximation of the interface Poisson equation
%
%   input:
%       node, N*2 matrix, node(i,:) are the xy coordinates of i-th node;
%       telem, NTT*3 matrix, telem(j,:) are the three  global indices of the
%              vertices ofj-th triangle element;
%       selem,NTS*4 matrix, selem(j,:) are the four global indices of the
%             vertices of j-th quad element;
%
%

[N,Dim] = size(node);
Ndof = N;

%% Geometry structures of interface meshes

NTS = size(selem,1);
isExteriorSElem = false(NTS,1);
c = (node(selem(:,1),:) + node(selem(:,2),:) + node(selem(:,3),:)+node(selem(:,4),:))/4;
isExteriorSElem(pde.phi(c)>0) = true;

NTT = size(telem,1);
isExteriorTElem = false(NTT,1);
c = (node(telem(:,1),:) + node(telem(:,2),:) + node(telem(:,3),:))/3;
isExteriorTElem(pde.phi(c)>0) = true;

[sAE,sbE] = getstiffmatrixandrhsonquad(node,selem(isExteriorSElem,:));
[sAI,sbI] = getstiffmatrixandrhsonquad(node,selem(~isExteriorSElem,:));
[tAE,tbE] = getstiffmatrixandrhsontri(node,telem(isExteriorTElem,:));
[tAI,tbI] = getstiffmatrixandrhsontri(node,telem(~isExteriorTElem,:));

A = sAE + sAI + tAE + tAI;
b = sbE + sbI + tbE + tbI;

AI = sAI + tAI;
AE = sAE + tAE;

flux = getfluxconditiononinterface(node,interfaceEdge);
b = b - flux;

%% Extend w to the whole domain
isInterfaceNode = false(Ndof,1);
isInterfaceNode(interfaceEdge(:)) = true;
interfaceNode = find(isInterfaceNode);

isInNode = false(Ndof,1);
interiorSElem = selem(~isExteriorSElem,:);
interiorTElem = telem(~isExteriorTElem,:);
isInNode([interiorSElem(:);interiorTElem(:)]) = true;
inNode = find(isInNode & ~isInterfaceNode);

w = zeros(Ndof,1);
FI= zeros(Ndof,1);
w(interfaceNode) = pde.exactw(node(interfaceNode,:));
FI = FI - AI*w;
extensionoption.printlevel = 0;
w(inNode) = amg(AI(inNode, inNode),FI(inNode),extensionoption);

%% Dirichlet boundary condition
isBdNode = false(Ndof,1);
isBdNode(bdEdge(:)) = true;
bdNode = find(isBdNode);

u = zeros(Ndof,1);
u(bdNode) = pde.g_D(node(bdNode,:));
b = b - A*u+AI*w;
b(bdNode) = u(bdNode);

freeNode = find(~isBdNode);
bdidx = zeros(Ndof,1);
bdidx(bdNode) = 1;
Tbd = spdiags(bdidx,0,Ndof,Ndof);
T = spdiags(1-bdidx,0,Ndof,Ndof);
AD = T*A*T + Tbd;

%% Solve the system of linear equations
if isempty(freeNode), return; end
% Set up solver type
if isempty(option) || ~isfield(option,'solver')    % no option.solver
    if Ndof <= 1e3  % Direct solver for small size systems
        option.solver = 'direct';
    else            % MGCG  solver for large size systems
        option.solver = 'mg';
    end
end
solver = option.solver;
% solve
switch solver
    case 'direct'
        tic;
        u(freeNode) = AD(freeNode,freeNode)\b(freeNode);
        residual = norm(b - AD*u);
        info = struct('solverTime',toc,'itStep',0,'err',residual,'flag',2,'stopErr',residual);
    case 'none'
        info = struct('solverTime',[],'itStep',0,'err',[],'flag',3,'stopErr',[]);
    case 'amg'
        option.solver = 'CG';
        [u(freeNode),info] = amg(AD(freeNode,freeNode),b(freeNode),option);
end

%%
eqn = struct('A',AD,'b',b,'freeNode',freeNode);

   %% Assemble stiffness matrix and rhs on quad mesh
    function [A,b] = getstiffmatrixandrhsonquad(node,elem)
        [NT,NV] = size(elem);
        % generate sparse pattern
        ii = zeros(10*NT,1); jj = zeros(10*NT,1);
        index = 0;
        for i = 1:4
            for j = i:4
                ii(index+1:index+NT) = double(elem(:,i));
                jj(index+1:index+NT) = double(elem(:,j));
                index = index + NT;
            end
        end
        % quadrature points
        if ~isfield(pde,'d'), pde.d = []; end
        if ~isfield(option,'dquadorder')
            option.dquadorder = 2;        % default order is exact for quadratic function
        end
        [pts, weight] = quadptsquad(option.dquadorder);
        nQuad = size(pts,1);
        % compute non-zeros
        sA = zeros(10*NT,nQuad);
        for p = 1:nQuad
            % Dphi at quadrature points
            [phi, Dphip, J] = quadbasis(node,elem,pts(p,:));
            index = 0;
            for i = 1:4
                for j = i:4
                    Aij = 0;
                    if isempty(pde.d) || isnumeric(pde.d)
                        Aij = Aij + weight(p)*dot(Dphip(:,:,i),Dphip(:,:,j),2);
                    else
                        pxy = zeros(NT, Dim);
                        for ip = 1:Dim
                            xi = node(:,ip);
                            pxy(:,ip) = xi(elem)*phi;
                        end
                        Aij = Aij + weight(p)*dot(Dphip(:,:,i),Dphip(:,:,j),2).*pde.d(pxy);
                    end
                    if ~isempty(pde.d) && isnumeric(pde.d) % d is piecewise constant
                        Aij = pde.d.*Aij;
                    end
                    Aij = Aij.*J;
                    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),Ndof,Ndof);
        AU = sparse(ii(upperIdx),jj(upperIdx),sA(upperIdx),Ndof,Ndof);
        A = A + AU + AU';
        clear Aij ii jj Dphip

        %% Assemble the right hand side
        b = zeros(Ndof,1);
        if ~isfield(option,'fquadorder')
            option.fquadorder = 3;   % default order
        end
        if ~isfield(pde,'f') || (isreal(pde.f) && (pde.f==0))
            pde.f = [];
        end

        if ~isempty(pde.f)
            [pts, weight] = quadptsquad(option.fquadorder);
            nQuad = size(pts,1);
            bt = zeros(NT,NV);
            for p = 1:nQuad
                % quadrature points in the x-y coordinate
                [phi, ~, J] = quadbasis(node,elem, pts(p,:));
                pxy = zeros(NT, Dim);
                for i = 1:Dim
                    xi = node(:,i);
                    pxy(:,i) = xi(elem)*phi; % ? questionable
                end
                fp = pde.f(pxy);
                bt = bt + (weight(p)*fp.*J)*phi';
            end
            b = accumarray(elem(:),bt(:),[Ndof 1]);
        end

    end

    %% Assemble the stiffmatrix and right hand side on triangle mesh
    function [A,b] = getstiffmatrixandrhsontri(node,elem)
        NT = size(elem,1);
        % quadrature points
        center = (node(elem(:,1),:) + node(elem(:,2),:) + node(elem(:,3),:))/3;
       % Diffusion coefficient
        if isfield(pde,'d') && ~isempty(pde.d)
            if isnumeric(pde.d)
                K = pde.d;                   % d is an array
            else                            % d is a function
                K = pde.d(center);
            end
        else
            K = [];
        end
        [Dphi,area] = gradbasis(node,elem);
        A = sparse(Ndof,Ndof);
        for i = 1:3
            for j = i:3
                Aij = (Dphi(:,1,i).*Dphi(:,1,j)+Dphi(:,2,i).*Dphi(:,2,j)).*area;
                if ~isempty(K)
                    Aij = K.*Aij;
                end
                if (j==i)
                    A = A + sparse(elem(:,i),elem(:,j),Aij,Ndof,N);
                else
                    A = A + sparse([elem(:,i);elem(:,j)],[elem(:,j);elem(:,i)],[Aij; Aij],Ndof,Ndof);
                end
            end
        end

        b = zeros(Ndof,1);
        if ~isfield(option,'fquadorder')
            option.fquadorder = 3;   % default order
        end
        if ~isfield(pde,'f') || (isreal(pde.f) && (pde.f==0))
            pde.f = [];
        end
        if ~isempty(pde.f)
            [lambda,weight] = quadpts(option.fquadorder);
            nQuad = size(lambda,1);
            ft = zeros(NT,3);
            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:3
                    ft(:,j) = ft(:,j) + lambda(p,j)*weight(p)*fp;
                end
            end
            ft = ft.*[area,area,area];
            b = accumarray(elem(:),ft(:),[Ndof 1]);
        end
    end

    %% Neumann boundary condition on interface edges
    function b = getfluxconditiononinterface(node,interfaceEdge)
        ve = node(interfaceEdge(:,1),:) - node(interfaceEdge(:,2),:);
        ve = [-ve(:,2), ve(:,1)];
        edgeLen = sqrt(sum(ve.^2,2));
        if ~isfield(option,'gNquadorder')
            option.gNquadorder = 5;   % default order
        end
        [lambda,weight] = quadpts1(option.gNquadorder);
        nQuad = length(weight);
        ge = zeros(size(interfaceEdge,1),2);
        for i = 1:nQuad
            pxy=lambda(i,1)*node(interfaceEdge(:,1),:)+lambda(i,2)*node(interfaceEdge(:,2),:);
            leftpt = pxy - ve/2;
            rightpt = pxy + ve/2;
            pxy = findintersectbisect(pde.phi,leftpt,rightpt);
            q = pde.exactq(pxy);
            ge(:,1)=ge(:,1)+weight(i)*lambda(i,1)*q;
            ge(:,2)=ge(:,2)+weight(i)*lambda(i,2)*q;
        end
        ge = ge.*[edgeLen,edgeLen];
        b = accumarray(interfaceEdge(:), ge(:),[Ndof,1]);
    end

end


function [vfN, vfR, vrN, vrR, vnetN, vnetR, uL, uC, u0L, u0C, lnxL, lnxC] = projectOntoSubspace(A, vf, vr, vnet, u, u0, lnx,printLevel,rowBool,colBool)
% Projects flux, net flux, potential and logarithmic concentration onto
% their respective subspaces of A using projection matrices generated either
% derived from SVD, or by using the Moore-Penrose pseudoinverse
%
% Optionally, a subset of the matrix A may be chosen by using A(rowBool,colBool)
% but then only the true rows of u, u0, lnx, and true columns of vf,vr,vnet are
% projected and the remaining rows and columns are not affected
%
% Let `M` denote the Moore-Penrose pseudoinverse of A and the subscripts are the following
% `_R` row space,
% `_N` nullspace,
% `_C` column space,
% `_L` left nullspace,
%
% Example for flux of net flux
%
% Let
%
% .. math::
%      vf   &= vf_R + vf_N \\
%      vf_R &= M A vf = PR vf \\
%      vf_N &= (I - M A) vf = PN vf
%
% Example for potential or logarithmic concentration
%
% Let
%
% .. math::
%      u   &= u_C + u_L \\
%      u_C &= A M u = PC u \\
%      u_L &= (I - A M) u = PL u
%
% USAGE:
%
%    [vfN, vfR, vrN, vrR, vnetN, vnetR, uL, uC, u0L, u0C, lnxL, lnxC] = projectOntoSubspace(modelT, vf, vr, vnet, u, u0, lnx)
%
% INPUTS:
%    A          `m x n` matrix
%    vf:        `n x 1` - forward flux
%    vr:        `n x 1` - reverse flux
%    vnet:      `n x 1` - net flux
%    u:         `m x 1` - chemical potential
%    u0:        `m x 1` - standard chemical potential
%    lnx:       `m x 1` - logarithmic concentration
% OPTIONAL INPUTS
%    rowBool    `m x 1` - boolean indicating the subset of rows of A
%    colBool    'n x 1' - boolean indicating the subset of cols of A
%
% OUTPUTS:
%    vfN:       forward flux - nullspace
%    vfR:       forward flux - row space
%    vrN:       reverse flux - nullspace
%    vrR:       reverse flux - row space
%    vnetN:     net flux - nullspace
%    vnetR:     net flux - row space
%    uL:        chemical potential - left nullspace
%    uC:        chemical potential - column space
%    lnxL:      logarithmic concentration - left nullspace
%    lnxC:      logarithmic concentration - column space

if ~isempty(vf)
    if any((vnet(colBool)- vf +vr)>1e-12) %sanity check
    error('Net flux does not equal the difference between forward and reverse flux')
    end
end

if ~isempty(lnx)
    if any((u - u0 - lnx)>1e-12)
        error('Chemical potential does not equal standard chemical potential plus logarithmic conc.')
    end
end

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

[m,n]=size(A);

if ~exist('rowBool','var')
    rowBool=true(m,1);
end

if ~exist('colBool','var')
    colBool=true(n,1);
end

%generate fake outputs, or populate with unprojected vectors, part of which
%will be overwritten with the projected vectos further below.
if ~exist('u','var')
    uL=NaN*ones(m,1);
    uC=NaN*ones(m,1);
else
    uL=u;
    uC=u;
end
if ~exist('u0','var')
    u0L=NaN*ones(m,1);
    u0C=NaN*ones(m,1);
else
    u0L=u0;
    u0C=u0;
end
if ~exist('lnx','var')
    lnxL=NaN*ones(m,1);
    lnxC=NaN*ones(m,1);
else
    lnxL=lnx;
    lnxC=lnx;
end
if ~exist('vf','var')
    vfN=NaN*ones(n,1);
    vfR=NaN*ones(n,1);
else
    vfN=vf;
    vfR=vf;
end
if ~exist('vr','var')
    vrN=NaN*ones(n,1);
    vrR=NaN*ones(n,1);
else
    vrN=vr;
    vrR=vr;
end
if ~exist('vnet','var')
    vnetN=NaN*ones(n,1);
    vnetR=NaN*ones(n,1);
else
    vnetN=vnet;
    vnetR=vnet;
end

%generate projection matrices
sub_space='all';

[PR,PN,PC,PL]=subspaceProjector(A(rowBool,colBool),printLevel,sub_space);

%potential
if ~isempty(u)
    %concentration
    uC(rowBool)=PC*u(rowBool);
    uL(rowBool)=PL*u(rowBool);
end

if ~isempty(u0)
    %standard potential
    u0L(rowBool)=PL*u0(rowBool);
    u0C(rowBool)=PC*u0(rowBool);
end

if ~isempty(lnx)
    %concentration
    lnxC(rowBool)=PC*lnx(rowBool);
    lnxL(rowBool)=PL*lnx(rowBool);
end

%flux
if ~isempty(vnet)
    vnetN(colBool)=PN*vnet(colBool);
    vnetR(colBool)=PR*vnet(colBool);
end
if ~isempty(vf)
    vfN(colBool)=PN*vf(colBool);
    vrR(colBool)=PR*vr(colBool);
end
if ~isempty(vr)
    vrN(colBool)=PN*vr(colBool);
    vrR(colBool)=PR*vr(colBool);
end


function pos2 = nutate (tjd, pos1)

% this function nutates equatorial rectangular coordinates from
% the mean dynamical equator and equinox of epoch to the true
% equator and equinox of epoch. see explanatory supplement to the
% astronomical almanac, pp. 114-115.

% input

%  tjd  = tdb julian date of epoch

%  pos1 = position vector, geocentric equatorial rectangular
%         coordinates, referred to mean dynamical equator and
%         equinox of epoch

% output

%  pos2 = position vector, geocentric equatorial rectangular
%         coordinates, referred to true equator and equinox of epoch

% note:  if tjd is negative, inverse nutation (true to mean) is applied.

% ported from NOVAS 3.0

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

seccon = 180.d0 * 3600.d0 / pi;

tjd1 = abs(tjd);

[oblm, oblt, eqeq, dpsi, deps] = etilt (tjd1);

oblm = oblm * 3600.d0 / seccon;

oblt = oblt * 3600.d0 / seccon;

dpsi = dpsi / seccon;

deps = deps / seccon;

cobm = cos(oblm);

sobm = sin(oblm);

cobt = cos(oblt);

sobt = sin(oblt);

cpsi = cos(dpsi);

spsi = sin(dpsi);

% compute elements of nutation rotation matrix

xx =  cpsi;
yx = -spsi * cobm;
zx = -spsi * sobm;

xy =  spsi * cobt;
yy =  cpsi * cobm * cobt + sobm * sobt;
zy =  cpsi * sobm * cobt - cobm * sobt;

xz =  spsi * sobt;
yz =  cpsi * cobm * sobt - sobm * cobt;
zz =  cpsi * sobm * sobt + cobm * cobt;

if (tjd < 0.0d0)

    % perform rotation from true to mean

    pos2(1) = xx * pos1(1) + xy * pos1(2) + xz * pos1(3);

    pos2(2) = yx * pos1(1) + yy * pos1(2) + yz * pos1(3);

    pos2(3) = zx * pos1(1) + zy * pos1(2) + zz * pos1(3);

else

    % perform rotation from mean to true

    pos2(1) = xx * pos1(1) + yx * pos1(2) + zx * pos1(3);

    pos2(2) = xy * pos1(1) + yy * pos1(2) + zy * pos1(3);

    pos2(3) = xz * pos1(1) + yz * pos1(2) + zz * pos1(3);

end


% A Thin Plate Subjected to Uniform Traction
% T3 Implementation
% 2 elements
% clear memory
clear all;
clc;
close all;
% materials
E  = 30e6;     poisson = 0.30;  thickness = 1;

% matrix D
D=E/(1-poisson^2)*[1 poisson 0;poisson 1 0;0 0 (1-poisson)/2];

% trivial preprocessing
% numberElements: number of elements
numberElements=2;
% numberNodes: number of nodes
numberNodes=4;
% coordinates and connectivities
elementNodes=[1 3 2; 1 4 3];
nodeCoordinates=[0, 0; 0, 10; 20, 10; 20, 0];
drawingMesh(nodeCoordinates,elementNodes,'T3','k-o');

% GDof: global number of degrees of freedom
GDof=2*numberNodes;

% boundary conditions
prescribedDof=[1 2 3 4]';
% force vector
force=zeros(GDof,1);
force(5)=5000; force(7) =5000;

% calculation of the system stiffness matrix
stiffness=formStiffness2D(GDof,numberElements,...
    elementNodes,numberNodes,nodeCoordinates,D,thickness);

% solution
displacements=solution(GDof,prescribedDof,stiffness,force);

% output displacements
outputDisplacements(displacements, numberNodes, GDof);

outputStress(displacements,numberElements,...
    elementNodes,nodeCoordinates,D)


% RES = pointOp(IM, LUT, ORIGIN, INCREMENT, WARNINGS)
%
% Apply a point operation, specified by lookup table LUT, to image IM.
% LUT must be a row or column vector, and is assumed to contain
% (equi-spaced) samples of the function.  ORIGIN specifies the
% abscissa associated with the first sample, and INCREMENT specifies the
% spacing between samples.  Between-sample values are estimated via
% linear interpolation.  If WARNINGS is non-zero, the function prints
% a warning whenever the lookup table is extrapolated.
%
% This function is much faster than MatLab's interp1, and allows
% extrapolation beyond the lookup table domain.  The drawbacks are
% that the lookup table must be equi-spaced, and the interpolation is
% linear.

% Eero Simoncelli, 8/96.

function res = pointOp(im, lut, origin, increment, warnings)

%% NOTE: THIS CODE IS NOT ACTUALLY USED! (MEX FILE IS CALLED INSTEAD)

% fprintf(1,'WARNING: You should compile the MEX code for "pointOp", found in the MEX subdirectory.  It is MUCH faster.\n');

X = origin + increment*[0:size(lut(:),1)-1];
Y = lut(:);

res = reshape(interp1(X, Y, im(:), 'linear'),size(im));


function area = polySgnArea(x,y)
% area of an orientation with sign depending of orientation

area = 0.5*sum((y(2:end)-y(1:end-1)) .* (x(2:end)+x(1:end-1)));

end


function Vi = spm_get_vc(I,factor)
% Generate error covariance components for factorial designs
% FORMAT Vi = spm_get_vc(I,factor)
% I         - n x m matrix of factor level indicators
%             I(n,i) is the level of factor i for observation n
% factor(i) - structure array of sphericity assumptions for each factor
% .variance - 1 for different variance among levels of factor i
% .dept     - 1 for dependencies within levels of factor i
%
% Vi        - cell vector of covariance components
%__________________________________________________________________________
%
% spm_get_vc generates variance components for a given design. For each
% factor, the user specifies whether its levels have identical variances
% and are independent. The individual components for each factor are
% combined into covariance components by using the Kronecker tensor
% product. If there are unequal number of observations at different levels,
% the function specifies covariance components for a full factorial design
% first and subsequently removes unwanted rows and columns from the
% covariance matrices.
%
% The functionality of spm_get_vc is similar to that of spm_non_sphericity.
% The difference is that spm_get_vc can accommodate any number of factors
% and is more general, because it can cope with different number of
% observations under different levels of a factor.
%__________________________________________________________________________
% Copyright (C) 2006 Freiburg Brain Imaging
% Copyright (C) 2008-2013 Wellcome Trust Centre for Neuroimaging

% Volkmar Glauche
% $Id: spm_get_vc.m 5293 2013-03-01 16:41:46Z guillaume $

%-Numbers of scans and factors
%--------------------------------------------------------------------------
[nscan,nfactor] = size(I);

%-Make sure each row of Iin is unique
%==========================================================================
[Iu,Ii,Ij]  = unique(I,'rows');
if size(Iu,1) < nscan
    nfactor = nfactor + 1;
    uf      = zeros(nscan, 1);
    for k = 1:max(Ij)
        uf(Ij==k) = 1:sum(Ij==k);
    end
    I       = [I uf];
end
nlevel      = max(I);

%-Non-sphericity assumptions
%--------------------------------------------------------------------------
% First factor is replications, assume identical variance and independence.
% Pad with zeroes in case there are less than nfactor factors specified.
variance    = [0 cat(2, factor.variance) zeros(1,nfactor)];
dept        = [0 cat(2, factor.dept) zeros(1,nfactor)];

% (i) generate generic index
%==========================================================================
Igen = zeros(prod(nlevel), nfactor);
Igen(:,1) = kron(ones(1,prod(nlevel(2:end))),1:nlevel(1))';
for cf = 2:(nfactor-1)
    Igen(:,cf) = kron(ones(1,prod(nlevel((cf+1):end))),kron(1:nlevel(cf),ones(1,prod(nlevel(1:(cf-1))))))';
end
Igen(:,nfactor) = kron(1:nlevel(nfactor),ones(1,prod(nlevel(1:(nfactor-1)))))';

% (ii) generate error variance components
%==========================================================================
Vi = {};
for f=1:nfactor

    % identical/non-identical variances
    % for each factor, create a single variance component if variances are
    % identical across levels, and level specific variance components if
    % variances are non-identical
    %----------------------------------------------------------------------
    nVi = {};
    if ~variance(f)
        nVi{1} = speye(nlevel(f),nlevel(f));
    else
        for l1=1:nlevel(f)
            nVi{l1} = sparse(l1,l1,1,nlevel(f),nlevel(f));
        end
    end
    if dept(f)
        for l1 = 1:nlevel(f)
            for l2 = 1:(l1-1)
                nVi{end+1} = sparse([l1 l2],[l2 l1],1,nlevel(f), ...
                                         nlevel(f));
            end
        end
    end

    % combine current factor components with previous ones, thus building
    % up covariance components block by block
    %----------------------------------------------------------------------
    if isempty(Vi)
        Vi = nVi;
    else
        oVi = Vi;
        Vi = {};
        for nv = 1:numel(nVi)
            for ov = 1:numel(oVi)
                Vi{end+1} = kron(nVi{nv}, oVi{ov});
            end
        end
    end
end

% (iii) sort out rows/columns & remove all-zero variance components
%==========================================================================
[unused,ind] = ismember(I,Igen,'rows');
az = false(size(Vi));

for cVi = 1:numel(Vi)
    Vi{cVi} = Vi{cVi}(ind,ind);
    az(cVi) = full(all(Vi{cVi}(:) == 0));
end
Vi = Vi(~az);

dupl = false(size(Vi));
for cVi = 1:numel(Vi)
    if ~dupl(cVi)
        for cVi1 = (cVi+1):numel(Vi)
            dupl(cVi1) = dupl(cVi1)||full(all(Vi{cVi}(:) == Vi{cVi1}(:)));
        end
    end
end
Vi = Vi(~dupl);


function res = imGeodesicPropagation(img, varargin)
%IMGEODESICPROPAGATION Compute geodesic propagation for each foreground pixel
%
%   RES = imGeodesicPropagation(IMG);
%   IMG is a binary image. For each foreground pixel, the geodesic
%   progagation is defined as the maximum geodesic distance to another
%   pixel of the foreground. If the foreground is not connected this
%   distance equals infinity.
%
%   RES = imGeodesicPropagation(IMG, WEIGHTS);
%   use different weights for the computation of distances. See
%   imChamferDistance for further details.
%
%   Note:
%   * As the algorithm propagates geodesic distances from each foreground
%       pixel, the computation time may be expensive.
%   * the function is defined for both 2D and 3D images
%
%
%   Example
%     % Compute geodesic propagation in a L-shape
%     img = zeros(20, 20);
%     img(4:16, 4:9) = 1;
%     img(11:16, 4:16) = 1;
%     prop = imGeodesicPropagation(img);
%     imagesc(prop);
%     colormap([1 1 1 ; jet]);
%
%     % Compute geodesic propagation in a set of particles
%     prop = zeros(size(img));
%     lbl = bwlabel(img);
%     for i = 1:max(lbl(:))
%         prop = max(prop, imGeodesicPropagation(lbl==i));
%     end
%     imagesc(prop);
%     colormap([1 1 1 ; jet]);
%
%   See also
%   imGeodesics, imGeodesicDistanceMap, imGeodesicRadius, imGeodesicExtremities

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2009-05-22,    using Matlab 7.7.0.471 (R2008b)
% Copyright 2009 INRA - Cepia Software Platform.

img = img > 0;
res = zeros(size(img));

dim = size(img);
if length(dim) == 2
    for i = 1:dim(1)
        for j = 1:dim(2)
            if ~img(i,j)
                continue;
            end

            marker = false(size(img));
            marker(i, j) = true;

            dist = imGeodesicDistanceMap(img, marker, varargin{:});
            res(i, j) = max(dist(img));
        end
    end

elseif length(dim) == 3
    for k = 1:dim(3)
        for j = 1:dim(2)
            for i = 1:dim(1)
                if ~img(i,j,k)
                    continue;
                end

                marker = false(size(img));
                marker(i, j, k) = true;

                dist = imGeodesicDistanceMap3d(img, marker, varargin{:});
                res(i, j, k) = max(dist(img));
            end
        end
    end

else
    error('Requires a 2D or a 3D image');
end


% Gets the 3D coordinates of the corners of a 3D bounding box.
%
% Args:
%   bb3d - 3D bounding box struct.
%
% Returns:
%   corners - 8x3 matrix of 3D coordinates.
%
% See:
%   create_bounding_box_3d.m
%
% Author: Nathan Silberman (silberman@cs.nyu.edu)
function corners = get_corners_of_bb3d(bb3d)
  corners = zeros(8, 3);

  % Order the bases.
  [~, inds] = sort(abs(bb3d.basis(:,1)), 'descend');
  basis = bb3d.basis(inds, :);
  coeffs = bb3d.coeffs(inds);

  [~, inds] = sort(abs(basis(2:3,2)), 'descend');
  if inds(1) == 2
    basis(2:3,:) = flipdim(basis(2:3,:), 1);
    coeffs(2:3) = flipdim(coeffs(2:3), 2);
  end

  % Now, we know the basis vectors are orders X, Y, Z. Next, flip the basis
  % vectors towards the viewer.
  basis = flip_towards_viewer(basis, repmat(bb3d.centroid, [3 1]));

  coeffs = abs(coeffs);

  corners(1,:) = -basis(1,:) * coeffs(1) + basis(2,:) * coeffs(2) + basis(3,:) * coeffs(3);
  corners(2,:) = basis(1,:) * coeffs(1) + basis(2,:) * coeffs(2) + basis(3,:) * coeffs(3);
  corners(3,:) = basis(1,:) * coeffs(1) + -basis(2,:) * coeffs(2) + basis(3,:) * coeffs(3);
  corners(4,:) = -basis(1,:) * coeffs(1) + -basis(2,:) * coeffs(2) + basis(3,:) * coeffs(3);

  corners(5,:) = -basis(1,:) * coeffs(1) + basis(2,:) * coeffs(2) + -basis(3,:) * coeffs(3);
  corners(6,:) = basis(1,:) * coeffs(1) + basis(2,:) * coeffs(2) + -basis(3,:) * coeffs(3);
  corners(7,:) = basis(1,:) * coeffs(1) + -basis(2,:) * coeffs(2) + -basis(3,:) * coeffs(3);
  corners(8,:) = -basis(1,:) * coeffs(1) + -basis(2,:) * coeffs(2) + -basis(3,:) * coeffs(3);

  corners = corners + repmat(bb3d.centroid, [8 1]);
end

function normals = flip_towards_viewer(normals, points)
  points = points ./ repmat(sqrt(sum(points.^2, 2)), [1, 3]);

  proj = sum(points .* normals, 2);

  flip = proj > 0;
  normals(flip, :) = -normals(flip, :);
end


function [centers, mincenter, mindist, lower, computed] = anchors(firstcenter,k,data)
% choose k centers by the furthest-first method
% URL: http://cseweb.ucsd.edu/~elkan/fastkmeans.html

[n,dim] = size(data);
centers = zeros(k,dim);
lower = zeros(n,k);
mindist = Inf*ones(n,1);
mincenter = ones(n,1);
computed = 0;
centdist = zeros(k,k);

for j = 1:k
    if j == 1
        newcenter = firstcenter;
    else
        [maxradius,i] = max(mindist);
        newcenter = data(i,:);
    end

    centers(j,:) = newcenter;
    centdist(1:j-1,j) = calcdist(centers(1:j-1,:),newcenter);
    centdist(j,1:j-1) = centdist(1:j-1,j)';
    computed = computed + j-1;

    inplay = find(mindist > centdist(mincenter,j)/2);
    newdist = calcdist(data(inplay,:),newcenter);
    computed = computed + size(inplay,1);
    lower(inplay,j) = newdist;

    move = find(newdist < mindist(inplay));
    shift = inplay(move);
    mincenter(shift) = j;
    mindist(shift) = newdist(move);
end


function [X] = DCA(networks, dim, rsp, maxiter)
	Q = [];
	for i = 1 : length(networks)
		fileID = char(strcat('../network/', networks(i), '.txt'));
		net = load(fileID);
		tQ = diffusionRWR(net, maxiter, rsp);
		Q = [Q, tQ];
	end

	nnode = size(Q, 1);
	alpha = 1 / nnode;
	Q = log(Q + alpha) - log(alpha);

	Q = Q * Q';
	[U, S] = svds(Q, dim);
	X = U * sqrt(sqrt(S));
end


function nonlocalarr = blockmatch(patchmat,neighborindarr,neighbornumarr,selfindarr,para)
%BLOCKMATCH Block matching of patches with non-local similarity.
%   nonlocalarr=BLOCKMATCH(patchmat,neighborindarr,neighbornumarr,
%   selfindarr,para) returns the indexes of the patches with non-local
%   similarity.
%   See slao WNNM_IMDENOISE.
gridsize = length(neighbornumarr);

nonlocalarr = zeros([para.patchnum,gridsize],'uint32');
for i = 1:gridsize
    patch = patchmat(:,selfindarr(i)); % key patch
    neighbors = patchmat(:,neighborindarr(1:neighbornumarr(i),i)); % all neighbors
    distance = sum(bsxfun(@minus,neighbors,patch).^2); % ell_2 distance
    [~,ascendind] = sort(distance); % sort distance in an ascending order
    % indexes of the most similar (shortest distance) para.patchnum of neighbors
    nonlocalarr(:,i) = neighborindarr(ascendind(1:para.patchnum),i);
end

end


% Description:
%
%     Create a design matrix for group-covariate interactions.
%
% Syntax:
%
%     [ X, terms ] = mGC2X(groups, covariates, a, b)
%
% Inputs:
%
%     groups     - [ N x G ] (int)    - columns of qualitative variables
%     covariates - [ N x C ] (double) - columns of quantitative variables
%     a          - [ 1 x 1 ] (int)    - group offset index for terms
%     b          - [ 1 x 1 ] (int)    - covariate offset index for terms
%     options    - [ 1 x P ] (cell)   - see Options
%     columns    - [ T x 2 ] (int)    - see Details
%
% Outputs:
%
%     X     - [ N x M ] (double)
%     terms - [ 1 x M ] (cell)
%
% Details:
%
%     The first column of the columns input in this case is an index into the
%     columns of groups.  The second column of the columns input is an index
%     into the columns of covariates.  For example, [ 2 1 ] would refer to an
%     interaction between column 2 of groups and column 1 of covariates.
%
% Options:
%
%     'over-determined'  - use over-determined coding for the design matrix
%     'sigma-restricted' - use sigma-restricted coding for the design matrix
%     'verbose'          - display extra information to the command window
%
% Examples:
%
% Notes:
%
% Author(s):
%
%     William Gruner (williamgruner@gmail.com)
%
% References:
%
% Acknowledgements:
%
%     Many thanks to Dr. Erik Erhardt and Dr. Elena Allen of the Mind Research
%     Network (www.mrn.org) for their continued collaboration.
%
% Version:
%
%     $Author: williamgruner $
%     $Date: 2010-04-12 07:14:07 -0600 (Mon, 12 Apr 2010) $
%     $Revision: 494 $

function [ X, terms ] = mGC2X(groups, covariates, a, b, options, columns)

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

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

    if ~exist('options', 'var')
        options = {};
    end

    if ~exist('columns', 'var')

        columns = [];

        for i = 1 : size(groups, 2)
            for j = 1 : size(covariates, 2)
                columns(end + 1, :) = [ i j ];
            end
        end

    end

    terms = {};
    X     = [];

    [ G, g ] = mG2X(groups, 0, intersect(options, ...
        { 'over-determined' 'sigma-restricted' }));

    if ~isempty(strmatch('verbose', options, 'exact'))
        fprintf('\n')
    end

    for i = 1 : size(columns, 1)

        if ~isempty(strmatch('verbose', options, 'exact'))
            fprintf('Factor %d%d represents the interaction between column %d of groups and column %d of covariates.\n', ...
                columns(i, 1) + a, columns(i, 2) + b, columns(i, 1), columns(i, 2))
        end

        I = mFindTerms(columns(i, 1), g);
        x = G(:, I) .* repmat(covariates(:, columns(i, 2)), 1, length(I));
        X = cat(2, X, x);

        for j = 1 : length(I)
            terms{end + 1} = columns(i, :) + [ a b ];
        end

    end


function [cepstra,aspectrum,pspectrum] = melfcc(samples, sr, varargin)
%[cepstra,aspectrum,pspectrum] = melfcc(samples, sr[, opts ...])
%  Calculate Mel-frequency cepstral coefficients by:
%   - take the absolute value of the STFT
%   - warp to a Mel frequency scale
%   - take the DCT of the log-Mel-spectrum
%   - return the first <ncep> components
%  This version allows a lot of options to be controlled, as optional
%  'name', value pairs from the 3rd argument on: (defaults in parens)
%    'wintime' (0.025): window length in sec
%    'hoptime' (0.010): step between successive windows in sec
%    'numcep'     (13): number of cepstra to return
%    'lifterexp' (0.6): exponent for liftering; 0 = none; < 0 = HTK sin lifter
%    'sumpower'    (1): 1 = sum abs(fft)^2; 0 = sum abs(fft)
%    'preemph'  (0.97): apply pre-emphasis filter [1 -preemph] (0 = none)
%    'dither'      (0): 1 = add offset to spectrum as if dither noise
%    'minfreq'     (0): lowest band edge of mel filters (Hz)
%    'maxfreq'  (4000): highest band edge of mel filters (Hz)
%    'nbands'     (40): number of warped spectral bands to use
%    'bwidth'    (1.0): width of aud spec filters relative to default
%    'dcttype'     (2): type of DCT used - 1 or 2 (or 3 for HTK or 4 for feac)
%    'fbtype'  ('mel'): frequency warp: 'mel','bark','htkmel','fcmel'
%    'usecmp'      (0): apply equal-loudness weighting and cube-root compr.
%    'modelorder'  (0): if > 0, fit a PLP model of this order
%    'broaden'     (0): flag to retain the (useless?) first and last bands
%    'useenergy'   (0): overwrite C0 with true log energy
% The following non-default values nearly duplicate Malcolm Slaney's mfcc
% (i.e. melfcc(d,16000,opts...) =~= log(10)*2*mfcc(d*(2^17),16000) )
%       'wintime': 0.016
%     'lifterexp': 0
%       'minfreq': 133.33
%       'maxfreq': 6855.6
%      'sumpower': 0
% The following non-default values nearly duplicate HTK's MFCC
% (i.e. melfcc(d,16000,opts...) =~= 2*htkmelfcc(:,[13,[1:12]])'
%  where HTK config has PREEMCOEF = 0.97, NUMCHANS = 20, CEPLIFTER = 22,
%  NUMCEPS = 12, WINDOWSIZE = 250000.0, USEHAMMING = T, TARGETKIND = MFCC_0)
%     'lifterexp': -22
%        'nbands': 20
%       'maxfreq': 8000
%      'sumpower': 0
%        'fbtype': 'htkmel'
%       'dcttype': 3
% For more detail on reproducing other programs' outputs, see
% http://www.ee.columbia.edu/~dpwe/resources/matlab/rastamat/mfccs.html
%
% 2005-04-19 dpwe@ee.columbia.edu after rastaplp.m.
% Uses Mark Paskin's process_options.m from KPMtools
% $Header: /Users/dpwe/matlab/rastamat/RCS/melfcc.m,v 1.3 2012/09/03 14:01:26 dpwe Exp dpwe $

if nargin < 2;   sr = 16000;    end

% Parse out the optional arguments
[wintime, hoptime, numcep, lifterexp, sumpower, preemph, dither, ...
 minfreq, maxfreq, nbands, bwidth, dcttype, fbtype, usecmp, modelorder, ...
 broaden, useenergy] = ...
    process_options(varargin, 'wintime', 0.025, 'hoptime', 0.010, ...
          'numcep', 13, 'lifterexp', 0.6, 'sumpower', 1, 'preemph', 0.97, ...
	  'dither', 0, 'minfreq', 0, 'maxfreq', 4000, ...
	  'nbands', 40, 'bwidth', 1.0, 'dcttype', 2, ...
	  'fbtype', 'mel', 'usecmp', 0, 'modelorder', 0, ...
          'broaden', 0, 'useenergy', 0);

if preemph ~= 0
  samples = filter([1 -preemph], 1, samples);
end

% Compute FFT power spectrum
[pspectrum,logE] = powspec(samples, sr, wintime, hoptime, dither);

aspectrum = audspec(pspectrum, sr, nbands, fbtype, minfreq, maxfreq, sumpower, bwidth);

if (usecmp)
  % PLP-like weighting/compression
  aspectrum = postaud(aspectrum, maxfreq, fbtype, broaden);
end

if modelorder > 0

  if (dcttype ~= 1)
    disp(['warning: plp cepstra are implicitly dcttype 1 (not ', num2str(dcttype), ')']);
  end

  % LPC analysis
  lpcas = dolpc(aspectrum, modelorder);

  % convert lpc to cepstra
  cepstra = lpc2cep(lpcas, numcep);

  % Return the auditory spectrum corresponding to the cepstra?
%  aspectrum = lpc2spec(lpcas, nbands);
  % else return the aspectrum that the cepstra are based on, prior to PLP

else

  % Convert to cepstra via DCT
  cepstra = spec2cep(aspectrum, numcep, dcttype);

end

cepstra = lifter(cepstra, lifterexp);

if useenergy
  cepstra(1,:) = logE;
end


function [ m, p, t ] = naca4_mpt ( code )

%*****************************************************************************80
%
%% NACA4_MPT returns the parameters stored in a NACA 4 digit airfoil code.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 May 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer CODE, the NACA4 code.
%    0 <= CODE <= 9999.
%
%    Output, real M, the maximum camber, as a percentage of the chord length.
%    0 <= M <= 1.0.
%
%    Output, real P, the relative distance of the occurrence of the maximum
%    camber from the beginning of the chord.
%    0 <= P <= 1.0.
%
%    Output, real T, the maximum thickness relative to the chord length.
%    0 <= T <= 1.0.
%
  if ( code < 0 || 9999 < code )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'NACA4_MPT - Fatal error!\n' );
    fprintf ( 1, '  CODE should be an integer between 0 and 9999.\n' );
    error ( 'NACA4_MPT - Fatal error!' );
  end

  m = floor ( code / 1000 );
  code = code - m * 1000;
  m = m / 100.0;

  p = floor ( code / 100 );
  code = code - p * 100;
  p = p / 10.0;

  t = code / 100.0;

  return
end


function tests = test_VBA_random
% Unit Tests for VBA_random

tests = functiontests (localfunctions);

% -------------------------------------------------------------------------
function test_arbitrary (testCase)
    K = 3;
    p = rand (K, 1);
    p = p / sum (p);
    vals_1 = 'abc';
    vals_k = ['ab'; 'cd'; 'ef'];
    M = size(vals_k, 2);

    % should fail on invalid parameters
    shouldFail = @() VBA_random ('Arbitrary', [0.5; 0.5], vals_1);
    testCase.verifyError(shouldFail, 'VBA:invalidInput');
    shouldFail = @() VBA_random ('Arbitrary', zeros (3, 1), vals_1);
    testCase.verifyError(shouldFail, 'VBA:invalidInput');
    shouldFail = @() VBA_random ('Arbitrary', p, vals_k, 8, 8);
    testCase.verifyError(shouldFail, 'VBA:invalidInput');

    % should return one sample by default
    actual = VBA_random ('Arbitrary', p, vals_1);
    testCase.verifyNumElements (actual, 1);
    actual = VBA_random ('Arbitrary', p, vals_k);
    testCase.verifySize (actual, [M, 1]);

    % should return matrix on scalar N
    N = 8;
    actual = VBA_random ('Arbitrary', p, vals_1, N);
    testCase.verifySize (actual, [N, N]);
    actual = VBA_random ('Arbitrary', p, vals_k, N);
    testCase.verifySize (actual, [M, N]);

    % should return matrix of asked dimension
    N = num2cell (1 + randi (5, 1, 4));
    actual = VBA_random ('Arbitrary', p, vals_1, N{:});
    testCase.verifySize (actual, [N{:}]);

    % should return sample according to distribution
    % + univariate
    actual = VBA_random ('Arbitrary', p, vals_1, 1, 1e6);
    % - support
    testCase.verifyEqual (unique (actual), vals_1);
    % + density
    for i = 1 : numel (p)
        testCase.verifyEqual (mean (actual == vals_1(i)), p(i), 'AbsTol', 1e-2);
    end
    % + multivariate
    actual = VBA_random ('Arbitrary', p, vals_k, 1e6);
    % - support
    testCase.verifyEqual (unique (actual', 'rows'), vals_k);
    % + density
    for i = 1 : numel (p)
        isEq = all(bsxfun(@eq, actual, vals_k(i,:)'));
        testCase.verifyEqual (mean (isEq, 2), p(i), 'AbsTol', 1e-2);
    end

% -------------------------------------------------------------------------
function test_bernoulli (testCase)
    p = rand ();

    % should fail on invalid probability
    shouldFail = @() VBA_random ('Bernoulli', -3);
    testCase.verifyError(shouldFail, 'VBA:invalidInput');
    % should fail on invalid size
    shouldFail = @() VBA_random ('Bernoulli', rand (3), 2);
    testCase.verifyError(shouldFail, 'VBA:invalidInput');

    % should return one sample by default
    actual = VBA_random ('Bernoulli', p);
    testCase.verifyNumElements (actual, 1);

    % should return matrix on scalar N
    N = 3;
    actual = VBA_random ('Bernoulli', p, N);
    testCase.verifySize (actual, [N, N]);

    % should return matrix of asked dimension
    N = num2cell (1 + randi (5, 1, 4));
    actual = VBA_random ('Bernoulli', p,  N{:});
    testCase.verifySize (actual, [N{:}]);

    % should return matrix on matrix p
    actual = VBA_random ('Bernoulli', rand (N{:}));
    testCase.verifySize (actual, [N{:}]);

    % should return sample according to distribution
    actual = VBA_random ('Bernoulli', p, 1, 1e6);
    % + support
    testCase.verifyEqual (unique (actual), [0 1]);
    % + mean
    testCase.verifyEqual (mean (actual), p, 'AbsTol', 1e-2);

    % should deal with nan
    actual = VBA_random ('Bernoulli', [p NaN]);
    testCase.verifyTrue (~ isnan (actual(1)));
    testCase.verifyTrue (isnan (actual(2)));

% -------------------------------------------------------------------------
function test_binomial (testCase)
    n = 2;
    p = rand ();

    % should fail on invalid parameters
    shouldFail = @() VBA_random ('Binomial',0, p);
    testCase.verifyError(shouldFail, 'VBA:invalidInput');
    shouldFail = @() VBA_random ('Binomial',n, -3);
    testCase.verifyError(shouldFail, 'VBA:invalidInput');

    % should return one sample by default
    actual = VBA_random ('Binomial', n, p);
    testCase.verifyNumElements (actual, 1);

    % should return matrix on scalar N
    N = 3;
    actual = VBA_random ('Binomial', n, p, N);
    testCase.verifySize (actual, [N, N]);

    % should return matrix of asked dimension
    N = num2cell (1 + randi (5, 1, 4));
    actual = VBA_random ('Binomial', n, p,  N{:});
    testCase.verifySize (actual, [N{:}]);

    % should return sample according to distribution
    actual = VBA_random ('Binomial', n, p, 1, 1e6);
    % + support
    testCase.verifyEqual (unique (actual), 0 : n);

    % + mean
    testCase.verifyEqual (mean (actual), n * p, 'AbsTol', 1e-2);
    % + variance
    testCase.verifyEqual (var (actual), n * p * (1 - p), 'AbsTol', 1e-2);
    % + density
    for k = 0 : n
        expected = nchoosek (n, k) * (p ^ k) * ((1 - p) ^ (n - k));
        testCase.verifyEqual (mean (actual == k), expected, 'AbsTol', 1e-2);
    end

% -------------------------------------------------------------------------
function test_categorical (testCase)
    K = 3;
    p = rand (K, 1);
    p = p / sum(p);

    % should fail on invalid parameters
    shouldFail = @() VBA_random ('Categorical', zeros (K, 1));
    testCase.verifyError(shouldFail, 'VBA:invalidInput');

    % should return one sample by default
    actual = VBA_random ('Categorical', p);
    testCase.verifyNumElements (actual, 1);

    % should return matrix on scalar N
    N = 3;
    actual = VBA_random ('Categorical', p, N);
    testCase.verifySize (actual, [N, N]);

    % should return matrix of asked dimension
    N = num2cell (1 + randi (5, 1, 4));
    actual = VBA_random ('Categorical', p,  N{:});
    testCase.verifySize (actual, [N{:}]);

    % should return sample according to distribution
    actual = VBA_random ('Categorical', p, 1, 1e6);
    % + support
    testCase.verifyEqual (unique (actual), 1 : K);
    % + mean
    testCase.verifyEqual (mean (actual), (1 : K) * p, 'AbsTol', 1e-2);
   % + density
    for k = 1 : K
        testCase.verifyEqual (mean (actual == k), p(k), 'AbsTol', 1e-2);
    end

    % -------------------------------------------------------------------------
function test_dirichlet (testCase)
    K = 3;
    alpha = 1 + randi (10, K, 1);

    % should fail on invalid parameters
    shouldFail = @() VBA_random ('Dirichlet', rand);
    testCase.verifyError(shouldFail, 'VBA:invalidInput');
    shouldFail = @() VBA_random ('Dirichlet', alpha, 2, 3);
    testCase.verifyError(shouldFail, 'VBA:invalidInput');

    % should return one sample by default
    actual = VBA_random ('Dirichlet', alpha);
    testCase.verifySize (actual, [K, 1]);

    % should return N samples
    N = 8;
    actual = VBA_random ('Dirichlet', alpha, N);
    testCase.verifySize (actual, [K, N]);

    % should return sample according to distribution
    N = 1e6;
    actual = VBA_random ('Dirichlet', alpha, N);
    % + support
    testCase.verifyTrue (VBA_isInRange (actual, [0 1]));
    testCase.verifyEqual (sum (actual), ones (1, N), 'AbsTol', 1e-13);
    % + moments
    [expected.m, expected.v] = VBA_dirichlet_moments(alpha);
    testCase.verifyEqual (mean (actual, 2), expected.m , 'AbsTol', 1e-2);
    testCase.verifyEqual (var (actual, [], 2), expected.v , 'AbsTol', 1e-2);

% -------------------------------------------------------------------------
function test_gamma (testCase)
    a = 10 * rand ();
    b = 10 * rand ();

    % should fail on invalid parameters
    shouldFail = @() VBA_random ('Gamma', - 1, b);
    testCase.verifyError(shouldFail, 'VBA:invalidInput');
    shouldFail = @() VBA_random ('Gamma', a, - 1);
    testCase.verifyError(shouldFail, 'VBA:invalidInput');

    % should return one sample by default
    actual = VBA_random ('Gamma', a, b);
    testCase.verifyNumElements (actual, 1);

    % should return matrix on scalar N
    N = 3;
    actual = VBA_random ('Gamma', a, b, N);
    testCase.verifySize (actual, [N, N]);

    % should return matrix of asked dimension
    N = num2cell (1 + randi (5, 1, 4));
    actual = VBA_random ('Gamma', a, b,  N{:});
    testCase.verifySize (actual, [N{:}]);

    % should return sample according to distribution
    actual = VBA_random ('Gamma', a, b, 1, 1e6);
    % + support
    testCase.verifyTrue (all (actual > 0));
    % + mean
    testCase.verifyEqual (mean (actual), a * b, 'RelTol', 1e-2);
    % + variance
    testCase.verifyEqual (var (actual), a * b ^ 2, 'RelTol', 1e-2);

  % -------------------------------------------------------------------------
function test_gaussian (testCase)

    % + univariate
    mu_1 = randn ();
    Sigma_1 = rand () ^ 2;
    % + multivariate
    k = 2;
    mu_k = randn (k, 1);
    Sigma_k = rand (k);
    Sigma_k = Sigma_k * Sigma_k';

     % should fail on invalid parameters
    shouldFail = @() VBA_random ('Gaussian', mu_1, Sigma_k);
    testCase.verifyError(shouldFail, 'VBA:invalidInput');
    shouldFail = @() VBA_random ('Gaussian', mu_k, Sigma_k, 2, 2);
    testCase.verifyError(shouldFail, 'VBA:invalidInput');

    % should return one sample by default
    % + univariate
    actual = VBA_random ('Gaussian', mu_1, Sigma_1);
    testCase.verifyNumElements (actual, 1);
    % + multivariate
    actual = VBA_random ('Gaussian', mu_k, Sigma_k);
    testCase.verifySize (actual, [k, 1]);

    % should return matrix on scalar N
    N = 5;
    % + univariate
    actual = VBA_random ('Gaussian', mu_1, Sigma_1, N);
    testCase.verifySize (actual, [N, N]);
    % + multivariate
    actual = VBA_random ('Gaussian', mu_k, Sigma_k, N);
    testCase.verifySize (actual, [k, N]);

    % should return matrix of asked dimension
    N = num2cell (1 + randi (5, 1, 4));
    actual = VBA_random ('Gaussian', mu_1, Sigma_1,  N{:});
    testCase.verifySize (actual, [N{:}]);

    % should return sample according to distribution
    % + univariate
    actual = VBA_random ('Gaussian', mu_1, Sigma_1, 1, 1e6);
    % - mean
    testCase.verifyEqual (mean (actual), mu_1, 'AbsTol', 1e-2);
    % - variance
    testCase.verifyEqual (var (actual), Sigma_1, 'AbsTol', 1e-2);
    % + multivariate
    actual = VBA_random ('Gaussian', mu_k, Sigma_k, 1e6);
    % - mean
    testCase.verifyEqual (mean (actual, 2), mu_k, 'AbsTol', 1e-2);
    % - variance
    testCase.verifyEqual (cov (actual'), Sigma_k, 'AbsTol', 1e-2);

% -------------------------------------------------------------------------
function test_multinomial (testCase)
    n = 2;
    K = 3;
    p = rand (K, 1);
    p = p / sum(p);

    % should fail on invalid parameters
    shouldFail = @() VBA_random ('Multinomial',0, p);
    testCase.verifyError(shouldFail, 'VBA:invalidInput');
    shouldFail = @() VBA_random ('Multinomial',n, .5);
    testCase.verifyError(shouldFail, 'VBA:invalidInput');
    shouldFail = @() VBA_random ('Multinomial',n, [1; 1]);
    testCase.verifyError(shouldFail, 'VBA:invalidInput');
    shouldFail = @() VBA_random ('Multinomial',n, p, 8, 8);
    testCase.verifyError(shouldFail, 'VBA:invalidInput');

    % should return one sample by default
    actual = VBA_random ('Multinomial', n, p);
    testCase.verifySize (actual, [K, 1]);

    % should return matrix on scalar N
    N = 8;
    actual = VBA_random ('Multinomial', n, p, N);
    testCase.verifySize (actual, [K, N]);

    % should return sample according to distribution
    N = 1e6;
    actual = VBA_random ('Multinomial', n, p, N);
    % + support
    testCase.verifyEqual (unique (actual)', 0 : n);
    testCase.verifyEqual (sum (actual), n * ones (1, N), 'AbsTol', 1e-13);
    % + density
    for i = 1 : K
        expected = n * p(i);
        testCase.verifyEqual (mean (actual(i, :)), expected, 'AbsTol', 1e-2);
        expected = n * p(i) * (1 - p(i));
        testCase.verifyEqual (var (actual(i, :)), expected, 'AbsTol', 1e-2);
    end

    % should deal with nan
    actual = VBA_random ('Multinomial', n, nan(K, 1));
    testCase.verifySize (actual, [K, 1]);
    testCase.verifyTrue (all (isnan (actual)));


function eps = eps_RCUs( i_s, n, r )
eps = mean( exp( - max(0, i_s - log(2^(n*r)-1) ) ) ); % avg pr err
% eps = mean( exp( - max(0, i_s - log(2^(n*r)-1)-log(2)) ) ); % max pr err
end


%%
% \documentclass[12pt]{article}
%
% \title{ODEbox: A Toolbox for Ordinary Differential Equations\\
% Sturm Liouville Example 2\\
% The Mathieu Differential Equation}
%
% \author{Matthew Harker and Paul O'Leary\\
% Institute for Automation\\
% University of Leoben\\
% A-8700 Leoben,
% Austria\\
% URL: automation.unileoben.ac.at\\
% \\
% Original: January 9, 2013\\
% $\copyright$ 2013\\
% \\
% Last Modified: \today}
%%
% \section{The Mathieu Differential Equation}
%
% This example addresses the solution of the Mathieu differential equation.
% This equation occurs in conjunction with the modelling of the vibration
% of a ellptical membrane. The problem has no known analytical solution.
% %
% \begin{equation}
%    -\ddot{y} + 2 \,r \, \cos( 2 \, x )= \lambda \, y
%    \hspace{5mm}
%    \text{with}
%    \hspace{5mm}
%    y(0) = 0}
%   \hspace{5mm}
%   \text{and}
%   \hspace{5mm}
%   \dot{y}(\pi) = 0
% \end{equation}
% %
% in the closed interval $0 \leq x \leq \pi$.
%
%%
% \section{tidy up the Workspace}
%
clear all;
close all;
setUpGraphics;
%%
% \section{Define the Nodes and Compute the Basis Functions}
%
% Define the number of nodes and the number of basis functions used.
%
noPts = 1000;
noBfs = round( noPts/2);
%%
% Compute the Chebyshev points, but scaled to form a closed interval
% $0 \leq x \leq \pi$.
%
x = dopNodes( noPts, 'chebyends' );
x = pi * (x + 1)/2;
%%
% Synthesize a complete set of basis functions
%
B = dop(x);
%%
% \subsection{Generate the Differentiating Matrix}
%
% Synthesize the differentiating matrix with support length $l_s$
%
ls = 13;
D = dopDiffLocal( x, ls, ls );
%%
% \subsection{Define the Constraints and Compute the Admissible Functions}
%
% Define the constraints
%
C = zeros( noPts, 2 );
C(1,1) = 1;
C(end,end) = 1;
%%
% Compute the set of constrained basis functions, i.e., admissible
% functions.
%
Bc = dopConstrain( C, B );
%%
% Trucate the basis functions
%
Bc = Bc(:,1:noBfs);
%
%%
% \section{Setup the Sturm-Liouville Matrix Linear Differential Operator}
%
% Setup the Linear differential operator for the Mathieu differential
% equation.
%
c1 = 0;
c2 = -50;
gx = diag(c1 + c2 * cos(2*x)) ;
L = Bc' * (D * D - gx )* Bc;
%
%%
% \section{Solve the Eigenvector Problem}
%
% Solve the eigenvalue problem
%
[Vec, Val] = eig( L );
vals = -diag( Val );
%%
% and sort the solutions.
%
[vals, inds] = sort(vals);
Vec = Vec(:,inds);
%%
% and compute the final eigenfunctions.
%
sols = Bc * Vec;
%%
% \section{Close Pair of Eigenvalues}
%
% The Mathieu differential equation as paramatized here is known to produce
% a close pair of eigenvalues. We now show this pair.
%
noEVals = 2;
for k=1:noEVals
    numStr = num2str(vals(k),'%2.10E') ;
    str = ['\lambda_{', int2str(k-1), '} = ', numStr]
end;
%noV = length(vals);
%n = [1:noV]';
%valsT = n.^2;
%
%%
% \section{Plot a Few Eigenfunctions}
%
setUpGraphics(10)
FigureSize=[1 1 10 6];
set(0,'DefaultFigureUnits','centimeters');
set(0,'DefaultFigurePosition',FigureSize);
set(0,'DefaultFigurePaperUnits','centimeters');
set(0,'DefaultFigurePaperPosition',FigureSize);
MyAxesPosition=[0.18 0.17 0.8 0.8];
set(0,'DefaultaxesPosition',MyAxesPosition);
%
fig1 = figure;
plot( x, sols(:,1), 'r');
hold on
plot( x, sols(:,2), 'g');
plot( x, sols(:,3), 'b');
plot( x, sols(:,4), 'k');
grid on;
xlabel('$$x$$');
ylabel('$$y8x)$$');
axis([0,pi,-0.1,0.1]);
%
% \caption{The first $4$ eigen functions of the Mathieu differential equation.}
%%
%
setUpGraphics(10)
FigureSize=[1 1 10 10];
set(0,'DefaultFigureUnits','centimeters');
set(0,'DefaultFigurePosition',FigureSize);
set(0,'DefaultFigurePaperUnits','centimeters');
set(0,'DefaultFigurePaperPosition',FigureSize);
MyAxesPosition=[0.16 0.17 0.8 0.8];
set(0,'DefaultaxesPosition',MyAxesPosition);
%
fig2 = figure;
P1 = [0.16 0.4 0.8 0.55] ;
A = axes('position',P1);
imagesc( log10(abs(Vec) ));
colorbar;
ylabel(['Basis function number $$m$$, $$n_b = ',int2str(noBfs),'$$']);
P2 = [0.16 0.1 0.615 0.25] ;
A = axes('position',P2);
plot( vals(1:noBfs), 'k');
range = axis;
axis([0,noBfs,-1000,900000]);
grid on;
xlabel('Eigenvector number $$n$$');
ylabel('$$\lambda$$');
%
% \caption{The Rayleigh-Ritz Spectrum of the Eigenfunvtions with respect to
% the admissible functions and the eigenvalues for the Mathieu differential
% equation.}
%%
% \section{Save the Figure to Disk.}
fileType = 'eps';
printFigure( fig1, ['SLEx2EigFnsNb',int2str(noBfs)], fileType);
printFigure( fig2, ['SLEx2SpectrumNb',int2str(noBfs)], fileType);


function [ n_data, mu, sigma, x, fx ] = cauchy_cdf_values ( n_data )

%*****************************************************************************80
%
%% CAUCHY_CDF_VALUES returns some values of the Cauchy CDF.
%
%  Discussion:
%
%    In Mathematica, the function can be evaluated by:
%
%      Needs["Statistics`ContinuousDistributions`"]
%      dist = CauchyDistribution [ mu, sigma ]
%      CDF [ dist, x ]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real MU, the mean of the distribution.
%
%    Output, real SIGMA, the standard deviation of the distribution.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 12;

  fx_vec = [ ...
     0.5000000000000000E+00, ...
     0.8524163823495667E+00, ...
     0.9220208696226307E+00, ...
     0.9474315432887466E+00, ...
     0.6475836176504333E+00, ...
     0.6024163823495667E+00, ...
     0.5779791303773693E+00, ...
     0.5628329581890012E+00, ...
     0.6475836176504333E+00, ...
     0.5000000000000000E+00, ...
     0.3524163823495667E+00, ...
     0.2500000000000000E+00 ];

  mu_vec = [ ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.4000000000000000E+01, ...
     0.5000000000000000E+01 ];

  sigma_vec = [ ...
     0.5000000000000000E+00, ...
     0.5000000000000000E+00, ...
     0.5000000000000000E+00, ...
     0.5000000000000000E+00, ...
     0.2000000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.4000000000000000E+01, ...
     0.5000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.2000000000000000E+01 ];

  x_vec = [ ...
     0.1000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.4000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.3000000000000000E+01 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    mu = 0.0;
    sigma = 0.0;
    x = 0.0;
    fx = 0.0;
  else
    mu = mu_vec(n_data);
    sigma = sigma_vec(n_data);
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function [S2G, W, M2] = quadratureS2Grid(bandwidth, varargin)
%
% Syntax
%   [S2G, W, M2] = quadratureS2Grid(M) quadrature grid of type chebyshev
%   [S2G, W, M2] = quadratureS2Grid(M, 'gauss') quadrature grid of type gauss
%

persistent S2G_p;
persistent W_p;
persistent M2_p;

if check_option(varargin, 'gauss')
  load(fullfile(mtexDataPath,'vector3d','quadratureS2Grid_gauss.mat'),'gridIndex');
else
  load(fullfile(mtexDataPath,'vector3d','quadratureS2Grid_chebyshev.mat'),'gridIndex');
end
index = find(gridIndex.bandwidth >= bandwidth, 1);
if isempty(index)
  index = size(gridIndex,1);
  warning('M is too large, instead we are giving you the largest quadrature grid we got.');
end

M2 = gridIndex.bandwidth(index);

if ~isempty(M2_p) && M2_p == M2
  S2G = S2G_p;
  W = W_p;
  M2 = M2_p;

else
  name = cell2mat(gridIndex.name(index));
  if check_option(varargin, 'gauss')
    data = load(fullfile(mtexDataPath,'vector3d','quadratureS2Grid_gauss.mat'),name);
  else
    data = load(fullfile(mtexDataPath,'vector3d','quadratureS2Grid_chebyshev.mat'),name);
  end

  data = data.(name);
  S2G = vector3d.byPolar(data(:, 1), data(:, 2));

  if check_option(varargin, 'gauss')
    W = data(:,3);
  else
    W = 4*pi/size(data, 1) .* ones(size(S2G));
  end

  % store the data
  S2G_p = S2G;
  W_p = W;
  M2_p = M2;
end

end


function [omckk,Tckk,Rckk] = compute_extrinsic_init_fisheye(x_kk,X_kk,fc,cc,kc,alpha_c)

%compute_extrinsic
%
%[omckk,Tckk,Rckk] = compute_extrinsic_init_fisheye(x_kk,X_kk,fc,cc,kc,alpha_c)
%
%Computes the extrinsic parameters attached to a 3D structure X_kk given its projection
%on the image plane x_kk and the intrinsic camera parameters fc, cc and kc.
%Works with planar and non-planar structures.
%
%INPUT: x_kk: Feature locations on the images
%       X_kk: Corresponding grid coordinates
%       fc: Camera focal length
%       cc: Principal point coordinates
%       kc: Distortion coefficients
%       alpha_c: Skew coefficient
%
%OUTPUT: omckk: 3D rotation vector attached to the grid positions in space
%        Tckk: 3D translation vector attached to the grid positions in space
%        Rckk: 3D rotation matrices corresponding to the omc vectors
%
%Method: Computes the normalized point coordinates, then computes the 3D pose
%
%Important functions called within that program:
%
%normalize_pixel: Computes the normalize image point coordinates.
%
%pose3D: Computes the 3D pose of the structure given the normalized image projection.
%
%project_points.m: Computes the 2D image projections of a set of 3D points

if nargin < 6,
   alpha_c = 0;
	if nargin < 5,
   	kc = zeros(5,1);
   	if nargin < 4,
      	cc = zeros(2,1);
      	if nargin < 3,
         	fc = ones(2,1);
         	if nargin < 2,
            	error('Need 2D projections and 3D points (in compute_extrinsic.m)');
            	return;
         	end;
      	end;
   	end;
	end;
end;

%keyboard;

% Compute the normalized coordinates:

xn = normalize_pixel_fisheye(x_kk,fc,cc,kc,alpha_c);

Np = size(xn,2);

%% Check for planarity of the structure:
%keyboard;

X_mean = mean(X_kk')';

Y = X_kk - (X_mean*ones(1,Np));

YY = Y*Y';

[U,S,V] = svd(YY);

r = S(3,3)/S(2,2);

%keyboard;

if (r < 1e-3)|(Np < 5), %1e-3, %1e-4, %norm(X_kk(3,:)) < eps, % Test of planarity

   %fprintf(1,'Planar structure detected: r=%f\n',r);

   % Transform the plane to bring it in the Z=0 plane:

   R_transform = V';

   %norm(R_transform(1:2,3))

   if norm(R_transform(1:2,3)) < 1e-6,
      R_transform = eye(3);
   end;

   if det(R_transform) < 0, R_transform = -R_transform; end;

	T_transform = -(R_transform)*X_mean;

	X_new = R_transform*X_kk + T_transform*ones(1,Np);

   % Compute the planar homography:

   H = compute_homography(xn,X_new(1:2,:));

   % De-embed the motion parameters from the homography:

   sc = mean([norm(H(:,1));norm(H(:,2))]);

   H = H/sc;

   % Extra normalization for some reasons...
   %H(:,1) = H(:,1)/norm(H(:,1));
   %H(:,2) = H(:,2)/norm(H(:,2));

   if 0, %%% Some tests for myself... the opposite sign solution leads to negative depth!!!

       % Case#1: no opposite sign:

       omckk1 = rodrigues([H(:,1:2) cross(H(:,1),H(:,2))]);
       Rckk1 = rodrigues(omckk1);
       Tckk1 = H(:,3);

       Hs1 = [Rckk1(:,1:2) Tckk1];
       xn1 = Hs1*[X_new(1:2,:);ones(1,Np)];
       xn1 = [xn1(1,:)./xn1(3,:) ; xn1(2,:)./xn1(3,:)];
       e1 = xn1 - xn;

       % Case#2: opposite sign:

       omckk2 = rodrigues([-H(:,1:2) cross(H(:,1),H(:,2))]);
       Rckk2 = rodrigues(omckk2);
       Tckk2 = -H(:,3);

       Hs2 = [Rckk2(:,1:2) Tckk2];
       xn2 = Hs2*[X_new(1:2,:);ones(1,Np)];
       xn2 = [xn2(1,:)./xn2(3,:) ; xn2(2,:)./xn2(3,:)];
       e2 = xn2 - xn;

       if 1, %norm(e1) < norm(e2),
           omckk = omckk1;
           Tckk = Tckk1;
           Rckk = Rckk1;
       else
           omckk = omckk2;
           Tckk = Tckk2;
           Rckk = Rckk2;
       end;

   else

       u1 = H(:,1);
       u1 = u1 / norm(u1);
       u2 = H(:,2) - dot(u1,H(:,2)) * u1;
       u2 = u2 / norm(u2);
       u3 = cross(u1,u2);
       RRR = [u1 u2 u3];
       omckk = rodrigues(RRR);

       %omckk = rodrigues([H(:,1:2) cross(H(:,1),H(:,2))]);
       Rckk = rodrigues(omckk);
       Tckk = H(:,3);

   end;

   %If Xc = Rckk * X_new + Tckk, then Xc = Rckk * R_transform * X_kk + Tckk + T_transform

   Tckk = Tckk + Rckk* T_transform;
   Rckk = Rckk * R_transform;

   omckk = rodrigues(Rckk);
   Rckk = rodrigues(omckk);

else

   %fprintf(1,'Non planar structure detected: r=%f\n',r);

   % Computes an initial guess for extrinsic parameters (works for general 3d structure, not planar!!!):
   % The DLT method is applied here!!

   J = zeros(2*Np,12);

	xX = (ones(3,1)*xn(1,:)).*X_kk;
	yX = (ones(3,1)*xn(2,:)).*X_kk;

	J(1:2:end,[1 4 7]) = -X_kk';
	J(2:2:end,[2 5 8]) = X_kk';
	J(1:2:end,[3 6 9]) = xX';
	J(2:2:end,[3 6 9]) = -yX';
	J(1:2:end,12) = xn(1,:)';
	J(2:2:end,12) = -xn(2,:)';
	J(1:2:end,10) = -ones(Np,1);
	J(2:2:end,11) = ones(Np,1);

	JJ = J'*J;
	[U,S,V] = svd(JJ);

   RR = reshape(V(1:9,12),3,3);

   if det(RR) < 0,
      V(:,12) = -V(:,12);
      RR = -RR;
   end;

   [Ur,Sr,Vr] = svd(RR);

   Rckk = Ur*Vr';

   sc = norm(V(1:9,12)) / norm(Rckk(:));
   Tckk = V(10:12,12)/sc;

	omckk = rodrigues(Rckk);
   Rckk = rodrigues(omckk);

end;


%-------------------------------------------------------------------------
% Coupling Kinetic and Fluid System of Euler Equations
% To solve Shock Tube Problem
%
% Kinetic equation use SBBGK and Hydrodynamic equation use Roe Euler
%
% Based on:
%  [1] Pierre Degond, Giacomo Dimarco and Luc Mieussens
%      A moving interface method for dynamic kinetic-fluid coupling
%      Journal of Computation Physics 227(2007)1176-1208
%
% By Manuel Diaz and Yun-Da Tsai
% 007@IAM 25.01.2013
%-------------------------------------------------------------------------

clear all; clc; close all;

%% Global Variables
global CFL r_time theta dt dtdx nx
global w k nv
global gamma etpfix

%% Controling Parameters
name        ='SBBGK1d'; % Simulation Name
CFL         = 0.05;     % CFL condition
r_time      = 1/10000;  % Relaxation time
tEnd        = 0.04;      % End time
theta       = 0;        % {-1} BE, {0} MB, {1} FD.
quad        = 2;        % for NC = 1 , GH = 2
method      = 1;        % for TVD = 1, WENO3 = 2, WENO5 = 3
IC_case     = 1;        % IC: {1}Sod's, {2}LE, {3}RE, {4}DS, {5}SS, {6}Cavitation
plot_figs   = 1;        % 0: no, 1: yes please!
write_ans   = 0;        % 0: no, 1: yes please!
gamma       = 2;      % Ratio of specific heats
flxtype     = 2;        % {1} Roe, {2} LF, {3} LLF, {4} Upwind <-non-conservative!
etpfix      = 0.90;     % {#} Harten's sonic entropy fix value, {0} no entropy fix

%% Space Discretization
nx  = 100;                      % number of cells
x   = linspace(0,1,nx);         % Physical domain -x
dx  = max(x(2:end)-x(1:end-1)); % delta x

%% Load Initial Condition
[z0,ux0,t0,p0,rho0,E0] = SSBGK_IC1d(x,IC_case);

%% Load Initial Cut Function
 xa = 0.5;   % buffer left boundary
 xb = 0.5125;   % buffer right boundary
 h0  = cutfunc(x,xa,xb);  % Physical Cut Function

%  h0 = ones(size(x));
%  h0 = zeros(size(x));

%% Discretization of the Velocity Space
% Microscopic Velocity Discretization (using Discrete Ordinate Method)
% that is to make coincide discrete values of microscopic velocities with
% values as the value points for using a quadrature method, so that we can
% integrate the velocity probability distribution to recover our
% macroscopics properties.
switch quad

    case{1} % Newton Cotes Quadrature:
    V  = [-20,20];  % range: a to b
    nv = 200;       % nodes desired (may not the actual value)
    [c,w,k] = cotes_xw(V(1),V(2),nv,5); % Using Netwon Cotes Degree 5

    case{2} % Gauss Hermite Quadrature:
    nv = 100;          % nodes desired (the actual value)
    [c,w] = GaussHermite(nv); % for integrating range: -inf to inf
    k = 1;            % quadrature constant.
    w = w.*exp(c.^2); % weighting function of the Gauss-Hermite quadrature

    otherwise
        error('Order must be between 1 and 2');
end

%% Applying DOM
% The actual nv value will be computed using 'lenght' vector function:
nv = length(c);
% Remap velocity points
    c = repmat(c,1,nx);     w = repmat(w,1,nx);
% Remap classical IC
    [rho0,ux0,p0] = apply_DOM(rho0,ux0,p0,nv);
% Remap Semiclassical IC
    [z0,t0,E0] = apply_DOM(z0,t0,E0,nv);
% Remap h coeficient
  h0 = repmat(h0,nv,1);

%% Semi0-classical Equilibrium Distribution Function
M0 = f_equilibrium_1d(z0,ux0,c,t0,theta);

%% Load initial Conditions and Spliting of information
M = M0; rho = rho0; ux = ux0; t = t0; p = p0; z = z0; h = h0; E = E0;

%% Split ICs in R and L
rhor = h.*rho0;     rhol = (1-h).*rho0;
uxr = h.*ux0;       uxl = (1-h).*ux0;
pr = h.*p0;         pl = (1-h).*p0;

% [zr,~,tr,~] = macroproperties1d(rhor,rhor.*uxr,pr+0.5*rhor.*uxr.^2,nx,nv,theta);
% [zl,~,tl,~] = macroproperties1d(rhol,rhol.*uxl,pl+0.5*rhol.*uxl.^2,nx,nv,theta);
% Ml = f_equilibrium_1d(zl,uxl,c,tl,theta) ;
% fr = f_equilibrium_1d(zr,uxr,c,tr,theta);
 fr=h.*M;
 Ml=(1-h).*M;
%% Main Loop
% Compute next time step
 dt = dx*CFL/max(c(:,1));

% dt =1/20000;
time  = 0:dt:tEnd;
dtdx = dt/dx;
% Ml(isnan(Ml)) = 0;     fr(isnan(fr)) = 0;
f = fr + Ml; % computed here for ploting purposes

count = 1; % iteration counter
tic;
for tsteps = time

    CFL=dtdx*max(c(:,1));
% Plot IC
   if plot_figs == 1; surf(f); end;

% Compute vector 'q'
   q = [rhol(1,:) ; rhol(1,:).*uxl(1,:) ; pl(1,:)+0.5*rhol(1,:).*uxl(1,:).^2];

% Update Physical cut function 'h'
     h_next = h; % this means: fixed buffer assumption

%     Evaluate Modified Boltzmann BGK
    [fr_next] = ModSBBGK(h,h_next,M,fr,Ml,c,flxtype);

% Evaluate Modificed Roe Euler Solver
    [rhol,rhoul,El] = ModEuler(h,h_next,q,fr_next,c);

%     plot partial result
%     if plot_figs == 1;
%         subplot(1,3,1); plot(x,rhol,'o'); title('Density');
%         subplot(1,3,2); plot(x,rhoul./rhol,'o'); title('Velocity');
%         subplot(1,3,3); plot(x,El,'o'); title('Energy');
%     end

%     macroscopic properties
    [zl,uxl,tl,pl] = macroproperties1d(rhol,rhoul,El,nx,nv,gamma,theta);

    % Apply DOM
    [tl,zl,uxl] = apply_DOM(tl,zl,uxl,nv);

    % Compute Ml_next
     Ml_next = f_equilibrium_1d(zl,uxl,c,tl,theta) ;
%  ****************************************************

    % New time step info: sum left and righ values with 'NaN' filter sum
    % function.
    Ml_next(isnan(Ml_next)) = 0;     fr_next(isnan(fr_next)) = 0;
    % Total f
     f  = fr_next + Ml_next;

     % Update New macroquantity
   [rho,rhou,E] = macromoments1d(k,w,f,c);

    %Apply Neumann BC's in total variables
    f(:,1)  = f(:,2);                 f(:,end)  = f(:,end-1);
    rho(:,1)= rho(:,2);            rho(:,end)= rho(:,end-1);
    rhou(:,1) = rhou(:,2);       rhou(:,end) = rhou(:,end-1);
    E(:,1)  = E(:,2);                 E(:,end)  = E(:,end-1);

    [rho,rhou,E] = apply_DOM(rho,rhou,E,nv);

    % Recover Semiclassical Conditions for next time step
     [z,ux,t,p] = macroproperties1d(rho,rhou,E,nx,nv,gamma,theta);

%  update New equilibrium
     M=f_equilibrium_1d(z,ux,c,t,theta);
     fr=h.*M;

%  preparing new Ml
     rhol = (1-h).*rho;
     uxl = (1-h).*ux;
     pl = (1-h).*p;
     [zl,~,tl,~] = macroproperties1d(rhol,rhol.*uxl,pl+0.5*rhol.*uxl.^2,nx,nv,gamma,theta);
% update New Ml
     Ml=   f_equilibrium_1d(zl,uxl,c,tl,theta) ;
     Ml(isnan(Ml)) = 0;

% update counter
    count = count+1;

% update plot
    drawnow
end
toc;

% write/plot final output
%plot(x,r_total,'o');


function results = poly_segmentation_3d(record)
if nargin < 1
  record = false;
end
import iris.drawing.*;

lb = [0;0;0];
ub = [10;10;10];
dim = 3;

n_obs = 20;
% obstacles = iris.test.random_obstacles(dim, n_obs, lb, ub,3);
obstacles = iris.test.random_cubic_obstacles(dim, n_obs, lb, ub);

A_bounds = [-1,0,0;
            0,-1,0;
            0,0,-1;
            1,0,0;
            0,1,0;
            0,0,1];
b_bounds = [-lb;ub];

start = 0.5 * (ub + lb);

% profile on
[A,b,C,d,results] = iris.inflate_region(obstacles, A_bounds, b_bounds, start);
% profile viewer
if n_obs < 50
  animate_results(results, record);
end

end


%% This script is used to generate the Brain Network for user to perform further analysis by themselves.

clear all
addpath(genpath(pwd));
input_folder='./data/'; % this is the directory of the time course data for all subjects, each file for each subject, similar as you prepared for GUI
output_dir='./Generated_BrainNet/'; %this is the output directory;
meth_Net='aHOFC'; %Here you can set the brain network construction method;
%["PC","aHOFC","tHOFC","SR","WSR","SLR","SGR","WSGR","GSR","SSGSR","dHOFC"];
switch meth_Net
    case {'SR','WSR','GSR'}
        lambda=0.01:0.01:0.1; %User can change the parameter range by themselves;
    case {'SLR','SGR','WSGR','SSGSR'}
        lambda_1=0.01:0.01:0.1; %User can change the parameter range by themselves;
        lambda_2=0.01:0.01:0.1;
    case 'dHOFC'
        C=100:100:800;
        W=20:10:70;
        s=1; %step size is usually set to 1 or 2;
    case {'PC','tHOFC','aHOFC'}

end

dirOutput=dir(fullfile(input_folder,'*.txt'));
fileName={dirOutput.name}';
%folder={dirOutput.folder}';
for i=1:length(fileName)
    BOLD{i,1}=load([input_folder,'/',fileName{i}]);
end
%label=importdata(label_input);

[~,nROI]=size(BOLD{1});
nSubj=length(BOLD);

fprintf('Begin network construction\n');

switch meth_Net
    case 'PC'       % Pearson's correlation
        BrainNet{1}=PC(BOLD);
    case 'tHOFC'    % Topographical high-order FC
        BrainNet{1}=tHOFC(BOLD);
    case 'aHOFC'    % Associated high-order FC
        BrainNet{1}=aHOFC(BOLD);
    case 'SR'       % Sparse representation
        parfor i=1:length(lambda)
            BrainNet{i}=SR(BOLD,lambda(i));
        end

    case 'WSR'      % PC weighted SR
        parfor i=1:length(lambda)
            BrainNet{i}=WSR(BOLD,lambda(i));
        end

    case 'SLR'      % Sparse low-rank representation
        lambda1=lambda_1;
        lambda2=lambda_2;
        num_lambda1=length(lambda1);
        num_lambda2=length(lambda2);
        parfor i=1:num_lambda1
            for j=1:num_lambda2
                BrainNet{i,j}=SLR(BOLD,lambda1(i),lambda2(j));
            end
        end
        BrainNet=reshape(BrainNet,1,num_lambda1*num_lambda2);

    case 'SGR'      % Sparse group representation
        lambda1=lambda_1;
        lambda2=lambda_2;
        num_lambda1=length(lambda1);
        num_lambda2=length(lambda2);
        parfor i=1:num_lambda1
            for j=1:num_lambda2
                BrainNet{i,j}=SGR(BOLD,lambda1(i),lambda2(j));
            end
        end

        BrainNet=reshape(BrainNet,1,num_lambda1*num_lambda2);

    case 'WSGR'     % PC weighted SGR
        lambda1=lambda_1; % parameter for sparsity
        lambda2=lambda_2; % parameter for group sparsity
        num_lambda1=length(lambda1);
        num_lambda2=length(lambda2);

        parfor i=1:num_lambda1
            for j=1:num_lambda2
                BrainNet{i,j}=WSGR(BOLD,lambda1(i),lambda2(j));
            end
        end
        BrainNet=reshape(BrainNet,1,num_lambda1*num_lambda2);

    case 'GSR'      % Group sparse representation
        parfor i=1:length(lambda)
            BrainNet{i}=GSR(BOLD,lambda(i));
        end

    case 'SSGSR'    % Strength and Similarity guided GSR
        %lambda1=lambda_1(1:6); % parameter for group sparsity
        lambda1=lambda_1;
        lambda2=lambda_2; % parameter for inter-subject LOFC-pattern similarity
        num_lambda1=length(lambda1);
        num_lambda2=length(lambda2);
        parfor i=1:num_lambda1
            for j=1:num_lambda2
                BrainNet{i,j}=SSGSR(BOLD,lambda1(i),lambda2(j));
            end
        end
        BrainNet=reshape(BrainNet,1,num_lambda1*num_lambda2);

    case 'dHOFC'    % Dynamic high-order FC
        num_W=length(W);
        num_C=length(C);
        parfor i=1:num_W % number of clusters
            for j=1:num_C
                [BrainNet{i,j},IDX{i,j}]=dHOFC(BOLD,W(i),s,C(j));
            end
        end
        BrainNet=reshape(BrainNet,1,num_W*num_C);
end
save(char(strcat(output_dir,meth_Net,'.mat')),'BrainNet','-v7.3');
% All the generated networks (and those resulted from different parameters) are saved as a cell array, nROIxnROIxnSubject in each cell, different cells for results with different combinations of parameters
fprintf('Network construction finished\n');


function [ySqrtEst, PInvSqrtEst]=sqrtInfoBatchSmoother(ySqrtPred,PInvSqrtPred,z,u,H,F,SR,SQ,Gamma,kD)
%%SQRTINFOBATCHSMOOTHER Run the square root information smoother for linear
%                  dynamic and measurement models on a batch of
%                  measurements. The smoothed result at one time step or
%                  along the entire batch are available. The initial
%                  predicted states can not be uninformative.
%
%INPUTS: ySqrtPred The xDimX1 predicted square root information state at
%                 the time of the initial measurement in z. The predicted
%                 information state is always PInvSqrtPred times the
%                 predicted target state estimate.
%    PInvSqrtPred The inverse square root information matrix associated
%                 with the predicted information state at the time of the
%                 initial measurement in z. If P is the covariance matrix
%                 of a Gaussian state x, then P=PSqrt*PSqrt' and
%                 PInvSqrtPred=inv(PSqrt). This can be either upper
%                 triangular or lower triangular.
%               z The zDim X N matrix of measurements for the whole batch.
%               u The xDim X(N-1) matrix of control inputs for the whole
%                 batch. If there are no control inputs, then set u=[];
%               H The zDim X xDim X N hypermatrix of measurement matrices
%                 such that H(:,:,k)*x+w is the measurement at time k,
%                 where x is the state and w is zero-mean Gaussian noise
%                 with covariance matrix R (:,:,k). Alternatively, if all
%                 of the measurement matrices are the same, one can just
%                 pass a single zDim X xDim matrix.
%               F The xDim X xDim X (N-1) hypermatrix of state transition
%                 matrices. The state at discrete-time k+1 is modeled as
%                 F(:,:,k) times the state at time k plus zero-mean
%                 Gaussian process noise with covariance matrix Q(:,:,k).
%                 Alternatively, if all of the state transition matrices
%                 are the same, one can just pass a single xDim X xDim
%                 matrix.
%              SR The zDim X zDim X N hypermatrix of lower-triangular
%                 square-root of the measurement covariance matrices. The
%                 matrices must be invertible. Alternatively, if all of the
%                 measurement covariance matrices are the same, one can
%                 just pass a single zDim X zDim matrix.
%              SQ The xDim X xDim X (N-1) hypermatrix of lower-triangular
%                 square-root of the process noise covariance matrices.
%                 The matrices must be invertible. Alternatively, if all of
%                 the measurement covariance matrices are the same, one can
%                 just pass a single xDim X xDim matrix.
%           Gamma An optional xDim X xDim X (N-1) hypermatrix of matrices
%                 that transform the process noise to the state domain if
%                 the process noise covariance matrix is singular.
%                 Alternatively, if all of the transform matrices are the
%                 same, one can just pass a single xDim X xDim matrix. If
%                 this is omitted entirely, an identity matrix is used
%                 (i.e. there is no Gamma).
%              kD The discrete time-step at which the smoothed state
%                 estimate is desired, where z(:,1) is at discrete
%                 time-step 1 (not 0). If kD is omitted ot an empty matrix
%                 is passed, then results along the entire batch are
%                 obtained.
%
%OUTPUTS: ySqrtEst The xDimXN smoothed square root information state
%                  estimates at all steps if kD is not provided or the
%                  xDimX1 smoothed information state estimate at step kD if
%                  kD is provided.
%      PSqrtInvEst The inverse square root covariance matrices associated
%                  with the smoothed information state estimates. This is
%                  xDimXxDimXN for the whole batch if kD is not provided
%                  and is xDimXxDim if kD is provided.
%
%The algorithm is that of the linear square root information filter and
%smoother that is described in the book [1]. The forward pass steps can be
%found in Chapter IV, and the backwards smoother in Chapter X.
%
%The algorithm works by running a square root information filter forward
%in time and then smoothing backwards. The smoothing step depends on
%information stored during the forward pass. The smoothing step here is
%performed on a state residual rather than the state itself and applies the
%smoothed output to the previous updated measurement. This was based off of
%NASA's Orbit Determination Toolbox.
%
%z*(j+1)=PInvSqrt*(j+1)(x*(j+1)-xPred(j+1)
%A=[Rw(j)+Rwx(j),   Rwx(j)F(j+1,j),         u(j)
%   PInvSqrt*(j+1), PInvSqrt*(j+1)F(j+1,j), z*(j+1)]
%qr(A)=[Rw*(j),   Rwx*(j),      u*(j)
%       0,        PInvSqrt*(j), z*(j)]
%x*(j)=xUpd(j)+PInvSqrt*(j)\z*(j)
%
%where xPred is the forward predicted state, xUpd is the forward updated
%state, Rw and Rwx are outputs from the forward prediction step, u is the
%the control input (often zero), and x* and PInvSqrt* are the smoothed
%states. x*(N)=xUpd(N) and PInvSqrt*(N)=PInvSqrtUpd(N)
%
%REFERENCES:
%[1] G. J. Bierman, "Factorization Methods for Discrete Sequential
%    Estimation. Academic Press, New York, 1977.
%
%March 2015, David Karnick, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

xDim=size(H,2);
N=size(z,2);

if(nargin<9||isempty(Gamma))
    Gamma=eye(xDim);
end
if(nargin<10||isempty(kD))
    kD=[];
end
if(isempty(ySqrtPred))
    ySqrtPred=zeros(xDim,1);
end
if(isempty(PInvSqrtPred))
    PInvSqrtPred=zeros(xDim,xDim);
end
if(isempty(u))
    u=zeros(xDim,N-1);
end
if(size(H,3)==1)
    H=repmat(H,[1,1,N]);
end
if(size(F,3)==1)
    F=repmat(F,[1,1,N-1]);
end
if(size(SR,3)==1)
    SR=repmat(SR,[1,1,N]);
end
if(size(SQ,3)==1)
    SQ=repmat(SQ,[1,1,N-1]);
end
if(size(Gamma,3)==1)
    Gamma=repmat(Gamma,[1,1,N-1]);
end

%Run the SRIF forward until we have the predictions of step kD|kD-1 (going forwards).
ySqrtFwdPred=zeros(xDim,N);
PInvSqrtFwdPred=zeros(xDim,xDim,N);
ySqrtFwdUpd=zeros(xDim,N);
PInvSqrtFwdUpd=zeros(xDim,xDim,N);
Rw=zeros(xDim,xDim,N);
Rwx=zeros(xDim,xDim,N);

%The first step uses the priors
ySqrtFwdPred(:,1)=ySqrtPred;
PInvSqrtFwdPred(:,:,1)=PInvSqrtPred;

%The rest of the steps
for curStep=1:(N-1)
    [ySqrtFwdUpd(:,curStep),PInvSqrtFwdUpd(:,:,curStep)]=sqrtInfoFilterUpdate(ySqrtFwdPred(:,curStep),PInvSqrtFwdPred(:,:,curStep),z(:,curStep),SR(:,:,curStep),H(:,:,curStep));
    [ySqrtFwdPred(:,curStep+1),PInvSqrtFwdPred(:,:,curStep+1),Rw(:,:,curStep+1),Rwx(:,:,curStep+1)]=sqrtInfoFilterDiscPred(ySqrtFwdUpd(:,curStep),PInvSqrtFwdUpd(:,:,curStep),F(:,:,curStep),SQ(:,:,curStep),u(:,curStep),Gamma(:,:,curStep));
end

%Run the SRIS backwards until we have the updated information state of
%step kD|kD (going backwards).
ySqrtEst=zeros(xDim,N);
PInvSqrtEst=zeros(xDim,xDim,N);

%ODTBX method
[ySqrtEst(:,end),PInvSqrtEst(:,:,end)]=sqrtInfoFilterUpdate(ySqrtFwdPred(:,N),PInvSqrtFwdPred(:,:,N),z(:,N),SR(:,:,N),H(:,:,N));
for curStep=(N-1):-1:1
    xPred=PInvSqrtFwdPred(:,:,curStep+1)\ySqrtFwdPred(:,curStep+1); %Forward predicted state_j+1
    xStar=PInvSqrtEst(:,:,curStep+1)\ySqrtEst(:,curStep+1); %Backward smoothed state_j+1
    zStar=PInvSqrtEst(:,:,curStep+1)*(xStar-xPred);

    [zSmooth,PInvSqrtEst(:,:,curStep)]=sqrtInfoSmoothStep(zStar,PInvSqrtEst(:,:,curStep+1),F(:,:,curStep),Rw(:,:,curStep),Rwx(:,:,curStep),u(:,curStep),Gamma(:,:,curStep));

    xUpd=PInvSqrtFwdUpd(:,:,curStep)\ySqrtFwdUpd(:,curStep); %Forward updated state_j
    ySqrtEst(:,curStep)=PInvSqrtEst(:,:,curStep)*(xUpd+PInvSqrtEst(:,:,curStep)\zSmooth);
end

if(~isempty(kD))
    ySqrtEst=ySqrtEst(:,kD);
    PInvSqrtEst=PInvSqrtEst(:,:,kD);
end
end

function [ySqrtPred, PInvSqrtPred]=sqrtInfoSmoothStep(ySqrtPrev,PInvSqrtPrev,F,Ru,Rux,u,Gamma)
%This is the basic Smoothing step shown in Bierman's book
xDim=size(ySqrtPrev,1);

A=[Ru+Rux*Gamma, Rux*F, u;
    PInvSqrtPrev*Gamma,   PInvSqrtPrev*F,   ySqrtPrev];
[~,T]=qr(A);

ySqrtPred=T((xDim+1):end,end);
PInvSqrtPred=T((xDim+1):end,(end-xDim):(end-1));
end

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


function [TW] = Btuph2TW(Btuph)
% Convert power from British thermal units per hour to terawatts.
% Chad A. Greene 2012
TW = Btuph*2.930710702e-13;


function [post nlZ dnlZ] = infKL_sprox(hyp, mean, cov, lik, x, y)
% PG-SVI

if hyp.is_cached==1
	global cache_post;
	global cache_nlz;
	global cache_idx;

	post=cache_post(cache_idx);
	nlZ=cache_nlz(cache_idx);
	if nargout>2
		warning('to be implemented\n');
		dnlZ = NaN;
	end
	return
end

snu2=hyp.snu2;
n=size(x,1);

% GP prior
K = feval(cov{:}, hyp.cov, x);                  % evaluate the covariance matrix
m = feval(mean{:}, hyp.mean, x);                      % evaluate the mean vector
K=snu2*eye(n)+K;

lik_name = func2str(lik{1});
mini_batch_size=hyp.mini_batch_size;
assert (mini_batch_size>0)
mini_batch_num=ceil(n/mini_batch_size);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%init value
post_m=hyp.init_m;%k=0
tW = zeros(n,1);%k=-1
post_v=diag(hyp.init_V);%k=0
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

iter = 0;%iteration
pass=0;%pass
max_pass=hyp.max_pass;
beta = hyp.learning_rate;
r = 1/(beta+1);
index=1:n;
while pass<max_pass
	if mini_batch_size<n
		index=randperm(n);
	end
	offset=0;
	mini_batch_counter=0;
	pass=pass+1;
	while mini_batch_counter<mini_batch_num
		mini_batch_counter=mini_batch_counter+1;
		iter=iter+1;
		%mini batch
		tmp_idx = mini_batch_counter*mini_batch_size;
		idx=index( (tmp_idx-mini_batch_size+1):min(tmp_idx,n) );

		weight=double(n)/size(x(idx,:),1);

		if hyp.stochastic_approx==1
			[ll, gf, gv] = sampling_E(y(idx), post_m(idx), post_v(idx), lik, hyp.sample_size, hyp.lik);
		else
			switch lik_name
			case {'laplace','likLaplace','poisson','bernoulli_logit','likLogistic'}
				[ll, gf, gv] = E_log_p(lik_name, y(idx), post_m(idx), post_v(idx), hyp.lik);
			otherwise
				[ll,gf,d2f,gv] = likKL(post_v(idx), lik, hyp.lik, y(idx), post_m(idx));
			end
		end

		% pseudo observation
		pseudo_y = m + K(:,idx)*(weight*gf) - post_m;
		tW = r.*tW;
		tW(idx) = tW(idx)+(1-r).*((-2*weight)*gv);%tW^{k}, where W=-2*gv
		sW = sqrt(abs(tW)) .* sign(tW);
		L = chol(eye(n)+sW*sW'.*K); %L = chol(sW*K*sW + eye(n));

		%use this following line if we approximate r^{k} .* tW.^{k-1} by tW.{k}
		post_m = post_m + (1-r).*(pseudo_y - K*(sW.*(L\(L'\(sW.*pseudo_y)))));%m^{k+1}
		T = L'\(repmat(sW,1,n).*K); %T  = L'\(sW*K);
		post_v = diag(K) - sum(T.*T,1)'; % v = diag(inv(inv(K)+diag(W))); %v^{k+1}

		if isfield(hyp,'save_iter') && hyp.save_iter==1
			global cache_nlz_iter
			global cache_iter

			alpha=K\(post_m-m);
			nlZ2=compute_nlz(lik, hyp, sW, K, m, alpha, post_m, y);
			cache_iter=[cache_iter; iter];
			cache_nlz_iter=[cache_nlz_iter; nlZ2];
		end

	end
	alpha=K\(post_m-m);
	nlZ=compute_nlz(lik, hyp, sW, K, m, alpha, post_m, y);

	if isfield(hyp,'save_iter') && hyp.save_iter==1
		if pass==1
			global num_iters_at_pass;
			num_iters_at_pass=iter;
		end
		fprintf('pass:%d) at %d iter %.4f %.4f\n', pass, iter, nlZ, nlZ2);
	else
		fprintf('pass:%d) %.4f\n', pass, nlZ);
	end

	if hyp.is_save==1
		global cache_post;
		global cache_nlz;

		post.sW = sW;                                             % return argument
		post.alpha = alpha;
		post.L = L;

		cache_post=[cache_post; post];
		cache_nlz=[cache_nlz; nlZ];
	end
end

alpha=K\(post_m-m);
post.sW = sW;                                             % return argument
post.alpha = alpha;
post.L = L;                                              % L'*L=B=eye(n)+sW*K*sW

nlZ=compute_nlz(lik, hyp, sW, K, m, alpha, post_m, y);
fprintf('final: %.4f\n', nlZ);

if nargout>2
  warning('to be implemented\n');
  dnlZ = NaN;
end


function [fb_sim] = acc_gen (ref, imu)
% acc_gen: generates simulated accelerometers measurements from reference
%  data and IMU error profile.
%
% INPUT
%	ref: data structure with true trajectory.
%	imu: data structure with IMU error profile.
%
% OUTPUT
%	fb_sim: Nx3 matrix with simulated accelerations in the
%		body frame [X Y Z] (m/s^2, m/s^2, m/s^2).
%
%   Copyright (C) 2014, Rodrigo González, all rights reserved.
%
%   This file is part of NaveGo, an open-source MATLAB toolbox for
%   simulation of integrated navigation systems.
%
%   NaveGo is free software: you can redistribute it and/or modify
%   it under the terms of the GNU Lesser General Public License (LGPL)
%   version 3 as published by the Free Software Foundation.
%
%   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 Lesser General Public License for more details.
%
%   You should have received a copy of the GNU Lesser General Public
%   License along with this program. If not, see
%   <http://www.gnu.org/licenses/>.
%
% References:
%
%	R. Gonzalez, J. Giribet, and H. Patiño. NaveGo: a
% simulation framework for low-cost integrated navigation systems,
% Journal of Control Engineering and Applied Informatics, vol. 17,
% issue 2, pp. 110-120, 2015. Sec. 2.2.
%
%   Aggarwal, P. et al. MEMS-Based Integrated Navigation. Artech
% House. 2010.
%
%   Thinking about accelerometers and gravity by Dave Redell
% http://www.lunar.org/docs/LUNARclips/v5/v5n1/Accelerometers.html
%
% Version: 012
% Date:    2022/08/22
% Author:  Rodrigo Gonzalez <rodralez@frm.utn.edu.ar>
% URL:     https://github.com/rodralez/navego

N = max(size(ref.t));
M = [N, 3];

%% SIMULATION OF ACC

% If true, accelerations are provided...
if (isfield(ref, 'fb'))

    acc_b = ref.fb;

% If not, accelerations are obtained from velocity
elseif (isfield(ref, 'vel'))

    acc_raw = (diff(ref.vel)) ./ [diff(ref.t) diff(ref.t) diff(ref.t)];
    acc_raw = [ 0 0 0; acc_raw; ];

    % Noise introduced by derivatives should be smoothed
    acc_ned = my_sgolayfilt(acc_raw);
    acc_b = acc_nav2body(acc_ned, ref.DCMnb_m);

% If not, accelerations are obtained from position
else

    % Method: LLH > ECEF > NED
    [~, acc_ned] = pllh2vned (ref);
    acc_b = acc_nav2body(acc_ned, ref.DCMnb_m);
end

%% SIMULATION OF GRAVITY AND CORIOLIS

% Gravity and Coriolis in nav-ref
g_n = -gravity(ref.lat, ref.h);              % Accelerometer in Z axis senses an
                                                % acceleration of 1.0 G straight up
cor_n = coriolis(ref.lat, ref.vel, ref.h);

% Gravity and Coriolis from nav-ref to body-ref
g_b = zeros(M);
cor_b = zeros(M);
for i = 1:N
    dcmnb = reshape(ref.DCMnb_m(i,:), 3, 3);
    gb = dcmnb * g_n(i,:)';
    corb =  dcmnb * cor_n(i,:)';
    g_b(i,:) = gb';
    cor_b(i,:) = corb';
end

%% SIMULATION OF NOISES

% -------------------------------------------------------------------------
% Simulation of static bias as a constant random variable

ab_sta = noise_b_sta (imu.ab_sta, N);

% -------------------------------------------------------------------------
% Simulation of white noise

wn = randn(M);
acc_wn = zeros(M);

for i=1:3

    acc_wn(:, i) = imu.a_std(i).* wn(:,i);
end

% -------------------------------------------------------------------------
% Simulation of dynamic bias (bias instability) as a first-order Gauss-Markov model

dt = 1/ref.freq;
ab_dyn = noise_b_dyn (imu.ab_corr, imu.ab_dyn, dt, M);

% -------------------------------------------------------------------------
% Simulation of rate random walk

acc_rrw = noise_rrw (imu.vrrw, dt, M);

% -------------------------------------------------------------------------

fb_sim = acc_b + cor_b + g_b + acc_wn + ab_sta + ab_dyn + acc_rrw;

end


function [mh,mw]=tropmapf(time,pos,azel)

if pos(3)<-1000||pos(3)>20000
    mh=0; mw=0; return;
end

% hydro-ave-a,b,c, hydro-amp-a,b,c, wet-a,b,c at latitude 15,30,45,60,75
coef=[ 1.2769934E-3, 1.2683230E-3, 1.2465397E-3, 1.2196049E-3, 1.2045996E-3;...
       2.9153695E-3, 2.9152299E-3, 2.9288445E-3, 2.9022565E-3, 2.9024912E-3;...
       62.610505E-3, 62.837393E-3, 63.721774E-3, 63.824265E-3, 64.258455E-3;...

       0.0000000E-0, 1.2709626E-5, 2.6523662E-5, 3.4000452E-5, 4.1202191E-5;...
       0.0000000E-0, 2.1414979E-5, 3.0160779E-5, 7.2562722E-5, 11.723375E-5;...
       0.0000000E-0, 9.0128400E-5, 4.3497037E-5, 84.795348E-5, 170.37206E-5;...

       5.8021897E-4, 5.6794847E-4, 5.8118019E-4, 5.9727542E-4, 6.1641693E-4;...
       1.4275268E-3, 1.5138625E-3, 1.4572752E-3, 1.5007428E-3, 1.7599082E-3;...
       4.3472961E-2, 4.6729510E-2, 4.3908931E-2, 4.4626982E-2, 5.4736038E-2];

% height correction
aht=[2.53E-5, 5.49E-3, 1.14E-3];
ah=zeros(3,1); aw=zeros(3,1);

if azel(2)<=0,mh=0; mw=0; return;end

lat=pos(1)*180/pi;
if lat<0,yy=0.5;else,yy=0;end
y=(time2doy(time)-28.0)/365.25+yy;
cosy=cos(2.0*pi*y);
lat=abs(lat);

for i=1:3
    ah(i)=interpc(coef(i,:),lat)-interpc(coef(i+3,:),lat)*cosy;
    aw(i)=interpc(coef(i+6,:),lat);
%     j=fix(lat/15);
%     ah(i)=(coef(i,j)+(coef(i,j+1)-coef(i,j))*(lat/15-j))-...
%           (coef(i+3,j)+(coef(i+3,j+1)-coef(i+3,j))*(lat/15-j))*cosy;
%     aw(i)=(coef(i+6,j)+(coef(i+6,j+1)-coef(i+6,j))*(lat/15-j));
end

el=azel(2);
sinel=sin(el);

mh1=(1+ah(1)/(1+ah(2)/(1+ah(3))))/(sinel+(ah(1)/(sinel+ah(2)/(sinel+ah(3)))));
mh2=(1+aht(1)/(1+aht(2)/(1+aht(3))))/(sinel+(aht(1)/(sinel+aht(2)/(sinel+aht(3)))));
mh=mh1+(1/sinel-mh2)*pos(3)/1000;

mw=(1+aw(1)/(1+aw(2)/(1+aw(3))))/(sinel+(aw(1)/(sinel+ah(2)/(sinel+aw(3)))));

return


function result = TLDA_test_LR(traindata, testdata, trainlabel, testlabel)
    %% use the model test the target domain data
    tempTrainXY = scale_cols(traindata, trainlabel);

    % train the classifier
    c00 = zeros(size(tempTrainXY,1),1);
    lambdaLG = exp(linspace(-0.5,6,20));
    wbest=c00;
    f1max = -inf;
    for j = 1 : length(lambdaLG)
        c_0 = train_cg(tempTrainXY,c00,lambdaLG(j));
        f1 = logProb(tempTrainXY,c_0);
        if f1 > f1max
            f1max = f1;
            wbest = c_0;
        end
    end
    C = wbest;

    result = zeros(1,2);
    % test the train data
    probability = 1./(1+1./(exp(C'*traindata)));
    probability(probability >= 0.5) = 1;
    probability(probability < 0.5) = -1;
    result(1,1) = mean(probability(:) == trainlabel(:));

    % test the test data
    probability = 1./(1+1./(exp(C'*testdata)));
    probability(probability >= 0.5) = 1;
    probability(probability < 0.5) = -1;
    result(1,2) = mean(probability(:) == testlabel(:));


function estimation_results = FEKF_SLAM(data)
% FEJ-EKF SLAM
% load pre-given data: odometry and observations
if nargin < 1
    load('./data.mat');
end

data_matrix = data.state;
odo_cov = data.odom_cov;   % constant variable
obs_cov = data.obse_cov;   % constant variable

odom_sigma = data.odom_sigma;
obsv_sigma = data.obsv_sigma;

%%%%%%%%%%%%%%%%%%%% Estimation_X is used to save the state in each step %%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%% In every step, all elements of Estimation_X will be changed %%%%%%%%%%%%
estimation_x.orientation = data.poses.orientation(1:3,1:3);
estimation_x.position    = data.poses.position(:,1);
estimation_x.cov         = sparse(6,6);
estimation_x.landmarks   = [];       % the landmarks observed until this step (included), 4*N format, the 4-th row is the index
estimation_x0.IndexObservedNew=[];
estimation_x0.IndexObservedAlreadyThis=[];
%Estimation_X.IndexOfFeature=[];     % the names(indexes) of the landmarks observed until this step (included)
%%%%%%%%%%%%%%%%%%%% Estimation_X is used to save the state in each step %%%%%%%%%%%%%%%%%%%%
FirstPosition=[]; % store  p_{n|n-1}
FirstLandmarks=[]; % store f_{k0}

% Initialize
n_steps = max(data_matrix(:,4));  % step instead of pose,  hence, it does not include pose 0
estimation_results = cell(1, n_steps+1);
estimation_results{1} = estimation_x;

for i = 0:n_steps
    IndexOfCurrentStepInDataMatrix = find(data_matrix(:,4) == i);
    m = size(IndexOfCurrentStepInDataMatrix, 1);
    if ( mod(i, 50) == 0 )
        disp(['Processing pose ', int2str(i)]);
    end
    % det(Estimation_X.cov)
    if i ~= n_steps
        OdometryFromThis2Next = data_matrix(IndexOfCurrentStepInDataMatrix(m-5):IndexOfCurrentStepInDataMatrix(m),1);
        if m > 6
            CameraMeasurementThis = [ data_matrix( IndexOfCurrentStepInDataMatrix(1): IndexOfCurrentStepInDataMatrix(m-6) , 1 ),...
                                      data_matrix( IndexOfCurrentStepInDataMatrix(1): IndexOfCurrentStepInDataMatrix(m-6) , 3 )];
            [estimation_x,FirstLandmarks] = FEKF_update(estimation_x, CameraMeasurementThis, obsv_sigma, FirstLandmarks );
        end

        estimation_results{i+1} = estimation_x;

        [estimation_x, FirstPosition] = FEKF_propagate(estimation_x, OdometryFromThis2Next, odom_sigma, FirstPosition );

    else
        if m > 6
            CameraMeasurementThis = [ data_matrix( IndexOfCurrentStepInDataMatrix(1): IndexOfCurrentStepInDataMatrix(end) , 1 ) , data_matrix( IndexOfCurrentStepInDataMatrix(1): IndexOfCurrentStepInDataMatrix(end) , 3 )];
           [estimation_x,FirstLandmarks] = FEKF_update(estimation_x, CameraMeasurementThis, obsv_sigma, FirstLandmarks);
        end
        estimation_results{i+1} = estimation_x;
    end
end

clearvars -except estimation_results;