function [x, infos] = sagmu_nmf(V, rank, in_options)
% Stochastic averaging gradient multiplicative update for non-negative matrix factorization (SAGMU-NMF) algorithm.
%
% Inputs:
%       matrix      V
%       rank        rank
%       options     options
% Output:
%       w           solution of w
%       infos       information
%
%
% Reference:
%       H. Kasai,
%       "Accelerated stochastic multiplicative update with gradient averaging for nonnegative matrix factorizations,"
%       EUSIPCO, 2018.
%
%
% This file is part of NMFLibrary.
%
% Created by H.Kasai on March 22, 2017
%
%       Feb. 26, 2018 (Hiroyuki Kasai): Fixed algorithm.
%
%       Jul. 12, 2022 (Hiroyuki Kasai): Modified code structures.
%

    m = size(V, 1);
    n = size(V, 2);

    % set local options
    local_options.fast_calc             = true;
    local_options.permute_on            = true;
    local_options.sub_mode              = 'STD';
    local_options.accel                 = false;
    local_options.ls                    = false;
    local_options.h_repeat              = 1;
    local_options.rep_mode              = 'fix';
    local_options.stepsize_ratio        = 1; % stepsize ratio
    local_options.robust                = false;
    local_options.lambda                = 1;

    % check input options
    if ~exist('in_options', 'var') || isempty(in_options)
        in_options = struct();
    end
    % merge options
    options = mergeOptions(get_nmf_default_options(), local_options);
    options = mergeOptions(options, in_options);

    number_of_batches = floor(n/options.batch_size);

    if ~isfield(options, 'accel')
        options.accel = false;
        if options.ls
            options.sub_mode = 'LS';
        else
            options.sub_mode = 'STD';
        end
    else
        if options.accel
            options.sub_mode = 'ACC';
        else
            if options.ls
                options.sub_mode = 'LS';
            else
                options.sub_mode = 'STD';
            end
        end
    end

    if options.accel
        if ~isfield(options, 'h_repeat')
            options.h_repeat = 1;
        else
        end

        if ~isfield(options, 'rep_mode')
            options.rep_mode = 'fix';
        else
        end
    else
        options.h_repeat = 1;
    end

    if options.h_repeat == 1
        if options.ls
            options.sub_mode = 'LS';
        else
            options.sub_mode = 'STD';
        end
    end

    if strcmp(options.rep_mode, 'adaptive')
        rhoh = 1+(m+m*rank)/(1*(rank+1));
        alpha = 2;
        delta = 0.01;
    end

    if ~isfield(options, 'x_init')
        Wt  = rand(m, rank);
        H   = rand(rank, n);
        R   = rand(m, n);
    else
        Wt  = options.x_init.W;
        H   = options.x_init.H;
        R   = options.x_init.R;
    end

    % permute samples
    if options.permute_on
        perm_idx = randperm(n);
    else
        perm_idx = 1:n;
    end
    V = V(:, perm_idx);
    H = H(:, perm_idx);
    R = R(:, perm_idx);

    if options.robust
        mode = 'R-SAGMU-NMF';
    else
        mode = 'SAGMU-NMF';
        R = zeros(m, n);
    end

    % initialize
    method_name = sprintf('%s (%s)', mode, options.sub_mode);
    epoch = 0;
    l = zeros(m, options.batch_size) + options.lambda;
    grad_calc_count = 0;

    if options.verbose > 0
        fprintf('# %s: started ...\n', method_name);
    end

    % prepare arrays for vht and Whht
    vht = cell(number_of_batches,1);
    Whht = cell(number_of_batches,1);

    % store vht and Whht
    cnt = 0;
    for t=1: options.batch_size : n - 1
        cnt = cnt + 1;
        vt = V(:,t:t+options.batch_size-1);
        ht = H(:,t:t+options.batch_size-1);
        rt = R(:,t:t+options.batch_size-1);
        vht{cnt} = vt * ht';
        Whht{cnt} = (Wt * ht + rt) * ht';
    end

    % prepare Delta_minus and Delta_plus
    if options.fast_calc
        Delta_minus = zeros(m, rank);
        Delta_plus = zeros(m, rank);
    end

    % store initial info
    clear infos;
    [infos, f_val, optgap] = store_nmf_info(V, Wt, H, R, options, [], epoch, grad_calc_count, 0);

    if options.verbose > 1
        fprintf('%s: Epoch = 0000, cost = %.16e, optgap = %.4e\n', method_name, options.sub_mode, f_val, optgap);
    end

    % set start time
    start_time = tic();

    % main outer loop
    while true

        % check stop condition
        [stop_flag, reason, max_reached_flag] = check_stop_condition(epoch, infos, options);
        if stop_flag
            display_stop_reason(epoch, infos, options, method_name, reason, max_reached_flag);
            break;
        end

        cnt = 0;
        % main inner loop
        for t = 1 : options.batch_size : n - 1
            cnt = cnt + 1;

            % retrieve vt and ht
            vt = V(:,t:t+options.batch_size-1);
            ht = H(:,t:t+options.batch_size-1);

            if ~options.robust

                % uddate ht
                Wtv = Wt' * vt;
                WtW = Wt' * Wt;
                if strcmp(options.sub_mode, 'ACC')
                    if strcmp(options.rep_mode, 'adaptive')
                        gamma = 1;
                        eps0 = 1;
                        j = 1;
                        rhoh_alpha = rhoh*alpha;
                        %while j <= floor(1+rhoh*alpha) &&  gamma >= delta*eps0
                        ht0 = ht;
                        while j <= rhoh_alpha && gamma >= delta*eps0
                            ht = ht .* (Wtv) ./ (WtW * ht);
                            ht = ht + (ht<eps) .* eps;
                            if j == 1
                                eps0 = norm(ht0-ht);
                            end
                            gamma = norm(ht0-ht);
                            j = j+1;
                        end
                    else
                        for iii=1:options.h_repeat
                            ht = ht .* (Wtv) ./ (WtW * ht);
                            ht = ht + (ht<eps) .* eps;
                        end
                        %ht = ht + (ht<eps) .* eps;
                    end
                elseif strcmp(options.sub_mode, 'LS')
                    ht = calc_nls_nmf(vt, Wt, 1e-16);
                    ht = ht + (ht<eps) * eps;
                else
                    ht = ht .* (Wtv) ./ (WtW * ht);
                    ht = ht + (ht<eps) .* eps;
                end
            else

                rt = R(:, t:t+options.batch_size-1);

                % uddate ht
                Wtv = Wt' * vt;
                %WtW = Wt' * Wt;
                if strcmp(options.sub_mode, 'ACC')
                    if strcmp(options.rep_mode, 'adaptive')
                        gamma = 1;
                        eps0 = 1;
                        j = 1;
                        rhoh_alpha = rhoh*alpha;
                        %while j <= floor(1+rhoh*alpha) &&  gamma >= delta*eps0
                        ht0 = ht;
                        while j <= rhoh_alpha && gamma >= delta*eps0
    %                         ht = ht .* (Wtv) ./ (Wt' * (Wt * ht + rt));
    %                         ht = ht + (ht<eps) .* eps;
    %                         rt = rt .* vt ./ (Wt * ht + rt + l);
                            Wh_r = Wt * ht + rt;
                            ht = ht .* (Wtv) ./ (Wt' * Wh_r);
                            ht = ht + (ht<eps) .* eps;
                            rt = rt .* vt ./ (Wh_r + l);
                            if j == 1
                                eps0 = norm(ht0-ht);
                            end
                            gamma = norm(ht0-ht);
                            j = j+1;
                        end
                    else
                        for iii=1:options.h_repeat
    %                         ht = ht .* (Wtv) ./ (Wt' * (Wt * ht + rt));
    %                         ht = ht + (ht<eps) .* eps;
    %                         rt = rt .* vt ./ (Wt * ht + rt + l);
                            Wh_r = Wt * ht + rt;
                            ht = ht .* (Wtv) ./ (Wt' * Wh_r);
                            ht = ht + (ht<eps) .* eps;
                            rt = rt .* vt ./ (Wh_r + l);
                        end
                    end
                else
    %                 ht = ht .* Wtv ./ (Wt' * (Wt * ht + rt));
    %                 ht = ht + (ht<eps) .* eps;
    %                 rt = rt .* vt ./ (Wt * ht + rt + l);
                    Wh_r = Wt * ht + rt;
                    ht = ht .* (Wtv) ./ (Wt' * Wh_r);
                    ht = ht + (ht<eps) .* eps;
                    rt = rt .* vt ./ (Wh_r + l);
                end

            end

            % store vht and Whht
            Whht_org = Whht{cnt};
            vht_org = vht{cnt};

            % update
            vht{cnt} = vt * ht';
            if ~options.robust
                Whht{cnt} = Wt * (ht * ht');
            else
                Whht{cnt} = (Wt * ht + rt) * ht';
            end

            if epoch > 0
                if options.fast_calc
                    delta_minus = Delta_minus/n + (vht{cnt} + Whht_org)/options.batch_size;
                    delta_plus = Delta_plus/n + (Whht{cnt} + vht_org)/options.batch_size;

                    % update Delta_minus and Delta_plus
                    Delta_minus = Delta_minus + (vht{cnt} - vht_org);
                    Delta_plus = Delta_plus + (Whht{cnt} - Whht_org);
                else
                    delta_minus = zeros(m, rank);
                    delta_plus = zeros(m, rank);

                    for jj=1:number_of_batches
                        delta_minus = delta_minus + vht{jj};
                        delta_plus = delta_plus + Whht{jj};
                    end

                    delta_minus = delta_minus/n;
                    delta_plus = delta_plus/n;

                    delta_minus = delta_minus + (vht{cnt} + Whht_org)/options.batch_size;
                    delta_plus = delta_plus + (Whht{cnt} + vht_org)/options.batch_size;
                end

            else
                delta_minus = vht{cnt}/options.batch_size;
                delta_plus = Whht{cnt}/options.batch_size;

                if options.fast_calc
                    % update Delta_minus and Delta_plus
                    Delta_minus = Delta_minus + vht{cnt};
                    Delta_plus = Delta_plus + Whht{cnt};
                end
            end

            % update W
            if options.stepsize_ratio == 1
                Wt = Wt .* (delta_minus ./ delta_plus);
            else
                Wt = (1-ratio)* Wt + ratio * Wt .* (delta_minus ./ delta_plus);
            end

            Wt = Wt + (Wt<eps) .* eps;

            % store new h
            H(:,t:t+options.batch_size-1) = ht;

            % update R
            if options.robust
                R(:,t:t+options.batch_size-1) = rt;
            end

            grad_calc_count = grad_calc_count + m * options.batch_size;
        end

        % measure elapsed time
        elapsed_time = toc(start_time);

        % update epoch
        epoch = epoch + 1;

        % store info
        infos = store_nmf_info(V, Wt, H, R, options, infos, epoch, grad_calc_count, elapsed_time);

        % display info
        display_info(method_name, epoch, infos, options);

    end

    x.W = Wt;
    x.H(:,perm_idx) = H;
    x.R = R;

end


function a = asin(a)
%ASIN         Gradient inverse sine asin(a)
%

% written  10/16/98     S.M. Rump
% modified 10/14/00     S.M. Rump  use Tony's trick
% modified 04/04/04     S.M. Rump  set round to nearest for safety
%                                    accelaration for sparse input
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 11/20/05     S.M. Rump  fast check for rounding to nearest
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
% modified 10/08/08     S.M. Rump  improved sparse multiplication: not using intval data type
% modified 08/26/12     S.M. Rump  global variables removed
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  N = getappdata(0,'INTLAB_GRADIENT_NUMVAR');

  % use full(a.x(:)): cures Matlab V6.0 bug
  % a=7; i=[1 1]; x=a(i), b=sparse(a); y=b(i)  yields row vector x but column vector y
  % ax is full anyway
  ax = 1 ./ sqrt( 1 - sqr(full(a.x(:))) );
  a.x = asin(a.x);
  if issparse(a.dx)
    sizeax = size(a.dx,1);
    [ia,ja,sa] = find(a.dx);
    if isa(a.x,'intval')
      adx = times(ax(ia),sa,0);
      if adx.complex
        a.dx = intval( sparse(ia,ja,adx.mid,sizeax,N) , sparse(ia,ja,adx.rad,sizeax,N) , 'midrad' );
      else
        a.dx = intval( sparse(ia,ja,adx.inf,sizeax,N) , sparse(ia,ja,adx.sup,sizeax,N) , 'infsup' );
      end
    else
      a.dx = sparse(ia,ja,ax(ia).*sa,sizeax,N);
    end
  else
    a.dx = a.dx .* ax(:,ones(1,N));
  end

  if rndold
    setround(rndold)
  end


function s = obj2segs(obj)

% OBJ2SEGS Get segments matrix from 3D object.
%   OBJ2SEGS(OBJ) returns a 3-by-N matrix of segments from the 3D
%   information in structure OBJ, stored in the N-by-3 matrix OBJ.vert,
%   with the object vertices, and the M-by-2 matrix OBJ.seg, with the
%   couples of indices in OBJ.vert defining each segment.
%
%   For example, the square:
%
%     1 +-----+ 2
%       |     |
%       |     |
%     3 +-----+ 4
%
%   is defined in OBJ as
%       OBJ.vert = [0 0 0; 1 0 0; 0 1 0; 1 1 0]
%       OBJ.seg  = [1 2; 2 4; 4 3; 3 1]
%   then the produced segments matrix SEG = OBJ2SEGS(OBJ) is
%       SEG = [ 0 1 1 0
%               0 0 1 1
%               0 0 0 0
%               1 1 0 0
%               0 1 1 0
%               0 0 0 0 ]

%   Copyright 2009 Joan Sola @ LAAS-CNRS.

p = obj2pnts(obj);    % endpoints in 3-by-N format
M = size(obj.seg,1);  % number of segments M
s = zeros(6,M);       % initialize output segments matrix

for i = 1:M  % for each segment ...

    j = obj.seg(i,1); % first endpoint's index
    k = obj.seg(i,2); % second endpoint's index
    s(:,i) = [p(:,j);p(:,k)]; % the segment!

end

% ========== 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 [post,nlZ,dnlZ] = infExact_fastrobust(kmax, hyp, mean, cov, lik, x, y, s)

% Fast, robust inference for a GP with Gaussian likelihood. Compute a
% parametrization of the posterior, the negative log marginal likelihood
% and its derivatives w.r.t. the hyperparameters. See also "help infMethods".
%
% To be used in combination with 'fast' covariance functions (e.g.
% covSEard_fast and such).
%
% This implementation also handles badly conditioned covariance matrices,
% which sometimes become non-positive definite and cause a failure of the
% Cholesky decomposition. In these cases, the covariance matrix is corrected
% to the nearest symmetric positive semidefinite (SPD) matrix in Frobenius
% norm (see [1]; based on an implementation by John d'Errico).
%
% [1] Higham NJ, "Computing a nearest symmetric positive semidefinite
% matrix", Linear Algebra Appl, 1988.
% http://www.sciencedirect.com/science/article/pii/0024379588902236
%
% Added support for 'fast' inference and robust Cholesky decomposition by
% Luigi Acerbi, 2016-01-03.
% Original code by Carl Edward Rasmussen and Hannes Nickisch, 2015-07-13.
%
% See also INFMETHODS.M.

if iscell(lik), likstr = lik{1}; else likstr = lik; end
if ~ischar(likstr), likstr = func2str(likstr); end
if strcmp(likstr,'likGauss')    % NOTE: no explicit call to likGauss
    henoise_flag = false;
elseif strcmp(likstr,'likGaussHe')
    henoise_flag = true;        % Heteroskedastic noise
else
    error('Exact inference only possible with Gaussian likelihood');
end

if isempty(kmax); kmax = 5; end

[n, D] = size(x);
if nargout > 2                                      % do we want derivatives?
  [K,dK] = feval(cov{:}, hyp.cov, x);                 % evaluate covariance matrix and derivatives
else
  K = feval(cov{:}, hyp.cov, x);                      % evaluate covariance matrix
end
m = feval(mean{:}, hyp.mean, x);                          % evaluate mean vector

sn2_base = exp(2*hyp.lik);                               % noise variance of likGauss

if henoise_flag && ~isempty(s)
    % if isempty(s); error('Input-dependent vector S is empty.'); end
    sn2 = sn2_base + s.^2;                               % Vector of observation variance
else
    sn2 = sn2_base;
end

Lchol = min(sn2) >= 1e-6;       % tiny sn2 can lead to numerical trouble

% smallnoise = sn2 < 1e-6;
if Lchol
    if isscalar(sn2)
        sn2div = sn2;
        sn2_mat = eye(n);
    else
        sn2div = min(sn2);
        sn2_mat = diag(sn2/sn2div);
    end

    M = K/sn2div+sn2_mat;       % B matrix
else
    if isscalar(sn2)
        sn2_mat = sn2*eye(n);
    else
        sn2_mat = diag(sn2);
    end
    M = K+sn2_mat;           % Covariance with noise
end

[L,p] = chol(M);                % Try computing Cholesky factor

if p~=0     % Failed Cholesky decomposition, compute nearest SPD matrix
    if kmax <= 0; error('Cannot compute Cholesky decomposition.'); end
    % Mold = M;

    M = (M + M')/2;         % Ensure M is symmetric
    [U,Sigma,V] = svd(M);
    H = V*Sigma*V';         % Symmetric polar factor H is SPD
    M = (M+H)/2;
    M = (M + M')/2;         % Ensure symmetry again
    [L,p] = chol(M);        % Retry Cholesky decomposition
    k = 0;
    while p ~= 0            % Failed again, add a small diagonal component
        k = k + 1;
        % We do not want to change the matrix too much
        if k > kmax-1; error('Cannot compute Cholesky decomposition.'); end
        lambda = eig(M);
        mineig = min(lambda);
        kappa = 0.05;        % Sometimes even a small nudge is sufficient
        M = M + kappa*abs(mineig)*k.^2*eye(size(M));
        [L,p] = chol(M);
    end

    % [sort(eig(M))'; sort(eig(Mold))']

end

if Lchol
    sl = sn2div;
    pL = L;                         % L = chol(eye(n)+sW*sW'.*K)
else
    sl = 1;
    pL = -solve_chol(L,eye(n));     % L = -inv(K+inv(sW^2))
end

alpha = solve_chol(L,y-m)/sl;

post.alpha = alpha;                            % return the posterior parameters
post.sW = ones(n,1)/sqrt(min(sn2));            % sqrt of noise precision vector
post.L = pL;
post.Lchol = Lchol;

if nargout>1                               % do we want the marginal likelihood?
  nlZ = (y-m)'*alpha/2 + sum(log(diag(L))) + n*log(2*pi*sl)/2;   % -log marg lik
  if nargout>2                                         % do we want derivatives?
    dnlZ = hyp;                                 % allocate space for derivatives
    Q = solve_chol(L,eye(n))/sl - alpha*alpha';     % precompute for convenience
    for i = 1:numel(hyp.cov)
      dnlZ.cov(i) = sum(sum(Q.*dK(:,:,i)))/2;
    end
    dnlZ.lik = sn2_base*trace(Q);
    for i = 1:numel(hyp.mean)
      dnlZ.mean(i) = -feval(mean{:}, hyp.mean, x, i)'*alpha;
    end
  end
end


function [neighbors] = BuildNonCon2D(NGauss, tol)

% function [neighbors] = BuildNonCon2D(NGauss, tol)
% Purpose: find element to element connections through non-conforming interfaces
%          (** elements assumed straight sided **)

Globals2D;

% 1. Build Gauss nodes
[gz, gw] = JacobiGQ(0, 0, NGauss-1);

% 1.1 Find location of vertices of boundary faces
vx1 = VX(EToV(:,[1,2,3])); vx2 = VX(EToV(:,[2,3,1]));
vy1 = VY(EToV(:,[1,2,3])); vy2 = VY(EToV(:,[2,3,1]));

idB = find(EToE==((1:K)'*ones(1,Nfaces)));
x1 = vx1(idB)'; y1 = vy1(idB)';
x2 = vx2(idB)'; y2 = vy2(idB)';

% 1.2 Find those element-faces that are on boundary faces
[elmtsB,facesB] = find(EToE==((1:K)'*ones(1,Nfaces)));
Nbc = length(elmtsB);

sk = 1;
% 2.1 For each boundary face
for b1=1:Nbc
  % 2.2 Find element and face of this boundary face
  k1 = elmtsB(b1); f1 = facesB(b1);

  % 2.3 Find end coordinates of b1'th boundary face
  x11 = x1(b1);  y11 = y1(b1);  x12 = x2(b1);  y12 = y2(b1);

  % 2.4 Compute areas, lengths and face coordinates used in intersection
  % tests comparing b1'th boundary face with all boundary faces
  area1 = abs((x12-x11)*(y1-y11) - (y12-y11)*(x1-x11)); %scale
  area2 = abs((x12-x11)*(y2-y11) - (y12-y11)*(x2-x11));
  L   = (x12-x11)^2 + (y12-y11)^2 ;
  r21 = ((2*x1-x11-x12)*(x12-x11) + (2*y1-y11-y12)*(y12-y11))/L;
  r22 = ((2*x2-x11-x12)*(x12-x11) + (2*y2-y11-y12)*(y12-y11))/L;

  % 2.5 Find range of local face coordinate (bracketed between -1 and 1)
  r1 = max(-1,min(r21,r22)); r2 = min(1,max(r21,r22));

  % 2.6 Compute flag for overlap of b1 face with all other boundary faces
  flag = area1+area2+(r1<= -1 & r2<= -1)+(r1>=1 & r2>=1)+(r2-r1<tol);

  % 2.7 Find other faces with partial matches
  matches = setdiff(find(flag < tol),b1);
  Nmatches = length(matches(:));

  if(Nmatches>0)
    % 3.1 Find matches
    r1 = r1(matches); r2 = r2(matches);

    % 3.2 Find end points of boundary-boundary intersections
    xy11 = 0.5*[x11;y11]*(1-r1) +  0.5*[x12;y12]*(1+r1);
    xy12 = 0.5*[x11;y11]*(1-r2) +  0.5*[x12;y12]*(1+r2);

    % 3.3 For each face-face match
    for n=1:Nmatches

      % 3.4 Store which elements intersect
      k2 = elmtsB(matches(n)); f2 = facesB(matches(n));
      neighbors{sk}.elmtM = k1; neighbors{sk}.faceM = f1;
      neighbors{sk}.elmtP = k2; neighbors{sk}.faceP = f2;

      % 3.5 Build physical Gauss nodes on face fragment
      xg = 0.5*(1-gz)*xy11(1,n) + 0.5*(1+gz)*xy12(1,n);
      yg = 0.5*(1-gz)*xy11(2,n) + 0.5*(1+gz)*xy12(2,n);

      % 3.6 Find local coordinates of Gauss nodes
      [rg1,sg1] = FindLocalCoords2D(k1, xg, yg);
      [rg2,sg2] = FindLocalCoords2D(k2, xg, yg);

      % 3.7 Build interpolation matrices for volume nodes ->Gauss nodes
      gVM = InterpMatrix2D(rg1,sg1); neighbors{sk}.gVM  = gVM;
      gVP = InterpMatrix2D(rg2,sg2); neighbors{sk}.gVP  = gVP;

      % 3.8 Find face normal
      neighbors{sk}.nx = nx(1+(f1-1)*Nfp,k1);
      neighbors{sk}.ny = ny(1+(f1-1)*Nfp,k1);

      % 4.0 Build partial face data lift operator

      % 4.1 Compute weights for lifting
      partsJ = sqrt( (xy11(1,n)-xy12(1,n))^2 + (xy11(2,n)-xy12(2,n))^2 )/2;
      dgw = gw*partsJ/J(1,k1);

      % 4.2 Build matrix to lift Gauss data to volume data
      neighbors{sk}.lift = V*V'*(gVM')*diag(dgw);

      sk = sk+1;
    end
  end
end
return


function imgOut = WardHistAdjTMO(img, nBin, LdMin, LdMax, bPlotHistogram, bDownsampling)
%
%        imgOut = WardHistAdjTMO(img, nBin, LdMin, LdMax, bPlotHistogram, bDownsampling)
%
%
%        Input:
%           -img: input HDR image
%           -nBin: number of bins for calculating the histogram (1,+Inf)
%           -LdMin: minimum luminance value of the dispLay
%           -LdMax: maximum luminance value of the dispLay
%           -bPlotHistogram:
%
%        Output:
%           -imgOut: tone mapped image
%
%     Copyright (C) 2010-21  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/>.
%
%     The paper describing this technique is:
%     "A Visibility Matching Tone Reproduction Operator for High Dynamic Range Scenes"
% 	  by Gregory Ward Larson, Holly Rushmeier, Christine Piatko
%     in IEEE Transactions on Visualization and Computer Graphics 1997
%

%is it a gray/three color channels image?
check13Color(img);

checkNegative(img);

if(~exist('nBin', 'var'))
    nBin = 100;
end

if(nBin < 1)
    nBin = 100;
end

if(~exist('LdMin', 'var'))
    LdMin = 1; %cd/m^2
end

if(LdMin < 0.0)
    LdMin = 1;
end

if(~exist('LdMax', 'var'))
    LdMax = 100; %cd/m^2
end

if(LdMax <= 0.0)
    LdMax = 100;
end

if(LdMax < LdMin)
    tmp = LdMin;
    LdMin = LdMax;
    LdMax = tmp;
end

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

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

epsilon = 1e-6;

%compute luminance channel
L = lum(img);

%downsample according to fovea...
if(bDownsampling)
    L2 = WardDownsampling(L + epsilon);
else
    L2 = L + 1e-6;
end

%compute stastistics
LMin = min(L2(:));
LMax = max(L2(:));

Llog  = log(L2);

LlMin = log(LMin);
LlMax = log(LMax);

LldMin = log(LdMin + epsilon);
LldMax = log(LdMax + epsilon);

%compute the histogram H
H = zeros(nBin, 1);
delta = (LlMax - LlMin) / nBin;

for i=1:nBin
    indx = find(Llog > (delta * (i - 1) + LlMin) & Llog <= (delta * i + LlMin));
    H(i) = numel(indx);
end

%apply the histogram ceiling
maxH = max(H);
x_vis = LlMin:((LlMax - LlMin) / (nBin -1)):LlMax;

if(bPlotHistogram)
    bar(x_vis, H/maxH);
    hold on;
end

H = histogram_ceiling(H, delta / (LldMax - LldMin));

if(bPlotHistogram)
    bar(x_vis, H / maxH);
    hold off;
end

%compute P(x)
P = cumsum(H);
P = P / max(P);

%calculate tone mapped luminance
L(L > LMax) = LMax;
x = (LlMin:((LlMax - LlMin) / (nBin - 1)):LlMax)';
P_L = interp1(x , P , real(log(L)), 'linear');
Ld  = exp(LldMin + (LldMax - LldMin) * P_L);
%normalize in [0,1]
Ld  = (Ld - LdMin) / (LdMax - LdMin);

%change luminance
imgOut = ChangeLuminance(img, L, Ld);
end


function [ nr, nt, nc ] = circle_rt_size ( rule )

%*****************************************************************************80
%
%% CIRCLE_RT_SIZE sizes an R, THETA product quadrature rule in the unit circle.
%
%  Discussion:
%
%    For a given value of RULE, here are the number of points used at the
%    center (NC), the number of points along the radial direction (NR) and
%    the number of points along the theta direction (NT).  The total number
%    of points in the rule will be
%
%      Total = NC + NR * NT.
%
%    The user, when choosing RULE, must allocate enough space in the arrays
%    RA, RW, TA and TW for the resulting values of NR and NT.
%
%    RULE  NC  NR  NT  Total
%    ----  --  --  --  -----
%       1   1   0   0      1
%       2   0   1   4      4
%       3   1   1   4      5
%       4   1   1   6      7
%       5   1   2   4      9
%       6   0   3   4     12
%       7   1   2  10     21
%       8   0   4  16     64
%       9   0   5  20    120
%
%    The integral of F(X,Y) over the unit circle is approximated by
%
%      Integral ( X*X + Y*Y <= 1 ) F(X,Y) dx dy
%      = Integral ( 0 <= R <= 1, 0 <= T <= 2PI ) F(R*cos(T),R*sin(T)) r dr dt
%      = approximately
%        ZW * F(0,0)
%        + sum ( 1 <= I <= NR ) Sum ( 1 <= J <= NT )
%        RW(I) * TW(J) * F ( R(I) * cos ( TA(J) ), R(I) * sin ( TA(J) ) )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 April 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz, Irene Stegun,
%    Handbook of Mathematical Functions,
%    National Bureau of Standards, 1964,
%    ISBN: 0-486-61272-4,
%    LC: QA47.A34.
%
%    Arthur Stroud,
%    Approximate Calculation of Multiple Integrals,
%    Prentice Hall, 1971,
%    ISBN: 0130438936,
%    LC: QA311.S85.
%
%  Parameters:
%
%    Input, integer RULE, the rule desired.
%
%    Output, integer NR, the number of R abscissas.
%
%    Output, integer NT, the number of Theta abscissas.
%
%    Output, integer NC, the number of center abscissas (0 or 1).
%
  if ( rule == 1 )

    nr = 0;
    nt = 0;
    nc = 1;

  elseif ( rule == 2 )

    nr = 1;
    nt = 4;
    nc = 0;

  elseif ( rule == 3 )

    nr = 1;
    nt = 4;
    nc = 1;

  elseif ( rule == 4 )

    nr = 1;
    nt = 6;
    nc = 1;

  elseif ( rule == 5 )

    nr = 2;
    nt = 4;
    nc = 1;

  elseif ( rule == 6 )

    nr = 3;
    nt = 4;
    nc = 0;

  elseif ( rule == 7 )

    nr = 2;
    nt = 10;
    nc = 1;

  elseif ( rule == 8 )

    nr = 4;
    nt = 16;
    nc = 0;

  elseif ( rule == 9 )

    nr = 5;
    nt = 20;
    nc = 0;

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, 'CIRCLE_RT_SIZE - Fatal error!\n' );
    fprintf ( 1, '  There is no rule of index %d\n', rule );
    error ( 'CIRCLE_RT_SIZE - Fatal error!' );

  end

  return
end


function [Y, X, incrementalCertainty] = ...
    estimate_displacement_based_upon_vector_summation2d(solution,...
    filterDirection,...
    sizeImage,...
    numberOfFilterDirections,...
    dk,ck,...
    t11,t22,t12)
% ESTIMATE_DISPLACEMENT_BASED_UPON_VECTOR_SUMMATION2D Estimates a displacement field based upon vector summation
%
% [Y, X, incrementalCertainty] = ...
%    estimate_displacement_based_upon_vector_summation2d(solution,...
%                                                        filterDirection,...
%                                                        sizeImage,...
%                                                        numberOfFilterDirections,...
%                                                        dk,ck,...
%                                                        t11,t22,t12)
%
% INPUT ARGUMENTS
% solution                      - Solution to use (6-7)
% sizeImage                     - Image size
% dk                            - Phase-difference for k different filter
%                                 directions
% ck                            - Corresponding certainties for k different
%                                 filter directions
% filterDirection               - k filter directions
% numberOfFilterDirections      - Number of filter directions
% t11                           - Tensor element (1,1)
% t12                           - Tensor element (1,2)
% t22                           - Tensor element (2,2)
%
% OPTIONAL INPUT ARGUMENTS
% N/A
%
% OUTPUT ARGUMENTS
% Y	 							- Displacement along Y
% X 							- Displacement along X
% incrementalCertainty 			- Certainty of displacement

% Copyright (c) 2011 Daniel Forsberg
% danne.forsberg@outlook.com
%
% 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/>.

X = zeros(sizeImage);
Y = zeros(sizeImage);
incrementalCertainty = zeros(sizeImage);

switch solution
    case 6 %************************************************
        for k = 1:numberOfFilterDirections
            ckprim(:,:,k) = ck(:,:,k).*((filterDirection{k}(1).*t11 + filterDirection{k}(2).*t12).*filterDirection{k}(1)...
                +(filterDirection{k}(1).*t12 + filterDirection{k}(2).*t22).*filterDirection{k}(2));
        end

        for k = 1:numberOfFilterDirections
            X = X+ckprim(:,:,k).*dk(:,:,k)*filterDirection{k}(2);
            Y = Y+ckprim(:,:,k).*dk(:,:,k)*filterDirection{k}(1);
            incrementalCertainty = incrementalCertainty+ckprim(:,:,k);
        end
    case 7 %************************************************
        for k = 1:numberOfFilterDirections
            X = X+ck(:,:,k).*dk(:,:,k)*filterDirection{k}(2);
            Y = Y+ck(:,:,k).*dk(:,:,k)*filterDirection{k}(1);
            incrementalCertainty = incrementalCertainty+ck(:,:,k);
        end
end

incrementalCertainty = incrementalCertainty + eps;
X = X./incrementalCertainty;
Y = Y./incrementalCertainty;


function product_mixed_weight_test ( dim_num, order_1d, order_nd, rule, alpha, beta )

%*****************************************************************************80
%
%% PRODUCT_MIXED_WEIGHT_TEST computes the weights of a mixed factor product rule.
%
%  Discussion:
%
%    This routine gets the sparse grid indices and determines the
%    corresponding sparse grid weights.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 March 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer ORDER_1D(DIM_NUM), the order of the 1D rules.
%
%    Input, integer ORDER_ND, the order of the product rule.
%
%    Input, integer RULE(DIM_NUM), the rule in each dimension.
%     1, "CC",  Clenshaw Curtis, Closed Fully Nested rule.
%     2, "F2",  Fejer Type 2, Open Fully Nested rule.
%     3, "GP",  Gauss Patterson, Open Fully Nested rule.
%     4, "GL",  Gauss Legendre, Open Weakly Nested rule.
%     5, "GH",  Gauss Hermite, Open Weakly Nested rule.
%     6, "GGH", Generalized Gauss Hermite, Open Weakly Nested rule.
%     7, "LG",  Gauss Laguerre, Open Non Nested rule.
%     8, "GLG", Generalized Gauss Laguerre, Open Non Nested rule.
%     9, "GJ",  Gauss Jacobi, Open Non Nested rule.
%    10, "GW",  Golub Welsch, (presumed) Open Non Nested rule.
%    11, "CC_SE", Clenshaw Curtis Slow Exponential, Closed Fully Nested rule.
%    12, "F2_SE", Fejer Type 2 Slow Exponential, Closed Fully Nested rule.
%    13, "GP_SE", Gauss Patterson Slow Exponential, Closed Fully Nested rule.
%    14, "CC_ME", Clenshaw Curtis Moderate Exponential, Closed Fully Nested rule.
%    15, "F2_ME", Fejer Type 2 Moderate Exponential, Closed Fully Nested rule.
%    16, "GP_ME", Gauss Patterson Moderate Exponential, Closed Fully Nested rule.
%    17, "CCN", Clenshaw Curtis Nested, Linear, Closed Fully Nested rule.
%
%    Input, real ALPHA(DIM_NUM), BETA(DIM_NUM), parameters used for
%    Generalized Gauss Hermite, Generalized Gauss Laguerre, and Gauss Jacobi rules.
%
  weight_sum_exact = 1.0;

  for dim = 1 : dim_num

    if ( rule(dim) == 1 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 2 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 3 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 4 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 5 )
      weight_sum_exact = weight_sum_exact * sqrt ( pi );
    elseif ( rule(dim) == 6 )
      weight_sum_exact = weight_sum_exact * r8_gamma ( 0.5 * ( alpha(dim) + 1.0 ) );
    elseif ( rule(dim) == 7 )
      weight_sum_exact = weight_sum_exact * 1.0;
    elseif ( rule(dim) == 8 )
      weight_sum_exact = weight_sum_exact * r8_gamma ( alpha(dim) + 1.0 );
    elseif ( rule(dim) == 9 )
      arg1 = - alpha(dim);
      arg2 = 1.0;
      arg3 = beta(dim) + 2.0;
      arg4 = - 1.0;
      value1 = r8_hyper_2f1 ( arg1, arg2, arg3, arg4 );
      arg1 = - beta(dim);
      arg2 = 1.0;
      arg3 = alpha(dim) + 2.0;
      arg4 = - 1.0;
      value2 = r8_hyper_2f1 ( arg1, arg2, arg3, arg4 );
      weight_sum_exact = weight_sum_exact * ( ...
        value1 / ( beta(dim) + 1.0 ) + value2 / ( alpha(dim) + 1.0 ) );
    elseif ( rule(dim) == 10 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'PRODUCT_MIXED_WEIGHT_TEST - Fatal error!\n' );
      fprintf ( 1, '  Do not know how to handle rule 10.\n' );
      error ( 'PRODUCT_MIXED_WEIGHT_TEST - Fatal error!' );
    elseif ( rule(dim) == 11 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 12 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 13 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 14 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 15 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 16 )
      weight_sum_exact = weight_sum_exact * 2.0;
    elseif ( rule(dim) == 17 )
      weight_sum_exact = weight_sum_exact * 2.0;
    else
      fprintf ( 1, '\n' );
      fprintf ( 1, 'PRODUCT_MIXED_WEIGHT_TEST - Fatal error!\n' );
      fprintf ( 1, '  Unexpected value of RULE = %d\n', rule(dim) );
      error ( 'PRODUCT_MIXED_WEIGHT_TEST - Fatal error!' );
    end

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, 'PRODUCT_MIXED_WEIGHT_TEST:\n' );
  fprintf ( 1, '  Compute the weights of a mixed factor product grid.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  As a simple test, sum these weights.\n' );
  fprintf ( 1, '  They should sum to exactly %f\n', weight_sum_exact );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Spatial dimension DIM_NUM = %d\n', dim_num );

  fprintf ( 1, '\n' );
  fprintf ( 1, ' Dimension      Rule     Order        Alpha          Beta\n' );
  fprintf ( 1, '\n' );

  for dim = 1 : dim_num
    fprintf ( 1, '  %8d  %8d  %8d', dim, rule(dim), order_1d(dim) );
    if ( rule(dim) == 6 | rule(dim) == 8 | rule(dim) == 9 )
      fprintf ( 1, '  %12e', alpha(dim) );
    end
    if ( rule(dim) == 9 )
      fprintf ( 1, '  %12e', beta(dim) );
    end
    fprintf ( 1, '\n' );
  end
%
%  Compute the weights and points.
%
  weight = product_mixed_weight ( dim_num, order_1d, order_nd, rule, alpha, beta );
%
%  Sum the weights.
%
  weight_sum = sum ( weight(1:order_nd) );

  weight_sum_error = abs ( weight_sum - weight_sum_exact );

  fprintf ( 1, '\n' );
  fprintf ( 1, '    Weight sum  Expected sum    Difference\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  %14e  %14e  %14e\n', ...
    weight_sum, weight_sum_exact, weight_sum_error );

  return
end


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

function f = RegCoeff(S, g, df, B, Nb, Nr)
% calculates regression coefficients to be used with longstaff-schwartz

v = g(:,end);   % start for backward induction

f = zeros(Nb, Nr-1);

% backward induction and regression from t_{Nr-1} up to t_1
for i = Nr-1:-1:1
        index = find(g(:,i) > 0); % all ITM paths
        s = S(index,i+1);         % values of S at given time point
        v = v * df(i+1);          % option value at t_i

        Acell = B(s);             % evaluate basis function in cell array B
        A = cell2mat(Acell{:,:}); % convert to matrix

        f(:,i) = (A'*A)\(A'*v(index)); % determine coefficients
        c = A*f(:,i);                   % continuation value
        exercise = g(index,i) >= c;    % early exercise
        v(index(exercise)) = g(index(exercise),i);
end

end


function [lb_e, sigma_e, tf_conv] = estimate_MP2(s, pout, gap,...
    niter, nx_target, flag_distance, tf_verbose)
% fitting with histogram, using L2 or dKL
if ~exist('pout', 'var')
    pout = 0.5;
end
if ~exist('gap', 'var')
    gap = 0.2;
end
if ~exist('niter', 'var')
    niter = 10;
end
if ~exist('tf_verbose', 'var')
    tf_verbose = false;
end
if ~exist('nx_target', 'var')
    nx_target = 3.2;
end

if ~exist('flag_distance', 'var')
    flag_distance = 'L2';
end

ep = 1e-4;
tol = 1e-3;
se = sort(s(s>ep),'descend');

% start with the moment estimator
[lb_e, sigma_e, ~] = estimate_MP(s, pout, gap, 50, false);
if tf_verbose; display(lb_e, sigma_e); end
tf_conv = false;
for i = 1: niter-1
    lb1_e = (1 + sqrt(lb_e))^2 * sigma_e^2;
    nbulk = sum(se < lb1_e);
    se_bulk = se(se < lb1_e);
    nbin = nbulk / nx_target;
    nbin = max(10, nbin);
    [Ns, bin_edges] = histcounts(se_bulk, ceil(nbin));

    bin_centers = (bin_edges(1:end-1) + bin_edges(2:end)) / 2;
    s_pdf = Ns ./ diff(bin_edges) / nbulk;

    switch flag_distance
        case 'L2'
            f_dist = @(p2) sum((s_pdf - MPdistr(bin_centers, p2(1), p2(2))).^2);
        case 'dKL'
            f_dist = @(p2) f_dist_dKL(p2, s_spdf, bin_centers);
    end;

    [p2_fit, ~] = fminsearch(f_dist, [lb_e, sigma_e], optimset('MaxFunEvals',1e5));
    lb_e_new = p2_fit(1);
    sigma_e_new = p2_fit(2);

    if abs(sigma_e_new - sigma_e) < tol && abs(lb_e_new-lb_e) < tol
        tf_conv = true;
        break;
    else
        lb_e = lb_e_new;
        sigma_e = sigma_e_new;
        if tf_verbose; display([lb_e, sigma_e]); end
    end
end
end

% Local functions
% not yet updated...
function d=f_dist_dKL(p2,nf,bin_centers)
nf_MP=prob_MP_bin(p2(1),p2(2),bin_centers);
d_list=nf_MP.*log(nf_MP./nf);
d_list(nf_MP==0)=0;
d=sum(d_list);
% % special treatment of nf=0 bins near 0
if isinf(d)
    d=1e5+sum((nf-prob_MP_bin(p2(1),p2(2),bin_centers)).^2);
end;
end


%     Steffen Urban email: steffen.urban@kit.edu
%     Copyright (C) 2014  Steffen Urban
%
%     This program is free software; you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation; either version 2 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License along
%     with this program; if not, write to the Free Software Foundation, Inc.,
%     51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.

% 04.03.2014 by Steffen Urban
% error function for the center of distortion search

function [error] = errCenterUrban(x, calib_data)

error = 0;
xc = x(1);
yc = x(2);
% call calibration function
[RRfin, ss] = calibrate(calib_data.Xt, calib_data.Yt, calib_data.Xp_abs, ...
                                                        calib_data.Yp_abs, xc,yc, ...
                                                        calib_data.taylor_order, ...
                                                        calib_data.ima_proc);
lauf = 1;
M = [calib_data.Xt,calib_data.Yt,ones(size(calib_data.Xt))];

for i = 1:size(RRfin,3)
    % if calibration was not possible add a high value
    % to penalize the minimization away from that point
    if calib_data.RRfin(:,:,i)==0
      error= error+sum(ones(length(calib_data.Xp_abs),1)*sqrt( (calib_data.ocam_model.width/2)^2 + (calib_data.ocam_model.height/2)^2));
    else
        Mc = RRfin(:,:,i)*M';
        Xpp=calib_data.Xp_abs(:,:,i);
        Ypp=calib_data.Yp_abs(:,:,i);
        [xp1,yp1] = omni3d2pixel(ss, Mc, calib_data.ocam_model.width, calib_data.ocam_model.height);
        if (isinf(xp1) | isinf(yp1))
             error = error+sum(ones(length(calib_data.Xp_abs),1)*sqrt( (calib_data.ocam_model.width/2)^2 + (calib_data.ocam_model.height/2)^2));
        else
            xp = xp1 + xc;
            yp = yp1 + yc;
            lauf = lauf+length(Xpp);
            error = error + sum((Xpp-xp').^2) + sum((Ypp-yp').^2);
        end

    end
end
error = sqrt(error / lauf);

end


function [mMLS] = calc_FitComp(mCatalog, fMinRad, fMaxRad, fRadIncr, fX, fY)
% function [mMLS] = calc_FitComp(mCatalog, fMinRad, fMaxRad, fRadIncr, fX, fY);
% -----------------------------------------------------------------------------
%
% Function to calculate the MLS fit and the Goodness of fit rate to discriminate which
% method works best exploring increasing radii
%
% Incoming variables:
%
% Author: J. Woessner
% woessner@seismo.ifg.ethz.ch
% last update: 23.01.03

% Initialize
mMlS = [];

% Start value
fRadius = fMinRad;
while fRadius <= fMaxRad
    % Create catalog
    vDistances_ = sqrt(((mCatalog(:,1)-fX)*cos(pi/180*fY)*111).^2 + ((mCatalog(:,2)-fY)*111).^2);
    % Select those in between the maximum radius
    vSel = (vDistances_ <= fRadius);
    vCheckDist = vDistances_(vSel, :);
    mRadCatalog = mCatalog(vSel, :);
    % Determine Mc-values
    [result]=sv_NodeCalcMc(mRadCatalog); % For parameters in result see sv_NodeCalcMc

    % Calculate normal and lognormal fit
    [mResult, fProbNorm, fMcNorm, vX_resNorm, fNmaxNorm, mDatPredNorm] = calc_McCdfnormal(mRadCatalog, 0.1);
    [mResult2, fProbLog, fMcLog, fMuLog, fSigmaLog, mDatPredLog, vPredBest] = calc_McCdflognormal(mCat, fBinning);
    [fProbExp, fMcExp, vX_resExp, fNmaxExp, mDatPredExp] = calc_McCdfexp(mRadCatalog, 0.1);

    % Show data fit below Mc
    vSel = (mDatPredNorm(:,2) < fMcNorm);
    mTmpNorm = mDatPredNorm(vSel,:);
    vSel = (mDatPredLog(:,2) < fMcLog);
    mTmpLog = mDatPredLog(vSel,:);
    vSel = (mDatPredExp(:,2) < fMcExp);
    mTmpExp = mDatPredExp(vSel,:);
    figure;
    plot(mTmpNorm(:,2), mTmpNorm(:,3),'k*',mTmpNorm(:,2), mTmpNorm(:,1),'-b');
    hold on;
    plot(mTmpLog(:,2), mTmpLog(:,1),'-r');
    plot(mTmpExp(:,2), mTmpExp(:,1),'-g');
    legend('Data','Normal CDF', 'Lognorm. CDF', 'Exponential func.');
    sTitlestr = ['McNorm = ' num2str(fMcNorm) ', McLog = ' num2str(fMcLog) ', McExp = ' num2str(fMcExp) ', R = ' num2str(fRadius)];
    title(sTitlestr)
    xlabel('Magnitude')
    drawnow;
    hold off;
    sPrintstr = ['Fit_kobe_1986_92_135.6_35' num2str(fRadius) 'km.eps'];
    print('-deps2c', '-tiff','-r400', sPrintstr);

    % Result array
    mMLS = [mMLS; fRadius result.fProbMcNorm result.fProbMcLog fProbExp result.fMc_max result.fMc_90 result.fMc_95...
            result.fMc_com result.fMcNorm result.fMcLog fMcExp];

    % Plot Non-cumulative distribution, original and predicted
    % Time period
    [vFMD, vNonCFMD] = calc_FMD(mRadCatalog);
    vNonCFMD = fliplr(vNonCFMD);
    fPeriod1 = max(mRadCatalog(:,3)) - min(mRadCatalog(:,3));
    %     figure_w_normalized_uicontrolunits('tag','ncumdist','Name','Best model','Units','normalized','Nextplot','add',...
    %         'Numbertitle','off','visible','on');
    figure;
    semilogy(vNonCFMD(1,:)', vNonCFMD(2,:)', '-k^',mDatPredNorm(:,2) ,mDatPredNorm(:,1).*fPeriod1,'-bo');
    hold on;
    semilogy(mDatPredLog(:,2) ,mDatPredLog(:,1).*fPeriod1,'-r*');
    semilogy(mDatPredExp(:,2) ,mDatPredExp(:,1).*fPeriod1,'-gs');
    legend('Data','Normal CDF', 'Lognorm. CDF', 'Exponential func.');
    sTitlestr = ['McNorm = ' num2str(fMcNorm) ', McLog = ' num2str(fMcLog) ', McExp = ' num2str(fMcExp) ', R = ' num2str(fRadius)];
    title(sTitlestr)
    xlabel('Magnitude')
    ylabel('Non-cumulative FMD')
    drawnow;
    sPrintstr = ['FMD_kobe_1986_92_135.6_35' num2str(fRadius) 'km.eps'];
    print('-deps2c', '-tiff','-r400', sPrintstr);
    % Increase radius
    fRadius = fRadius+fRadIncr;
end

figure;
subplot(2,1,1);
plot(mMLS(:,1), mMLS(:,8),'-bd', mMLS(:,1), mMLS(:,9),'-rd', mMLS(:,1), mMLS(:,10),'-gd');
xlabel('Radius / [km]')
ylabel('Mc')
legend('Normal CDF', 'Lognorm. CDF', 'Exponential func.');
subplot(2,1,2);
plot(mMLS(:,1), mMLS(:,2),'-bd', mMLS(:,1), mMLS(:,3),'-rd', mMLS(:,1), mMLS(:,4),'-gd');
xlabel('Radius / [km]')
ylabel('MLS')
drawnow;
sPrintstr = ['Radius_kobe_1986_92_135.6_35.eps'];
print('-deps2c', '-tiff','-r400', sPrintstr);


function [x, infos] = div_admm_nmf(V, rank, in_options)
% Divergence-based ADMM algorithm for non-negative matrix factorization (KL-FPA-NMF).
%
% Inputs:
%       matrix      V
%       rank        rank
%       options     options
%           d_beta  : parameter of beta divergence
%                       (only beta=0 (IS) and beta=1 (KL) are supported)
%           rho     : smothing parameter
%           fixed   : vector containing the indices of the basis vectors in
%                       W to hold fixed (e.g., when W is known a priori)
% Output:
%       w           solution of w
%       infos       information
%
% References:
%       D.L. Sun and C. Fvotte,
%       "Alternating direction method of multipliers for non-negative matrix
%       factorization with the beta divergence,"
%       ICASSP 2014.
%
%
% This file is part of NMFLibrary
%
% This file has been ported from
%   nmf_kl_fpa.m at https://github.com/felipeyanez/nmf
%   by Felipe Yanez
%
%   Copyright (c) 2014-2016 Felipe Yanez
%
%   Permission is hereby granted, free of charge, to any person obtaining a
%   copy of this software and associated documentation files (the "Software"),
%   to deal in the Software without restriction, including without limitation
%   the rights to use, copy, modify, merge, publish, distribute, sublicense,
%   and/or sell copies of the Software, and to permit persons to whom the
%   Software is furnished to do so, subject to the following conditions:
%
%   The above copyright notice and this permission notice shall be included
%   in all copies or substantial portions of the Software.
%
%   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
%   OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
%   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
%   THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
%   OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
%   ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
%   OTHER DEALINGS IN THE SOFTWARE.
%
%
% Ported by M.Horie and H.Kasai on June 30, 2022
%
% Change log:
%
%       Jul. 12, 2022 (Hiroyuki Kasai): Modified code structures.
%

    % set dimensions and samples
    [m, n] = size(V);

    % set local options
    local_options = [];
    local_options.rho = 1;
    local_options.metric_type = 'beta-div';
    local_options.d_beta = 1; % parameter of beta divergence (only beta=0 (IS) and beta=1 (KL) are supported)

    % check input options
    if ~exist('in_options', 'var') || isempty(in_options)
        in_options = struct();
    end
    % merge options
    options = mergeOptions(get_nmf_default_options(), local_options);
    options = mergeOptions(options, in_options);

    % initialize factors
    init_options = options;
    [init_factors, ~] = generate_init_factors(V, rank, init_options);
    W = init_factors.W;
    H = init_factors.H;

    % initialize
    method_name = sprintf('Div-ADMM (%s=%.1f)', options.metric_type, options.d_beta);
    epoch = 0;
    grad_calc_count = 0;

    if options.verbose > 0
        fprintf('# %s: started ...\n', method_name);
    end

    % initialize for this algorithm
    fixed=[];
    % get the vector of indices to update
    free = setdiff(1:rank, fixed);

    X = W*H;
    Wplus = W;
    Hplus = H;
    alphaX = zeros(size(X));
    alphaW = zeros(size(W));
    alphaH = zeros(size(H));

    if options.d_beta == 0 && ~isfield(in_options, 'rho') % when IS divergence (d_beta == 0), rho should be much higher.
        local_options.rho = 1000;
    end

    % store initial info
    clear infos;

    %[options.metric_type, options.metric.param] = check_divergence(options);

    [infos, f_val, optgap] = store_nmf_info(V, W, H, [], options, [], epoch, grad_calc_count, 0);

    if options.verbose > 1
        fprintf('%s: Epoch = 0000, cost = %.16e, optgap = %.4e\n', method_name, f_val, optgap);
    end

    % set start time
    start_time = tic();

    % main loop
    while true

        % check stop condition
        [stop_flag, reason, max_reached_flag] = check_stop_condition(epoch, infos, options);
        if stop_flag
            display_stop_reason(epoch, infos, options, method_name, reason, max_reached_flag);
            break;
        end

        % update H
        H = (W'*W + eye(rank)) \ (W'*X + Hplus + 1/options.rho*(W'*alphaX - alphaH));

        % update W
        P = H*H' + eye(rank);
        Q = H*X' + Wplus' + 1/options.rho*(H*alphaX' - alphaW');
        W(:,free) = ( P(:,free) \ (Q - P(:,fixed)*W(:,fixed)') )';

        % update X (this is the only step that depends on beta)
        X_ap = W*H;
        if options.d_beta == 1

            b = options.rho*X_ap - alphaX - 1;
            X = (b + sqrt(b.^2 + 4*options.rho*V))/(2*options.rho);

        elseif options.d_beta == 0

            A = alphaX/options.rho - X_ap;
            B = 1/(3*options.rho) - A.^2/9;
            C = - A.^3/27 + A/(6*options.rho) + V/(2*options.rho);
            D = B.^3 + C.^2;

            X(D>=0) = nthroot(C(D>=0)+sqrt(D(D>=0)),3) + ...
                nthroot(C(D>=0)-sqrt(D(D>=0)),3) - ...
                A(D>=0)/3;

            phi = acos(C(D<0) ./ ((-B(D<0)).^1.5));
            X(D<0) = 2*sqrt(-B(D<0)).*cos(phi/3) - A(D<0)/3;

        else
            error('beta is not currently supported.')
        end

        % update for H_+ and W_+
        Hplus = max(H + 1/options.rho * alphaH, 0);
        Wplus = max(W + 1/options.rho * alphaW, 0);

        % update for dual variables
        alphaX = alphaX + options.rho * (X - X_ap);
        alphaH = alphaH + options.rho * (H - Hplus);
        alphaW = alphaW + options.rho * (W - Wplus);

        % measure gradient calc count
        grad_calc_count = grad_calc_count + m*n;

        % measure elapsed time
        elapsed_time = toc(start_time);

        % update epoch
        epoch = epoch + 1;

        % store info
        infos = store_nmf_info(V, W, H, [], options, infos, epoch, grad_calc_count, elapsed_time);

        % display info
        display_info(method_name, epoch, infos, options);

    end

    x.W = W;
    x.W(:,free) = Wplus(:,free);
    x.H = Hplus;

end


function value = r4_chu ( a, b, x )

%*****************************************************************************80
%
%% R4_CHU evaluates the confluent hypergeometric function of R4 arguments.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 October 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real A, B, the parameters.
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the function value.
%
  persistent eps

  if ( isempty ( eps ) )
    eps = r4_mach ( 3 );
  end

  if ( x < 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_CHU - Fatal error!\n' );
    fprintf ( 1, '  X < 0.\n' );
    error ( 'R4_CHU - Fatal error!' )
  end

  if ( x == 0.0 )
    if ( 1.0 <= b )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'R4_CHU - Fatal error!\n' );
      fprintf ( 1, '  X = 0 and 1 <= B.\n' );
      error ( 'R4_CHU - Fatal error!' )
    end
    value = r4_gamma ( 1.0 - b ) / r4_gamma ( 1.0 + a - b );
    return
  end

  if ( max ( abs ( a ), 1.0 ) * max ( abs ( 1.0 + a - b ), 1.0 ) < 0.99 * abs ( x ) )
    value = x^( - a ) * r4_chu_scaled ( a, b, x );
    return
  end
%
%  The ascending series will be used, because the descending rational
%  approximation (which is based on the asymptotic series) is unstable.
%
  if ( b < 0.0 )
    aintb = r4_aint ( b - 0.5 );
  else
    aintb = r4_aint ( b + 0.5 );
  end
  beps = b - aintb;
  n = aintb;

  alnx = log ( x );
  xtoeps = exp ( - beps * alnx );
%
%  Evaluate the finite sum.
%
%  Consider the case b < 1.0 first.
%
  if ( n < 1 )

    sum = 1.0;
    t = 1.0;
    m = - n;
    for i = 1 : m
      xi1 = i - 1;
      t = t * ( a + xi1 ) * x / ( ( b + xi1 ) * ( xi1 + 1.0 ) );
      sum = sum + t;
    end

    sum = r4_poch ( 1.0 + a - b, - a ) * sum;
%
%  Now consider the case b .ge. 1.0.
%
  else

    sum = 0.0;
    m = n - 2;

    if ( 0 <= m )

      t = 1.0;
      sum = 1.0;

      for i = 1 : m
        xi = i;
        t = t * ( a - b + xi ) * x / ( ( 1.0 - b + xi ) * xi );
        sum = sum + t;
      end

      sum = r4_gamma ( b - 1.0 ) * r4_gamr ( a ) ...
        * x^( 1 - n ) * xtoeps * sum;

    end

  end
%
%  Now evaluate the infinite sum.
%
  if ( n < 1 )
    istrt = 1 - n;
  else
    istrt = 0;
  end

  xi = istrt;

  factor = r4_mop ( n ) * r4_gamr ( 1.0 + a - b ) * x^istrt;

  if ( beps ~= 0.0 )
    factor = factor * beps * pi / sin ( beps * pi );
  end

  pochai = r4_poch ( a, xi );
  gamri1 = r4_gamr ( xi + 1.0 );
  gamrni = r4_gamr ( aintb + xi );
  b0 = factor * r4_poch ( a, xi - beps ) * gamrni ...
    * r4_gamr ( xi + 1.0 - beps );
%
%  x^(-beps) is close to 1.0, so we must be careful in evaluating
%  the differences.
%
  if ( abs ( xtoeps - 1.0 ) <= 0.5 )

    pch1ai = r4_poch1 ( a + xi, - beps );
    pch1i = r4_poch1 ( xi + 1.0 - beps, beps );
    c0 = factor * pochai * gamrni * gamri1 * ( ...
      - r4_poch1 ( b + xi, -beps ) + pch1ai ...
      - pch1i + beps * pch1ai * pch1i );
%
%  xeps1 = (1.0 - x^(-beps)) / beps = (x^(-beps) - 1.0)/(-beps)
%
    xeps1 = alnx * r4_exprel ( - beps * alnx );
    value = sum + c0 + xeps1 * b0;
    xn = n;

    for i = 1 : 1000
      xi = istrt + i;
      xi1 = istrt + i - 1;
      b0 = ( a + xi1 - beps ) * b0 * x ...
        / ( ( xn + xi1 ) * ( xi - beps ) );
      c0 = ( a + xi1 ) * c0 * x / ( ( b + xi1 ) * xi ) ...
        - ( ( a - 1.0 ) * ( xn + 2.0 * xi - 1.0 )...
        + xi * ( xi - beps ) ) * b0 ...
        / ( xi * ( b + xi1 ) * ( a + xi1 - beps ) );
      t = c0 + xeps1 * b0;
      value = value + t;
      if ( abs ( t ) < eps * abs ( value ) )
        return
      end
    end

    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_CHU - Fatal error!\n' );
    fprintf ( 1, '  No convergence in 1000 terms.\n' );
    error ( 'R4_CHU - Fatal error!' )

  end
%
%  x^(-beps) is very different from 1.0, so the straightforward
%  formulation is stable.
%
  a0 = factor * pochai * r4_gamr ( b + xi ) * gamri1 / beps;
  b0 = xtoeps * b0 / beps;

  value = sum + a0 - b0;

  for i = 1 : 1000
    xi = istrt + i;
    xi1 = istrt + i - 1;
    a0 = ( a + xi1 ) * a0 * x / ( ( b + xi1 ) * xi );
    b0 = ( a + xi1 - beps ) * b0 * x ...
      / ( ( aintb + xi1 ) * ( xi - beps ) );
    t = a0 - b0;
    value = value + t;
    if ( abs ( t ) < eps * abs ( value ) )
      return
    end
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R4_CHU - Fatal error!\n' );
  fprintf ( 1, '  No convergence in 1000 terms.'\n' );

  error ( 'R4_CHU - Fatal error!' )
end


% Residue Curve Map for Reactive Systems / Methyl Acetate Chemistry
% Author's Data: Housam BINOUS
% Department of Chemical Engineering
% National Institute of Applied Sciences and Technology
% Tunis, TUNISIA
% Email: binoushousam@yahoo.com

c=rand(50,3);

for i=0:9,

xO1=0.00867394;
x02=0.608674;
x03=0.191326;
x04=0.191326;

T0=330;
y01=0.00025;

X01=0.1*i;
X02=0.1*(9-i);

tf=20;

x0 = [xO1  x02  x03  x04 y01 T0 X01 X02];

opts = odeset('Mass','M','MassSingular','yes');

[t,x] = ode15s('RCM_MethylAcetate',[0 tf],x0,opts);

x1=x(:,7);
x2=x(:,8);

figure(1);
x1=x(:,7);
x2=x(:,8);

AXIS([0 1 0 1])
hold on

plot(x1,x2,'color',[c(i+1,1) c(i+1,2) c(i+1,3)],'LineWidth',2);

end

for i=0:9,

xO1=0.00867394;
x02=0.608674;
x03=0.191326;
x04=0.191326;

T0=330;
y01=0.00025;

X01=0.1*i;
X02=0.1*(9-i);

tf=20;

x0 = [xO1  x02  x03  x04 y01 T0 X01 X02];

opts = odeset('Mass','M','MassSingular','yes');

[t,x] = ode15s('RCM_MethylAcetate2',[0 tf],x0,opts);

x1=x(:,7);
x2=x(:,8);

figure(1);
x1=x(:,7);
x2=x(:,8);

AXIS([0 1 0 1])
hold on

plot(x1,x2,'color',[c(i+1,1) c(i+1,2) c(i+1,3)],'LineWidth',2);

end


function [mResult] = calc_GridMcBboot(vResults, nMethod)
% function [mResult] = calc_GridMcBboot(vResults, nMethod)
% --------------------------------------------------------
% Calculate Mc and b-value using the bootstrap mean value for an already
% existing grid with another Mc method
%
% Incoming variables:
% vResults : Struct array from grid calculated with sv_calcMc / sv_calc
% nMethod  : Method to calculate Mc
%
% Outgoing variables:
% mResult : [nNodeGridPoint fMc fStd_Mc fBvalue fStd_B fAvalue fStd_A]
%          Standard deviations by bootstrap
%          fMc, fBvalue, fAvalue are mean values from the bootstrap
%
% J. Woessner: woessner@seismo.ifg.ethz.ch
% last update: 25.11.03

mResult = [];

for nNodeGridPoint=1:length(vResults.mPolygon(:,1))
    % Get the data for the grid node
    mNodeCatalog_ = vResults.mCatalog(vResults.caNodeIndices{nNodeGridPoint}, :);
    % Create the frequency magnitude distribution
    [vFMD, vNonCFMD] = calc_FMD(mNodeCatalog_);
    [nY,nX]=size(mNodeCatalog_);
    if nY > vResults.nMinimumNumber
       [fMc, fStd_Mc, fBvalue, fStd_B, fAvalue, fStd_A, vMc, mBvalue] = calc_McBboot(mNodeCatalog_,vResults.fBinning, vResults.fBstnum, nMethod);
       mResult = [mResult; nNodeGridPoint fMc fStd_Mc fBvalue fStd_B fAvalue fStd_A];
    else
       mResult = [mResult; nNodeGridPoint NaN NaN NaN NaN NaN NaN];
    end
    if rem(nNodeGridPoint,500) == 0
        save(['result' num2str(nNodeGridPoint) '.mat'],'mResult');
    end
end
save(['result' num2str(nNodeGridPoint) '.mat'],'mResult');


% DEMO_VBRFA - A simple comparison of VB PCA and two robust extensions.
%
% The algorithms do not use ARD prior for the loadings W in order to keep
% the visualizations of the subspaces more clear. In addition, it helps
% avoiding pruning out relevant components too easily.
%
% NOTE: The robust algorithms might be less sensitive to the initialization
% if they estimate many components. With only one estimated component, they
% might kill all the components.
%
% This demo doesn't always show good results for robust algorithms. You may
% want to run a few times. In addition, sometimes there are several good
% interpretations of the data and the robust algorithms pick one.

% Last modified 2010-06-10
% Copyright (c) Jaakko Luttinen (jaakko.luttinen@tkk.fi)

function Q = demo_vbrfa(seed)

n = 50;
m = 2;
d = 1;
dh = d + 0; % number of components to estimate

if nargin >= 1
  randn('state', seed);
  rand('state', seed);
end

mu = [10;20];
W = [2; 1];
X = orth(randn(d,n)')' * sqrt(n);
s2 = 0.1;

% Generate normal data
Y = W*X + repmat(mu,1,n);

% Add noise
Yn = Y + sqrt(s2)*randn(m,n);

% Generate some outliers
Yno = Yn;
p = (rand(m,n) < 0.05);
Yno(p) = Yno(p) + (5 + 8 * rand(sum(p(:)),1));

% Generate some missing values
Ynom = Yno;
pmv = (rand(m,n) < 0.0);
Ynom(pmv) = NaN;

% Run different algorithms
options.init.tau = 1e1*ones(m,1); % the initialization of this can be crucial..
options.init.nu = 1;
options.prior.a_alpha = 1e10; % "fix" w to non-informative for W by
options.prior.b_alpha = 1e15; % setting strong prior for w
options.update_nu = 1;
options.update_alpha = 1;
options.update_beta = 1;
options.rotate = false;
options.common_nu = false;
options.common_tau = true;
options.maxiter = 100;

% PCA
disp('Run PCA')
options.robustness = 'none';
results_p = vbrfa(Ynom, dh, options);
W_p = results_p.W;
X_p = results_p.X;
Mu_p = results_p.Mu;
nu_p = results_p.nu;

% Robust PCA with multivariate Student-t
disp('Run robust PCA with multivariate Student-t')
options.robustness = 'multivariate-t';
results_rp = vbrfa(Ynom, dh, options);
W_rp = results_rp.W;
X_rp = results_rp.X;
Mu_rp = results_rp.Mu;
nu_rp = results_rp.nu;

options.maxiter = 1000;

% Robust PCA with independent Student-t
disp('Run robust PCA with independent Student-t')
options.robustness = 'independent-t';
results_rvb = vbrfa(Ynom, dh, options);
W_rvb = results_rvb.W;
X_rvb = results_rvb.X;
Mu_rvb = results_rvb.Mu;
nu_rvb = results_rvb.nu;

% Reconstruct
Y_p = bsxfun(@plus, W_p*X_p, Mu_p);
Y_rp = bsxfun(@plus, W_rp*X_rp, Mu_rp);
Y_rvb = bsxfun(@plus, W_rvb*X_rvb, Mu_rvb);

plot_results(Ynom, []);
plot_results(Ynom, Y_p)
plot_results(Ynom, Y_rp)
plot_results(Ynom, Y_rvb)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%55
function plot_results(Y, Yh)
%subspace2d(Yh, [], Y)
figure
plot(Y(1,:), Y(2,:), 'x', 'markersize', 5, 'color', [.2 .2 .2]);
if ~isempty(Yh)
  hold on
  plot(Yh(1,:), Yh(2,:), 'ko', 'markersize', 3);
  nans = nan*ones(1,size(Y,2));
  plot([Y(1,:);Yh(1,:);nans], [Y(2,:);Yh(2,:);nans], ':', 'color', [.0 .0 .0]);
end
set(gca, 'DataAspectRatioMode', 'manual');
set(gca, 'DataAspectRatio', [1 1 1]);
set(gca, 'XTick', [], 'YTick', []);
set(gcf, 'units', 'centimeters');
% $$$ pos = get(gcf, 'position');
% $$$ set(gcf, 'position', [pos(1), pos(2), 5 5])
% $$$ set(gcf, 'PaperPositionMode', 'auto', 'paperunits', 'centimeters', 'PaperSize', [5 5]);

xmg = 0.05 * (max(Y(1,:)) - min(Y(1,:)));
ymg = 0.05 * (max(Y(2,:)) - min(Y(2,:)));
xl = [min(Y(1,:))-xmg, max(Y(1,:))+xmg];
yl = [min(Y(2,:))-ymg, max(Y(2,:))+ymg];
set(gca, 'xlim', xl, 'ylim', yl);

% $$$ % Mark outliers
% $$$ hold on
% $$$ indeces = sum(p,1)>0;
% $$$ plot(VYno(1,indeces), VYno(2, indeces), 'go', 'MarkerSize', 8);

% $$$ % Mark observations with missing values
% $$$ indeces = sum(pmv,1)>0;
% $$$ scatter(VYno(1,indeces), VYno(2, indeces), 'yo');

return


function fx = p12_fun ( option, nvar, x )

%*****************************************************************************80
%
%% P12_FUN evaluates the function for problem 12.
%
%  Title:
%
%    Materially nonlinear problem.
%
%  Description:
%
%    The problem is the two point boundary value problem
%
%      U'' + LAMBDA * SIN ( U + U**2 + U**3 ) = 0
%
%    with boundary conditions
%
%      U(0) = 0.0
%      U(1) = 0.0
%
%    U is approximated by piecewise polynomials whose coefficients are
%    the unknowns U(1), ..., U(NVAR-1), and the value of LAMBDA is
%    stored as U(NVAR).
%
%  Options:
%
%    OPTION  Polynomials   Continuity
%      1     linear         1
%      2     cubic          1
%      3     cubic          2
%      4     quintic        1
%      5     quintic        2
%      6     quintic        3
%
%    All options use 8 intervals.
%
%  Comments:
%
%    The current program has zero as solution for all X(nvar).
%    Must find bifurcation branch and jump on to it.
%    Perhaps add X(nvar+1) a perturbation to right hand side.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 September 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Ivo Babuska, Werner Rheinboldt,
%    Reliable Error Estimations and Mesh Adaptation for the Finite
%    Element Method,
%    in International Conference on Computational Methods
%    in Nonlinear Mechanics,
%    edited by John Oden,
%    Elsevier, 1980,
%    ISBN: 0444853820,
%    LC: QA808.I57.
%
%  Parameters:
%
%    Input, integer OPTION, the option index.
%
%    Input, integer NVAR, the number of variables.
%
%    Input, real X(NVAR), the argument of the function.
%
%    Output, real FX(NVAR-1), the value of the function at X.
%
  nbco = 1;
  nbcz = 1;
  nint = 8;
  maxpolys = 6;

  bcone(1) = 0.0;
  bczero(1) = 0.0;

  fx(1:nvar-1) = 0.0;

  if ( option == 1 )
    npolys = 2;
    nderiv = 1;
  elseif ( option == 2 )
    npolys = 4;
    nderiv = 1;
  elseif ( option == 3 )
    npolys = 4;
    nderiv = 2;
  elseif ( option == 4 )
    npolys = 6;
    nderiv = 1;
  elseif ( option == 5 )
    npolys = 6;
    nderiv = 2;
  elseif ( option == 6 )
    npolys = 6;;
    nderiv = 3;
  end

  nvary = nint * npolys;
%
%  Get the Gauss quadrature rule.
%
  [ gcoef, gpoint ] = p12_gauss8 ( );
%
%  Set up the terms A * Y involving the bivariate form
%
%  For each interval I:
%
  for i = 1 : nint

    iskip = ( i - 1 ) * npolys;
    xl = ( i - 1 ) / nint;
    xr = i / nint;
    dtdx = 2.0 / ( xr - xl );
%
%  For each Gauss point, J, evaluate the integrand.
%
    for j = 1 : 8

      t = gpoint(j);
      coef = gcoef(j) * ( xr - xl ) / 2.0;
      [ pl, pld ] = p12_legendre_val ( t, dtdx, npolys );

      u = 0.0;
      uprym = 0.0;
      for k = 1 : npolys
        u = u + x(iskip+k) * pl(k);
        uprym = uprym + x(iskip+k) * pld(k);
      end

      phi = - uprym;
      psi = x(nvar) * sin ( u * ( 1.0 + u * ( 1.0 + u ) ) );
      lskip = iskip;
%
%  Project onto each test function L.
%
      for l = 1 : npolys
        ieqn = lskip + l;
        fx(ieqn) = fx(ieqn) + coef * ( psi * pl(l) + phi * pld(l) );
      end

      lskip = lskip + npolys;

    end

  end
%
%  2. Add the terms B * Z for the continuity of the test functions.
%
%  For each interval I:
%
  for i = 1 : nint

    if ( i == 1 )
      ncl = nvary;
    else
      ncl = nvary + nbcz + ( i - 2 ) * nderiv;
    end

    ncr = nvary + nbcz + ( i - 1 ) * nderiv;
    xl = ( i - 1 ) / nint;
    xr = i / nint;
    dtdx = 2.0 / ( xr - xl );
%
%  Count conditions at the left endpoint, LHIL, and at right, LHIR.
%  If we are in the first or last interval, one of
%  these will be boundary conditions.
%
    if ( i == 1 )
      lhil = nbcz;
    else
      lhil = nderiv;
    end

    if ( i == nint )
      lhir = nbco;
    else
      lhir = nderiv;
    end
%
%  For each test function PL(K):
%
    for k = 1 : npolys

      s = r8_mop ( k + 1 );
      ieqn = ( i - 1 ) * npolys + k;
%
%  Apply the boundary conditions.
%
      h2i = 1.0;
      for l = 1 : lhil
        s = - s;
        ivar = ncl + l;
        fx(ieqn) = fx(ieqn) + s * x(ivar) * h2i * p12_theta ( l, k );
        h2i = h2i * dtdx;
      end

      h2i = 1.0;
      for l = 1 : lhir
        ivar = ncr + l;
        fx(ieqn) = fx(ieqn) + x(ivar) * h2i * p12_theta ( l, k );
        h2i = h2i * dtdx;
      end

    end

  end
%
%  3. Create the C * Y terms for U and its derivatives.
%  One equation is generated for component and condition.
%
  npsum = 0;
  dtdxr = 0.0;
  dtdxl = 0.0;
%
%  For each node:
%
  ndsum = nvary;

  for i = 1 : nint + 1

    if ( 1 < i )
      xl = ( i - 2 ) / nint;
    end

    xc = ( i - 1 ) / nint;

    if ( i < nint + 1 )
      xr = i / ( nint );
    end

    if ( xc ~= xl )
      dtdxl = 2.0 / ( xc - xl );
    end

    if ( xr ~= xc )
      dtdxr = 2.0 / ( xr - xc );
    end

    h2il = 1.0;
    h2ir = 1.0;
%
%  Count the conditions:
%
    if ( i == 1 )
      khi = nbcz;
    elseif ( i < nint + 1 )
      khi = nderiv;
    elseif ( i == nint + 1 )
      khi = nbco;
    end

    for k = 1 : khi

      s = r8_mop ( k + 1 );
%
%  Set up the term from the left hand interval.
%
      ieqn = ndsum + k;

      if ( i == 1 )

        fx(ieqn) = fx(ieqn) + bczero(k);

      else

        for l = 1 : npolys
          ivar = npsum + l - npolys;
          fx(ieqn) = fx(ieqn) + x(ivar) * h2il * p12_theta ( k, l );
        end

      end
%
%  Set up the term from the right hand interval.
%
      if ( i == nint + 1 )

        fx(ieqn) = fx(ieqn) - bcone(k);

      else

        for l = 1 : npolys
          ivar = npsum + l;
          s = - s;
          fx(ieqn) = fx(ieqn) + s * x(ivar) * h2ir * p12_theta(k,l);
        end
      end

      h2il = h2il * dtdxl;
      h2ir = h2ir * dtdxr;

    end

    ndsum = ndsum + khi;
    npsum = npsum + npolys;

  end

  return
end


% inverse dynamics (recursive Newton-Euler) using spatial vector notation
function  tau = ID( model, q, qd, qdd )

    a_grav = SpatialAcceleration([0;0;-9.81;0;0;0]);

    for i = 1:model.NB
        [ XJ, S(:,i) ] = jcalc( model.pitch(i), q(i) );
        XJ
        S(:,i)
        vJ = SpatialVelocity(S(:,i)*qd(i));
        Xup(i) = XJ*model.Xtree(i);
        if model.parent(i) == 0
            v(i) = vJ;
            a(i) = Xup(i)*(-a_grav) + SpatialAcceleration(S(:,i)*qdd(i));
            a
        else
            v(i) = Xup(i)*v(model.parent(i)) + vJ;
            a(i) = Xup(i)*a(model.parent(i)) ...
                + SpatialAcceleration(S(:,i)*qdd(i)) ...
                + cross(v(i),vJ);
        end
        f(i) = model.I(i)*a(i) + cross( v(i), model.I(i)*v(i) );
    end

    v
    a
    f

    for i = model.NB:-1:1
        tau(i,1) = S(:,i)' * double(f(i));
        if model.parent(i) ~= 0
            f(model.parent(i)) = f(model.parent(i)) + Xup(i)*f(i);
        end
    end
end

function  [Xj,S] = jcalc( pitch, q )  %FIXED VW ORDER

    % jcalc  Calculate joint transform and motion subspace.
    % [Xj,S]=jcalc(pitch,q) calculates the joint transform and motion subspace
    % matrices for a revolute (pitch==0), prismatic (pitch==inf) or helical
    % (pitch==any other value) joint.  For revolute and helical joints, q is
    % the joint angle.  For prismatic joints, q is the linear displacement.

    if pitch == 0				% revolute joint
        Xj = Twist(SE3.Rz(q));
        S = [0;0;0;0;0;1];
    elseif pitch == inf			% prismatic joint
        Xj = Twist(SE3([0 0 q]));
        S = [0;0;1;0;0;0];
    else					% helical joint
        Xj = Twist(SE3.Rz(q) * SE3([0 0 q*pitch]));
        S = [0;0;pitch0;0;1;];
    end
end


function [Q_k,T_k,r,anorm,ierr,work] = lanpro(A,nin,kmax,r,options,...
    Q_k,T_k,anorm)

%LANPRO   Lanczos tridiagonalization with partial reorthogonalization
%   LANPRO computes the Lanczos tridiagonalization of a real symmetric
%   matrix using the symmetric Lanczos algorithm with partial
%   reorthogonalization.
%
%   [Q_K,T_K,R,ANORM,IERR,WORK] = LANPRO(A,K,R0,OPTIONS,Q_old,T_old)
%   [Q_K,T_K,R,ANORM,IERR,WORK] = LANPRO('Afun',N,K,R0,OPTIONS,Q_old,T_old)
%
%   Computes K steps of the Lanczos algorithm with starting vector R0,
%   and returns the K x K tridiagonal T_K, the N x K matrix Q_K
%   with semiorthonormal columns and the residual vector R such that
%
%        A*Q_K = Q_K*T_K + R .
%
%   Partial reorthogonalization is used to keep the columns of Q_K
%   semiorthogonal:
%        MAX(DIAG((eye(k) - Q_K'*Q_K))) <= OPTIONS.delta.
%
%
%   The first input argument is either a real symmetric matrix, a struct with
%   components A.L and A.U or a string containing the name of an M-file which
%   applies a linear operator to the columns of a given matrix.  In the latter
%   case, the second input argument must be N, the order of the problem.
%
%   If A is a struct with components A.L and A.U, such that
%   L*U = (A - sigma*I), a shift-and-invert Lanczos iteration is performed
%
%   The OPTIONS structure is used to control the reorthogonalization:
%     OPTIONS.delta:  Desired level of orthogonality
%                     (default = sqrt(eps/K)).
%     OPTIONS.eta  :  Level of orthogonality after reorthogonalization
%                     (default = eps^(3/4)/sqrt(K)).
%     OPTIONS.cgs  :  Flag for switching between different reorthogonalization
%                     algorithms:
%                      0 = iterated modified Gram-Schmidt  (default)
%                      1 = iterated classical Gram-Schmidt
%     OPTIONS.elr  :  If OPTIONS.elr = 1 (default) then extended local
%                     reorthogonalization is enforced.
%     OPTIONS.Y    :  The lanczos vectors are reorthogonalized against
%                     the columns of the matrix OPTIONS.Y.
%
%   If both R0, Q_old and T_old are provided, they must contain
%   a partial Lanczos tridiagonalization of A on the form
%
%        A Q_old = Q_old T_old + R0 .
%
%   In this case the factorization is extended to dimension K x K by
%   continuing the Lanczos algorithm with R0 as starting vector.
%
%   On exit ANORM contains an approximation to ||A||_2.
%     IERR = 0  :  K steps were performed succesfully.
%     IERR > 0  :  K steps were performed succesfully, but the algorithm
%                  switched to full reorthogonalization after IERR steps.
%     IERR < 0  :  Iteration was terminated after -IERR steps because an
%                  invariant subspace was found, and 3 deflation attempts
%                  were unsuccessful.
%   On exit WORK(1) contains the number of reorthogonalizations performed, and
%   WORK(2) contains the number of inner products performed in the
%   reorthogonalizations.
%
%   See also LANEIG, REORTH, COMPUTE_INT

% References:
% R.M. Larsen, Ph.D. Thesis, Aarhus University, 1998.
%
% G. H. Golub & C. F. Van Loan, "Matrix Computations",
% 3. Ed., Johns Hopkins, 1996.  Chapter 9.
%
% B. N. Parlett, ``The Symmetric Eigenvalue Problem'',
% Prentice-Hall, Englewood Cliffs, NJ, 1980.
%
% H. D. Simon, ``The Lanczos algorithm with partial reorthogonalization'',
% Math. Comp. 42 (1984), no. 165, 115--142.

% Rasmus Munk Larsen, DAIMI, 1998

% Check input arguments.
if nargin<1, error('Not enough input arguments.');  end
if isnumeric(A) | isstruct(A)
  if isnumeric(A)
    [m n] = size(A);
    if m~=n | ~isequal(A,A') | ~isreal(A)
      error('A must be real symmetric')
    end
  elseif isstruct(A)
    [m n] = size(A.L);
  end

  if nargin<7 | isempty(T_k),
    anorm = []; est_anorm=1;
  else
    anorm = T_k; est_anorm=0;
  end
  if nargin<6,  Q_k=[]; T_k=[]; else,  T_k = Q_k; Q_k = options; end
  if nargin<4 | isempty(r),  options = []; else,  options = r;  end
  if nargin<3 | isempty(kmax),
    r = rand(n,1)-0.5;
  else
    r = kmax;
  end
  if nargin<2 | isempty(nin);  kmax = max(10,n/10); else,  kmax = nin;  end
else
  if nargin<2
    error('Not enough input arguments.');
  end
  % Check input functions and parse to create an internal object
  % if an explicit expression is given.
  [A, msg] = fcnchk(A);
  if ~isempty(msg)
    error(msg);
  end
  n = nin;
  if nargin<8 | isempty(anorm), anorm = []; est_anorm=1; else est_anorm=0; end
  if nargin<7,  Q_k=[]; T_k=[]; end
  if nargin<5 | isempty(options),  options = [];          end
  if nargin<4 | isempty(r),  r = rand(n,1)-0.5;   end
  if nargin<3 | isempty(kmax);  kmax = max(10,n/10); end
end

% Set options.
delta = sqrt(eps/kmax); % Desired level of orthogonality.
eta = eps^(3/4)/sqrt(kmax);     % Level of orth. after reorthogonalization.
cgs = 0;                % Flag for switching between iterated CGS and MGS.
elr = 1;                % Flag for switching extended local
                        % reorthogonalization on and off.
deflate = 0;              % Flag for deflation against OPTIONS.Y

% Parse options struct
if ~isempty(options) & isstruct(options)
  c = fieldnames(options);
  for i=1:length(c)
    if strmatch(c(i),'delta'), delta = getfield(options,'delta');  end
    if strmatch(c(i),'eta'), eta = getfield(options,'eta'); end
    if strmatch(c(i),'cgs'), cgs = getfield(options,'cgs'); end
    if strmatch(c(i),'elr'), elr = getfield(options,'elr'); end
    if strmatch(c(i),'Y'), deflate = ~isempty(options.Y);  end
  end
end

np = 0;  nr = 0; ierr=0;

% Rule-of-thumb estimate on the size of round-off terms:
eps1 = sqrt(n)*eps/2; % Notice that {\bf u} == eps/2.
gamma = 1/sqrt(2);

% Prepare Lanczos iteration
if isempty(Q_k) % New Lanczos tridiagonalization.
  % Allocate space
  alpha = zeros(kmax+1,1);  beta = zeros(kmax+1,1);
  Q_k = zeros(n,kmax);
  q = zeros(n,1); beta(1)=norm(r);
  omega = zeros(kmax,1); omega_max = omega;  omega_old = omega;
  omega(1) = 0;   force_reorth= 0;
  j0 = 1;
else            % Extending existing Lanczos tridiagonalization.
  j = size(Q_k,2); % Size of existing factorization
  % Allocate space
  Q_k = [Q_k zeros(n,kmax-j)];
  alpha = zeros(kmax+1,1);  beta = zeros(kmax+1,1);
  alpha(1:j) = diag(T_k);
  if j>1
    beta(2:j) = diag(T_k,-1);
  end
  q = Q_k(:,j);
  % Reorthogonalize r.
  beta(j+1) = norm(r);
  if j<kmax & beta(j+1)*delta < anorm*eps1,
    fro = 1;
  end
  if isfinite(delta)
    int = 1:j;
    [r,beta(j+1),rr] = reorth(Q_k,r,beta(j+1),int,gamma,cgs);
    np = rr*j;    nr = 1;   force_reorth = 1;
  else
     force_reorth = 0;
  end
  % Compute Gerscgorin bound on ||T_k||_2 as SQRT(||T_k'*T_k||_1)
  if est_anorm
    anorm = sqrt(norm(T_k'*T_k,1));
  end
  omega = eps1*ones(kmax,1); omega_max = omega;  omega_old = omega;
  j0 = j+1;
end

if delta==0
  fro = 1; % The user has requested full reorthogonalization.
else
  fro = 0;
end

for j=j0:kmax,
  % Lanczos Step:
  q_old = q;
  if beta(j)==0
    q = r;
  else
    q = r / beta(j);
  end
  Q_k(:,j) = q;
  if isnumeric(A)
    u = A*q;
  elseif isstruct(A)
    u = A.U \ ( A.L \ q);
  else
    u = feval(A,q);
  end
  r = u - beta(j)*q_old;
  alpha(j) = q'*r;
  r = r - alpha(j)*q;

  % Extended local reorthogonalization:
  beta(j+1) = sqrt(r'*r); % Quick and dirty estimate.
  if beta(j+1)<gamma*beta(j) & elr
    if  j==1
      t1=0;
      for i=1:2
	t = q'*r;
	r = r-q*t;
	t1 = t1+t;
      end
      alpha(j) = alpha(j) + t1;
    elseif j>1
      t1 = q_old'*r;
      t2 = q'*r;
      r = r  - (q_old*t1 + q*t2); % Add small terms together first to
      if beta(j)~=0               % reduce risk of cancellation.
	beta(j) = beta(j) + t1;
      end
      alpha(j) = alpha(j) + t2;
    end
    beta(j+1) = sqrt(r'*r); % Quick and dirty estimate.
  end

  % Update Gersgorin estimate of ||T_k|| if required
%  if est_anorm & beta(j+1)~=0
%    T_k = spdiags([[beta(2:j);0] alpha(1:j) beta(1:j)],-1:1,j,j);
%    anorm = sqrt(norm(T_k'*T_k,1))
%  end
  if  est_anorm & beta(j+1)~=0
    anorm = update_gbound(anorm,alpha,beta,j);
  end

  % Update omega-recurrence
  if j>1 & ~fro & beta(j+1)~=0
    [omega,omega_old] = update_omega(omega,omega_old,j,alpha,beta,...
	eps1,anorm);
    omega_max(j) = max(abs(omega));
  end

  % Reorthogonalize if required
  if j>1 & (fro  | force_reorth | omega_max(j)>delta) & beta(j+1)~=0
    if fro
      int = 1:j;
    else
      if force_reorth == 0
	force_reorth= 1; % Do forced reorth to avoid spill-over from q_{j-1}.
	int = compute_int(omega,j,delta,eta,0,0,0);
      else
	force_reorth= 0;
      end
    end
    [r,beta(j+1),rr] = reorth(Q_k,r,beta(j+1),int,gamma,cgs);
    omega(int) = eps1;
    np = np + rr*length(int(:));    nr = nr + 1;
  else
    beta(j+1) = norm(r); % compute norm accurately.
  end

  if deflate
    [r,beta(j+1),rr] = reorth(options.Y,r,beta(j+1),1:size(options.Y,2), ...
			      gamma,cgs);
  end

  if  j<kmax & beta(j+1) < n*anorm*eps  ,
    % If beta is "small" we deflate by setting the off-diagonals of T_k
    % to 0 and attempt to restart with a basis for a new
    % invariant subspace by replacing r with a random starting vector:
    beta(j+1) = 0;
    bailout = 1;
    for attempt=1:3
      r = rand(n,1)-0.5;
      if isnumeric(A)
	r = A*r;
      elseif isstruct(A)
	r = A.U \ ( A.L \ r);
      else
	r = feval(A,r);
      end
      nrm=sqrt(r'*r); % not necessary to compute the norm accurately here.
      int = 1:j;
      [r,nrmnew,rr] = reorth(Q_k,r,nrm,int,gamma,cgs);
      omega(int) = eps1;
      np = np + rr*length(int(:));    nr = nr + 1;
      if nrmnew > 0
	% A vector numerically orthogonal to span(Q_k(:,1:j)) was found.
	% Continue iteration.
	bailout=0;
	break;
      end
    end
    if bailout
      ierr = -j;
      break;
    else
      r=r/nrmnew; % Continue with new normalized r as starting vector.
      force_reorth = 1;
      if delta>0
	fro = 0;    % Turn off full reorthogonalization.
      end
    end
  elseif j<kmax & ~fro & beta(j+1)*delta < anorm*eps1,
    % If anorm*eps1/beta(j+1) > delta then  omega(j+1) will
    % immediately exceed delta, and thus forcing a reorth. to occur at the
    % next step. The components of omega will mainly be determined
    % by the initial value and not the recurrence, and therefore we
    % cannot tell reliably which components exceed eta => we might
    % as well switch to full reorthogonalization to avoid trouble.
    % The user is probably trying to determine pathologically
    % small ( < sqrt(eps)*||A||_2 ) eigenvalues.
    %    warning(['Semiorthogonality cannot be maintained at iteration ', ...
    %	  num2str(j),'. The matrix is probably ill-conditioned.', ...
    %	  ' Switching to full reorthogonalization.'])
    fro = 1;
    ierr = j;
  end
end

% Set up tridiagonal T_k in sparse matrix data structure.
T_k = spdiags([[beta(2:j);0] alpha(1:j) beta(1:j)],-1:1,j,j);
if nargout<2
  Q_k = T_k;
elseif j~=size(Q_k,2)
  Q_k = Q_k(:,1:j);
end
work = [nr np];

function [omega,omega_old] = update_omega(omega, omega_old, j, ...
    alpha,beta,eps1,anorm)
% UPDATE_OMEGA:  Update Simon's omega_recurrence for the Lanczos vectors.
%
% [omega,omega_old] = update_omega(omega, omega_old,j,eps1,alpha,beta,anorm)
%

% Rasmus Munk Larsen, DAIMI, 1998.

% Estimate of contribution to roundoff errors from A*v
%   fl(A*v) = A*v + f,
% where ||f|| \approx eps1*||A||.
% For a full matrix A, a rule-of-thumb estimate is eps1 = sqrt(n)*eps.
T = eps1*anorm;
binv = 1/beta(j+1);

omega_old = omega;
% Update omega(1) using omega(0)==0.
omega_old(1)= beta(2)*omega(2)+ (alpha(1)-alpha(j))*omega(1) -  ...
    beta(j)*omega_old(1);
omega_old(1) = binv*(omega_old(1) + sign(omega_old(1))*T);
% Update remaining components.
k=2:j-2;
omega_old(k) = beta(k+1).*omega(k+1) + (alpha(k)-alpha(j)).*omega(k) ...
     + beta(k).*omega(k-1) - beta(j)*omega_old(k);
omega_old(k) = binv*(omega_old(k) + sign(omega_old(k))*T);
omega_old(j-1) = binv*T;
% Swap omega and omega_old.
temp = omega;
omega = omega_old;
omega_old = omega;
omega(j) =  eps1;

function anorm = update_gbound(anorm,alpha,beta,j)
%UPDATE_GBOUND   Update Gerscgorin estimate of 2-norm
%  ANORM = UPDATE_GBOUND(ANORM,ALPHA,BETA,J) updates the Gerscgorin bound
%  for the tridiagonal in the Lanczos process after the J'th step.
%  Applies Gerscgorins circles to T_K'*T_k instead of T_k itself
%  since this gives a tighter bound.

if j==1 % Apply Gerscgorin circles to T_k'*T_k to estimate || A ||_2
  i=j;
  % scale to avoid overflow
  scale = max(abs(alpha(i)),abs(beta(i+1)));
  alpha(i) = alpha(i)/scale;
  beta(i+1) = beta(i+1)/scale;
  anorm = 1.01*scale*sqrt(alpha(i)^2+beta(i+1)^2 + abs(alpha(i)*beta(i+1)));
elseif j==2
  i=1;
  % scale to avoid overflow
  scale = max(max(abs(alpha(1:2)),max(abs(beta(2:3)))));
  alpha(1:2) = alpha(1:2)/scale;
  beta(2:3) = beta(2:3)/scale;

  anorm = max(anorm, scale*sqrt(alpha(i)^2+beta(i+1)^2 + ...
      abs(alpha(i)*beta(i+1) + alpha(i+1)*beta(i+1)) + ...
      abs(beta(i+1)*beta(i+2))));
  i=2;
  anorm = max(anorm,scale*sqrt(abs(beta(i)*alpha(i-1) + alpha(i)*beta(i)) + ...
      beta(i)^2+alpha(i)^2+beta(i+1)^2 +  ...
      abs(alpha(i)*beta(i+1))) );
elseif j==3
  % scale to avoid overflow
  scale = max(max(abs(alpha(1:3)),max(abs(beta(2:4)))));
  alpha(1:3) = alpha(1:3)/scale;
  beta(2:4) = beta(2:4)/scale;
  i=2;
  anorm = max(anorm,scale*sqrt(abs(beta(i)*alpha(i-1) + alpha(i)*beta(i)) + ...
      beta(i)^2+alpha(i)^2+beta(i+1)^2 +  ...
      abs(alpha(i)*beta(i+1) + alpha(i+1)*beta(i+1)) + ...
      abs(beta(i+1)*beta(i+2))) );
  i=3;
  anorm = max(anorm,scale*sqrt(abs(beta(i)*beta(i-1)) + ...
      abs(beta(i)*alpha(i-1) + alpha(i)*beta(i)) + ...
      beta(i)^2+alpha(i)^2+beta(i+1)^2 +  ...
      abs(alpha(i)*beta(i+1))) );
else
  % scale to avoid overflow
  %  scale = max(max(abs(alpha(j-2:j)),max(abs(beta(j-2:j+1)))));
  %  alpha(j-2:j) = alpha(j-2:j)/scale;
  %  beta(j-2:j+1) = beta(j-2:j+1)/scale;

  % Avoid scaling, which is slow. At j>3 the estimate is usually quite good
  % so just make sure that anorm is not made infinite by overflow.
  i = j-1;
  anorm1 = sqrt(abs(beta(i)*beta(i-1)) + ...
      abs(beta(i)*alpha(i-1) + alpha(i)*beta(i)) + ...
      beta(i)^2+alpha(i)^2+beta(i+1)^2 +  ...
      abs(alpha(i)*beta(i+1) + alpha(i+1)*beta(i+1)) + ...
      abs(beta(i+1)*beta(i+2)));
  if isfinite(anorm1)
    anorm = max(anorm,anorm1);
  end
  i = j;
  anorm1 = sqrt(abs(beta(i)*beta(i-1)) + ...
      abs(beta(i)*alpha(i-1) + alpha(i)*beta(i)) + ...
      beta(i)^2+alpha(i)^2+beta(i+1)^2 +  ...
      abs(alpha(i)*beta(i+1)));
  if isfinite(anorm1)
    anorm = max(anorm,anorm1);
  end
end


function A = slkfd(K, nums, varargin)
%SLKFD Perform Kernelized Fisher Discriminant Analysis
%
% $ Syntax $
%   - A = slkfd(K, nums, ...)
%
% $ Arguments $
%   - K:        the kernel gram matrix of the training samples
%   - nums:     the numbers of samples in all classes
%   - A:        the projection coefficient matrix
%
% $ Description $
%   - A = slkfd(K, nums, ...) performs Kernerlized Fisher discriminant
%     analysis on the samples X according to the specified properties.
%     \*
%     \t   Table 1.  The properties of Fisher Discriminant Analysis   \\
%     \h     name     &     description                                \\
%           'sol'     &  The cell containing the arguments for solving
%                        the generalized eigen-problem by slsymgeig.
%                        default = {}.                                 \\
%           'dimset'  &  The cell containing the arguments for determining
%                        the output feature dimension. default = {}.
%                        (refer to sldim_by_eigval).                   \\
%           'Sb'      &  The pre-computed kernelized between-class
%                        scattering matrix or the cell containing
%                        the arguments for computing the kernelized
%                        scatter matrix in the form {type, ...},
%                        which is input to slkernelscatter.     \\
%           'Sw'      &  The pre-computed kernelized within-class
%                        scattering matrix or the cell containing
%                        the arguments for computing the scatter
%                        matrix in the form {type, ...}, which is
%                        input to slkernelscatter.     \\
%         'weights'   &  The sample weights. default = [].   \\
%     \*
%
% $ History $
%   - Created by Dahua Lin on May 03, 2006
%

%% parse and verify input arguments

if nargin < 2
    raise_lackinput('slkfd', 2);
end

% for K
n = size(K, 1);
if ~isequal(size(K), [n, n])
    error('sltoolbox:invaliddims', ...
        'K should be a square matrix');
end

% for nums
nc = length(nums);
if ~isequal(size(nums), [1 nc])
    error('sltoolbox:sizmismatch', ...
        'nums should be a 1 x nc row vector');
end

% for options

opts.sol = {};
opts.dimset = {};
opts.Sb = {'Sb'};
opts.Sw = {'Sw'};
opts.weights = [];
opts = slparseprops(opts, varargin{:});

has_Sb = ~isempty(opts.Sb) && isnumeric(opts.Sb);
has_Sw = ~isempty(opts.Sw) && isnumeric(opts.Sw);
if has_Sb
    Sb = opts.Sb;
    if ~isequal(size(Sb), [n n])
        error('sltoolbox:sizmismatch', ...
            'Sb should be a n x n matrix');
    end
end
if has_Sw
    Sw = opts.Sw;
    if ~isequal(size(Sw), [n n])
        error('sltoolbox:sizmismatch', ...
            'Sw should be a n x n matrix');
    end
end

if ~isempty(opts.weights)
    w = opts.weights;
    if ~isequal(size(w), [1 n])
        error('sltoolbox:sizmismatch', ...
            'The weights should be a 1 x n row vector');
    end
else
    w = [];
end

%% Compute

%% Step 1: Construct the eigen-problem

if ~has_Sb
    Sb = slscatter(K, opts.Sb{:}, 'sweights', w, 'nums', nums);
end

if ~has_Sw
    Sw = slscatter(K, opts.Sw{:}, 'sweights', w, 'nums', nums);
end

%% Step 2: Resolve the eigen-problem

[evs, A] = slsymgeig(Sb, Sw, opts.sol{:});

rk = sldim_by_eigval(evs, opts.dimset{:});
A = A(:, 1:rk);