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

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

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

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

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

%% Assemble matrix for divergence operator
% Since Dphi is linear, 1-pt quadrature is exact for the integral
% int(div(phi)). We evaluate each basis at the barycenter.
Dphic(:,:,6) = 4/3*(Dlambda(:,:,1) + Dlambda(:,:,2));
Dphic(:,:,1) = 1/3*Dlambda(:,:,1); % (4*lambda(p,1)-1).*Dlambda(:,:,1);
Dphic(:,:,2) = 1/3*Dlambda(:,:,2);
Dphic(:,:,3) = 1/3*Dlambda(:,:,3);
Dphic(:,:,4) = 4/3*(Dlambda(:,:,2) + Dlambda(:,:,3));
Dphic(:,:,5) = 4/3*(Dlambda(:,:,3) + Dlambda(:,:,1));
clear Dlambda
% divergence matrix
Dx = sparse(Np,Nu);
Dy = sparse(Np,Nu);
for i = 1:6
    Dx = Dx + sparse(1:NT,double(elem2dof(:,i)),Dphic(:,1,i).*area,Np,Nu);
    Dy = Dy + sparse(1:NT,double(elem2dof(:,i)),Dphic(:,2,i).*area,Np,Nu);
end
B = -[Dx Dy]; %#ok<*NASGU>
clear Dphi Dx Dy

%% Assemble right hand side
f1 = zeros(Nu,1);
f2 = zeros(Nu,1);
if ~isfield(option,'fquadorder')
    option.fquadorder = 3;   % default order
end
if ~isfield(pde,'f') || (isreal(pde.f) && (pde.f==0))
    pde.f = [];
end
if ~isempty(pde.f)
    % quadrature points in the barycentric coordinate
    [lambda,weight] = quadpts(option.fquadorder);
    % basis values at quadrature points
    phi(:,6) = 4*lambda(:,1).*lambda(:,2);
    phi(:,1) = lambda(:,1).*(2*lambda(:,1)-1);
    phi(:,2) = lambda(:,2).*(2*lambda(:,2)-1);
    phi(:,3) = lambda(:,3).*(2*lambda(:,3)-1);
    phi(:,4) = 4*lambda(:,2).*lambda(:,3);
    phi(:,5) = 4*lambda(:,3).*lambda(:,1);
    nQuad = size(lambda,1);
    ft1 = zeros(NT,6);
    ft2 = zeros(NT,6);
    for p = 1:nQuad
        % quadrature points in the x-y coordinate
        pxy = lambda(p,1)*node(elem(:,1),:) ...
            + lambda(p,2)*node(elem(:,2),:) ...
            + lambda(p,3)*node(elem(:,3),:);
        % function values at quadrature points
        fp = pde.f(pxy);
        for j = 1:6
            ft1(:,j) = ft1(:,j) + fp(:,1).*phi(p,j)*weight(p);
            ft2(:,j) = ft2(:,j) + fp(:,2).*phi(p,j)*weight(p);
        end
    end
    ft1 = ft1.*repmat(area,1,6);
    ft2 = ft2.*repmat(area,1,6);
    f1 = accumarray(elem2dof(:),ft1(:),[Nu 1]);
    f2 = accumarray(elem2dof(:),ft2(:),[Nu 1]);
end

%% Boundary condition
[AD,BD,f,g,u,p,ufreeDof,pDof] = getbdStokesP2P0;

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

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

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

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

%% Output
soln = struct('u',u,'p',p);
eqn = struct('A',AD,'B',BD,'Lap',A,'f',f,'g',g,...
             'edge',edge,'ufreeDof',ufreeDof,'pDof',pDof);
info.assembleTime = assembleTime;

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

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

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

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

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

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

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

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

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

    ufreeDof = [ufreeDof; Nu+ufreeDof];
    end % end of function getbdStokesP2P0
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end
% TODO: impose compatible condition int(pde.g_D*n)=0 numerically.
% NOTE: the data should be compatible, which is not easy. One
% special case is the enclose flow i.e. pde.g_D*n=0 everywhere.


function S2G = HEALPixS2Grid(varargin)
% defines an equispaced spherical grid
%
% Syntax
%   equispacedS2Grid('points',300)
%   equispacedS2Grid('resolution',5*degree)
%
% Options
%  points     - number of points to be generated
%  resolution - resolution of the grid
%  hemisphere - 'lower', 'uper', 'complete', 'sphere', 'identified'
%  minRho     - starting rho angle (default 0)
%  maxRho     - maximum rho angle (default 2*pi)
%  minTheta   - starting theta angle (default 0)
%  maxTheta   - maximum theta angle (default pi)
%
% Flags
%  antipodal  - include <VectorsAxes.html antipodal symmetry>
%  restrict2MinMax - restrict margins to min / max
%  no_center  - ommit point at center
%
% See also
% regularS2Grid plotS2Grid

%  ind = rhoInside(rho,minrho,maxrho) & theta >= mintheta;
%  theta = theta(ind);
%  rho = rho(ind);
%
%  if isnumeric(maxtheta)
%    ind = theta <= maxtheta;
%  else
%    ind = theta <= maxtheta(rho);
%  end
%  theta = theta(ind);
%  rho = rho(ind);

res = get_option(varargin,'resolution',2.5*degree);

n = round(log(1/res)/log(2));

polar = HealpixGenerateSampling(2^n,1);

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

return

% extract options
bounds = getPolarRange(varargin{:});

% get number of points
if check_option(varargin,'points') % calculate resolution
  ntheta = N2ntheta(fix(get_option(varargin,'points')),...
    bounds.VR{2},bounds.VR{4}-bounds.VR{3});%Check this
  res =  (bounds.VR{2}-bounds.VR{1}) / ntheta;
else
  res = get_option(varargin,'resolution',2.5*degree);
  res =  2* bounds.VR{2} / round(2 * bounds.VR{2} / res);
  ntheta = fix(round(2 * (bounds.VR{2}-bounds.VR{1}) / res + ...
    check_option(varargin,'no_center') )/2);
end

% define polar angle
if check_option(varargin,'no_center')
  theta =  (0.5:ntheta-0.5)*res;
else
  theta = (0:ntheta)*res;
end
theta = bounds.VR{1} + theta;

% define azimuth angles
identified = check_option(varargin,'antipodal');

rhGrid = repmat(S1Grid([],bounds.FR{3},bounds.FR{4} + pi,'periodic'),...
  1,length(theta));
for j = 1:length(theta)

  th = theta(j);
  if isappr(th,pi/2) && isappr(bounds.drho,2*pi) && identified
    rhGrid(j).max = bounds.FR{3} + pi;
    rhGrid(j).points = bounds.VR{3} + res*(0.5*mod(j,2)+(0:2*ntheta-1));
  else
    steps = max(round(sin(th) * bounds.drho / bounds.dtheta * ntheta),1);
    rhGrid(j).points = bounds.VR{3} + (0:steps-1 )* bounds.drho /steps + ...
      mod(j,2) * bounds.drho/steps/2;
  end
end

theta = S1Grid(theta,bounds.FR{1},bounds.FR{2});

if identified, opt = {'antipodal'}; else, opt = {}; end
S2G = S2Grid(theta,rhGrid,opt{:});
S2G = S2G.setOption('resolution',res);

% restrict to spherical region if specified
sR = getClass(varargin,'sphericalRegion');
if ~isempty(sR), S2G = S2G.subGrid(sR.checkInside(S2G,'noAntipodal')); end

end

% ---------------------------------------------------------
function ntheta = N2ntheta(N,maxtheta,maxrho)
ntheta = 1;
while calcAnz(ntheta,0,maxtheta,maxrho) < N
  ntheta = ntheta + 1;
end
if (calcAnz(ntheta,0,maxtheta,maxrho) - N) > (N-calcAnz(ntheta-1,0,maxtheta,maxrho))
  ntheta = ntheta-1;
end

end

function c = calcAnz(N,tmin,dt,dr)
c = sum(round(sin(tmin+dt/N*(1:N)) * dr/dt * N));
end


function lik = lik_qgp(varargin)
%LIK_QGP  Create a Quantile Gaussian Process likelihood (utility) structure
%
%  Description
%    LIK = LIK_QGP('PARAM1',VALUE1,'PARAM2,VALUE2,...)
%    creates a quantile gp likelihood structure in which the named
%    parameters have the specified values. Any unspecified
%    parameters are set to default values.
%
%    LIK = LIK_QGP(LIK,'PARAM1',VALUE1,'PARAM2,VALUE2,...)
%    modify a likelihood function structure with the named
%    parameters altered with the specified values.
%
%    Parameters for QGP likelihood function [default]
%      sigma2       - variance [0.1]
%      sigma2_prior - prior for sigma2 [prior_logunif]
%      quantile     - Quantile of interest [0.5]
%
%    Note! If the prior is 'prior_fixed' then the parameter in
%    question is considered fixed and it is not handled in
%    optimization, grid integration, MCMC etc.
%
%    The likelihood is defined as follows:
%                            __ n
%      p(y|f, sigma2, tau) = || i=1 tau*(1-tau)/sigma*exp(-(y-f)/sigma*
%                                 (tau - I(t <= f)))
%
%    where tau is the quantile of interest, sigma is the standard deviation
%    of the distribution and I(t <= f) = 1 if t <= f, 0 otherwise.
%
%    Note that because the form of the likelihood, second order derivatives
%    with respect to latent values are 0. Because this, EP should be used
%    instead of Laplace approximation.
%
%  See also
%    GP_SET, PRIOR_*, LIK_*
%
%   Reference
%     Boukouvalas et al. (2012). Direct Gaussian Process Quantile
%     Regression Using Expectation Propagation. In Proceedings of the
%     29th International Conference on Machine Learning, Edinburgh,
%     Scotland, UK, 2012.
%
%
% Copyright (c) 2012 Ville Tolvanen

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

  ip=inputParser;
  ip.FunctionName = 'LIK_QGP';
  ip.addOptional('lik', [], @isstruct);
  ip.addParamValue('sigma2',0.1, @(x) isscalar(x) && x>0);
  ip.addParamValue('sigma2_prior',prior_logunif(), @(x) isstruct(x) || isempty(x));
  ip.addParamValue('quantile',0.5, @(x) isscalar(x) && x>0 && x<1);
  ip.parse(varargin{:});
  lik=ip.Results.lik;

  if isempty(lik)
    init=true;
    lik.type = 'QGP';
  else
    if ~isfield(lik,'type') || ~isequal(lik.type,'QGP')
      error('First argument does not seem to be a valid likelihood function structure')
    end
    init=false;
  end

  % Initialize parameters
  if init || ~ismember('sigma2',ip.UsingDefaults)
    lik.sigma2 = ip.Results.sigma2;
  end
  if init || ~ismember('quantile',ip.UsingDefaults)
    lik.quantile = ip.Results.quantile;
  end
  % Initialize prior structure
  if init
    lik.p=[];
  end
  if init || ~ismember('sigma2_prior',ip.UsingDefaults)
    lik.p.sigma2=ip.Results.sigma2_prior;
  end
  if init
    % Set the function handles to the subfunctions
    lik.fh.pak = @lik_qgp_pak;
    lik.fh.unpak = @lik_qgp_unpak;
    lik.fh.lp = @lik_qgp_lp;
    lik.fh.lpg = @lik_qgp_lpg;
    lik.fh.ll = @lik_qgp_ll;
    lik.fh.llg = @lik_qgp_llg;
    lik.fh.llg2 = @lik_qgp_llg2;
    lik.fh.llg3 = @lik_qgp_llg3;
    lik.fh.tiltedMoments = @lik_qgp_tiltedMoments;
    lik.fh.siteDeriv = @lik_qgp_siteDeriv;
    lik.fh.predy = @lik_qgp_predy;
    lik.fh.invlink = @lik_qgp_invlink;
    lik.fh.recappend = @lik_qgp_recappend;
  end

end

function [w s h] = lik_qgp_pak(lik)
%LIK_QGP_PAK  Combine likelihood parameters into one vector.
%
%  Description
%    W = LIK_QGP_PAK(LIK) takes a likelihood structure LIK
%    and combines the parameters into a single row vector W.
%    This is a mandatory subfunction used for example in
%    energy and gradient computations.
%
%       w = [ log(lik.sigma2)
%             (hyperparameters of lik.magnSigma2)]'
%
%  See also
%    LIK_QGP_UNPAK

  w = []; s = {}; h=[];
  if ~isempty(lik.p.sigma2)
    w = [w log(lik.sigma2)];
    s = [s; 'log(qgp.sigma2)'];
    h = [h 0];
    % Hyperparameters of sigma2
    [wh, sh, hh] = lik.p.sigma2.fh.pak(lik.p.sigma2);
    w = [w wh];
    s = [s; sh];
    h = [h hh];
  end

end

function [lik, w] = lik_qgp_unpak(lik, w)
%LIK_QGP_UNPAK  Extract likelihood parameters from the vector.
%
%  Description
%    W = LIK_QGP_UNPAK(W, LIK) takes a likelihood structure
%    LIK and extracts the parameters from the vector W to the LIK
%    structure. This is a mandatory subfunction used for example
%    in energy and gradient computations.
%
%    Assignment is inverse of
%       w = [ log(lik.sigma2)
%             (hyperparameters of lik.magnSigma2)]'
%
%  See also
%    LIK_QGP_PAK

  if ~isempty(lik.p.sigma2)
    lik.sigma2 = exp(w(1));
    w = w(2:end);

    % Hyperparameters of sigma2
    [p, w] = lik.p.sigma2.fh.unpak(lik.p.sigma2, w);
    lik.p.sigma2 = p;
  end
end

function lp = lik_qgp_lp(lik)
%LIK_QGP_LP  Evaluate the log prior of likelihood parameters
%
%  Description
%    LP = LIK_QGP_LP(LIK) takes a likelihood structure LIK and
%    returns log(p(th)), where th collects the parameters. This
%    subfunction is needed when there are likelihood parameters.
%
%  See also
%    LIK_QGP_PAK, LIK_QGP_UNPAK, LIK_QGP_G, GP_E

  lp = 0;

  if ~isempty(lik.p.sigma2)
    likp=lik.p;
    lp = likp.sigma2.fh.lp(lik.sigma2, likp.sigma2) + log(lik.sigma2);
  end
end

function lpg = lik_qgp_lpg(lik)
%LIK_QGP_LPG  Evaluate gradient of the log prior with respect
%                  to the parameters.
%
%  Description
%    LPG = LIK_QGP_LPG(LIK) takes a QGP likelihood
%    function structure LIK and returns LPG = d log (p(th))/dth,
%    where th is the vector of parameters. This subfunction is
%    needed when there are likelihood parameters.
%
%  See also
%    LIK_QGP_PAK, LIK_QGP_UNPAK, LIK_QGP_E, GP_G

  lpg = [];

  if ~isempty(lik.p.sigma2)
    likp=lik.p;

    lpgs = likp.sigma2.fh.lpg(lik.sigma2, likp.sigma2);
    lpg = lpgs(1).*lik.sigma2 + 1;
    if length(lpgs) > 1
      lpg = [lpg lpgs(2:end)];
    end
  end
end

function ll = lik_qgp_ll(lik, y, f, z)
%LIK_QGP_LL  Log likelihood
%
%  Description
%    LL = LIK_QGP_LL(LIK, Y, F, Z) takes a likelihood
%    structure LIK, observations Y and latent values F.
%    Returns the log likelihood, log p(y|f,z). This subfunction
%    is needed when using Laplace approximation or MCMC for
%    inference with non-Gaussian likelihoods. This subfunction
%    is also used in information criteria (DIC, WAIC) computations.
%
%  See also
%    LIK_QGP_LLG, LIK_QGP_LLG3, LIK_QGP_LLG2, GPLA_E

  tau=lik.quantile;
  sigma=sqrt(lik.sigma2);
  ll = sum(log(tau*(1-tau)/sigma) - (y-f)./sigma.*(tau-(y<=f)));
end

function llg = lik_qgp_llg(lik, y, f, param, z)
%LIK_QGP_LLG  Gradient of the log likelihood
%
%  Description
%    LLG = LIK_QGP_LLG(LIK, Y, F, PARAM) takes a likelihood
%    structure LIK, observations Y and latent values F. Returns
%    the gradient of the log likelihood with respect to PARAM.
%    At the moment PARAM can be 'param' or 'latent'. This subfunction
%    is needed when using Laplace approximation or MCMC for inference
%    with non-Gaussian likelihoods.
%
%  See also
%    LIK_QGP_LL, LIK_QGP_LLG2, LIK_QGP_LLG3, GPLA_E

  tau=lik.quantile;
  sigma2=sqrt(lik.sigma2);
  switch param
    case 'param'
      llg = sum(-1/(2.*sigma2) + (y-f)./(2.*sigma2^(3/2)).*(tau-(y<=f)));

      % correction for the log transformation
      llg = llg.*lik.sigma2;
    case 'latent'
      llg = (tau-(y<=f))/sqrt(sigma2);
  end
end

function llg2 = lik_qgp_llg2(lik, y, f, param, z)
%LIK_QGP_LLG2  Second gradients of the log likelihood
%
%  Description
%    LLG2 = LIK_QGP_LLG2(LIK, Y, F, PARAM) takes a likelihood
%    structure LIK, observations Y and latent values F. Returns
%    the Hessian of the log likelihood with respect to PARAM.
%    At the moment PARAM can be 'param' or 'latent'. LLG2 is
%    a vector with diagonal elements of the Hessian matrix
%    (off diagonals are zero). This subfunction is needed
%    when using Laplace approximation or EP for inference
%    with non-Gaussian likelihoods.
%
%  See also
%    LIK_QGP_LL, LIK_QGP_LLG, LIK_QGP_LLG3, GPLA_E

  tau=lik.quantile;
  sigma2=lik.sigma2;
  switch param
    case 'param'
      llg2 = sum(1/(2*sigma2^2) - 3.*(tau-(y<=f)).*(y-f)./(4.*sigma2^(5/2)));

      % correction due to the log transformation
      llg2 = llg2.*lik.sigma2;
    case 'latent'
      llg2 = zeros(size(f));
    case 'latent+param'
      llg2 = -(tau-(y<=f))./(2*sigma2^(3/2));

      % correction due to the log transformation
      llg2 = llg2.*lik.disper;
  end
end

function llg3 = lik_qgp_llg3(lik, y, f, param, z)
%LIK_QGP_LLG3  Third gradients of the log likelihood
%
%  Description
%    LLG3 = LIK_QGP_LLG3(LIK, Y, F, PARAM) takes a likelihood
%    structure LIK, observations Y and latent values F and
%    returns the third gradients of the log likelihood with
%    respect to PARAM. At the moment PARAM can be 'param' or
%    'latent'. LLG3 is a vector with third gradients. This
%    subfunction is needed when using Laplace approximation for
%    inference with non-Gaussian likelihoods.
%
%  See also
%    LIK_QGP_LL, LIK_QGP_LLG, LIK_QGP_LLG2, GPLA_E, GPLA_G

  tau=lik.quantile;
  sigma2=lik.sigma2;
  switch param
    case 'param'
      llg3 = sum(-1/sigma2^3 + 15.*(tau-(y<=f)).*(y-f)./(8.*sigma2^(7/2)));
    case 'latent'
      llg3 = 0;
    case 'latent2+param'
      llg3 = 0;

      % correction due to the log transformation
      llg3 = llg3.*lik.sigma2;
  end
end

function [logM_0, m_1, sigm2hati1] = lik_qgp_tiltedMoments(lik, y, i1, sigma2_i, myy_i, z)
%LIK_QGP_TILTEDMOMENTS  Returns the marginal moments for EP algorithm
%
%  Description
%    [M_0, M_1, M2] = LIK_QGP_TILTEDMOMENTS(LIK, Y, I, S2,
%    MYY, Z) takes a likelihood structure LIK, observations
%    Y, index I and cavity variance S2 and mean MYY. Returns
%    the zeroth moment M_0, mean M_1 and variance M_2 of the
%    posterior marginal (see Rasmussen and Williams (2006):
%    Gaussian processes for Machine Learning, page 55). This
%    subfunction is needed when using EP for inference with
%    non-Gaussian likelihoods.
%
%  See also
%    GPEP_E

  yy = y(i1);
  sigma2 = lik.sigma2;
  tau=lik.quantile;
  logM_0=zeros(size(yy));
  m_1=zeros(size(yy));
  sigm2hati1=zeros(size(yy));

  for i=1:length(i1)
    if isscalar(sigma2_i)
      sigma2_ii = sigma2_i;
    else
      sigma2_ii = sigma2_i(i);
    end

    % get a function handle of an unnormalized tilted distribution
    % (likelihood * cavity = Quantile-GP * Gaussian)
    % and useful integration limits
    [tf,minf,maxf]=init_qgp_norm(yy(i),myy_i(i),sigma2_ii,sigma2,tau);

    % Integrate with quadrature
    RTOL = 1.e-6;
    ATOL = 1.e-10;
    [m_0, m_1(i), m_2] = quad_moments(tf, minf, maxf, RTOL, ATOL);
    if isnan(m_0)
      logM_0=NaN;
      return
    end
    sigm2hati1(i) = m_2 - m_1(i).^2;

    % If the second central moment is less than cavity variance
    % integrate more precisely. Theoretically for log-concave
    % likelihood should be sigm2hati1 < sigm2_i.
    if sigm2hati1(i) >= sigma2_ii
      ATOL = ATOL.^2;
      RTOL = RTOL.^2;
      [m_0, m_1(i), m_2] = quad_moments(tf, minf, maxf, RTOL, ATOL);
      sigm2hati1(i) = m_2 - m_1(i).^2;
      if sigm2hati1(i) >= sigma2_ii
        error('lik_qgp_tilted_moments: sigm2hati1 >= sigm2_i');
      end
    end
    logM_0(i) = log(m_0);
  end
end

function [g_i] = lik_qgp_siteDeriv(lik, y, i1, sigm2_i, myy_i, z)
%LIK_QGP_SITEDERIV  Evaluate the expectation of the gradient
%                      of the log likelihood term with respect
%                      to the likelihood parameters for EP
%
%  Description [M_0, M_1, M2] =
%    LIK_QGP_SITEDERIV(LIK, Y, I, S2, MYY, Z) takes a
%    likelihood structure LIK, observations Y, index I
%    and cavity variance S2 and mean MYY. Returns E_f
%    [d log p(y_i|f_i) /d a], where a is the likelihood
%    parameter and the expectation is over the marginal posterior.
%    This term is needed when evaluating the gradients of
%    the marginal likelihood estimate Z_EP with respect to
%    the likelihood parameters (see Seeger (2008):
%    Expectation propagation for exponential families).This
%    subfunction is needed when using EP for inference with
%    non-Gaussian likelihoods and there are likelihood parameters.
%
%  See also
%    GPEP_G

  yy = y(i1);
  sigma2=lik.sigma2;
  tau=lik.quantile;

  % get a function handle of an unnormalized tilted distribution
  % (likelihood * cavity = Quantile-GP * Gaussian)
  % and useful integration limits
  [tf,minf,maxf]=init_qgp_norm(yy,myy_i,sigm2_i,sigma2,tau);
  % additionally get function handle for the derivative
  td = @deriv;

  % Integrate with quadgk
  [m_0, fhncnt] = quadgk(tf, minf, maxf);
  [g_i, fhncnt] = quadgk(@(f) td(f).*tf(f)./m_0, minf, maxf);
  g_i = g_i.*sigma2;

  function g = deriv(f)

    g = -1/(2.*sigma2) + (yy-f)./(2.*sigma2^(3/2)).*(tau-(yy<=f));

  end
end

function [lpy, Ey, Vary] = lik_qgp_predy(lik, Ef, Varf, yt, zt)
%LIK_QGP_PREDY  Returns the predictive mean, variance and density of y
%
%  Description
%    LPY = LIK_QGP_PREDY(LIK, EF, VARF YT, ZT)
%    Returns logarithm of the predictive density PY of YT, that is
%        p(yt | zt) = \int p(yt | f, zt) p(f|y) df.
%    This subfunction is needed when computing posterior predictive
%    distributions for future observations.
%
%    [LPY, EY, VARY] = LIK_QGP_PREDY(LIK, EF, VARF) takes a
%    likelihood structure LIK, posterior mean EF and posterior
%    Variance VARF of the latent variable and returns the
%    posterior predictive mean EY and variance VARY of the
%    observations related to the latent variables. This
%    subfunction is needed when computing posterior predictive
%    distributions for future observations.
%

%
%  See also
%    GPLA_PRED, GPEP_PRED, GPMC_PRED

  sigma2=lik.sigma2;
  tau=lik.quantile;

  Ey=[];
  Vary=[];

  % Evaluate the posterior predictive densities of the given observations
  lpy = zeros(length(yt),1);
  if (size(Ef,2) > 1) && (size(Ef,2) > 1) && size(yt,2) == 1
    % Approximate integral with sum of grid points when using corrected
    % marginal posterior
    for i1=1:length(yt)
      py = arrayfun(@(f) exp(lik.fh.ll(lik, yt(i1), f, [])), Ef(i1,:));
      pf = Varf(i1,:)./sum(Varf(i1,:));
      lpy(i1) = log(sum(py.*pf));
    end
  else
    for i1=1:length(yt)
      % get a function handle of the likelihood times posterior
      % (likelihood * posterior = Quantile-GP * Gaussian)
      % and useful integration limits
      [pdf,minf,maxf]=init_qgp_norm(...
        yt(i1),Ef(i1),Varf(i1),sigma2, tau);
      % integrate over the f to get posterior predictive distribution
      lpy(i1) = log(quadgk(pdf, minf, maxf));
    end
  end
end

function [df,minf,maxf] = init_qgp_norm(yy,myy_i,sigm2_i,sigma2,tau)
%INIT_QGP_NORM
%
%  Description
%    Return function handle to a function evaluating
%    Quantile-GP * Gaussian which is used for evaluating
%    (likelihood * cavity) or (likelihood * posterior) Return
%    also useful limits for integration. This is private function
%    for lik_qgp. This subfunction is needed by subfunctions
%    tiltedMoments, siteDeriv and predy.
%
%  See also
%    LIK_QGP_TILTEDMOMENTS, LIK_QGP_SITEDERIV,
%    LIK_QGP_PREDY

  sigma=sqrt(sigma2);
% avoid repetitive evaluation of constant part
  ldconst = log(tau*(1-tau)/sigma) ...
            - log(sigm2_i)/2 - log(2*pi)/2;
  % Create function handle for the function to be integrated
  df = @qgp_norm;
  % use log to avoid underflow, and derivates for faster search
  ld = @log_qgp_norm;
  ldg = @log_qgp_norm_g;
%   ldg2 = @log_qgp_norm_g2;

  % Set the limits for integration
  % Quantile-GP likelihood is log-concave so the qgp_norm
  % function is unimodal, which makes things easier
  if yy==0
    % with yy==0, the mode of the likelihood is not defined
    % use the mode of the Gaussian (cavity or posterior) as a first guess
    modef = myy_i;
  else
    % use precision weighted mean of the Gaussian approximation
    % of the Quantile-GP likelihood and Gaussian
    modef = (myy_i/sigm2_i + yy/sigma2)/(1/sigm2_i + 1/sigma2);
  end
  % find the mode of the integrand using Newton iterations
  % few iterations is enough, since the first guess in the right direction
  niter=8;       % number of Newton iterations

  minf=modef-6*sigm2_i;
  while ldg(minf) < 0
    minf=minf-2*sigm2_i;
  end
  maxf=modef+6*sigm2_i;
  while ldg(maxf) > 0
    maxf=maxf+2*sigm2_i;
  end
  for ni=1:niter
%     h=ldg2(modef);
    modef=0.5*(minf+maxf);
    if ldg(modef) < 0
      maxf=modef;
    else
      minf=modef;
    end
  end
  % integrand limits based on Gaussian approximation at mode
  minf=modef-6*sqrt(sigm2_i);
  maxf=modef+6*sqrt(sigm2_i);
  modeld=ld(modef);
  iter=0;
  % check that density at end points is low enough
  lddiff=20; % min difference in log-density between mode and end-points
  minld=ld(minf);
  step=1;
  while minld>(modeld-lddiff)
    minf=minf-step*sqrt(sigm2_i);
    minld=ld(minf);
    iter=iter+1;
    step=step*2;
    if iter>100
      error(['lik_qgp -> init_qgp_norm: ' ...
             'integration interval minimun not found ' ...
             'even after looking hard!'])
    end
  end
  maxld=ld(maxf);
  iter=0;
  step=1;
  while maxld>(modeld-lddiff)
    maxf=maxf+step*sqrt(sigm2_i);
    maxld=ld(maxf);
    iter=iter+1;
    step=step*2;
    if iter>100
      error(['lik_qgp -> init_qgp_norm: ' ...
             'integration interval maximun not found ' ...
             'even after looking hard!'])
    end
  end

  function integrand = qgp_norm(f)
  % Quantile-GP * Gaussian
    integrand = exp(ldconst ...
                    -(yy-f)./sqrt(sigma2).*(tau-(yy<=f)) ...
                    -0.5*(f-myy_i).^2./sigm2_i);
  end

  function log_int = log_qgp_norm(f)
  % log(Quantile-GP * Gaussian)
  % log_qgp_norm is used to avoid underflow when searching
  % integration interval
    log_int = ldconst...
              -(yy-f)./sqrt(sigma2).*(tau-(yy<=f)) ...
              -0.5*(f-myy_i).^2./sigm2_i;
  end

  function g = log_qgp_norm_g(f)
  % d/df log(Quantile-GP * Gaussian)
  % derivative of log_qgp_norm
    g = (tau-(yy<=f))/sqrt(sigma2) ...
        + (myy_i - f)./sigm2_i;
  end

end

function mu = lik_qgp_invlink(lik, f, z)
%LIK_QGP_INVLINK  Returns values of inverse link function
%
%  Description
%    MU = LIK_QGP_INVLINK(LIK, F) takes a likelihood structure LIK and
%    latent values F and returns the values MU of inverse link function.
%    This subfunction is needed when using function gp_predprctmu.
%
%     See also
%     LIK_QGP_LL, LIK_QGP_PREDY

  mu = f;
end

function reclik = lik_qgp_recappend(reclik, ri, lik)
%RECAPPEND  Append the parameters to the record
%
%  Description
%    RECLIK = LIK_QGP_RECAPPEND(RECLIK, RI, LIK) takes a
%    likelihood record structure RECLIK, record index RI and
%    likelihood structure LIK with the current MCMC samples of
%    the parameters. Returns RECLIK which contains all the old
%    samples and the current samples from LIK.  This subfunction
%    is needed when using MCMC sampling (gp_mc).
%
%  See also
%    GP_MC

  if nargin == 2
    % Initialize the record
    reclik.type = 'Quantile-GP';

    % Initialize parameter
    reclik.sigma2 = [];

    % Set the function handles
    reclik.fh.pak = @lik_qgp_pak;
    reclik.fh.unpak = @lik_qgp_unpak;
    reclik.fh.lp = @lik_qgp_lp;
    reclik.fh.lpg = @lik_qgp_lpg;
    reclik.fh.ll = @lik_qgp_ll;
    reclik.fh.llg = @lik_qgp_llg;
    reclik.fh.llg2 = @lik_qgp_llg2;
    reclik.fh.llg3 = @lik_qgp_llg3;
    reclik.fh.tiltedMoments = @lik_qgp_tiltedMoments;
    reclik.fh.predy = @lik_qgp_predy;
    reclik.fh.invlink = @lik_qgp_invlink;
    reclik.fh.recappend = @lik_qgp_recappend;
    reclik.p=[];
    reclik.p.sigma2=[];
    if ~isempty(ri.p.sigma2)
      reclik.p.sigma2 = ri.p.sigma2;
    end
  else

    % Append to the record
    reclik.sigma2(ri,:)=lik.sigma2;
    if ~isempty(lik.p.sigma2)
      reclik.p.sigma2 = lik.p.sigma2.fh.recappend(reclik.p.sigma2, ri, lik.p.sigma2);
    end
    reclik.quantile = lik.quantile;
  end
end


function [err_p,elerr_p] = diffpost_bc(aez,fez,elerror,xy,ev,ebound);
%diffpost_bc postprocesses local Poisson error estimator
%   [err_p,elerr_p] = diffpost_bc(aez,fez,elerror,xy,ev,ebound);
%   input
%          aez       elementwise Poisson problem matrices
%          fez       elementwise rhs vectors
%          elerror   elementwise error estimate (without BC imposition)
%          xy        vertex coordinate vector
%          ev        element mapping matrix
%          ebound    element edge boundary matrix
%   output
%          err_p     global error estimate
%          elerr_p   elementwise error estimate
%   IFISS function: DJS; 4 March 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
      x=xy(:,1); y=xy(:,2);
      nel=length(ev(:,1));
      lev=[ev,ev(:,1)]; elerr_p=elerror;
%
% recompute contributions from elements with Dirichlet boundaries
      nbde=length(ebound(:,1));
      ebdy = zeros(nel,1);
      edge = zeros(nel,1);
% isolate boundary elements
      for el = 1:nbde
      ee = ebound(el,1);
      ebdy(ee) = ebdy(ee)+1; edge(ee)=ebound(el,2);
      end
%
% two edge elements
      k2=find(ebdy==2);
      nel2b=length(k2);
% loop over two edge elements
      for el = 1:nel2b
      el2e=k2(el);
      kk=find(ebound(:,1) == el2e);
      edges=ebound(kk,2);
% set up original matrix and RHS vector
	  ae=squeeze(aez(el2e,1:5,1:5));
      fe=fez(el2e,:)';
% set up local coordinates and impose interpolated error as Dirichlet bc
      xl=x(lev(el2e,:)); yl=y(lev(el2e,:));
      [bae,fe] = localbc_p(ae,fe,edges,xl,yl);
% solve local problem
      err=bae\fe;
      elerr_p(el2e,1) = err'*fe;
      end
% end of element loop
%
% one edge elements
      k1=find(ebdy==1);
      nel1b=length(k1);
% loop over one edge elements
      for el = 1:nel1b
      el1e=k1(el);
      kk=find(ebound(:,1) == el1e);
      edges=ebound(kk,2);
% set up original matrix and RHS vector
      fe=fez(el1e,:)';
	  ae=squeeze(aez(el1e,1:5,1:5));
% set up local coordinates and impose interpolated error as Dirichlet bc
      xl=x(lev(el1e,:)); yl=y(lev(el1e,:));
      [bae,fe] = localbc_p(ae,fe,edges,xl,yl);
% solve local problem
      err=bae\fe;
      elerr_p(el1e,1) = err'*fe;
      end
% end of element loop
%
      err_p = sqrt(sum(elerr_p));
      elerr_p = sqrt(elerr_p);
      fprintf('done\n')
      fprintf('estimated global error (in energy):  %10.6e\n',err_p)
 return


function p = randperm(n)
%RANDPERM Random permutation.
%	RANDPERM(n) is a random permutation of 1:n.
[ignore,p] = sort(rand(1,n));


function [outerproduct] = outer(tensor1, tensor2, squeezedimensions)
%C = OUTER(A, B, squeezedimensions) Computes the outer product
%C(i[1],...,i[m],j[1],...,j[n]) = A(i[1],...,i[m]) * B(j[1],...j[n]).
%Discards superfluous singleton dimensions if squeezedimensions ~= 0.
%
%Note: thanks to Emese Toth for suggesting the algorithm used here.
%
%Wynton Moore, January 2006

%store initial dimensions
dim1=size(tensor1);dim2=size(tensor2);

%evaluate
outerproduct=reshape(reshape(tensor1, [], 1)*reshape(tensor2, 1, []), [dim1 dim2]);

%discard superfluous singleton dimensions
if squeezedimensions
    outerproduct=squeeze(outerproduct);
end


function dawson_test ( )

%*****************************************************************************80
%
%% DAWSON_TEST tests R4_DAWSON and R8_DAWSON.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2011
%
%  Author:
%
%    John Burkardt
%
  addpath ( '../test_values' );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'DAWSON_TEST:\n' );
  fprintf ( 1, '  Test DAWSON_VALUES, R4_DAWSON, R8_DAWSON.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '             X      DAWSON(X)\n' );
  fprintf ( 1, '                 R4_DAWSON(X)         Diff\n' );
  fprintf ( 1, '                 R8_DAWSON(X)         Diff\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, fx1 ] = dawson_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fx2 = r4_dawson ( single ( x ) );
    fx3 = r8_dawson ( x );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  %14.4f  %14.6g\n', x, fx1 );
    fprintf ( 1, '                  %14.6g  %14.6g\n', fx2, abs ( fx1 - fx2 ) );
    fprintf ( 1, '                  %14.6g  %14.6g\n', fx3, abs ( fx1 - fx3 ) );

  end

  rmpath ( '../test_values' );

  return
end


% Driver script for solving the 1D Euler equations
Globals1D;

% Polynomial order used for approximation
N = 4;

% Generate simple mesh
[Nv, VX, K, EToV] = MeshGen1D(0, 2*pi, 80);

% Initialize solver and construct grid and metric
StartUp1D;

% Set up initial conditions -- Sod's problem
MassMatrix = inv(V')/V;
%cx = ones(Np,1)*sum(MassMatrix*x,1)/2;

%u = ones(Np,K).*( sin(2*pi*cx) );
u0 = 0.5 + sin(x);
FinalTime = 1.5;

% Solve Problem
[u] = iBurgers1D(u0,FinalTime);
snapnow;

% Plot Figure
%plotrange=[0,2*pi,min(min(u0))-0.2,max(max(u0))+0.2];
%plot(x,u,x,u0,'-+'); axis(plotrange); ylabel('u(x)'); xlabel('x');


function V = lumScotopic(imgXYZ)
%
%       V = lumScotopic(imgXYZ)
%
%       This function calculates the scotopic luminance
%
%       input:
%           img: an image in the XYZ color space
%
%       output:
%           V: scotopic luminance approximation by Larson, Rushmeier and Piatko 1997
%
%     Copyright (C) 2011-14  Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

check3Color(imgXYZ);

eps = 1e-6;
t = (imgXYZ(:,:,2) + imgXYZ(:,:,3)) ./ (imgXYZ(:,:,1) + eps);
V = imgXYZ(:,:,2) .* (1.33 * (1.0 + t) - 1.68);

end


function [M_c,C_c_nu_c,g_c,Jc_c,dJc_nu_c,nu_c] = fromFloatingToCentroidalDynamics(M, h, g, Jc, dJc_nu, nu, T, dT)
%FROMFLOATINGTOCENTROIDALDYNAMICS  converts dynamic equation parameters to the
%                       corresponding values in centroidal frame of reference.
%
% [M_c,C_c_nu_c,g_c,Jc_c,dJc_nu_c,nu_c] = FROMFLOATINGTOCENTROIDALDYNAMICS
% (M, h, g, Jc, dJc_nu, nu, T, dT) takes as an input the robot dynamics.
%  The output is the robot dynamics in centroidal frame of reference.
%

% ------------Initialization----------------
ndof   = size(g,1)-6;
invT   = eye(ndof+6)/T;
invTt  = eye(ndof+6)/(T');

%% Control terms conversion
% mass matrix
M_c            = invTt*M*invT;
M_c(1:6,7:end) = zeros(6,ndof);
M_c(7:end,1:6) = zeros(ndof,6);
M_c(1:3,1:3)   = M(1,1)*eye(3);
M_c(1:3,4:6)   = zeros(3);
M_c(4:6,1:3)   = zeros(3);
Mb             = M(1:6,1:6);
Mbj            = M(1:6,7:end);

nu_c           = T*nu;
gravAcc        = norm(invTt*g)/M(1,1);
e3             = zeros(ndof+6,1);
e3(3)          = 1;
g_c            = M(1,1)*gravAcc*e3;

%coriolis terms
C_nu           = h - g;
C_nu_j         = C_nu(7:end);
C_nu_b         = C_nu(1:6);
C_c_nu_c_dT    = invTt*C_nu - M_c*dT*nu;
C_c_nu_c       = [ zeros(3,1);
                  C_c_nu_c_dT(4:6);
                  C_nu_j-(Mbj')*(Mb\C_nu_b)];

%new dT*nu computation for Jacobian
dT_nu          = M_c\(C_nu-C_c_nu_c); % M_c\(inv(T)*C_nu-C_c_nu_c);
Jc_c           = Jc*invT;
dJc_nu_c       = dJc_nu - Jc*invT*dT_nu;

end


function result = cross_entropy(head_embedding, tail_embedding, head, tail, weights, a, b, same_embedding)
%CROSS_ENTROPY Given a distance for each 1-simplex in low-dimensional space
% and the original weights of the 1-simplices in high-dimensional space,
% compute the approximation to the cross-entropy between the two simplicial
% complexes. This calculation uses the modified smooth formula Phi for
% low-dimensional weight that is used in the stochastic gradient descent.
%
% result = cross_entropy(head_embedding, tail_embedding, head, tail, weights, a, b, same_embedding)
%
% Parameters
% ----------
% dists: array of size (n_1_simplices, 1)
%     The current distance between the two endpoints of the 1-simplex in
%     low-dimensional Euclidean space.
%
% weights: array of size (n_1_simplices, 1)
%     The original weights assigned to the 1-simplices in high-dimensional
%     space.
%
% a: double
%     Parameter of differentiable approximation of right adjoint functor.
%
% b: double
%     Parameter of differentiable approximation of right adjoint functor.
%
% Returns
% -------
% result: double
%     The total approximated cross entropy between the two simplicial complexes.
%
% See also: NEG_SAMPLING_OBJECTIVE
%
%   AUTHORSHIP
%   Math Lead & Primary Developer:  Connor Meehan <connor.gw.meehan@gmail.com>
%   Secondary Developer: Stephen Meehan <swmeehan@stanford.edu>
%   Bioinformatics Lead:  Wayne Moore <wmoore@stanford.edu>
%   Provided by the Herzenberg Lab at Stanford University
%   License: BSD 3 clause
%
    n1 = size(head_embedding, 1);
    n2 = size(tail_embedding, 1);

    if n1*n2 > 1e8
        error('HALTED: MATLAB usually freezes for embeddings this large.');
    end

    full_dists = pdist2(head_embedding, tail_embedding);
    full_weights = full(sparse(head, tail, weights, n1, n2));
    if same_embedding
        full_weights = full_weights + eye(n1);
    end
    Phi = ones(size(full_weights))./(1 + a*(full_dists.^(2*b)));
    fw0 = full_weights == 0;
    fw1 = full_weights == 1;
    other = ~fw0 & ~fw1;
    Phi_summands = zeros(size(full_weights));
    Phi_summands(fw0) = log(1-Phi(fw0));
    Phi_summands(fw1) = log(Phi(fw1));
    Phi_summands(other) = full_weights(other).*log(Phi(other)) + (1-full_weights(other)).*log(1-Phi(other));

    result = -sum(sum(Phi_summands));
end


% input: rvqs0: r s in s0, v s in s0, q s0 2 s, note s0 is a fixed local
% world frame, also called w-frame
% rqs02e: rs0 in e, q s0 2 e, a, acceleration by IMU, w, angular rate by
% gyro, dt, sample interval from the last measurement to the current
% measurement.
% gwomegaw, $g^w$, gravity in local world frame, 3-vector, and $w_{iw}^w$, 3-vector,
% if all of them set 0, this amount to integration in inertial frame

function rvqs0_new=strapdown_local_quat_bias(rvqs0, rqs02e, a, w, dt, gwomegaw)
rvqs0_new=zeros(size(rvqs0));
gs0=gwomegaw (1:3); % gravity in s0 frame
wie2s0=gwomegaw (4:6);
%Update attitude
%% method (1) second order integration
qe=rvec2quat_v000(wie2s0*dt);
vr_a=quatmult_v000(rvqs0(7:10),qe);
qb=rvec2quat_v000(-w*dt);
rvqs0_new(7:10)=quatmult_v000(qb,vr_a);
%% method (2) Runge-Kutta 4th order integration, empirically, this sometimes
% gives worse result than second order integration
% wie2s=quatrot_v000(rvqs0(7:10),wie2s0,0);
% omega=zeros(4,2);
% omega(1,2)=dt;
% omega(2:4,1)=w-wie2s;
% omega(2:4,2)=lastw-wie2s;
% qs2s0=rotationRK4( omega, [rvqs0(7); -rvqs0(8:10)]);
% rvqs0_new(7:10)=[qs2s0(1); -qs2s0(2:4)];

%% better velocity and position integration than first order rectanglar rule
%Update Velocity
vel_inc1=(quatrot_v000(rvqs0(7:10),a*dt,1)+quatrot_v000(rvqs0_new(7:10),a*dt,1))/2;
vel_inc2=(gs0+2*cross(rvqs0(4:6),wie2s0))*dt;
rvqs0_new(4:6)=rvqs0(4:6)+vel_inc1+vel_inc2;
%Update_pos
rvqs0_new(1:3)=rvqs0(1:3)+(rvqs0(4:6)+rvqs0_new(4:6))*dt/2;


function imdb = getCifarImdb(opts)
% -------------------------------------------------------------------------
% Preapre the imdb structure, returns image data with mean image subtracted
unpackPath = fullfile(opts.dataDir, 'cifar-10-batches-mat');
files = [arrayfun(@(n) sprintf('data_batch_%d.mat', n), 1:5, 'UniformOutput', false) ...
  {'test_batch.mat'}];
files = cellfun(@(fn) fullfile(unpackPath, fn), files, 'UniformOutput', false);
file_set = uint8([ones(1, 5), 3]);

if any(cellfun(@(fn) ~exist(fn, 'file'), files))
  url = 'http://www.cs.toronto.edu/~kriz/cifar-10-matlab.tar.gz' ;
  fprintf('downloading %s\n', url) ;
  untar(url, opts.dataDir) ;
end

data = cell(1, numel(files));
labels = cell(1, numel(files));
sets = cell(1, numel(files));
for fi = 1:numel(files)
  fd = load(files{fi}) ;
  data{fi} = permute(reshape(fd.data',32,32,3,[]),[2 1 3 4]) ;
  labels{fi} = fd.labels' + 1; % Index from 1
  sets{fi} = repmat(file_set(fi), size(labels{fi}));
end

set = cat(2, sets{:});
data = single(cat(4, data{:}));

% remove mean in any case
dataMean = mean(data(:,:,:,set == 1), 4);
data = bsxfun(@minus, data, dataMean);

% normalize by image mean and std as suggested in `An Analysis of
% Single-Layer Networks in Unsupervised Feature Learning` Adam
% Coates, Honglak Lee, Andrew Y. Ng

if isfield(opts,'contrastNormalization')&&opts.contrastNormalization
  z = reshape(data,[],60000) ;
  z = bsxfun(@minus, z, mean(z,1)) ;
  n = std(z,0,1) ;
  z = bsxfun(@times, z, mean(n) ./ max(n, 40)) ;
  data = reshape(z, 32, 32, 3, []) ;
end

if isfield(opts,'whitenData') &&opts.whitenData
  z = reshape(data,[],60000) ;
  W = z(:,set == 1)*z(:,set == 1)'/60000 ;
  [V,D] = eig(W) ;
  % the scale is selected to approximately preserve the norm of W
  d2 = diag(D) ;
  en = sqrt(mean(d2)) ;
  z = V*diag(en./max(sqrt(d2), 10))*V'*z ;
  data = reshape(z, 32, 32, 3, []) ;
end

clNames = load(fullfile(unpackPath, 'batches.meta.mat'));

imdb.images.data = data ;
imdb.images.labels = single(cat(2, labels{:})) ;
imdb.images.set = set;
imdb.meta.sets = {'train', 'val', 'test'} ;
imdb.meta.classes = clNames.label_names;


function [x, resvec, lsvec] = dense_overdetermined_lsqr(A, b, R, tol, maxit)
% x = dense_overdetermined_lsqr(A, b, R, tol, maxit)
% [x, resvec, lsvec] = dense_overdetermined_lsqr(A, b, R, tol, maxit)
%
% LSQR on a dense overdetermined matrix with a dense preconditioner R.
% Solves x = arg min (A*x - b)
% Inputs:
%   A, b
%   R - Upper triangular preconditioner. kappa(A * inv(R)) governs
%       the convergence.
%   tol - tolerance. Stop when
%      norm(inv(R') * A' * r) / (norm(A * inv(R), 'fro') * norm(r)) < tol
%   maxit - Maximum number of iterations
%
% Outputs:
%   x
%   optional: resvec, lsvec - values of norm(A' * r) and norm(r).
%
% 6-December 2009, Version 1.3
% Copyright (C) 2009, Haim Avron and Sivan Toledo.


function BandTrader(sys)
%BANDTRADER - Band Trading Strategy
%
%  This MATLAB function implements a simple band trading strategy. A band
%  consists of two lines that form the upper and lower boundaries of the band.
%  The upper and lower boundaries are used to to enter and exit trades.
%  For example, if prices fall below the lower boundary a buy signal is
%  generated.
%
%  Requirements:
%  - MATLAB 2012b
%  - TA Developer Toolbox (http://www.tadeveloper.com)
%
%  Please refer to http://www.tadeveloper.com/docs/80-matlab-algo-trading
%  for a full description of the strategy
%

% custom trading parameters
if sys.TradingParameters.isKey('movingAverage'),
    movingAverage = sys.TradingParameters('movingAverage');
else
    movingAverage = 10; % default value if unset
end;
if sys.TradingParameters.isKey('bandWidth'),
    bandWidth = sys.TradingParameters('bandWidth');
else
    bandWidth = 2; % default value if unset
end;

% Construct the center line
Average = (sys.High + sys.Low)/2;
CenterLine = Ema(Average, movingAverage);
PlotDataSeries(sys, CenterLine, 'blue', 'solid', 2, '');

% Construct the bands
AverageTrueRange = Atr(sys.BarData, movingAverage);
UpperBand = CenterLine + bandWidth * AverageTrueRange;
LowerBand = CenterLine - bandWidth * AverageTrueRange;
PlotDataSeries(sys, UpperBand, 'magenta', 'solid', 2, '');
PlotDataSeries(sys, LowerBand, 'magenta', 'solid', 2, '');

% Entry and Exit Signals
LongSignal = sys.Close < LowerBand;
ShortSignal = sys.Close > UpperBand;
AddLongSignal(sys, LongSignal);
AddShortSignal(sys, ShortSignal);

end


function q=qrdivide(q1,q2)
%QRDIVIDE divdes two real quaternions q=[q1,q2]
%
% Inputs:
%
%     q1(4,1), q2(4,1)  Two real quaternions in the form [r, i, j, k]' where i^2=j^2=k^2=ijk=-1
%
% Outputs:
%
%     q(4,1)   Quotient of q1/q2 such that q1=q*q2.
%              Note that q*q2 ~= q2*q since quaternion multiplication does not commute.

%      Copyright (C) Mike Brookes 2000-2008
%      Version: $Id: qrdivide.m,v 1.1 2008/12/03 10:07:53 dmb Exp $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
persistent a b c d
if isempty(a)
    a=[5 8 9 10 15 13];
    b=[6 7 11 12 14 16];
    c=[1 2 3 4 6 7 11 12 16 14];
    d=[1 2 3 4 5 8 9 10 13 15];
end
if nargin<2
    %    just take the inverse of the only input argument
    q=q1/(q1'*q1);
    q(2:4)=-q(2:4);
else
    %    invert q2 and do a multiply
    q=q2/(q2'*q2);
    q(2:4)=-q(2:4);
    t=q1*q.';
    s=zeros(4,4);
    s(a)=-t(b);
    s(c)=t(d);
    q=sum(s,2);
end


function diff_vol = dtiSmoothAnisoPM(vol, num_iter, delta_t, kappa, option, voxel_spacing)
%ANISODIFF2D Conventional anisotropic diffusion
%   DIFF_VOL = ANISODIFF3D(VOL, NUM_ITER, DELTA_T, KAPPA, OPTION, VOXEL_SPACING) perfoms
%   conventional anisotropic diffusion (Perona & Malik) upon a stack of gray scale images.
%   A 3D network structure of 26 neighboring nodes is considered for diffusion conduction.
%
%       ARGUMENT DESCRIPTION:
%               VOL      - gray scale volume data (MxNxP).
%               NUM_ITER - number of iterations.
%               DELTA_T  - integration constant (0 <= delta_t <= 3/44).
%                          Usually, due to numerical stability this
%                          parameter is set to its maximum value.
%               KAPPA    - gradient modulus threshold that controls the conduction.
%               OPTION   - conduction coefficient functions proposed by Perona & Malik:
%                          1 - c(x,y,z,t) = exp(-(nablaI/kappa).^2),
%                              privileges high-contrast edges over low-contrast ones.
%                          2 - c(x,y,z,t) = 1./(1 + (nablaI/kappa).^2),
%                              privileges wide regions over smaller ones.
%          VOXEL_SPACING - 3x1 vector column with the x, y and z dimensions of
%                          the voxel (milimeters). In particular, only cubic and
%                          anisotropic voxels in the z-direction are considered.
%                          When dealing with DICOM images, the voxel spacing
%                          dimensions can be extracted using MATLAB's dicominfo(.).
%
%       OUTPUT DESCRIPTION:
%               DIFF_VOL - (diffused) volume with the largest scale-space parameter.
%
%   Example
%   -------------
%   vol = randn(100,100,100);
%   num_iter = 4;
%   delta_t = 3/44;
%   kappa = 70;
%   option = 2;
%   voxel_spacing = ones(3,1);
%   diff_vol = anisodiff3D(vol, num_iter, delta_t, kappa, option, voxel_spacing);
%   figure, subplot 121, imshow(vol(:,:,50),[]), subplot 122, imshow(diff_vol(:,:,50),[])
%
% See also anisodiff1D, anisodiff2D.

% References:
%   P. Perona and J. Malik.
%   Scale-Space and Edge Detection Using Anisotropic Diffusion.
%   IEEE Transactions on Pattern Analysis and Machine Intelligence,
%   12(7):629-639, July 1990.
%
%   G. Grieg, O. Kubler, R. Kikinis, and F. A. Jolesz.
%   Nonlinear Anisotropic Filtering of MRI Data.
%   IEEE Transactions on Medical Imaging,
%   11(2):221-232, June 1992.
%
%   MATLAB implementation based on Peter Kovesi's anisodiff(.):
%   P. D. Kovesi. MATLAB and Octave Functions for Computer Vision and Image Processing.
%   School of Computer Science & Software Engineering,
%   The University of Western Australia. Available from:
%   <http://www.csse.uwa.edu.au/~pk/research/matlabfns/>.
%
% Credits:
% Daniel Simoes Lopes
% ICIST
% Instituto Superior Tecnico - Universidade Tecnica de Lisboa
% danlopes (at) civil ist utl pt
% http://www.civil.ist.utl.pt/~danlopes
%
% May 2007 original version.

% Convert input volume to double.
vol = double(vol);

% Useful variables.
[rows cols pags] = size(vol);

% PDE (partial differential equation) initial condition.
diff_vol = vol;
clear vol

% Center voxel distances.
x = voxel_spacing(1);
y = voxel_spacing(2);
z = voxel_spacing(3);
dx = 1;
dy = 1;
dz = z/x;
dd = sqrt(dx^2+dy^2);
dh = sqrt(dx^2+dz^2);
dc = sqrt(dd^2+dz^2);

% 3D convolution masks - finite differences.
h1 = zeros(3,3,3); h1(2,2,2) = -1; h1(2,2,1) = 1;
h2 = zeros(3,3,3); h2(2,2,2) = -1; h2(2,2,3) = 1;
h3 = zeros(3,3,3); h3(2,2,2) = -1; h3(2,1,2) = 1;
h4 = zeros(3,3,3); h4(2,2,2) = -1; h4(2,3,2) = 1;
h5 = zeros(3,3,3); h5(2,2,2) = -1; h5(3,2,2) = 1;
h6 = zeros(3,3,3); h6(2,2,2) = -1; h6(1,2,2) = 1;

h7 = zeros(3,3,3); h7(2,2,2) = -1; h7(3,1,1) = 1;
h8 = zeros(3,3,3); h8(2,2,2) = -1; h8(2,1,1) = 1;
h9 = zeros(3,3,3); h9(2,2,2) = -1; h9(1,1,1) = 1;
h10 = zeros(3,3,3); h10(2,2,2) = -1; h10(3,2,1) = 1;
h11 = zeros(3,3,3); h11(2,2,2) = -1; h11(1,2,1) = 1;
h12 = zeros(3,3,3); h12(2,2,2) = -1; h12(3,3,1) = 1;
h13 = zeros(3,3,3); h13(2,2,2) = -1; h13(2,3,1) = 1;
h14 = zeros(3,3,3); h14(2,2,2) = -1; h14(1,3,1) = 1;

h15 = zeros(3,3,3); h15(2,2,2) = -1; h15(3,1,2) = 1;
h16 = zeros(3,3,3); h16(2,2,2) = -1; h16(1,1,2) = 1;
h17 = zeros(3,3,3); h17(2,2,2) = -1; h17(3,3,2) = 1;
h18 = zeros(3,3,3); h18(2,2,2) = -1; h18(1,3,2) = 1;

h19 = zeros(3,3,3); h19(2,2,2) = -1; h19(3,1,3) = 1;
h20 = zeros(3,3,3); h20(2,2,2) = -1; h20(2,1,3) = 1;
h21 = zeros(3,3,3); h21(2,2,2) = -1; h21(1,1,3) = 1;
h22 = zeros(3,3,3); h22(2,2,2) = -1; h22(3,2,3) = 1;
h23 = zeros(3,3,3); h23(2,2,2) = -1; h23(1,2,3) = 1;
h24 = zeros(3,3,3); h24(2,2,2) = -1; h24(3,3,3) = 1;
h25 = zeros(3,3,3); h25(2,2,2) = -1; h25(2,3,3) = 1;
h26 = zeros(3,3,3); h26(2,2,2) = -1; h26(1,3,3) = 1;

% Anisotropic diffusion.
for t = 1:num_iter

    % Finite differences. [imfilter(.,.,'conv') can be replaced by convn(.,.,'same')]
    % Due to possible memory limitations, the diffusion
    % will be calculated at each page/slice of the volume.
    for p = 1:pags-2
        diff3pp = diff_vol(:,:,p:p+2);
        aux = imfilter(diff3pp,h1,'conv'); nabla1 = aux(:,:,2);
        aux = imfilter(diff3pp,h2,'conv'); nabla2 = aux(:,:,2);
        aux = imfilter(diff3pp,h3,'conv'); nabla3 = aux(:,:,2);
        aux = imfilter(diff3pp,h4,'conv'); nabla4 = aux(:,:,2);
        aux = imfilter(diff3pp,h5,'conv'); nabla5 = aux(:,:,2);
        aux = imfilter(diff3pp,h6,'conv'); nabla6 = aux(:,:,2);
        aux = imfilter(diff3pp,h7,'conv'); nabla7 = aux(:,:,2);
        aux = imfilter(diff3pp,h8,'conv'); nabla8 = aux(:,:,2);
        aux = imfilter(diff3pp,h9,'conv'); nabla9 = aux(:,:,2);
        aux = imfilter(diff3pp,h10,'conv'); nabla10 = aux(:,:,2);
        aux = imfilter(diff3pp,h11,'conv'); nabla11 = aux(:,:,2);
        aux = imfilter(diff3pp,h12,'conv'); nabla12 = aux(:,:,2);
        aux = imfilter(diff3pp,h13,'conv'); nabla13 = aux(:,:,2);
        aux = imfilter(diff3pp,h14,'conv'); nabla14 = aux(:,:,2);
        aux = imfilter(diff3pp,h15,'conv'); nabla15 = aux(:,:,2);
        aux = imfilter(diff3pp,h16,'conv'); nabla16 = aux(:,:,2);
        aux = imfilter(diff3pp,h17,'conv'); nabla17 = aux(:,:,2);
        aux = imfilter(diff3pp,h18,'conv'); nabla18 = aux(:,:,2);
        aux = imfilter(diff3pp,h19,'conv'); nabla19 = aux(:,:,2);
        aux = imfilter(diff3pp,h20,'conv'); nabla20 = aux(:,:,2);
        aux = imfilter(diff3pp,h21,'conv'); nabla21 = aux(:,:,2);
        aux = imfilter(diff3pp,h22,'conv'); nabla22 = aux(:,:,2);
        aux = imfilter(diff3pp,h23,'conv'); nabla23 = aux(:,:,2);
        aux = imfilter(diff3pp,h24,'conv'); nabla24 = aux(:,:,2);
        aux = imfilter(diff3pp,h25,'conv'); nabla25 = aux(:,:,2);
        aux = imfilter(diff3pp,h26,'conv'); nabla26 = aux(:,:,2);

        % Diffusion function.
        if option == 1
            c1 = exp(-(nabla1/kappa).^2);
            c2 = exp(-(nabla2/kappa).^2);
            c3 = exp(-(nabla3/kappa).^2);
            c4 = exp(-(nabla4/kappa).^2);
            c5 = exp(-(nabla5/kappa).^2);
            c6 = exp(-(nabla6/kappa).^2);
            c7 = exp(-(nabla7/kappa).^2);
            c8 = exp(-(nabla8/kappa).^2);
            c9 = exp(-(nabla9/kappa).^2);
            c10 = exp(-(nabla10/kappa).^2);
            c11 = exp(-(nabla11/kappa).^2);
            c12 = exp(-(nabla12/kappa).^2);
            c13 = exp(-(nabla13/kappa).^2);
            c14 = exp(-(nabla14/kappa).^2);
            c15 = exp(-(nabla15/kappa).^2);
            c16 = exp(-(nabla16/kappa).^2);
            c17 = exp(-(nabla17/kappa).^2);
            c18 = exp(-(nabla18/kappa).^2);
            c19 = exp(-(nabla19/kappa).^2);
            c20 = exp(-(nabla20/kappa).^2);
            c21 = exp(-(nabla21/kappa).^2);
            c22 = exp(-(nabla22/kappa).^2);
            c23 = exp(-(nabla23/kappa).^2);
            c24 = exp(-(nabla24/kappa).^2);
            c25 = exp(-(nabla25/kappa).^2);
            c26 = exp(-(nabla26/kappa).^2);
        elseif option == 2
            c1 = 1./(1 + (nabla1/kappa).^2);
            c2 = 1./(1 + (nabla2/kappa).^2);
            c3 = 1./(1 + (nabla3/kappa).^2);
            c4 = 1./(1 + (nabla4/kappa).^2);
            c5 = 1./(1 + (nabla5/kappa).^2);
            c6 = 1./(1 + (nabla6/kappa).^2);
            c7 = 1./(1 + (nabla7/kappa).^2);
            c8 = 1./(1 + (nabla8/kappa).^2);
            c9 = 1./(1 + (nabla9/kappa).^2);
            c10 = 1./(1 + (nabla10/kappa).^2);
            c11 = 1./(1 + (nabla11/kappa).^2);
            c12 = 1./(1 + (nabla12/kappa).^2);
            c13 = 1./(1 + (nabla13/kappa).^2);
            c14 = 1./(1 + (nabla14/kappa).^2);
            c15 = 1./(1 + (nabla15/kappa).^2);
            c16 = 1./(1 + (nabla16/kappa).^2);
            c17 = 1./(1 + (nabla17/kappa).^2);
            c18 = 1./(1 + (nabla18/kappa).^2);
            c19 = 1./(1 + (nabla19/kappa).^2);
            c20 = 1./(1 + (nabla20/kappa).^2);
            c21 = 1./(1 + (nabla21/kappa).^2);
            c22 = 1./(1 + (nabla22/kappa).^2);
            c23 = 1./(1 + (nabla23/kappa).^2);
            c24 = 1./(1 + (nabla24/kappa).^2);
            c25 = 1./(1 + (nabla25/kappa).^2);
            c26 = 1./(1 + (nabla26/kappa).^2);
        end

    % Discrete PDE solution.
    diff_vol(:,:,p+1) = diff_vol(:,:,p+1) + ...
                        delta_t*(...
                        (1/(dz^2))*c1.*nabla1 + (1/(dz^2))*c2.*nabla2 + ...
                        (1/(dx^2))*c3.*nabla3 + (1/(dx^2))*c4.*nabla4 + ...
                        (1/(dy^2))*c5.*nabla5 + (1/(dy^2))*c6.*nabla6 + ...
                        ...
                        (1/(dc^2))*c7.*nabla7 + (1/(dh^2))*c8.*nabla8 + ...
                        (1/(dc^2))*c9.*nabla9 + (1/(dh^2))*c10.*nabla10 + ...
                        (1/(dh^2))*c11.*nabla11 + (1/(dc^2))*c12.*nabla12 + ...
                        (1/(dh^2))*c13.*nabla13 + (1/(dc^2))*c14.*nabla14 + ...
                        ...
                        (1/(dd^2))*c15.*nabla15 + (1/(dd^2))*c16.*nabla16 + ...
                        (1/(dd^2))*c17.*nabla17 + (1/(dd^2))*c18.*nabla18 + ...
                        ...
                        (1/(dc^2))*c19.*nabla19 + (1/(dh^2))*c20.*nabla20 + ...
                        (1/(dc^2))*c21.*nabla21 + (1/(dh^2))*c22.*nabla22 + ...
                        (1/(dh^2))*c23.*nabla23 + (1/(dc^2))*c24.*nabla24 + ...
                        (1/(dh^2))*c25.*nabla25 + (1/(dc^2))*c26.*nabla26);
    end
end

return;


function [u, s, U_p, U_k, U_d] = pinHole(p,k,d)

% PINHOLE Pin-hole camera model, with optional radial distortion.
%   U = PINHOLE(P) gives the projected pixel U of a point P in a canonical
%   pin-hole camera, that is, with calibration parameters
%     u0 = 0
%     v0 = 0
%     au = 1
%     av = 1
%   It uses reference frames {RDF,RD} (right-down-front for the 3D world
%   points and right-down for the pixel), according to this scheme:
%
%         / z (forward)
%        /
%       +------- x                 +------- u
%       |                          |
%       |      3D : P=[x;y;z]      |     image : U=[u;v]
%       | y                        | v
%
%   U = PINHOLE(P,K) allows the introduction of the camera's calibration
%   parameters:
%     K = [u0 v0 au av]'.
%
%   U = PINHOLE(P,K,D) allows the introduction of the camera's radial
%   distortion parameters:
%     D = [K2 K4 K6 ...]'
%   so that the new pixel is distorted following the distortion equation:
%     U_D = U * (1 + K2*R^2 + K4*R^4 + ...)
%   with R^2 = sum(U.^2), being U the projected point in the image plane
%   for a camera with unit focal length.
%
%   [U,S] = PINHOLE(...) returns the depth S from the camera center.
%
%   If P is a points matrix, PINHOLE(P,...) returns a pixel matrix U and a
%   depths row-vector S. P, U and S are defined as
%     P = [P1 ... Pn];   Pi = [xi;yi;zi]
%     U = [U1 ... Un];   Ui = [ui;vi]
%     S = [S1 ... Sn]
%
%   [U,S,U_p,U_k,U_d] returns the Jacobians of U wrt P, K and D. It only
%   works for single points P=[x;y;z].
%
%   See also PERSP_PROJECT, DISTORT, PIXELLISE, INVPINHOLE, PINHOLEIDP, INVDISTORTION.

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

% Point's depth
s = p(3,:);

if nargout <= 2 % only pixel

    switch nargin
        case 1
            u = persp_project(p);
        case 2
            u = pixellise(persp_project(p),k);
        case 3
            if ~isempty(d)
                u = pixellise(distort(persp_project(p),d),k);
            else
                u = pixellise(persp_project(p),k);
            end
    end

else % Jacobians

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

        switch nargin
            case 1
                [u, U_p] = persp_project(p);

            case 2
                [u1, U1_p]     = persp_project(p);
                [u, U_u1, U_k] = pixellise(u1,k);
                U_p            = U_u1*U1_p;

            case 3
                if ~isempty(d)
                    [u1, U1_p]        = persp_project(p);
                    [u2, U2_u1, U2_d] = distort(u1,d);
                    [u, U_u2, U_k]    = pixellise(u2,k);
                    U_d               = U_u2*U2_d;
                    U_p               = U_u2*U2_u1*U1_p;
                else
                    [u1, U1_p]     = persp_project(p);
                    [u, U_u1, U_k] = pixellise(u1,k);
                    U_p            = U_u1*U1_p;
                    U_d            = zeros(2,0);
                end

        end

    end

end

return

%% jacobians
syms x y z u0 v0 au av d2 d4 d6 real
p = [x;y;z];
k = [u0;v0;au;av];
d = [d2;d4;d6];

[u, s, U_p, U_k, U_d] = pinHole(p,k,d);

simplify(U_p - jacobian(u,p))
simplify(U_k - jacobian(u,k))
simplify(U_d - jacobian(u,d))

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

%   # START GPL LICENSE

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

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

%   # END GPL LICENSE


function out=acot(x)

out=atan(1./x);


classdef Anodal2gausComputer < handle

    properties (Access = public)
        A_nodal_2_gauss
    end

    properties (Access = private)
        nnode
        nelem
        npnod
        ngaus
        connec
        shape
    end

    methods (Access = public)
        function obj = Anodal2gausComputer(cParams)
            obj.init(cParams);
        end

        function compute(obj)
            obj.computeA();
        end

        function intX = integrateP1FunctionWithShapeFunction(obj,cParams)
            ndof = size(obj.A_nodal_2_gauss{1},2);
            intX = zeros(ndof,1);
            for igaus = 1:obj.ngaus
                dVG  = cParams.dV(:,igaus);
                xG   = cParams.x(:,igaus);
                A    = obj.A_nodal_2_gauss{igaus};
                intX = intX + A'*(xG.*dVG);
            end
        end
    end

    methods (Access = private)
        function init(obj,cParams)
            obj.nnode  = cParams.nnode;
            obj.nelem  = cParams.nelem;
            obj.npnod  = cParams.npnod;
            obj.ngaus  = cParams.ngaus;
            obj.connec = cParams.connec;
            obj.shape  = cParams.shape;
        end

        function computeA(obj)
            A0    = sparse(obj.nelem,obj.npnod);
            A2g   = cell(obj.ngaus,1);
            nodes = obj.connec;
            for igaus = 1:obj.ngaus
                A2g{igaus} = A0;
                for inode = 1:obj.nnode
                    node   = nodes(:,inode);
                    shapeN = obj.shape(inode,igaus);
                    Ni = ones(obj.nelem,1)*shapeN;
                    A  = sparse(1:obj.nelem,node,Ni,obj.nelem,obj.npnod);
                    A2g{igaus} = A2g{igaus} + A;
                end
            end
            obj.A_nodal_2_gauss = A2g;
        end
    end

end


function [model] = BCPF_MP(Y, varargin)
% Bayesian CP Factorization using Gaussian Mixture Priors for Image Completion
% Author : Qibin Zhao  2014
%
% -----------------------------------------------------------------------
%  [model] = BCPF_MP(Y, 'PARAM1', val1, 'PARAM2', val2, ...)
%
%  INPUTS
%     Y              - Input tensor
%     'obs'          - Binary (0-1) tensor indicating missing entries
%                      (0: missing; 1: observed)
%     'init'         - Initialization method
%                     - 'ml'  : SVD initilization (default)
%                     - 'rand': Random matrices
%     'maxRank'      - The initialization of rank (larger than true rank)
%     'dimRed'       - 1: Remove unnecessary components automaticly (default)
%                    - 0: Not remove
%     'maxiters'     - max number of iterations (default: 100)
%     'tol'          - lower band change tolerance for convergence dection
%                      (default: 1e-5)
%     'noise'        - whether noise is updated
%                        - 'on': update noise parameter (default)
%                        - 'off': fixed noise parameter (1e-5)
%     'predVar'      - Predictive distribution
%                         - 1:  compute and output
%                         - 0:  doesnot compute  (default)
%     'verbose'      - visualization of results
%                       - 0: no
%                       - 1: text (default)
%                       - 2: online display image
%                       - 3: show factors by image
%                       - 4: show factors by hinton plot (very slow)
%   OUTPUTS
%      model         - Model parameters and hyperparameters
% -----------------------------------------------------------------------
%
%   Example:
%
%     [model] = BCPF_MP(Y, 'obs', O, 'init', 'rand', 'maxRank', 10, 'dimRed', 1, 'maxiters', 100, ...
%                                'tol', 1e-6, 'verbose', 3);
%
% < Bayesian CP Factorization of Incomplete Image using Gaussian Mixture Priors >
% Copyright (C) 2014  Qibin Zhao
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%
warning off; %#ok<WNOFF>
randn('state',1); rand('state',1); %#ok<RAND>
dimY = size(Y);
N = ndims(Y);

%% Set parameters from input or by using defaults
ip = inputParser;
ip.addParamValue('obs', ones(dimY), @(x) (isnumeric(x) || islogical(x)) );
ip.addParamValue('init', 'rand', @(x) (ismember(x,{'ml','rand'})));
ip.addParamValue('maxRank', max(dimY), @isscalar);
ip.addParamValue('maxiters', 100, @isscalar);
ip.addParamValue('tol', 1e-5, @isscalar);
ip.addParamValue('verbose', 1, @isscalar);
ip.addParamValue('noise', 'on', @(x)ismember(x,{'on','off'}));
ip.addParamValue('dimRed', 1, @isscalar);
ip.addParamValue('predVar', 0, @isscalar);
ip.addParamValue('nd', 1, @isscalar);
ip.parse(varargin{:});

O     = ip.Results.obs;
init  = ip.Results.init;
R   = ip.Results.maxRank;
maxiters  = ip.Results.maxiters;
tol   = ip.Results.tol;
verbose  = ip.Results.verbose;
DIMRED   = ip.Results.dimRed;
noise = ip.Results.noise;
predVar = ip.Results.predVar;
nd = ip.Results.nd;

%% Initialization
Y = tensor(Y.*O);
O = tensor(O);
nObs = sum(O(:));

a_gamma0     = 1e-6;
b_gamma0     = 1e-6;
if  strcmp(noise,'on')
    a_beta0      = 1e-6;
    b_beta0      = 1e-6;
else
    a_beta0      = 1e-1;
    b_beta0      = 1e-6;
end
gammas = ones(R,1);
beta = 1e4;
dscale =1;

W = cell(N,1);
W1 = cell(N,1);
oR = nObs/prod(dimY);
for n=1:N
    W{n} = zeros(dimY(n),dimY(n));
    for i=1:dimY(n)
        for j=1:dimY(n)
            W{n}(i,j) = exp(-2*oR*abs(i-j)^2);
        end
    end
    W1{n} = W{n} - diag(diag(W{n}));
    W1{n} = nd*5*bsxfun(@times, W1{n}, 1./sum(W1{n},2));
end

switch init,
    case 'ml'    % Maximum likelihood
        Z = cell(N,1);
        ZSigma = cell(N,1);
        if ~isempty(find(O==0))
            %   Y(find(O==0)) = sum(Y(:))/nObs;
            Y1 = Y;
            for n=1:N
                Y1 = ttm(Y1, W{n}, n);
            end
            Y(find(O==0)) = Y1(find(O==0));
        end
        for n = 1:N
            ZSigma{n} = (repmat(eye(R), [1 1 dimY(n)]));
            [U, S, V] = svd(double(tenmat(Y,n)), 'econ');
            if R <= size(U,2)
                Z{n} = U(:,1:R)*(S(1:R,1:R)).^(0.5);
            else
                Z{n} = [U*(S.^(0.5)) randn(dimY(n), R-size(U,2))];
            end
        end
        Y = Y.*O;
    case 'rand'   % Random initialization
        Z = cell(N,1);
        ZSigma = cell(N,1);
        for n = 1:N
            Z{n} = rand(dimY(n),R);
            ZSigma{n} = (repmat(eye(R), [1 1 dimY(n)]));
            if n<3
                %   Z{n} = cholcov(W{n})'*Z{n};
                Z{n} = Z{n} +  W1{n}*Z{n};
            end
        end
end

% --------- E(aa') = cov(a,a) + E(a)E(a')----------------
EZZT = cell(N,1);
for n=1:N
    EZZT{n} = (reshape(ZSigma{n}, [R*R, dimY(n)]))';
end

Fit =0;
LB = 0;
X = double(ktensor(Z));

%% Create figures
if verbose >2,
    scrsz = get(0,'ScreenSize');
    h1 = figure('Position',[scrsz(3)*0.2 scrsz(4)*0.3 scrsz(3)*0.6 scrsz(4)*0.4]);
    figure(h1);
    switch verbose,
        case 4,
            subplot(2,3,1); hintonDiagram(Z{1}); title('Mode-1'); ylabel('Length of #-mode');
            subplot(2,3,2); hintonDiagram(Z{2}); title('Mode-2'); xlabel('Latent dimensions');
            if N>=3, subplot(2,3,3); hintonDiagram(Z{3}); title('Mode-3'); end
        case 3,
            subplot(2,3,1); imagesc(Z{1}); title('Mode-1'); ylabel('Length of #-mode');
            subplot(2,3,2); imagesc(Z{2}); title('Mode-2'); xlabel('Latent dimensions');
            if N>=3, subplot(2,3,3); imagesc(Z{3}); title('Mode-3');end
    end
    subplot(2,3,4); bar(gammas); title('Posterior mean of \lambda'); xlabel('Latent components'); ylabel(''); axis tight;
    subplot(2,3,5); plot(LB, '-r.','LineWidth',1.5,'MarkerSize',10 ); title('Lower bound'); xlabel('Iteration');  grid on;
    subplot(2,3,6); plotGamma(a_beta0, a_beta0); title('Posterior pdf'); xlabel('Noise precision \tau');grid on;
    set(findall(h1,'type','text'),'fontSize',12);
    drawnow;
end
if verbose ==2;
    h3 = figure;
    temp = 255.*(X-min(X(:)))/(max(X(:))-min(X(:)));
    imshow(uint8(temp));
    title(['Iter.= '  num2str(0),',  Rank = ' num2str(R)],'FontSize', 13, 'color','b');
    tic;
    xlabel(['(BCPF-MP)  Time: ' num2str(round(toc)), ' seconds'],'FontSize', 13, 'color','b');
    drawnow;
end

%% Model learning
for it=1:maxiters,
    %% Update factor matrices
    Aw = diag(gammas);
    for n=1:N
        % compute E(Z_{\n}^{T} Z_{\n})
        ENZZT = reshape(khatrirao_fast(EZZT{[1:n-1, n+1:N]},'r')' * double(tenmat(O,n)'), [R,R,dimY(n)]);
        % compute E(Z_{\n})
        FslashY = khatrirao_fast(Z{[1:n-1, n+1:N]},'r')' * tenmat(Y.*O, n)';
        for i=1:dimY(n)
            ZSigma{n}(:,:,i) = (beta * ENZZT(:,:,i) + Aw )^(-1);
            Z{n}(i,:) = (beta * ZSigma{n}(:,:,i) * FslashY(:,i))';
        end
        if n<3
            Z{n} = Z{n} + W1{n}*Z{n};
        end
        EZZT{n} = (reshape(ZSigma{n}, [R*R, dimY(n)]) + khatrirao_fast(Z{n}',Z{n}'))';
    end

    %% Update latent tensor X
    X = double(ktensor(Z));

    %% Update hyperparameters gamma
    a_gammaN = (0.5*sum(dimY) + a_gamma0)*ones(R,1);
    b_gammaN = 0;
    for n=1:N
        b_gammaN = b_gammaN + diag(Z{n}'*Z{n}) + diag(sum(ZSigma{n},3));
    end
    b_gammaN = b_gamma0 + 0.5.* b_gammaN;
    gammas = a_gammaN./b_gammaN;

    %% update noise beta
    %  The most time and space consuming part
    if 0 % save time but large space needed
        EX2 =  O(:)' * khatrirao_fast(EZZT,'r') * ones(R*R,1);
    else  % save space but slow
        temp1 = cell(N,1);
        EX2 =0;
        for i =1:R
            for n=1:N
                temp1{n} = EZZT{n}(:,(i-1)*R+1: i*R);
            end
            EX2 = EX2 + O(:)' * khatrirao_fast(temp1,'r')* ones(R,1);
        end
    end
    err = Y(:)'*Y(:) - 2*Y(:)'*X(:) + EX2;
    if  strcmp(noise,'on')
        a_betaN = a_beta0 + 0.5*nObs;
        b_betaN = b_beta0 + 0.5*err;
    else
        a_betaN = a_beta0;
        b_betaN = b_beta0;
    end
    beta = a_betaN/b_betaN;
    Fit = 1 - sqrt(sum(err(:)))/norm(Y(:));

    %% Lower bound
    temp1 = -0.5*nObs*safelog(2*pi) + 0.5*nObs*(psi(a_betaN)-safelog(b_betaN)) - 0.5*(a_betaN/b_betaN)*err;
    temp22 =0;
    for n=1:N
        temp22= temp22 + Z{n}'*Z{n} + sum(ZSigma{n},3);
    end
    temp2 = -0.5*R*sum(dimY)*safelog(2*pi) + 0.5*sum(dimY)*sum(psi(a_gammaN)-safelog(b_gammaN)) -0.5*trace(diag(gammas)* temp22);
    temp3 = sum(-safelog(gamma(a_gamma0)) + a_gamma0*safelog(b_gamma0) -  b_gamma0.*(a_gammaN./b_gammaN) + (a_gamma0-1).*(psi(a_gammaN)-safelog(b_gammaN)));
    temp4 = -safelog(gamma(a_beta0)) + a_beta0*safelog(b_beta0) + (a_beta0-1)*(psi(a_betaN)-safelog(b_betaN)) - b_beta0*(a_betaN/b_betaN);
    temp5=0;
    for n=1:N
        for i=1:size(ZSigma{n},3)
            temp5 = temp5 + 0.5*safelog(det(ZSigma{n}(:,:,i))) + 0.5*R*(1+safelog(2*pi));
        end
    end
    temp6 = sum(safelog(gamma(a_gammaN)) - (a_gammaN-1).*psi(a_gammaN) -safelog(b_gammaN) + a_gammaN);
    temp7 = safelog(gamma(a_betaN)) - (a_betaN-1)*psi(a_betaN) -safelog(b_betaN) + a_betaN;
    LB(it) = temp1 + temp2 + temp3 + temp4 + temp5 + temp6 + temp7;

    %% Prune irrelevant dimensions?
    Zall = cell2mat(Z);
    comPower = diag(Zall' * Zall);
    comTol = sum(dimY)*eps(norm(Zall,'fro'));
    rankest = sum(comPower> comTol );
    if max(rankest)==0
        disp('Rank becomes 0 !!!');
        break;
    end
    if DIMRED==1  && it >=2,
        if R~= max(rankest)
            indices = comPower > comTol;
            gammas = gammas(indices);
            temp = ones(R,R);
            temp(indices,indices) = 0;
            temp = temp(:);
            for n=1:N
                Z{n} = Z{n}(:,indices);
                ZSigma{n} = ZSigma{n}(indices,indices,:);
                EZZT{n} = EZZT{n}(:, temp == 0);
            end
            R = max(rankest);
        end
    end

    %% Display progress
    if it>2
        LBRelChan = abs(LB(it) - 2*LB(it-1) + LB(it-2))/-LB(2);
    else
        LBRelChan = NaN;
    end
    if verbose,
        fprintf('Iter. %d: RelChan = %g, Fit = %g, R = %d \n', it, LBRelChan, Fit, rankest);
    end

    %% visualize online results
    if verbose >2 ,
        switch verbose,
            case 4,
                set(0,'CurrentFigure',h1);
                subplot(2,3,1); hintonDiagram(Z{1}); title('Mode-1'); ylabel('Length of #-mode');
                subplot(2,3,2); hintonDiagram(Z{2}); title('Mode-2'); xlabel('Latent dimensions');
                if N>=3, subplot(2,3,3); hintonDiagram(Z{3}); title('Mode-3'); end
            case 3,
                set(0,'CurrentFigure',h1);
                subplot(2,3,1); imagesc(Z{1}); title('Mode-1'); ylabel('Length of #-mode');
                subplot(2,3,2); imagesc(Z{2}); title('Mode-2'); xlabel('Latent dimensions');
                if N>=3, subplot(2,3,3); imagesc(Z{3}); title('Mode-3'); end
        end
        subplot(2,3,4); bar(gammas); title('Posterior mean of \lambda'); xlabel('Latent components'); ylabel(''); axis tight;
        subplot(2,3,5); plot(LB, '-r.','LineWidth',1.5,'MarkerSize',10 ); title('Lower bound'); xlabel('Iteration');  grid on;
        subplot(2,3,6); plotGamma(a_betaN, b_betaN); title('Posterior pdf'); xlabel('Noise precision \tau');grid on;
        set(findall(h1,'type','text'),'fontSize',12);
        drawnow;
    end
    if verbose==2
        set(0,'CurrentFigure',h3);
        figure(h3);
        %        temp = (X-min(X(:)))/(max(X(:))-min(X(:)));
        %        image(temp);
        %        axis off;
        imshow(uint8(X));
        title(['Iter.= '  num2str(it),',  Rank = ' num2str(max(rankest))],'FontSize', 13, 'color','b');
        xlabel(['(BCPF-MP)  Time: ' num2str(round(toc)), ' seconds'],'FontSize', 13, 'color','b');
        drawnow;
    end

    %% Convergence check
    if it>5 && abs(LBRelChan) < tol
        disp('\\\======= Converged===========\\\');
        break;
    end
end

%% Predictive distribution
if predVar==1
    Xvar =  tenzeros(size(Y));
    for n=1:N
        Xvar = tenmat(Xvar,n);
        Fslash = khatrirao_fast(Z{[1:n-1, n+1:N]},'r');
        if 1
            temp1 = double(tenmat(tensor(ZSigma{n}),3));
            temp2 = khatrirao_fast(Fslash', Fslash');
            Xvar(:,:) = Xvar(:,:) + temp1*temp2;
        else
            % ---  slow computation ------
            for i=1:size(Xvar,1)     %#ok
                Xvar(i,:) = Xvar(i,:) + diag(Fslash * ZSigma{n}(:,:,i) *Fslash')';
            end
            % ---  slow computation ------
        end
        Xvar = tensor(Xvar);
    end
    Xvar = Xvar + beta^(-1);
    Xvar = Xvar.*(2*a_betaN)/(2*a_betaN-2);
    Xvar = Xvar.*(dscale^2);
else
    Xvar =[];
end

%% Prepare the results
SNR = 10*log10(var(X(:))*beta);
X = ktensor(Z)*dscale;
X = arrange(X);

%% Output
model.X = X;
model.ZSigma = ZSigma;
model.gammas = gammas;
model.Fit = Fit;
model.SNR = SNR;
model.Xvar = double(Xvar);
model.TrueRank = rankest;
model.LowBound = max(LB);

function y = safelog(x)
x(x<1e-300)=1e-200;
x(x>1e300)=1e300;
y=log(x);


% correct_mc() - compute an upper limit for the number of independant
%                time-frequency estimate in a given time-frequency image.
%                This number can be used to correct for multiple comparisons.
%
% Usage:
%   [ncorrect array] = correct_mc( EEG, cycles, maxfreq, timesout);
%
% Inputs:
%    EEG       - EEGLAB structure
%    cycles    - [float] same as the cycle input to timef(). Default is [3 0.5].
%    freqrange - [float] minimum and maximum frequency. Default is [2 50] Hz.
%    timesout  - [integer] array of number of time points to test.
%
% Output:
%    ncorrect - number of independant tf estimate in the time-freq image
%    array    - array of size (freqs x timesout) containing pvalues.
%
% Method details:
%
% Dividing by the total number of time-frequency estimate in the 2-D
% time-frequency image decomposition would be too conservative since
% spectral estimates of neighboring time-frequency points are highly
% correlated. One must thus estimate the number of independent
% time-frequency points in the TF image. Here, I used geometrical wavelets
% which are optimal in terms of image compression, so neighboring
% frequencies can be assume to carry independent spectral estimates.
% We thus had time-frequency decompositions at only X frequencies (e.g. 120,
% 60, 30, 15, 7.5, 3.25, 1.625 Hz). For each frequency, I then found
% the minimum number of time points for which there was a significant
% correlation of the spectral estimates between neighboring time points
% (for each frequency and number of time point, I computed the correlation
% from 0 to 1 for all data channel to obtain an a probability distribution
% of correlation; we then fitted this distribution using a 4th order curve
% (Ramberg, J. S., E. J. Dudewicz, et al. (1979). "A probability
% distribution and its uses in fitting data." Technometrics 21(2)) and
% assessed the probability of significance for the value 0 (no correlation)
% to be within the distribution of estimated correlation). For instance,
% using 28 time points at 120 Hz, there was no significant (p>0.05 taking
% into account Bonferoni correction for multiple comparisons) correlation
% between neighboring time-frequency power estimate, but there was a
% significant correlation using 32 time points instead of 28 (p<0.05).
% Applying the same approach for the X geometrical frequencies and summing
% the minimum number of time points for observing a significant correlation
% at all frequencies, ones obtain in general a number below 200 (with the
% defaults above and 3-second data epochs) independent estimates. In all
% the time-frequency plots, one has to used a significance mask at p<0.00025
% (0.05/200). An alternative method for correcting for multiple comparisons
% is presented in Nichols & Holmes, Human Brain Mapping, 2001.
%
% Author: Arnaud Delorme, SCCN, Jan 17, 2004

% Copyright (C) 2004 Arnaud Delorme, SCCN, arno@salk.edu
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

function [ncorrect, pval] = correct_mc( EEG, cycles, freqrange, timesout);

    if nargin < 1
        help correct_mc;
        return;
    end;
    if nargin < 2
        cycles  = [3 0.5];
    end;
    if nargin < 3
        freqrange = [2 50];
    end;
    if nargin < 4
        % possible number of time outputs
        % -------------------------------
        timesout = [5 6 7 8 9 10 12 14 16 18 20 24 28 32 36 40];
    end;
    nfreqs = ceil(log2(freqrange(2)));

    % scan times
    % ----------
    for ti = 1:length(timesout)
        clear tmpf

        % scan data channels
        % ------------------
        for index = 1:EEG.nbchan

            clf; [ersp,itc,powbase,times,freqs,erspboot,itcboot] = newtimef(EEG.data(index,:),EEG.pnts, ...
                             [EEG.xmin EEG.xmax]*1000,EEG.srate, cycles, 'timesout', timesout(ti), ...
                             'freqscale', 'log', 'nfreqs', nfreqs, 'freqrange', freqrange, 'plotitc', 'off', 'plotersp', 'off');

            % compute correlation
            % -------------------
            for fi = 1:length(freqs)
                tmp      = corrcoef(ersp(fi,1:end-1), ersp(fi,2:end));
                tmpf(index,fi) = tmp(2,1);
            end;

        end;

        % fit curve and determine if the result is significant
        % ----------------------------------------------------
        for fi = 1:length(freqs)
            pval(fi, ti) = rsfit(tmpf(:,fi)', 0);
            if pval(fi,ti) > 0.9999, pval(fi,ti) = NaN; end;
        end;
    end;

    % find minimum number of points for each frequency
    % ------------------------------------------------
    ncorrect = 0;
    threshold = 0.05 / prod(size(pval));
    for fi = 1:size(pval,1)
        ti = 1;
        while ti <= size(pval,2)
            if pval(fi,ti) < threshold
                ncorrect = ncorrect +  timesout(ti);
                ti = size(pval,2)+1;
            end;
            ti = ti+1;
        end;
    end;


function e = strainRate(L)
% strain rate

if all(L.isSymmetric(:))
  e = 0.5*max(eig(L));
else

  for n = 1:length(L)

    e(n) = 0.5 * svds(L.M(:,:,n),1);

  end

end


function eclipVec=ICRS2Eliptic(vec,TT1,TT2,method)
%%ICRS2ECLIPTIC Convert a location vector from the International
%               Celestial Reference System (ICRS) to eliptic coordinates
%               either using the IAU 2006 precession model or the Vondrak
%               400 millennia precession model.
%
%INPUTS: x The NXnumVec collection of vectors in the ICRS to convert. N
%          can be 2, or 3. If the vectors are 2D, then they are assumed to
%          be azimuth and elevation in radians. 3D vectors are assumed to
%          be Cartesian position.
% Jul1, Jul2 Two parts of a Julian date given in terrestrial time (TT).
%          The units of the date are days. The full date is the sum of
%          both terms. The date is broken into two parts to provide more
%          bits of precision. It does not matter how the date is split.
%   method An optional parameter specifying which algorithm is to be used.
%          Possible values are
%          0 (The default if omitted or an empty matrix is passed) Use the
%            IAU 2006 precession model.
%          1 Use the long-term (Vondrak) precession model.
%
%OUTPUTS: xG The vectors rotated into the ecliptic coordinate system. If
%            the input was 2D azimuth and elevation, the output will be
%            the same. If the input was Cartesian, then the output will be
%            Cartesian.
%
%This function is a Matlab interface for the relevant functions in the
%International Astronomical Union's (IAU) Standard's of Fundamental
%Astronomy library.
%
%The ecliptic is defined in the IERS Conventions [1] to be the "the
%plane perpendicular to the mean heliocentric orbital angular momentum
%vector of the Earth-Moon barycentre in the BCRS".
%
%The algorithm can be compiled for use in Matlab  using the
%CompileCLibraries function.
%
%The algorithm is run in Matlab using the command format
%eclipVec=ICRS2Eliptic(vec,TT1,TT2,method);
%
%REFERENCES:
%[1] G. Petit and B. Luzum, IERS Conventions (2010), International Earth
%    Rotation and Reference Systems Service Std. 36, 2010.
%
%July 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

error('This function is only implemented as a mexed C or C++ function. Please run CompileCLibraries.m to compile the function for use.')

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 S = mk_naive_struct(n,C)
%
% S = mk_naive_struct(Number_of_nodes, Class_node)
%
S = zeros(n);
S(C,setdiff(1:n,C)) = 1;


% usage demo for

% Masked k-order statistic filter for double data
% -------------------------------------------------
% by Fabio Bellavia (fbellavia@unipa.it),
% refer to: F. Bellavia, D. Tegolo, C. Valenti,
% "Improving Harris corner selection strategy",
% IET Computer Vision 5(2), 2011.
% Only for academic or other non-commercial purposes.

% see kordstatfilt2.m for more details

% compile the mex file
% tested with Matlab R2012a x32 on Ubuntu 12.04
mex ordstatfilt2.c

% input data
im=double(rgb2gray(imread('donkey.jpg')));
figure;
imshow(im,[]);

% circular kernel
ker=fspecial('disk',15)>0;

% median filter
idx=sum(ker(:))/2+0.5;
res=kordstatfilt2(im,ker,idx);
figure;
imshow(res,[]);


% load an image from uiuc texture database
all_images = retrieve_uiuc(1,1);
x = all_images{1}{1};

% compute its 2d scattering
tic;
options.feat = 'scatt2d_uiuc';
[feat_f,outmeta,outprecomp] = gfeat(x,options);
toc; % about 20 seconds on a core i7 2.4Ghz

% check number of coefficients :
% there should be
% 9 (scales) * 8 (orientations) coefficients of order 1
% and
% (7+5+3+1) (scales) * 8^2 (orientations) coefficients of order 2
theoritical_size = 9*8 + (7+5+3+1)*8^2;

assert(numel(feat_f) == theoritical_size);


function [I1,wl1,rgb1,g,H1,extra,s,G,S] = estimate_PSF_SRF(I,wl,bbl,rgb,s,extra,options)
%ESTIMATE_PSF_SRF Estimate point spread function (PSF) and spectral
%response function (SRF)
disp('Start estimating PSF and SRF');
show_fig = parse_param(options,'show_fig',0);

I1 = I(:,:,bbl);
wl1 = wl(bbl);
rgb1 = rgb;

Y = reshape_hsi(I1);
X = reshape(rgb1, [size(rgb1,1)*size(rgb1,2), size(rgb1,3)]);
[N,B] = size(Y);
b = size(X,2);
R2 = s + extra*2;
R = prod(R2);

convergence_t = 1e-3;

SRF_range = parse_param(options,'SRF_range','color');
% construct S
[I_sel,bands_sel] = select_relevant_bands(I1,wl1,SRF_range);
wl_sel = wl1(bands_sel)';
S = zeros(B,length(wl_sel));
for i = 1:size(S,2)
    S(bands_sel(i),i) = 1;
end

% construct C
Cs = construct_C(I1,rgb1,s,extra);
C = cat(2,Cs{:});

Y1 = [ones(N,1),Y*S];

% construct D
D = zeros(N*b,R);
for i = 1:N
    D((i-1)*b+1:i*b,:) = X'*Cs{i}';
end

% contruct options
options = [];
options.D = D;
options.Y1 = Y1;
options.D1D = D'*D;
options.D1YI = D'*kron(Y1,eye(b));

g = (1/R) * ones(R,1);
G = g2G(C,g,N);

delta_t0 = 1e-4;
delta_t_g = delta_t0;

errors = [];
for iter = 1:200
    % Update H1
    H1 = solve_for_H1(G*X, Y1, struct('lambda',1e-4));
    options.H1 = H1;

    % Update G
    der_g = calc_der_g(g, options);
    options.der_g = der_g;
    delta_t_g = calc_time_step_adaptive(@eval_obj_fun_g, @update_g, ...
        g, options, delta_t_g, delta_t0);
    g = update_g(g, options, delta_t_g);
    options.g = g;
    G = g2G(C,g,N);

    % Test convergence
    errors(end+1) = eval_obj_fun(options);

    if test_convergence(errors, convergence_t)
        break;
    end

    if mod(iter,100) == 0
        disp(['Process iteration ',num2str(iter)]);
    end
end

if show_fig
    figure('name','PSF'), mesh(reshape(g,R2));
    figure('name','SRF'), plot(H1);
end

function der_g = calc_der_g(g, options)
vecH1 = (options.H1)';
vecH1 = vecH1(:);
der_g = options.D1D * g - options.D1YI * vecH1;

function val = eval_obj_fun_g(g, options)
vecYH = options.Y1 * options.H1;
vecYH = vecYH';
vecYH = vecYH(:);
val = sum((options.D * g - vecYH).^2);

function g_new = update_g(g, options, delta_t)
der_g = options.der_g;

g_new = g - delta_t * der_g;
g_new = project_to_simplex(g_new');
g_new = g_new';

function val = eval_obj_fun(options)
g = options.g;
val = eval_obj_fun_g(g,options);


% FUNCTION [idx,pnts] = kdtree_range(kdtree, range)
%
% AUTHOR:     Steven Michael
%             (smichael@ll.mit.edu)
%
% DATE:       2/17/05
%
% DESCRIPTION:
%
%  This function simply returns all the points of a kdtree that
%  are within an N-dimesional volume bounded by "range"
%
% INPUTS:
%
%   kdtree :    A KD Tree previously created with kdtree_create
%
%
%   range  :    An array (ndim X 2) of points that specify the bounds
%               of an "ndim" dimensional volume.  All the points of the
%               kdtree within this volume will be returned.
%               The range is boundary inclusive: [min,max]
%
%               Alternatively, range can be a (nsearches X ndim X 2)
%               array of points that describes multiple range boxes
%               -- the "nsearches" index references the range box.  The
%               output in this case will be put into "nsearches"
%               different cell arrays.
%
%
% OUTPUTS:
%
%   idx    :    A (1 X N) array of points, where N is the number of
%               points within the "ndim" dimensional volume. The value is
%               the index to the releavant point in the array from which
%               the tree was created.
%
%               If multiple range boxes are searches (a 3D array is input
%               for range), then "pnt" will be a cell array, where the
%               "nth" cell contains a (1XN) array of points ; N is the
%               number of points within the "ndim" dimensional volume
%               described by the "nth" index of the range input.
%
%
%   pnts:  :    A (N X ndim) array the actual points. If the actual point
%               instead of the index is desired, an optional second output
%               will hold the points.  Multiple range boxes are handled
%               as described above.
%
% Example:
%
%    % Create a list of 1000 random points in 3d space
%    r = rand(1000,3);
%
%    % Create a tree from this list
%    tree = kdtree(r);
%
%    % Find the point closest to the origin
%    [pntidx,pntval] = kdtree_closestpoint(tree,[0 0 0]);
%
%    % Create a list "r2" of 100 random points in 3d space and
%    % find the points in "r" that are closest to each point in "r2"
%    [pntidx,pntval] = kdtree_closestpoint(tree,r2);
%
%    % Find all the points within the cube defined by "rng"
%    rng = [ [.45 .55]; [.45 .55]; ; [.45 .55] ];
%    pntidx = kdtree_range(tree,rng);