function [D PD] = allfitdist(data,sortby,varargin)
%ALLFITDIST Fit all valid parametric probability distributions to data.
%   [D PD] = ALLFITDIST(DATA) fits all valid parametric probability
%   distributions to the data in vector DATA, and returns a struct D of
%   fitted distributions and parameters and a struct of objects PD
%   representing the fitted distributions. PD is an object in a class
%   derived from the ProbDist class.
%
%   [...] = ALLFITDIST(DATA,SORTBY) returns the struct of valid distributions
%   sorted by the parameter SORTBY
%        NLogL - Negative of the log likelihood
%        BIC - Bayesian information criterion (default)
%        AIC - Akaike information criterion
%        AICc - AIC with a correction for finite sample sizes
%
%   [...] = ALLFITDIST(...,'DISCRETE') specifies it is a discrete
%   distribution and does not attempt to fit a continuous distribution
%   to the data
%
%   [...] = ALLFITDIST(...,'PDF') or (...,'CDF') plots either the PDF or CDF
%   of a subset of the fitted distribution. The distributions are plotted in
%   order of fit, according to SORTBY.
%
%   List of distributions it will try to fit
%     Continuous (default)
%       Beta
%       Birnbaum-Saunders
%       Exponential
%       Extreme value
%       Gamma
%       Generalized extreme value
%       Generalized Pareto
%       Inverse Gaussian
%       Logistic
%       Log-logistic
%       Lognormal
%       Nakagami
%       Normal
%       Rayleigh
%       Rician
%       t location-scale
%       Weibull
%
%     Discrete ('DISCRETE')
%       Binomial
%       Negative binomial
%       Poisson
%
%   Optional inputs:
%   [...] = ALLFITDIST(...,'n',N,...)
%   For the 'binomial' distribution only:
%      'n'            A positive integer specifying the N parameter (number
%                     of trials).  Not allowed for other distributions. If
%                     'n' is not given it is estimate by Method of Moments.
%                     If the estimated 'n' is negative then the maximum
%                     value of data will be used as the estimated value.
%   [...] = ALLFITDIST(...,'theta',THETA,...)
%   For the 'generalized pareto' distribution only:
%      'theta'        The value of the THETA (threshold) parameter for
%                     the generalized Pareto distribution. Not allowed for
%                     other distributions. If 'theta' is not given it is
%                     estimated by the minimum value of the data.
%
%   Note: ALLFITDIST does not handle nonparametric kernel-smoothing,
%   use FITDIST directly instead.
%
%
%   EXAMPLE 1
%     Given random data from an unknown continuous distribution, find the
%     best distribution which fits that data, and plot the PDFs to compare
%     graphically.
%        data = normrnd(5,3,1e4,1);         %Assumed from unknown distribution
%        [D PD] = allfitdist(data,'PDF');   %Compute and plot results
%        D(1)                               %Show output from best fit
%
%   EXAMPLE 2
%     Given random data from a discrete unknown distribution, with frequency
%     data, find the best discrete distribution which would fit that data,
%     sorted by 'NLogL', and plot the PDFs to compare graphically.
%        data = nbinrnd(20,.3,1e4,1);
%        values=unique(data); freq=histc(data,values);
%        [D PD] = allfitdist(values,'NLogL','frequency',freq,'PDF','DISCRETE');
%        PD{1}
%
%  EXAMPLE 3
%     Although the Geometric Distribution is not listed, it is a special
%     case of fitting the more general Negative Binomial Distribution. The
%     parameter 'r' should be close to 1. Show by example.
%        data=geornd(.7,1e4,1); %Random from Geometric
%        [D PD]= allfitdist(data,'PDF','DISCRETE');
%        PD{1}
%
%  EXAMPLE 4
%     Compare the resulting distributions under two different assumptions
%     of discrete data. The first, that it is known to be derived from a
%     Binomial Distribution with known 'n'. The second, that it may be
%     Binomial but 'n' is unknown and should be estimated. Note the second
%     scenario may not yield a Binomial Distribution as the best fit, if
%     'n' is estimated incorrectly. (Best to run example a couple times
%     to see effect)
%        data = binornd(10,.3,1e2,1);
%        [D1 PD1] = allfitdist(data,'n',10,'DISCRETE','PDF'); %Force binomial
%        [D2 PD2] = allfitdist(data,'DISCRETE','PDF');       %May be binomial
%        PD1{1}, PD2{1}                             %Compare distributions
%

%    Mike Sheppard
%    Last Modified: 17-Feb-2012

%% Check Inputs
if nargin == 0
    data = 10.^((normrnd(2,10,1e4,1))/10);
    sortby='BIC';
    varargin={'CDF'};
end
if nargin==1
    sortby='BIC';
end
sortbyname={'NLogL','BIC','AIC','AICc'};
if ~any(ismember(lower(sortby),lower(sortbyname)))
    oldvar=sortby; %May be 'PDF' or 'CDF' or other commands
    if isempty(varargin)
        varargin={oldvar};
    else
        varargin=[oldvar varargin];
    end
    sortby='BIC';
end
if nargin < 2, sortby='BIC'; end
distname={'beta', 'birnbaumsaunders', 'exponential', ...
    'extreme value', 'gamma', 'generalized extreme value', ...
    'generalized pareto', 'inversegaussian', 'logistic', 'loglogistic', ...
    'lognormal', 'nakagami', 'normal', ...
    'rayleigh', 'rician', 'tlocationscale', 'weibull'};
if ~any(strcmpi(sortby,sortbyname))
    error('allfitdist:SortBy','Sorting must be either NLogL, BIC, AIC, or AICc');
end
%Input may be mixed of numeric and strings, find only strings
vin=varargin;
strs=find(cellfun(@(vs)ischar(vs),vin));
vin(strs)=lower(vin(strs));
%Next check to see if 'PDF' or 'CDF' is listed
numplots=sum(ismember(vin(strs),{'pdf' 'cdf'}));
if numplots>=2
    error('ALLFITDIST:PlotType','Either PDF or CDF must be given');
end
if numplots==1
    plotind=true; %plot indicator
    indxpdf=ismember(vin(strs),'pdf');
    plotpdf=any(indxpdf);
    indxcdf=ismember(vin(strs),'cdf');
    vin(strs(indxpdf|indxcdf))=[]; %Delete 'PDF' and 'CDF' in vin
else
    plotind=false;
end
%Check to see if discrete
strs=find(cellfun(@(vs)ischar(vs),vin));
indxdis=ismember(vin(strs),'discrete');
discind=false;
if any(indxdis)
    discind=true;
    distname={'binomial', 'negative binomial', 'poisson'};
    vin(strs(indxdis))=[]; %Delete 'DISCRETE' in vin
end
strs=find(cellfun(@(vs)ischar(vs),vin));
n=numel(data); %Number of data points
data = data(:);
D=[];
%Check for NaN's to delete
deldatanan=isnan(data);
%Check to see if frequency is given
indxf=ismember(vin(strs),'frequency');
if any(indxf)
    freq=vin{1+strs((indxf))}; freq=freq(:);
    if numel(freq)~=numel(data)
        error('ALLFITDIST:PlotType','Matrix dimensions must agree');
    end
    delfnan=isnan(freq);
    data(deldatanan|delfnan)=[]; freq(deldatanan|delfnan)=[];
    %Save back into vin
    vin{1+strs((indxf))}=freq;
else
    data(deldatanan)=[];
end

%% Run through all distributions in FITDIST function
warning('off','all'); %Turn off all future warnings
for indx=1:length(distname)
    try
        dname=distname{indx};
        switch dname
            case 'binomial'
                PD=fitbinocase(data,vin,strs); %Special case
            case 'generalized pareto'
                PD=fitgpcase(data,vin,strs); %Special case
            otherwise
                %Built-in distribution using FITDIST
                PD = fitdist(data,dname,vin{:});
        end

        NLL=PD.NLogL; % -Log(L)
        %If NLL is non-finite number, produce error to ignore distribution
        if ~isfinite(NLL)
            error('non-finite NLL');
        end
        num=length(D)+1;
        PDs(num) = {PD}; %#ok<*AGROW>
        k=numel(PD.Params); %Number of parameters
        D(num).DistName=PD.DistName;
        D(num).NLogL=NLL;
        D(num).BIC=-2*(-NLL)+k*log(n);
        D(num).AIC=-2*(-NLL)+2*k;
        D(num).AICc=(D(num).AIC)+((2*k*(k+1))/(n-k-1));
        D(num).ParamNames=PD.ParamNames;
        D(num).ParamDescription=PD.ParamDescription;
        D(num).Params=PD.Params;
        D(num).Paramci=PD.paramci;
        D(num).ParamCov=PD.ParamCov;
        D(num).Support=PD.Support;
    catch err %#ok<NASGU>
        %Ignore distribution
    end
end
warning('on','all'); %Turn back on warnings
if numel(D)==0
    error('ALLFITDIST:NoDist','No distributions were found');
end

%% Sort distributions
indx1=1:length(D); %Identity Map
sortbyindx=find(strcmpi(sortby,sortbyname));
switch sortbyindx
    case 1
        [~,indx1]=sort([D.NLogL]);
    case 2
        [~,indx1]=sort([D.BIC]);
    case 3
        [~,indx1]=sort([D.AIC]);
    case 4
        [~,indx1]=sort([D.AICc]);
end
%Sort
D=D(indx1); PD = PDs(indx1);

%% Plot if requested
if plotind;
    plotfigs(data,D,PD,vin,strs,plotpdf,discind)
end

end

function PD=fitbinocase(data,vin,strs)
%% Special Case for Binomial
% 'n' is estimated if not given
vinbino=vin;
%Check to see if 'n' is given
indxn=any(ismember(vin(strs),'n'));
%Check to see if 'frequency' is given
indxfreq=ismember(vin(strs),'frequency');
if ~indxn
    %Use Method of Moment estimator
    %E[x]=np, V[x]=np(1-p) -> nhat=E/(1-(V/E));
    if isempty(indxfreq)||~any(indxfreq)
        %Raw data
        mnx=mean(data);
        nhat=round(mnx/(1-(var(data)/mnx)));
    else
        %Frequency data
        freq=vin{1+strs(indxfreq)};
        m1=dot(data,freq)/sum(freq);
        m2=dot(data.^2,freq)/sum(freq);
        mnx=m1; vx=m2-(m1^2);
        nhat=round(mnx/(1-(vx/mnx)));
    end
    %If nhat is negative, use maximum value of data
    if nhat<=0, nhat=max(data(:)); end
    vinbino{end+1}='n'; vinbino{end+1}=nhat;
end
PD = fitdist(data,'binomial',vinbino{:});
end

function PD=fitgpcase(data,vin,strs)
%% Special Case for Generalized Pareto
% 'theta' is estimated if not given
vingp=vin;
%Check to see if 'theta' is given
indxtheta=any(ismember(vin(strs),'theta'));
if ~indxtheta
    %Use minimum value for theta, minus small part
    thetahat=min(data(:))-10*eps;
    vingp{end+1}='theta'; vingp{end+1}=thetahat;
end
PD = fitdist(data,'generalized pareto',vingp{:});
end

function plotfigs(data,D,PD,vin,strs,plotpdf,discind)
%Plot functionality for continuous case due to Jonathan Sullivan
%Modified by author for discrete case

%Maximum number of distributions to include
%max_num_dist=Inf;  %All valid distributions
max_num_dist=4;

%Check to see if frequency is given
indxf=ismember(vin(strs),'frequency');
if any(indxf)
    freq=vin{1+strs((indxf))};
end

figure

%% Probability Density / Mass Plot
if plotpdf
    if ~discind
        %Continuous Data
        nbins = max(min(length(data)./10,100),50);
        xi = linspace(min(data),max(data),nbins);
        dx = mean(diff(xi));
        xi2 = linspace(min(data),max(data),nbins*10)';
        fi = histc(data,xi-dx);
        fi = fi./sum(fi)./dx;
        inds = 1:min([max_num_dist,numel(PD)]);
        ys = cellfun(@(PD) pdf(PD,xi2),PD(inds),'UniformOutput',0);
        ys = cat(2,ys{:});
        bar(xi,fi,'FaceColor',[160 188 254]/255,'EdgeColor','k');
        hold on;
        plot(xi2,ys,'LineWidth',1.5)
        legend(['empirical',{D(inds).DistName}],'Location','NE')
        xlabel('Value');
        ylabel('Probability Density');
        title('Probability Density Function');
        grid on
    else
        %Discrete Data
        xi2=min(data):max(data);
        %xi2=unique(x)'; %If only want observed x-values to be shown
        indxf=ismember(vin(strs),'frequency');
        if any(indxf)
            fi=zeros(size(xi2));
            fi((ismember(xi2,data)))=freq; fi=fi'./sum(fi);
        else
            fi=histc(data,xi2); fi=fi./sum(fi);
        end
        inds = 1:min([max_num_dist,numel(PD)]);
        ys = cellfun(@(PD) pdf(PD,xi2),PD(inds),'UniformOutput',0);
        ys=cat(1,ys{:})';
        bar(xi2,[fi ys]);
        legend(['empirical',{D(inds).DistName}],'Location','NE')
        xlabel('Value');
        ylabel('Probability Mass');
        title('Probability Mass Function');
        grid on
    end
else

%Cumulative Distribution
    if ~discind
        %Continuous Data
        [fi xi] = ecdf(data);
        inds = 1:min([max_num_dist,numel(PD)]);
        ys = cellfun(@(PD) cdf(PD,xi),PD(inds),'UniformOutput',0);
        ys = cat(2,ys{:});
        if max(xi)/min(xi) > 1e4; lgx = true; else lgx = false; end
        subplot(2,1,1)
        if lgx
            semilogx(xi,fi,'k',xi,ys)
        else
            plot(xi,fi,'k',xi,ys)
        end
        legend(['empirical',{D(inds).DistName}],'Location','NE')
        xlabel('Value');
        ylabel('Cumulative Probability');
        title('Cumulative Distribution Function');
        grid on
        subplot(2,1,2)
        y = 1.1*bsxfun(@minus,ys,fi);
        if lgx
            semilogx(xi,bsxfun(@minus,ys,fi))
        else
            plot(xi,bsxfun(@minus,ys,fi))
        end
        ybnds = max(abs(y(:)));
        ax = axis;
        axis([ax(1:2) -ybnds ybnds]);
        legend({D(inds).DistName},'Location','NE')
        xlabel('Value');
        ylabel('Error');
        title('CDF Error');
        grid on
    else
        %Discrete Data
        indxf=ismember(vin(strs),'frequency');
        if any(indxf)
            [fi xi] = ecdf(data,'frequency',freq);
        else
            [fi xi] = ecdf(data);
        end
        %Check unique xi, combine fi
        [xi,ign,indx]=unique(xi); %#ok<ASGLU>
        fi=accumarray(indx,fi);
        inds = 1:min([max_num_dist,numel(PD)]);
        ys = cellfun(@(PD) cdf(PD,xi),PD(inds),'UniformOutput',0);
        ys=cat(2,ys{:});
        subplot(2,1,1)
        stairs(xi,[fi ys]);
        legend(['empirical',{D(inds).DistName}],'Location','NE')
        xlabel('Value');
        ylabel('Cumulative Probability');
        title('Cumulative Distribution Function');
        grid on
        subplot(2,1,2)
        y = 1.1*bsxfun(@minus,ys,fi);
        stairs(xi,bsxfun(@minus,ys,fi))
        ybnds = max(abs(y(:)));
        ax = axis;
        axis([ax(1:2) -ybnds ybnds]);
        legend({D(inds).DistName},'Location','NE')
        xlabel('Value');
        ylabel('Error');
        title('CDF Error');
        grid on
    end
end

end


%--------------------------------------------------------------------------
%	SurfBox-MATLAB (c)
%--------------------------------------------------------------------------
%
%	Yue M. Lu and Minh N. Do
%
%--------------------------------------------------------------------------
%
%	SurfEP.m
%
%	First created: 12-12-08
%	Last modified: 12-12-08
%
%--------------------------------------------------------------------------

function Ep = SurfEP(sz, Lev_array, Pyr_mode, bo, nRepeat)

%   Estimating the subband scaling coefficients of the surfacelet transform
%   using a Monte Carlo method.
%
%   Input:
%
%   sz: the size of a N-dimensional array (N >= 2)
%
%   Lev_array: an L by 1 cell array, with each cell being an N by N matrix
%   for NDFB decomposition, or a scaler (either 1 or 2) for dual-tree
%   wavelet decomposition.
%
%   Pyr_mode: type of multiscale pyramid, corresponding to different levels
%   of redundancy.
%   1:       ~ 6.4
%   1.5:     ~ 4.0
%   2:       ~ 3.4
%
%   bo: the order of the checkerboard filters. Default: bo = 12
%
%   nRepeat: the number of trials in the Monte Carlo method.
%
%   Output:
%
%   Ep: the estimated subband scaling coefficients

h = waitbar(0, ['Estimating the subband scaling coefficients.' ...
        'This can be a slow process, but it only needs to be done once for a given configuration.']);

for n = 1 : nRepeat
    X = randn(sz);
    [Y, Recinfo] = Surfdec(X, Pyr_mode, Lev_array, 'ritf', 'bo', bo);
    clear X; % to save some memory

    if n == 1
        Ep = Y;

        for j = 1 : length(Y) - 1
            for dd = 1 : length(Y{j})
                for sd = 1 : length(Y{j}{dd})
                    Ep{j}{dd}{sd} = Ep{j}{dd}{sd} .^ 2;
                end
            end
        end

    else
        for j = 1 : length(Y) - 1
            for dd = 1 : length(Y{j})
                for sd = 1 : length(Y{j}{dd})
                    Ep{j}{dd}{sd} = Ep{j}{dd}{sd} + Y{j}{dd}{sd} .^ 2;
                end
            end
        end
    end

    clear Y;

    waitbar(n / nRepeat, h);
end
close(h);

for j = 1 : length(Ep) - 1
    for dd = 1 : length(Ep{j})
        for sd = 1 : length(Ep{j}{dd})
            Ep{j}{dd}{sd} = sqrt(Ep{j}{dd}{sd} / (nRepeat - 1));
        end
    end
end


% function [Lh,Ph,Mu,Pi,LL]=mfa(X,M,K,cyc,tol);
%
% Maximum Likelihood Mixture of Factor Analysis using EM
%
% X - data matrix
% M - number of mixtures (default 1)
% K - number of factors in each mixture (default 2)
% cyc - maximum number of cycles of EM (default 100)
% tol - termination tolerance (prop change in likelihood) (default 0.0001)
%
% Lh - factor loadings
% Ph - diagonal uniquenesses matrix
% Mu - mean vectors
% Pi - priors
% LL - log likelihood curve
%
% Iterates until a proportional change < tol in the log likelihood
% or cyc steps of EM

function [Lh, Ph,  Mu, Pi, LL] = mfa(X,M,K,cyc,tol)

if nargin<5   tol=0.0001; end;
if nargin<4   cyc=100; end;
if nargin<3   K=2; end;
if nargin<2   M=1; end;

N=length(X(:,1));
D=length(X(1,:));
tiny=exp(-700);

%rand('state',0);

fprintf('\n');

if (M==1)
  [Lh,Ph,LL]=ffa(X,K,cyc,tol);
  Mu=mean(X);
  Pi=1;
else
  if N==1
    mX = X;
  else
    mX=mean(X);
  end
  cX=cov(X);
  scale=det(cX)^(1/D);
  randn('state',0);
  Lh=randn(D*M,K)*sqrt(scale/K);
  Ph=diag(cX)+tiny;
  Pi=ones(M,1)/M;
  %randn('state',0);
  Mu=randn(M,D)*sqrtm(cX)+ones(M,1)*mX;
  oldMu=Mu;
  I=eye(K);

  lik=0;
  LL=[];

  H=zeros(N,M); 	% E(w|x)
  EZ=zeros(N*M,K);
  EZZ=zeros(K*M,K);
  XX=zeros(D*M,D);
  s=zeros(M,1);
  const=(2*pi)^(-D/2);
  %%%%%%%%%%%%%%%%%%%%
  for i=1:cyc;

    %%%% E Step %%%%

    Phi=1./Ph;
    Phid=diag(Phi);
    for k=1:M
      Lht=Lh((k-1)*D+1:k*D,:);
      LP=Phid*Lht;
      MM=Phid-LP*inv(I+Lht'*LP)*LP';
      dM=sqrt(det(MM));
      Xk=(X-ones(N,1)*Mu(k,:));
      XM=Xk*MM;
      H(:,k)=const*Pi(k)*dM*exp(-0.5*rsum(XM.*Xk));
      EZ((k-1)*N+1:k*N,:)=XM*Lht;
    end;

    Hsum=rsum(H);
    oldlik=lik;
    lik=sum(log(Hsum+(Hsum==0)*exp(-744)));

    Hzero=(Hsum==0); Nz=sum(Hzero);
    H(Hzero,:)=tiny*ones(Nz,M)/M;
    Hsum(Hzero)=tiny*ones(Nz,1);

    H=rdiv(H,Hsum);
    s=csum(H);
    s=s+(s==0)*tiny;
    s2=sum(s)+tiny;

    for k=1:M
      kD=(k-1)*D+1:k*D;
      Lht=Lh(kD,:);
      LP=Phid*Lht;
      MM=Phid-LP*inv(I+Lht'*LP)*LP';
      Xk=(X-ones(N,1)*Mu(k,:));
      XX(kD,:)=rprod(Xk,H(:,k))'*Xk/s(k);
      beta=Lht'*MM;
      EZZ((k-1)*K+1:k*K,:)=I-beta*Lht +beta*XX(kD,:)*beta';
    end;

    %%%% log likelihood %%%%

    LL=[LL lik];
    fprintf('cycle %g   \tlog likelihood %g ',i,lik);

    if (i<=2)
      likbase=lik;
    elseif (lik<oldlik)
      fprintf(' violation');
    elseif ((lik-likbase)<(1 + tol)*(oldlik-likbase)||~isfinite(lik))
      break;
    end;

    fprintf('\n');

    %%%% M Step %%%%

    % means and covariance structure

    Ph=zeros(D,1);
    for k=1:M
      kD=(k-1)*D+1:k*D;
      kK=(k-1)*K+1:k*K;
      kN=(k-1)*N+1:k*N;

      T0=rprod(X,H(:,k));
      T1=T0'*[EZ(kN,:) ones(N,1)];
      XH=EZ(kN,:)'*H(:,k);
      T2=inv([s(k)*EZZ(kK,:) XH; XH' s(k)]);
      T3=T1*T2;
      Lh(kD,:)=T3(:,1:K);
      Mu(k,:)=T3(:,K+1)';
      T4=diag(T0'*X-T3*T1')/s2;
      Ph=Ph+T4.*(T4>0);
    end;

    Phmin=exp(-700);
    Ph=Ph.*(Ph>Phmin)+(Ph<=Phmin)*Phmin; % to avoid zero variances

    % priors
    Pi=s'/s2;

  end;
  fprintf('\n');
end;


%DEMO_REGRESSION_ADDITIVE2  Regression demonstration with additive Gaussian
%                           process using linear, squared exponential and
%                           neural network covariance fucntions
%
%  Description
%    Gaussian process solutions in 2D regression problem using
%    constant, linear, squared exponential (sexp) and neural
%    network covariance functions, and with various additive
%    combinations of these four covariance functions. The noisy
%    observations y are assumed to satisfy
%
%         y = f + e,    where e ~ N(0, s^2)
%
%    where f is an unknown underlying function. A zero mean Gaussian
%    process prior is assumed for f
%
%         f ~ N(0, K),
%
%    where K is the covariance matrix whose elements are given by one of
%    the following six covariance function:
%
%    - constant + linear
%    - costant + sexp for 1. input + linear for 2. input
%    - sexp for 1. input + sexp for 2. input
%    - sexp
%    - neural network for 1. input + neural network for 2. input
%    - neural network
%
%    A prior is assumed for parameters of the covariance functions,
%    and the inference is done with a MAP estimate for parameter
%    values.
%
%    For more detailed discussion of  covariance functions, see e.g.
%
%    Rasmussen, C. E. and Williams, C. K. I. (2006). Gaussian
%    Processes for Machine Learning. The MIT Press.
%
%
%  See also
%    DEMO_REGRESSION1
%
% Copyright (c) 2010 Jaakko Riihimäki, Aki Vehtari

% 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.

% REGRESSION TOY DATA
% Create an example regression data
x=rand(225,2)*2-1;
y=3*norm_cdf(2*x(:,2))+2*norm_cdf(4*x(:,1));
% add some noise
y=y+randn(size(y))*0.25;
y=y-mean(y);
[n, nin] = size(x);
% create equally spaced points to visualise the predictions:
[xt1,xt2]=meshgrid(-2:0.1:2,-2:0.1:2);
xt=[xt1(:) xt2(:)];
nxt=size(xt1,1);

% Assume a Student-t distribution for the GP parameters
pt = prior_t('nu', 4, 's2', 10);	% a prior structure

% Create a Gaussian noise model
lik = lik_gaussian('sigma2', 0.2^2);

% Set a small amount of jitter
jitter=1e-4;

% Set the options for the optimization
opt=optimset('TolFun',1e-3,'TolX',1e-3);

% CONSTANT + LINEAR COVARIANCE FUNCTION
disp('Constant + linear covariance function')
% constant covariance function
gpcf_c = gpcf_constant('constSigma2', 1, 'constSigma2_prior', pt);
% linear covariance function
gpcf_l = gpcf_linear('coeffSigma2_prior', pt);
gp = gp_set('lik', lik, 'cf', {gpcf_c gpcf_l}, 'jitterSigma2', jitter);

% Optimize with the scaled conjugate gradient method
gp=gp_optim(gp,x,y,'opt',opt);

% Compute predictions in a grid using the MAP estimate
Eft_map = gp_pred(gp, x, y, xt);

% Plot the prediction and data
figure
set(gcf, 'color', 'w')
mesh(xt1, xt2, reshape(Eft_map,nxt,nxt));
hold on
plot3(x(:,1), x(:,2), y, '*')
xlabel('x_1'), ylabel('x_2')
title('The predicted underlying function (constant + linear)');

% CONSTANT + SQUARED EXPONENTIAL COVARIANCE FUNCTION (W.R.T. THE
% FIRST INPUT DIMENSION) + LINEAR (W.R.T. THE SECOND INPUT
% DIMENSION)
fprintf(['Constant + squared exponential covariance function\n' ...
         '(w.r.t. the first input dimension) + linear (w.r.t.\n' ...
         'the second input dimension)\n'])

%Covariance function for the first input variable
gpcf_s1 = gpcf_sexp('selectedVariables', 1, 'lengthScale',0.5, ...
                    'lengthScale_prior', pt, 'magnSigma2', 0.15, ...
                    'magnSigma2_prior', pt);
% gpcf_s1 can be construted also as
%metric1 = metric_euclidean('components', {[1]}, 'lengthScale',[0.5], ...
%                           'lengthScale_prior', pt);
%gpcf_s1 = gpcf_sexp('magnSigma2', 0.15, 'magnSigma2_prior', pt, ...
%                    'metric', metric1);
%Covariance function for the second input variable
gpcf_l2 = gpcf_linear('selectedVariables', 2, 'coeffSigma2_prior', pt);
gp = gp_set('lik', lik, 'cf', {gpcf_c gpcf_s1 gpcf_l2}, 'jitterSigma2', jitter);

% Optimize with the scaled conjugate gradient method
gp=gp_optim(gp,x,y,'opt',opt);

% Compute predictions in a grid using the MAP estimate
Eft_map = gp_pred(gp, x, y, xt);

% Plot the prediction and data
figure
set(gcf, 'color', 'w')
mesh(xt1, xt2, reshape(Eft_map,nxt,nxt));
hold on
plot3(x(:,1), x(:,2), y, '*')
xlabel('x_1'), ylabel('x_2')
title('The predicted underlying function (sexp for 1. input + linear for 2. input )');

% ADDITIVE SQUARED EXPONENTIAL COVARIANCE FUNCTION
disp('Additive squared exponential covariance function')

% Covariance function for the second input variable
gpcf_s2 = gpcf_sexp('selectedVariables', 2,'lengthScale',0.5, ...
                    'lengthScale_prior', pt, 'magnSigma2', 0.15, ...
                    'magnSigma2_prior', pt);
% gpcf_s2 can be construted also as
%metric2 = metric_euclidean('components', {[2]},'lengthScale',[0.5], 'lengthScale_prior', pt);
%gpcf_s2 = gpcf_sexp('magnSigma2', 0.15, 'magnSigma2_prior', pt, 'metric', metric2);
gp = gp_set('lik', lik, 'cf', {gpcf_s1,gpcf_s2}, 'jitterSigma2', jitter);

% Optimize with the scaled conjugate gradient method
gp=gp_optim(gp,x,y,'opt',opt);

% Compute predictions in a grid using the MAP estimate
Eft_map = gp_pred(gp, x, y, xt);

% Plot the prediction and data
figure
set(gcf, 'color', 'w')
mesh(xt1, xt2, reshape(Eft_map,nxt,nxt));
hold on
plot3(x(:,1), x(:,2), y, '*')
xlabel('x_1'), ylabel('x_2')
title('The predicted underlying function (additive sexp)');

% SQUARED EXPONENTIAL COVARIANCE FUNCTION
disp('Squared exponential covariance function')

gpcf_s = gpcf_sexp('lengthScale', ones(1,nin), 'magnSigma2', 0.2^2, ...
                   'lengthScale_prior', pt, 'magnSigma2_prior', pt);
gp = gp_set('lik', lik, 'cf', {gpcf_s}, 'jitterSigma2', jitter);

% Optimize with the scaled conjugate gradient method
gp=gp_optim(gp,x,y,'opt',opt);

% Compute predictions in a grid using the MAP estimate
Eft_map = gp_pred(gp, x, y, xt);

% Plot the prediction and data
figure
set(gcf, 'color', 'w')
mesh(xt1, xt2, reshape(Eft_map,nxt,nxt));
hold on
plot3(x(:,1), x(:,2), y, '*')
xlabel('x_1'), ylabel('x_2')
title('The predicted underlying function (sexp)');

% ADDITIVE NEURAL NETWORK COVARIANCE FUNCTION
disp('Additive neural network covariance function');

gpcf_nn1 = gpcf_neuralnetwork('weightSigma2', 1, 'biasSigma2', 1, 'selectedVariables', [1], ...
                              'weightSigma2_prior', pt, 'biasSigma2_prior', pt);
gpcf_nn2 = gpcf_neuralnetwork('weightSigma2', 1, 'biasSigma2', 1, 'selectedVariables', [2], ...
                              'weightSigma2_prior', pt, 'biasSigma2_prior', pt);
gp = gp_set('lik', lik, 'cf', {gpcf_nn1,gpcf_nn2}, 'jitterSigma2', jitter);

% Optimize with the scaled conjugate gradient method
gp=gp_optim(gp,x,y,'opt',opt);

% Compute predictions in a grid using the MAP estimate
Eft_map = gp_pred(gp, x, y, xt);

% Plot the prediction and data
figure
set(gcf, 'color', 'w')
mesh(xt1, xt2, reshape(Eft_map,nxt,nxt));
hold on
plot3(x(:,1), x(:,2), y, '*')
xlabel('x_1'), ylabel('x_2')
title('The predicted underlying function (additive neural network)');

% NEURAL NETWORK COVARIANCE FUNCTION
disp('Neural network covariance function')

gpcf_nn = gpcf_neuralnetwork('weightSigma2', ones(1,nin), 'biasSigma2', 1, ...
                             'weightSigma2_prior', pt, 'biasSigma2_prior', pt);
gp = gp_set('lik', lik, 'cf', {gpcf_nn}, 'jitterSigma2', jitter);

% Optimize with the scaled conjugate gradient method
gp=gp_optim(gp,x,y,'opt',opt);

% Compute predictions in a grid using the MAP estimate
Eft_map = gp_pred(gp, x, y, xt);

% Plot the prediction and data
figure
set(gcf, 'color', 'w')
mesh(xt1, xt2, reshape(Eft_map,nxt,nxt));
hold on
plot3(x(:,1), x(:,2), y, '*')
xlabel('x_1'), ylabel('x_2')
title('The predicted underlying function (neural network)');


function precision = precision_table ( rule )

%*****************************************************************************80
%
%% PRECISION_TABLE returns the precision of a Lebedev rule.
%
%  Modified:
%
%    15 September 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Vyacheslav Lebedev, Dmitri Laikov,
%    A quadrature formula for the sphere of the 131st
%    algebraic order of accuracy,
%    Russian Academy of Sciences Doklady Mathematics,
%    Volume 59, Number 3, 1999, pages 477-481.
%
%  Parameters:
%
%    Input, integer RULE, the index of the rule, between 1 and 65.
%
%    Output, integer PRECISION, the precision of the rule.
%
  rule_max = 65;

  table = [ ...
      3,   5,   7,   9,  11,  13,  15,  17,  19,  21, ...
     23,  25,  27,  29,  31,  33,  35,  37,  39,  41, ...
     43,  45,  47,  49,  51,  53,  55,  57,  59,  61, ...
     63,  65,  67,  69,  71,  73,  75,  77,  79,  81, ...
     83,  85,  87,  89,  91,  93,  95,  97,  99, 101, ...
    103, 105, 107, 109, 111, 113, 115, 117, 119, 121, ...
    123, 125, 127, 129, 131 ]';

  if ( rule < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'PRECISION_TABLE - Fatal error!\n' );
    fprintf ( 1, '  RULE < 1.\n' );
    error ( 'PRECISION_TABLE - Fatal error!' );
  elseif ( rule_max < rule )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'PRECISION_TABLE - Fatal error!\n' );
    fprintf ( 1, '  RULE_MAX < RULE.\n' );
    error ( 'PRECISION_TABLE - Fatal error!' );
  end

  precision = table(rule);

  return
end


function Y = khatri_rao(A, B)
%KHATRI_RAO Performs Khatri-Rao product (column-wise Kronecker product).
%Syntax:
%   Y = KHATRI_RAO(A, B)

[r1, c1] = size(A);
[r2, c2] = size(B);
if c1 ~= c2
    error('Column size mismatch');
end

Y = zeros(r1 * r2, c1);
for i = 1:c1
    Y(:,i) = kron(A(:,i), B(:,i));
end

end


% Color Mixture Non-local Pixel Affinities
% This function implements the color-mixture information flow in
% Yagiz Aksoy, Tunc Ozan Aydin, Marc Pollefeys, "Designing Effective
% Inter-Pixel Information Flow for Natural Image Matting", CVPR, 2017
% when the input parameter 'useXYinLLEcomp' is false (default), and
% the affinity definition used in
% Xiaowu Chen, Dongqing Zou, Qinping Zhao, Ping Tan, "Manifold
% preserving edit propagation", ACM TOG, 2012
% when 'useXYinLLEcomp' is true.
% All parameters other than image are optional. The output is a sparse
% matrix which has non-zero element for the non-local neighbors of
% the pixels given by binary map inMap.
% - K defines the number of neighbors from which LLE weights are
%   computed.
% - outMap is a binary map that defines where the nearest neighbor
%   search is done.
% - xyWeight determines how much importance is given to the spatial
%   coordinates in the nearest neighbor selection.

function Wcm = colorMixtureAffinities(image, K, inMap, outMap, xyWeight, useXYinLLEcomp)

    [h, w, ~] = size(image);
    N = h * w;

    if ~exist('K', 'var') || isempty(K)
        K = 20;
    end
    if ~exist('inMap', 'var') || isempty(inMap)
        inMap = true(h, w);
    end
    if ~exist('outMap', 'var') || isempty(outMap)
        outMap = true(h, w);
    end
    if ~exist('xyWeight', 'var') || isempty(xyWeight)
        xyWeight = 1;
    end
    if ~exist('useXYinLLEcomp', 'var') || isempty(useXYinLLEcomp)
        useXYinLLEcomp = false;
    end

    [inInd, neighInd, features] = findNonlocalNeighbors(image, K, xyWeight, inMap, outMap);

    if ~useXYinLLEcomp
        features = features(:, 1 : end - 2);
    end
    flows = zeros(size(inInd, 1), size(neighInd, 2));

    for i = 1 : size(inInd, 1)
        flows(i, :) = localLinearEmbedding(features(inInd(i), :)', features(neighInd(i, :), :)', 1e-10);
    end
    flows = flows ./ repmat(sum(flows, 2), [1, K]);

    inInd = repmat(inInd, [1, K]);
    Wcm = sparse(inInd(:), neighInd(:), flows, N, N);
end


function [Q fcnEvals iter] = adaptiveLobatto(fcn, a, b, varargin)
% adaptiveLobatto - Numerically evaluate integral, adaptive Lobatto quadrature.
%
% function [Q fcnEvals iter] = adaptiveLobatto(fcn, a, b, varargin)
%
% (c) Matthias Conrad and Nils Papenberg (2007-08-03)
%
% Authors:
%   Matthias Conrad (e-mail: conrad@tiaco.de)
%   Nils Papenberg  (e-mail: papenber@math.uni-luebeck.de)
%
% Version:
%		Release date: 2008-08-12   Version: 1.2
%   MATLAB Version 7.5.0.338 (R2007b)
%
% Description:
%   The adaptive Lobatto algorithm programmed in an iterative not recursive
%   manner
%
% Input arguments:
%   fcn             - function to be integrated
%   a               - first point of interval
%   b               - final point of interval
%   #varargin       - further options of algorithm
%     tol           - tolerance accuracy of quadrature [ 1e-6 ]
%     parts         - initial number of partitions [ 2 ]
%     maxFcnEvals   - maximal number of function evaluations allowed [ 20000 ]
%     maxParts      - maximal number of partitions allowed [ 8000 ]
%
% Output arguments:
%   Q               - numerical integral of function fcn on [a,b]
%   fcnEvals        - number of function evaluations
%   iter            - number of iterations
%
% Details:
%   This function behavior is similar to of Matlab integrated function "quadv".
%
%   Example:
%     Q = adaptiveLobatto(@(x) [-cos(50*x); sin(x)], 0, pi, 'tol', 1e-6)
%
% References:
%   [1] Gander, W. & Gautschi, W. Adaptive Quadrature - Revisited
%       Eidgenoessische technische Hochschule Zuerich, 2000.

% check scalar limits of interval
if ~isscalar(a) || ~isscalar(b)
  error('Matlab:adaptiveLobatto:Limits',...
    'The limits of integration must be scalars.');
end

% default values
tol = 1e-6; parts = 2; maxFcnEvals = 20000; maxParts = 8000;

% rewrite default options if needed
for j = 1 : length(varargin) / 2
  eval([varargin{2 * j - 1},'=varargin{',int2str(2 * j),'};']);
end

% initial values, termination constant, parts of interval and integral value
m = parts; parts = 4 * parts + 1; Q = 0;
minH = eps(b - a) / 1024; maxResolution = 0; iter = 0;
poleWarning = 0;

% width constants
alpha = sqrt(2/3); beta = sqrt(1/5);

% check if interval has infinite boundaries, in case substitute function
if ~isfinite(a) || ~isfinite(b)
  warning('Matlab:adaptiveLobatto:infiniteInterval',...
    'The integral has an infinite interval; proceed with a substitution of function on finite interval.')
  if ~isfinite(a) && isfinite(b)
    [Q fcnEvals iter] = adaptiveLobatto(fcn, 0, b, varargin);
    fcn = @(t) infiniteLeft(t, fcn);
    a = 0; b = 1;
  elseif isfinite(a) && ~isfinite(b)
    [Q fcnEvals iter] = adaptiveLobatto(fcn, a, 0, varargin);
    fcn = @(t) infiniteRight(t, fcn);
    a = 0; b = 1;
  else
    fcn = @(t) infiniteBoth(t, fcn);
    a = - pi / 2; b = pi / 2;
  end
end

% initialize grid
t = linspace(a, b, m + 1);
A = t(1:end-1); B = t(2:end);

% widths and midpoints of intervals
H = diff(t)/2; J = (A + B) / 2;

% grid points
F = -alpha * H + J; D = -beta * H + J; C =  J;
E =  beta * H + J; G =  alpha * H + J;
t = [A; F; D; C; E; G; B]; t = t(:);

% function evaluations
y = fcn([A, F, D, C, E, G, B]); fcnEvals = 7 * m;

% avoid infinities at start point of interval
if any(~isfinite(y(:,1)))
  y(:,1) = fcn(a + eps(superiorfloat(a,b)) * (b - a));
  fcnEvals = fcnEvals + 1;
  poleWarning = 1;
end

% avoid infinities at end point of interval
if any(~isfinite(y(:, end)))
  y(:, end) = fcn(b - eps(superiorfloat(a,b)) * (b - a));
  fcnEvals = fcnEvals + 1;
  poleWarning = 1;
end

% poles at initial points
if ~isempty(find(~isfinite(max(abs(y))))), poleWarning = 1; end

% hand over function values
yA = y(:,     1 :   m); yF = y(:,   m+1 : 2*m); yD = y(:, 2*m+1 : 3*m);
yC = y(:, 3*m+1 : 4*m); yE = y(:, 4*m+1 : 5*m); yG = y(:, 5*m+1 : 6*m);
yB = y(:, 6*m+1 : end);

% dimension of parallel integration
n = size(yA,1);

% adaptive Lobatto iteration
while 1

  % number of iteration
  iter = iter + 1;

  % four point Lobatto formula
  Q1 = kron(H, ones(n,1)) / 6 .* (yA + 5 * (yD + yE) + yB);
  % seven point Kronrod formula
  Q2 = kron(H, ones(n,1)) / 1470 .* (77 * (yA + yB) + 432 * (yF + yG) + 625 * (yD + yE) + 672 * yC);

  % difference of Lobatto formulas
  diffQ = Q2 - Q1; diffQ(find(isnan(diffQ))) = 0;

  % intervals which do not fulfill termination criterion
  idx = find(max(abs(diffQ), [], 1) > tol);

  % intervals fulfill termination criterion
  idxQ = setdiff(1:length(A), idx);

  % check stop criterions
  STOP1 = isempty(idx); % check regular termination
  STOP2 = fcnEvals > maxFcnEvals;  % check maximal function evaluations
  STOP3 = 5 * length(idx) > maxParts; % check maximal partition

  % regular termination
  if STOP1
    Q = Q + sum(Q2, 2);
    break
  end

  % check if maximal resolution reached
  idxH = find(abs(H) < minH);
  if ~isempty(idxH)
    Q = Q + sum(Q2(idxH), 2);
    idx = setdiff(idx, idxH);
    idxQ = setdiff(idxQ, idxH);
    maxResolution = 1;
    % termination criterion
    if isempty(idx), break, end
  end

  % maximal function evaluations reached
  if STOP2
    warning('Matlab:adaptiveLobatto:MaxEvaluations',...
      'The maximal number of function evaluations reached; singularity likely.')
    Q = Q + sum(Q2, 2);
    break
  end

  % maximal partition reached
  if STOP3
    warning('Matlab:adaptiveLobatto:parts',...
      'The maximal number of parts reached.')
    Q = Q + sum(Q2, 2);
    break
  end

  % update quadrature value
  Q = Q + sum(Q2(:,idxQ) + diffQ(:,idxQ) / 15, 2);

  % number of intervals
  m = 6 * length(idx);

  % initialize t
  t = zeros(1, 6 * length(idx));

  % hand over new start points A
  t(1:6:end) = A(idx); t(2:6:end) = F(idx); t(3:6:end) = D(idx);
  t(4:6:end) = C(idx); t(5:6:end) = E(idx); t(6:6:end) = G(idx);
  A = t;

  % hand over new end points B
  t(1:6:end) = F(idx); t(2:6:end) = D(idx); t(3:6:end) = C(idx);
  t(4:6:end) = E(idx); t(5:6:end) = G(idx); t(6:6:end) = B(idx);
  B = t;

  y = zeros(n, 6 * length(idx));
  % hand over new start values A
  y(:,1:6:end) = yA(:,idx); y(:,2:6:end) = yF(:,idx); y(:,3:6:end) = yD(:,idx);
  y(:,4:6:end) = yC(:,idx); y(:,5:6:end) = yE(:,idx); y(:,6:6:end) = yG(:,idx);
  yA = y;

  % hand over new end values B
  y(:,1:6:end) = yF(:,idx); y(:,2:6:end) = yD(:,idx); y(:,3:6:end) = yC(:,idx);
  y(:,4:6:end) = yE(:,idx); y(:,5:6:end) = yG(:,idx); y(:,6:6:end) = yB(:,idx);
  yB = y;

  % widths and midpoints of intervals
  H = (B - A) / 2; J = (A + B) / 2;

  % calculate new mid points
  F = -alpha * H + J; D = -beta * H + J; C =  J;
  E =  beta * H + J; G =  alpha * H + J;

  % function evaluations
  y = fcn([F, D, C, E, G]); fcnEvals = fcnEvals + 5 * m;

  % poles at new points
  if ~isempty(find(~isfinite(max(abs(y))))), poleWarning = 1; end

  % hand over new midpoint values of F D C E and G
  yF = y(:,     1 :   m); yD = y(:,   m+1 : 2*m); yC = y(:, 2*m+1 : 3*m);
  yE = y(:, 3*m+1 : 4*m); yG = y(:, 4*m+1 : 5*m);

end

% display warnings
if any(~isfinite(Q))
  warning('Matlab:adaptiveLobatto:Infinite',...
    'The Quadrature of the function reached infinity or is Not-a-Number.')
end
if maxResolution
  warning('Matlab:adaptiveLobatto:MaxResolution',...
    'The maximal resolution of partial interval reached; singularity likely.')
end
if poleWarning
  warning('Matlab:adaptiveLobatto:PoleDetection',...
    'A detection of a pole; singularity likely.')
end

return

% substitute function interval [-inf, 0] on [0, 1]
function f = infiniteLeft(t, fcn)
f = fcn(log(t));
f = f ./ kron(ones(size(f,1),1), t);
return

% substitute function interval [0, inf] on [0, 1]
function f = infiniteRight(t, fcn)
f = fcn(-log(t));
f = f ./ kron(ones(size(f,1),1), t);
return

% substitute function interval [-inf, inf] on [-pi / 2, pi / 2]
function f = infiniteBoth(t, fcn)
f = fcn(tan(t));
f = f ./ kron(ones(size(f,1),1), cos(t).^2);
return


function gqu_sparse_test ( )

%*****************************************************************************80
%
%% GQU_SPARSE_TEST uses the GQU function to build a sparse grid.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2014
%
%  Author:
%
%    Original MATLAB version by Florian Heiss, Viktor Winschel.
%
%  Local parameters:
%
%    Local, integer D, the spatial dimension.
%
%    Local, integer MAXK, the maximum level to check.
%
  d = 6;
  maxk = 10;
  func = 'prod ( exp(-(x/2).^2/2)/2/sqrt(2*pi), 2 ) ';
  trueval = fu_integral ( d );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'GQU_SPARSE_TEST:\n' );
  fprintf ( 1, '  GQU sparse grid:\n' );
  fprintf ( 1, '  Sparse Gaussian unweighted quadrature over [0,1].\n' );
  fprintf ( 1, '  Exact integral is %g\n', trueval );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   D  Level   Nodes    SG error    MC error\n' );
  fprintf ( 1, '\n' );

  for k = 1 : maxk
%
%  Compute sparse grid estimate.
%
    [ x, w ] = nwspgr ( 'gqu', d, k );
    fx = eval ( func );
    SGappr = w' * fx;
    SGerror = sqrt ( ( SGappr - trueval ) .^ 2 ) / trueval;
%
%  Average 1000 Monte Carlo estimates.
%
    numnodes = length ( w );
    sim = zeros ( 1000, 1 );
    for r = 1 : 1000
      x = rand ( numnodes, d );
      fx = eval ( func );
      sim(r) = mean ( fx );
    end
    simerror = sqrt ( mean ( ( sim - trueval ) .^ 2 ) ) / trueval;

    fprintf( '  %2d     %2d  %6d  %10.5g  %10.5g\n', ...
      d, k, numnodes, SGerror, simerror )

  end

  return
end


function [z,s,exitflag] = smoothn(varargin)

%SMOOTHN Robust spline smoothing for 1-D to N-D data.
%   SMOOTHN provides a fast, automatized and robust discretized smoothing
%   spline for data of any dimension.
%
%   Z = SMOOTHN(Y) automatically smoothes the uniformly-sampled array Y. Y
%   can be any N-D noisy array (time series, images, 3D data,...). Non
%   finite data (NaN or Inf) are treated as missing values.
%
%   Z = SMOOTHN(Y,S) smoothes the array Y using the smoothing parameter S.
%   S must be a real positive scalar. The larger S is, the smoother the
%   output will be. If the smoothing parameter S is omitted (see previous
%   option) or empty (i.e. S = []), it is automatically determined using
%   the generalized cross-validation (GCV) method.
%
%   Z = SMOOTHN(Y,W) or Z = SMOOTHN(Y,W,S) specifies a weighting array W of
%   real positive values, that must have the same size as Y. Note that a
%   nil weight corresponds to a missing value.
%
%   Robust smoothing
%   ----------------
%   Z = SMOOTHN(...,'robust') carries out a robust smoothing that minimizes
%   the influence of outlying data.
%
%   [Z,S] = SMOOTHN(...) also returns the calculated value for S so that
%   you can fine-tune the smoothing subsequently if needed.
%
%   An iteration process is used in the presence of weighted and/or missing
%   values. Z = SMOOTHN(...,OPTION_NAME,OPTION_VALUE) smoothes with the
%   termination parameters specified by OPTION_NAME and OPTION_VALUE. They
%   can contain the following criteria:
%       -----------------
%       TolZ:       Termination tolerance on Z (default = 1e-3)
%                   TolZ must be in ]0,1[
%       MaxIter:    Maximum number of iterations allowed (default = 100)
%       Initial:    Initial value for the iterative process (default =
%                   initial data)
%       -----------------
%   Syntax: [Z,...] = SMOOTHN(...,'MaxIter',500,'TolZ',1e-4,'Initial',Z0);
%
%   [Z,S,EXITFLAG] = SMOOTHN(...) returns a boolean value EXITFLAG that
%   describes the exit condition of SMOOTHN:
%       1       SMOOTHN converged.
%       0       Maximum number of iterations was reached.
%
%   Class Support
%   -------------
%   Input array can be numeric or logical. The returned array is of class
%   double.
%
%   Notes
%   -----
%   The N-D (inverse) discrete cosine transform functions DCTN and IDCTN are required.
%
%   To be made
%   ----------
%   Estimate the confidence bands (see Wahba 1983, Nychka 1988).
%
%   Reference
%   ---------
%   Garcia D, Robust smoothing of gridded data in one and higher dimensions
%   with missing values. Computational Statistics & Data Analysis, 2010.
%   PDF download
%
%   Examples:
%   --------
%   % 1-D example
%   x = linspace(0,100,2^8);
%   y = cos(x/10)+(x/50).^2 + randn(size(x))/10;
%   y([70 75 80]) = [5.5 5 6];
%   z = smoothn(y); % Regular smoothing
%   zr = smoothn(y,'robust'); % Robust smoothing
%   subplot(121), plot(x,y,'r.',x,z,'k','LineWidth',2)
%   axis square, title('Regular smoothing')
%   subplot(122), plot(x,y,'r.',x,zr,'k','LineWidth',2)
%   axis square, title('Robust smoothing')
%
%   % 2-D example
%   xp = 0:.02:1;
%   [x,y] = meshgrid(xp);
%   f = exp(x+y) + sin((x-2*y)*3);
%   fn = f + randn(size(f))*0.5;
%   fs = smoothn(fn);
%   subplot(121), surf(xp,xp,fn), zlim([0 8]), axis square
%   subplot(122), surf(xp,xp,fs), zlim([0 8]), axis square
%
%   % 2-D example with missing data
%   n = 256;
%   y0 = peaks(n);
%   y = y0 + rand(size(y0))*2;
%   I = randperm(n^2);
%   y(I(1:n^2*0.5)) = NaN; % lose 1/2 of data
%   y(40:90,140:190) = NaN; % create a hole
%   z = smoothn(y); % smooth data
%   subplot(2,2,1:2), imagesc(y), axis equal off
%   title('Noisy corrupt data')
%   subplot(223), imagesc(z), axis equal off
%   title('Recovered data ...')
%   subplot(224), imagesc(y0), axis equal off
%   title('... compared with original data')
%
%   % 3-D example
%   [x,y,z] = meshgrid(-2:.2:2);
%   xslice = [-0.8,1]; yslice = 2; zslice = [-2,0];
%   vn = x.*exp(-x.^2-y.^2-z.^2) + randn(size(x))*0.06;
%   subplot(121), slice(x,y,z,vn,xslice,yslice,zslice,'cubic')
%   title('Noisy data')
%   v = smoothn(vn);
%   subplot(122), slice(x,y,z,v,xslice,yslice,zslice,'cubic')
%   title('Smoothed data')
%
%   % Cardioid
%   t = linspace(0,2*pi,1000);
%   x = 2*cos(t).*(1-cos(t)) + randn(size(t))*0.1;
%   y = 2*sin(t).*(1-cos(t)) + randn(size(t))*0.1;
%   z = smoothn(complex(x,y));
%   plot(x,y,'r.',real(z),imag(z),'k','linewidth',2)
%   axis equal tight
%
%   % Cellular vortical flow
%   [x,y] = meshgrid(linspace(0,1,24));
%   Vx = cos(2*pi*x+pi/2).*cos(2*pi*y);
%   Vy = sin(2*pi*x+pi/2).*sin(2*pi*y);
%   Vx = Vx + sqrt(0.05)*randn(24,24); % adding Gaussian noise
%   Vy = Vy + sqrt(0.05)*randn(24,24); % adding Gaussian noise
%   I = randperm(numel(Vx));
%   Vx(I(1:30)) = (rand(30,1)-0.5)*5; % adding outliers
%   Vy(I(1:30)) = (rand(30,1)-0.5)*5; % adding outliers
%   Vx(I(31:60)) = NaN; % missing values
%   Vy(I(31:60)) = NaN; % missing values
%   Vs = smoothn(complex(Vx,Vy),'robust'); % automatic smoothing
%   subplot(121), quiver(x,y,Vx,Vy,2.5), axis square
%   title('Noisy velocity field')
%   subplot(122), quiver(x,y,real(Vs),imag(Vs)), axis square
%   title('Smoothed velocity field')
%
%   See also SMOOTH, GSMOOTHN, DCTN, IDCTN.
%
%   -- Damien Garcia -- 2009/03, revised 2010/06
%   Visit my website for more details about SMOOTHN

% Check input arguments
error(nargchk(1,10,nargin));

%% Test & prepare the variables
%---
k = 0;
while k<nargin && ~ischar(varargin{k+1}), k = k+1; end
%---
% y = array to be smoothed
y = double(varargin{1});
sizy = size(y);
noe = prod(sizy); % number of elements
if noe<2, z = y; return, end
%---
% Smoothness parameter and weights
W = ones(sizy);
s = [];
if k==2
    if isempty(varargin{2}) || isscalar(varargin{2}) % smoothn(y,s)
        s = varargin{2}; % smoothness parameter
    else % smoothn(y,W)
        W = varargin{2}; % weight array
    end
elseif k==3 % smoothn(y,W,s)
        W = varargin{2}; % weight array
        s = varargin{3}; % smoothness parameter
end
if ~isequal(size(W),sizy)
        error('MATLAB:smoothn:SizeMismatch',...
            'Arrays for data and weights must have same size.')
elseif ~isempty(s) && (~isscalar(s) || s<0)
    error('MATLAB:smoothn:IncorrectSmoothingParameter',...
        'The smoothing parameter must be a scalar >=0')
end
%---
% "Maximal number of iterations" criterion
I = find(strcmpi(varargin,'MaxIter'),1);
if isempty(I)
    MaxIter = 100; % default value for MaxIter
else
    try
        MaxIter = varargin{I+1};
    catch
        error('MATLAB:smoothn:IncorrectMaxIter',...
            'MaxIter must be an integer >=1')
    end
    if ~isnumeric(MaxIter) || ~isscalar(MaxIter) ||...
            MaxIter<1 || MaxIter~=round(MaxIter)
        error('MATLAB:smoothn:IncorrectMaxIter',...
            'MaxIter must be an integer >=1')
    end
end
%---
% "Tolerance on smoothed output" criterion
I = find(strcmpi(varargin,'TolZ'),1);
if isempty(I)
    TolZ = 1e-3; % default value for TolZ
else
    try
        TolZ = varargin{I+1};
    catch
        error('MATLAB:smoothn:IncorrectTolZ',...
            'TolZ must be in ]0,1[')
    end
    if ~isnumeric(TolZ) || ~isscalar(TolZ) || TolZ<=0 || TolZ>=1
        error('MATLAB:smoothn:IncorrectTolZ',...
            'TolZ must be in ]0,1[')
    end
end
%---
% "Initial Guess" criterion
I = find(strcmpi(varargin,'Initial'),1);
if isempty(I)
    isinitial = false; % default value for TolZ
else
    isinitial = true;
    try
        z0 = varargin{I+1};
    catch
        error('MATLAB:smoothn:IncorrectInitialGuess',...
            'Z0 must be a valid initial guess for Z')
    end
    if ~isnumeric(z0) || ~isequal(size(z0),sizy)
        error('MATLAB:smoothn:IncorrectTolZ',...
            'Z0 must be a valid initial guess for Z')
    end
end
%---
% Weights. Zero weights are assigned to not finite values (Inf or NaN),
% (Inf/NaN values = missing data).
IsFinite = isfinite(y);
nof = nnz(IsFinite); % number of finite elements
W = W.*IsFinite;
if any(W<0)
    error('MATLAB:smoothn:NegativeWeights',...
        'Weights must all be >=0')
else
    W = W/max(W(:));
end
%---
% Weighted or missing data?
isweighted = any(W(:)<1);
%---
% Robust smoothing?
isrobust = any(strcmpi(varargin,'robust'));
%---
% Automatic smoothing?
isauto = isempty(s);
%---
% DCTN and IDCTN are required
test4DCTNandIDCTN

%% Creation of the Lambda tensor
%---
% Lambda contains the eingenvalues of the difference matrix used in this
% penalized least squares process.
d = ndims(y);
Lambda = zeros(sizy);
for i = 1:d
    siz0 = ones(1,d);
    siz0(i) = sizy(i);
    Lambda = bsxfun(@plus,Lambda,...
        cos(pi*(reshape(1:sizy(i),siz0)-1)/sizy(i)));
end
Lambda = -2*(d-Lambda);
if ~isauto, Gamma = 1./(1+s*Lambda.^2); end

%% Upper and lower bound for the smoothness parameter
% The average leverage (h) is by definition in [0 1]. Weak smoothing occurs
% if h is close to 1, while over-smoothing appears when h is near 0. Upper
% and lower bounds for h are given to avoid under- or over-smoothing. See
% equation relating h to the smoothness parameter (Equation #12 in the
% referenced CSDA paper).
N = sum(sizy~=1); % tensor rank of the y-array
hMin = 1e-6; hMax = 0.99;
sMinBnd = (((1+sqrt(1+8*hMax.^(2/N)))/4./hMax.^(2/N)).^2-1)/16;
sMaxBnd = (((1+sqrt(1+8*hMin.^(2/N)))/4./hMin.^(2/N)).^2-1)/16;

%% Initialize before iterating
%---
Wtot = W;
%--- Initial conditions for z
if isweighted
    %--- With weighted/missing data
    % An initial guess is provided to ensure faster convergence. For that
    % purpose, a nearest neighbor interpolation followed by a coarse
    % smoothing are performed.
    %---
    if isinitial % an initial guess (z0) has been provided
        z = z0;
    else
        z = InitialGuess(y,IsFinite);
    end

else
    z = zeros(sizy);
end
%---
z0 = z;
y(~IsFinite) = 0; % arbitrary values for missing y-data
%---
tol = 1;
RobustIterativeProcess = true;
RobustStep = 1;
nit = 0;
%--- Error on p. Smoothness parameter s = 10^p
errp = 0.1;
opt = optimset('TolX',errp);
%--- Relaxation factor RF: to speedup convergence
RF = 1 + 0.75*isweighted;

%% Main iterative process
%---
while RobustIterativeProcess
    %--- "amount" of weights (see the function GCVscore)
    aow = sum(Wtot(:))/noe; % 0 < aow <= 1
    %---
    while tol>TolZ && nit<MaxIter
        nit = nit+1;
        DCTy = dctn(Wtot.*(y-z)+z);
        if isauto && ~rem(log2(nit),1)
            %---
            % The generalized cross-validation (GCV) method is used.
            % We seek the smoothing parameter s that minimizes the GCV
            % score i.e. s = Argmin(GCVscore)^.
            % Because this process is time-consuming, it is performed from
            % time to time (when nit is a power of 2)
            %---
            fminbnd(@gcv,log10(sMinBnd),log10(sMaxBnd),opt);
        end
        z = RF*idctn(Gamma.*DCTy) + (1-RF)*z;

        % if no weighted/missing data => tol=0 (no iteration)
        tol = isweighted*norm(z0(:)-z(:))/norm(z(:));

        z0 = z; % re-initialization
    end
    exitflag = nit<MaxIter;

    if isrobust %-- Robust Smoothing: iteratively re-weighted process
        %--- average leverage
        h = sqrt(1+16*s); h = sqrt(1+h)/sqrt(2)/h; h = h^N;
        %--- take robust weights into account
        Wtot = W.*RobustWeights(y-z,IsFinite,h);
        %--- re-initialize for another iterative weighted process
        isweighted = true; tol = 1; nit = 0;
        %---
        RobustStep = RobustStep+1;
        RobustIterativeProcess = RobustStep<4; % 3 robust steps are enough.
    else
        RobustIterativeProcess = false; % stop the whole process
    end
end

z = cast(z, 'like', varargin{1});

%% Warning messages
%---
if isauto
    if abs(log10(s)-log10(sMinBnd))<errp
        warning('MATLAB:smoothn:SLowerBound',...
            ['s = ' num2str(s,'%.3e') ': the lower bound for s ',...
            'has been reached. Put s as an input variable if required.'])
    elseif abs(log10(s)-log10(sMaxBnd))<errp
        warning('MATLAB:smoothn:SUpperBound',...
            ['s = ' num2str(s,'%.3e') ': the upper bound for s ',...
            'has been reached. Put s as an input variable if required.'])
    end
end
if nargout<3 && ~exitflag
    warning('MATLAB:smoothn:MaxIter',...
        ['Maximum number of iterations (' int2str(MaxIter) ') has ',...
        'been exceeded. Increase MaxIter option or decrease TolZ value.'])
end

%% GCV score
%---
function GCVscore = gcv(p)
    % Search the smoothing parameter s that minimizes the GCV score
    %---
    s = 10^p;
    Gamma = 1./(1+s*Lambda.^2);
    %--- RSS = Residual sum-of-squares
    if aow>0.9 % aow = 1 means that all of the data are equally weighted
        % very much faster: does not require any inverse DCT
        RSS = norm(DCTy(:).*(Gamma(:)-1))^2;
    else
        % take account of the weights to calculate RSS:
        yhat = idctn(Gamma.*DCTy);
        RSS = norm(sqrt(Wtot(IsFinite)).*(y(IsFinite)-yhat(IsFinite)))^2;
    end
    %---
    TrH = sum(Gamma(:));
    GCVscore = RSS/nof/(1-TrH/noe)^2;
end

end

%% Robust weights
function W = RobustWeights(r,I,h)
    % weights for robust smoothing.
    MAD = median(abs(r(I)-median(r(I)))); % median absolute deviation
    u = abs(r/(1.4826*MAD)/sqrt(1-h)); % studentized residuals
    c = 4.685; W = (1-(u/c).^2).^2.*((u/c)<1); % bisquare weights
    % c = 2.385; W = 1./(1+(u/c).^2); % Cauchy weights
    % c = 2.795; W = u<c; % Talworth weights
    W(isnan(W)) = 0;
end

%% Test for DCTN and IDCTN
function test4DCTNandIDCTN
    if ~exist('dctn','file')
        error('MATLAB:smoothn:MissingFunction',...
            ['DCTN and IDCTN are required. Download DCTN here.'])
    elseif ~exist('idctn','file')
        error('MATLAB:smoothn:MissingFunction',...
            ['DCTN and IDCTN are required. Download IDCTN here.'])
    end
end

%% Initial Guess with weighted/missing data
function z = InitialGuess(y,I)
    %-- nearest neighbor interpolation (in case of missing values)
    if any(~I(:))
        if license('test','image_toolbox')
            [z,L] = bwdist(I);
            z = y;
            z(~I) = y(L(~I));
        else
        % If BWDIST does not exist, NaN values are all replaced with the
        % same scalar. The initial guess is not optimal and a warning
        % message thus appears.
            z = y;
            z(~I) = mean(y(I));
            warning('MATLAB:smoothn:InitialGuess',...
                ['BWDIST (Image Processing Toolbox) does not exist. ',...
                'The initial guess may not be optimal; additional',...
                ' iterations can thus be required to ensure complete',...
                ' convergence. Increase ''MaxIter'' criterion if necessary.'])
        end
    else
        z = y;
    end
    %-- coarse fast smoothing using one-tenth of the DCT coefficients
    siz = size(z);
    z = dctn(z);
    for k = 1:ndims(z)
        z(ceil(siz(k)/10)+1:end,:) = 0;
        z = reshape(z,circshift(siz,[0 1-k]));
        z = shiftdim(z,1);
    end
    z = idctn(z);
end


function [ p, l, u ] = oto_plu ( n )

%*****************************************************************************80
%
%% OTO_PLU returns the PLU factors of the OTO matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 June 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Output, real P(N,N), L(N,N), U(N,N), the PLU factors.
%
  p = zeros ( n, n );
 for j = 1 : n
   for i = 1 : n
      if ( i == j )
        p(i,j) = 1.0;
      else
        p(i,j) = 0.0;
      end
    end
  end

 l = zeros ( n, n );
 for j = 1 : n
   for i = 1 : n
      if ( i == j )
        l(i,j) = 1.0;
      elseif ( i == j + 1 )
        l(i,j) = j / ( j + 1 );
      else
        l(i,j) = 0.0;
      end
    end
  end

  u = zeros ( n, n );
  for j = 1 : n
    for i = 1 : n
      if ( i == j )
        u(i,j) = ( i + 1 ) / i;
      elseif ( i == j - 1 )
        u(i,j) = 1.0;
      else
        u(i,j) = 0.0;
      end
    end
  end

  return
end


function [ result, node_num ] = sphere01_triangle_quad_icos1v ( a_xyz, ...
  b_xyz, c_xyz, factor, fun )

%*****************************************************************************80
%
%% SPHERE01_TRIANGLE_QUAD_ICOS1V: vertex rule, subdivide then project.
%
%  Discussion:
%
%    This function estimates an integral over a spherical triangle on the
%    unit sphere.
%
%    This function sets up an icosahedral grid, and subdivides each
%    edge of the icosahedron into FACTOR subedges.  These edges define a grid
%    within each triangular icosahedral face.   All of these calculations are
%    done, essentially, on the FLAT faces of the icosahedron.  Only then are
%    the triangle vertices projected to the sphere.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 April 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A_XYZ(3), B_XYZ(3), C_XYZ(3), the vertices
%    of the spherical triangle.
%
%    Input, integer FACTOR, the subdivision factor, which must
%    be at least 1.
%
%    Input, function v = fun ( x ), evaluates the integrand at the point X.
%
%    Output, real RESULT, the estimated integral.
%
%    Output, integer NODE_NUM, the number of evaluation points.
%

%
%  Destroy all row vectors.
%
  a_xyz = a_xyz(:);
  b_xyz = b_xyz(:);
  c_xyz = c_xyz(:);
%
%  Initialize the integral data.
%
  result = 0.0;
  area_total = 0.0;
  node_num = 0;
%
%  Deal with subtriangles that have same orientation as face.
%
  for f1 = 0 : factor - 1
    for f2 = 0 : factor - f1 - 1
      f3 = factor - f1 - f2;

      a2_xyz = sphere01_triangle_project ( a_xyz, b_xyz, c_xyz, f1 + 1, f2,     f3 - 1 );
      b2_xyz = sphere01_triangle_project ( a_xyz, b_xyz, c_xyz, f1,     f2 + 1, f3 - 1 );
      c2_xyz = sphere01_triangle_project ( a_xyz, b_xyz, c_xyz, f1,     f2,     f3 );

      area = sphere01_triangle_vertices_to_area ( a2_xyz, b2_xyz, c2_xyz );

      node_num = node_num + 3;
      va = fun ( a2_xyz );
      vb = fun ( b2_xyz );
      vc = fun ( c2_xyz );
      result = result + area * ( va + vb + vc ) / 3.0;
      area_total = area_total + area;

      end
    end
%
%  Deal with subtriangles that have opposite orientation as face.
%
  for f3 = 0 : factor - 2
    for f2 = 1 : factor - f3 - 1
      f1 = factor - f2 - f3;

      a2_xyz = sphere01_triangle_project ( a_xyz, b_xyz, c_xyz, f1 - 1, f2,     f3 + 1 );
      b2_xyz = sphere01_triangle_project ( a_xyz, b_xyz, c_xyz, f1,     f2 - 1, f3 + 1 );
      c2_xyz = sphere01_triangle_project ( a_xyz, b_xyz, c_xyz, f1,     f2,     f3 );

      area = sphere01_triangle_vertices_to_area ( a2_xyz, b2_xyz, c2_xyz );

      node_num = node_num + 3;
      va = fun ( a2_xyz );
      vb = fun ( b2_xyz );
      vc = fun ( c2_xyz );
      result = result + area * ( va + vb + vc ) / 3.0;
      area_total = area_total + area;

    end
  end

  return
end


function combo_test37 ( )

%*****************************************************************************80
%
%% COMBO_TEST37 tests STIRLING_NUMBERS1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 January 2011
%
%  Author:
%
%    John Burkardt
%
  maxm = 6;
  maxn = 6;
  offset = 1;

  fprintf ( 1, ' \n' );
  fprintf ( 1, 'COMBO_TEST37\n' );
  fprintf ( 1, '  STIRLING_NUMBERS1 computes a table of Stirling\n' );
  fprintf ( 1, '  numbers of the first kind.\n' );

  s = stirling_numbers1 ( maxm, maxn );

  fprintf ( 1, ' \n' );
  fprintf ( 1, '    I S(I,0) S(I,1) S(I,2) S(I,3) S(I,4) S(I,5)\n' );
  fprintf ( 1, ' \n' );

  for i = 0 : maxm
    fprintf ( 1, '%5d', i );
    for j = 0 : maxn
      fprintf ( 1, '%5d', s(i+offset,j+offset) );
    end
    fprintf ( 1, '\n' );
  end

  return
end


function [ grid_index, grid_base ] = levels_index ( dim_num, level_max, rule, ...
  point_num )

%*****************************************************************************80
%
%% LEVELS_INDEX indexes a sparse grid.
%
%  Discussion:
%
%    The sparse grid is the logical sum of product grids with total LEVEL
%    between LEVEL_MIN and LEVEL_MAX.
%
%    The necessary dimensions of GRID_INDEX can be determined by
%    calling LEVELS_INDEX_SIZE first.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 March 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    A Sparse Grid Stochastic Collocation Method for Partial Differential
%    Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2309-2345.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer LEVEL_MAX, the maximum value of LEVEL.
%
%    Input, integer RULE, the index of the rule.
%    1, "CC", Clenshaw Curtis Closed Fully Nested rule.
%    2, "F1", Fejer 1 Open Fully Nested rule.
%    3, "F2", Fejer 2 Open Fully Nested rule.
%    4, "GP", Gauss Patterson Open Fully Nested rule.
%    5, "GL", Gauss Legendre Open Weakly Nested rule.
%    6, "GH", Gauss Hermite Open Weakly Nested rule.
%    7, "LG", Gauss Laguerre Open Non Nested rule.
%
%    Input, integer POINT_NUM, the total number of points
%    in the grids.
%
%    Output, integer GRID_INDEX(DIM_NUM,POINT_NUM), a list of
%    point indices, representing a subset of the product grid of level
%    LEVEL_MAX, representing (exactly once) each point that will show up in a
%    sparse grid of level LEVEL_MAX.
%
%    Output, integer GRID_BASE(DIM_NUM,POINT_NUM), a list of
%    the orders of the rules associated with each point and dimension.
%
  if ( rule == 1 )
    [ grid_index, grid_base ] = levels_index_cfn ( dim_num, level_max, ...
      point_num );
  elseif ( 2 <= rule & rule <= 4 )
    [ grid_index, grid_base ] = levels_index_ofn ( dim_num, level_max, ...
      point_num );
  elseif ( 5 <= rule & rule <= 6 )
    [ grid_index, grid_base ] = levels_index_own ( dim_num, level_max, ...
      point_num );
  elseif ( 7 == rule )
    [ grid_index, grid_base ] = levels_index_onn ( dim_num, level_max, ...
      point_num );
  else
    grid_index = [];
    grid_base = [];
    fprintf ( 1, '\n' );
    fprintf ( 1, 'LEVELS_INDEX - Fatal error!\n' );
    fprintf ( 1, '  Unrecognized rule number = %d\n', rule );
    error ( 'LEVELS_INDEX - Fatal error!' );
  end

  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.

% Script chap::3::script
% Density Variance Gamma model with Cox-Ingersoll-Ross stochastic clock
%
%
%
T = 1;              % maturity
f0 = 100;           % spot value
r=0;                % risk less rate
d=0;                % dividend yield
% parameters for applying fourier transform to compute the density
a = 600;                            % parameter to compute density via fft
N = 1024;                           % number of grid points
x = ( (0:N-1) - N/2 ) / a;          % range

C = 2;
G = 2;
M = 2;
Y = .75;

legend = 'Base';
title_plot = 'CGMY Density';

func = @(x) cf_cgmy(x,T,0,r,d, C, G, M, Y);
y = fftdensity(func,a,N);
%% Changing C
C_low = 1;
C_high = 3;

func_low = @(x) cf_cgmy(x,T,0,r,d, C_low,G,M,Y);
y_low = fftdensity(func_low,a,N);
func_high = @(x) cf_cgmy(x,T,0,r,d, C_high,G,M,Y);
y_high = fftdensity(func_high,a,N);
legend_low = 'Changing C low';
legend_high = 'Changing C high';

createfigure_density(x,y,y_low,y_high,title_plot,legend,legend_low,legend_high);

%% Changing G
G_low = 1.5;
G_high = 2.5;

func_low = @(x) cf_cgmy(x,T,0,r,d, C,G_low,M,Y);
y_low = fftdensity(func_low,a,N);
func_high = @(x) cf_cgmy(x,T,0,r,d, C,G_high,M,Y);
y_high = fftdensity(func_high,a,N);
legend_low = 'Changing G low';
legend_high = 'Changing G high';

createfigure_density(x,y,y_low,y_high,title_plot,legend,legend_low,legend_high);

%% Changing M
M_low = 1.5;
M_high = 2.5;

func_low = @(x) cf_cgmy(x,T,0,r,d, C,G,M_low,Y);
y_low = fftdensity(func_low,a,N);
func_high = @(x) cf_cgmy(x,T,0,r,d, C,G,M_high,Y);
y_high = fftdensity(func_high,a,N);
legend_low = 'Changing M low';
legend_high = 'Changing M high';

createfigure_density(x,y,y_low,y_high,title_plot,legend,legend_low,legend_high);

%% Changing Y
Y_low = -0.5;
Y_high = 1.2;

func_low = @(x) cf_cgmy(x,T,0,r,d, C,G,M,Y_low);
y_low = fftdensity(func_low,a,N);
func_high = @(x) cf_cgmy(x,T,0,r,d, C,G,M,Y_high);
y_high = fftdensity(func_high,a,N);
legend_low = 'Changing Y low';
legend_high = 'Changing Y high';

createfigure_density(x,y,y_low,y_high,title_plot,legend,legend_low,legend_high);


function [chi,omega_b,a_b,S] = lukfUpdate(chi,omega_b,a_b,...
    S,y,param,R,ParamFilter)
param.Pi = ParamFilter.Pi;
param.chiC = ParamFilter.chiC;

k = length(y);
q = length(S);
N_aug = q+k;
Rc = chol(kron(eye(k/2),R));
S_aug = blkdiag(S,Rc);

% scaled unsented transform
W0 = 1-N_aug/3;
Wj = (1-W0)/(2*N_aug);
gamma = sqrt(N_aug/(1-W0));
alpha = 1;
beta = 2;

% Compute transformed measurement
X = gamma*[zeros(N_aug,1) S_aug' -S_aug'];% sigma-points
Y = zeros(k,2*N_aug+1);
Y(:,1) = h(chi,zeros(q-6,1),param,zeros(N_aug-q,1));
for j = 2:2*N_aug+1
    xi_j = X([1:9 16:q],j);
    v_j = X(q+1:N_aug,j);
    Y(:,j) = h(chi,xi_j,param,v_j);
end
ybar = W0*Y(:,1) + Wj*sum(Y(:,2:end),2);% Measurement mean
Y(:,1) = sqrt(abs(W0+(1-alpha^2+beta)))*(Y(:,1)-ybar);
YY = sqrt(Wj)*(Y(:,2:2*N_aug+1)-ybar*ones(1,2*N_aug));
[~,Rs] = qr(YY');
Ss = Rs(1:k,1:k);
[Sy,~] = cholupdate(Ss,Y(:,1),'-'); % Sy'*Sy = Pyy
Pxy = zeros(q,k);
for j = 2:2*N_aug+1
    Pxy = Pxy + Wj*X(1:q,j)*(Y(:,j)-ybar)';
end

K = Pxy*Sy^-1*Sy'^-1; % Gain

xibar = K*(y-ybar);
omega_b = omega_b + xibar(10:12);
a_b = a_b + xibar(13:15);
xibar = xibar([1:9 16:q]);

% Covariance update
A = K*Sy';
for n = 1:k
    S = cholupdate(S,A(:,n),'-');
end

% Update mean state
chi = chi*exp_multiSE3(xibar);
J = xi2calJr(xibar);
S = S*J;
end

%--------------------------------------------------------------------------
function y = h(chi,xi,param,v)
Pi = param.Pi;
chiC = param.chiC;
RotC = chiC(1:3,1:3);
xC = chiC(1:3,4);

yAmers = param.yAmers;
NbAmers = length(yAmers);

chi_j = chi*exp_multiSE3(xi);
Rot = chi_j(1:3,1:3);
x = chi_j(1:3,5);
PosAmers = chi_j(1:3,6:end);
posAmers = PosAmers(:,yAmers);
z = Pi*( (Rot*RotC)'*(posAmers-kron(x,ones(1,NbAmers))) ...
    - kron(xC,ones(1,NbAmers)));
y = z(1:2,:)./z(3,:);
y = y(:) + v;
end


function [y] = ml_gmm_pdf(xs,Priors,Mu,Sigma)
%ML_GMM_PDF, Gaussian Mixture Model function
%
%   G(x;\{pi,mu,sigma\}) = \sum_{i=1}^{K} pi_{k} * g(x;mu_k,sigma_k)
%
%   input ---------------------------------------------------------------
%
%       o xs: (D x N), set of N data points of dimension D
%
%       o Priors: (1 x K), sum(Priors) = 1
%
%       o Mu: (D x K), set of Sigmas
%
%       o Sigmas: (D x D x K)
%
%   output ----------------------------------------------------------------
%
%       o ys: (N x 1), values
%

[N,M] = size(xs);
K = size(Priors,2);
ys = zeros(M,K);

if size(Mu,1) == 1
    % Univariate

    for k=1:K
        ys(:,k) = normpdf(xs,Mu(k),sqrt(Sigma(k)));
    end
else
    % Multivariate
    for k=1:K
        ys(:,k) = ml_gaussPDF(xs,Mu(:,k),Sigma(:,:,k));
    end

end

y = sum(ys .* repmat(Priors,M,1),2);

end


clear;
clc
close all;

%% INTPUS
p = [0; 0];
v = [0;  0];
yaw = deg2rad(45);

%% Epoch 1 Measurements
omgea = [0.5; 0.2; 0.1; -0.1];
t = 0.5;
a = [2  0.1; 5 0.1; 2 -0.05; 0 0];

for i = 1:4
% update heading
lyaw = yaw;
yaw = yaw + omgea(i)*t;

% transform acceration to p frame, get a average value
Cpb_before = [cos(lyaw) -sin(lyaw); sin(lyaw) cos(lyaw)];
Cpb = [cos(yaw) -sin(yaw); sin(yaw) cos(yaw)];
Cpb = (Cpb_before + Cpb) / 2;

% get acceralation in N frame
a_n_frame = Cpb*a(i,:)';

% update velocity
lv = v;
v = v + a_n_frame.*t;
% update position
p = p + (lv +v)*0.5*t;

end

p


function [MF, VF, KF, MC, VC] = garchfor2(data, resids, ht, kt, parameters, model,ar, ma, p, q, max_forecast)
%{
-----------------------------------------------------------------------
 PURPOSE:
 Mean, Volatility and Kurtosis Forecasting
-----------------------------------------------------------------------
 USAGE:
 [MF, VF, KF, MC, VC] = garchfor2(data, resids, ht, kt, parameters, model,ar, ma, p, q, max_forecast)

 INPUTS:
 data:          a vector of series
 resids:        a vector of residuals
 ht:            a vector of conditional variances
 kt:            a vector of conditional kurtosis
 parameters:    a vector of parameters
 model:         'GARCH', 'GJR', 'AGARCH', 'NAGARCH'
 ar:            positive scalar integer representing the order of AR
 am:            positive scalar integer representing the order of MA
 p:             positive scalar integer representing the order of ARCH
 q:             positive scalar integer representing the order of GARCH
 max_forecasts: maximum number of forecasts (i.e. 1-trading months 22 days)

 OUTPUTS:
 MF:            a vector of mean forecasts
 VF:            a vector of volatility forecasts
 KF:            a vector of kurtosis forecasts
 MC:            a vector of cumulative mean forecasts
 VC:            a vector of cumulative volatility forecasts
-----------------------------------------------------------------------
 Author:
 Alexandros Gabrielsen, a.gabrielsen@city.ac.uk
 Date:     08/2011
-----------------------------------------------------------------------
%}

if nargin == 0
    error('Data, Residuals, Variance, Kurtosis, GARCH Model, AR, MA, ARCH, GARCH, Maximum Number of Forecasts')
end

if size(data,2) > 1
   error('Data vector should be a column vector')
end

if (length(ar) > 1) | (length(ma) > 1) | ar < 0 | ma < 0
    error('AR and MA should be positive scalars')
end

if (length(p) > 1) | (length(q) > 1) | p < 0 | q < 0
    error('P and Q should be positive scalars')
end

z=ar+ma;
MF=[];
VF=[];
KF=[];

% Verifying that the vector of parameters is a column vector
[r,c]=size(parameters);
if r<c
    parameters=parameters';
end

% Forecasting the Mean
MF = parameters(1:1+z)'*[1; data(end-(1:ar)); resids(end-(0:ma-1))]; % 1-period ahead forecast
 for i = 2:max_forecast
     MF(i,1) = sum([parameters(1); ones(1,ar)*parameters(2:2+ar)*MF(i-1,1); ones(1,ma)*parameters(3+ar:2+ar+ma)*resids(end-(0:ma-1-i))]);
 end
 clear i

 % Forecasting Volatility and Kurtosis
 % Please note that for a given set of parameters it is possible to forecast
 % a smaller than 3 kurtosis, and therefore we floor the forecast at 3.
if isequal(strcat(model), 'GARCH')
    VF(1,1) = parameters(2+z:2+z+p+q)'*[1; resids(end-(0:(p-1))).^2;ht(end-(0:(q-1)))]; % 1-period ahead forecast
    KF(1,1) = parameters(3+z+p+q:3+z+2*p+2*q)'*[1; (resids(end-(0:(p-1))).^4)./(ht(end-(0:(p-1))).^2); kt(end-(0:(q-1)))]; % 1-period ahead forecast
    if KF(1,1) < 3
        KF(1,1) = 3
    end
    for i = 2:max_forecast
       VF(i,1) = parameters(2+z) + ones(1,p+q)*parameters(3+z:2+z+p+q)*VF(i-1,1);
       KF(i,1) = parameters(3+z+p+q) + ones(1,p+q)*parameters(4+z+p+q:3+z+2*p+2*q)*KF(i-1,1);
    end
clear i
elseif  isequal(strcat(model), 'GJR')
    VF(1,1) = parameters(2+z:2+z+2*p+q)'*[1; resids(end-(0:(p-1))).^2;  ht(end-(0:(q-1))); (resids(end-(0:(p-1)))<0).*resids(end-(0:(p-1))).^2];
    KF(1,1) = parameters(3+z+p+q:3+z+2*p+2*q)'*[1; (resids(end-(0:(p-1))).^4)./(ht(end-(0:(p-1))).^2); kt(end-(0:(q-1)))]; % 1-period ahead forecast
    if KF(1,1) < 3
        KF(1,1) = 3
    end
    for i = 2:max_forecast
         VF(i,1) = parameters(2+z) + ones(1,2*p+q)*parameters(3+z:2+z+2*p+q)*VF(i-1,1);
         KF(i,1) = parameters(3+z+p+q) + ones(1,p+q)*parameters(4+z+p+q:3+z+2*p+2*q)*KF(i-1,1);
    end
clear i
elseif isequal(strcat(model), 'AGARCH')
    asym=parameters(3+z+p+q:2+z+2*p+q);
    VF(1,1) = parameters(2+z:2+z+p+q)'*[1; (resids(end-(0:(p-1))) - asym).^2; ht(end-(0:(q-1)))]; % 1-period ahead forecast
    KF(1,1) = parameters(4+z+p+q:4+z+2*p+2*q)'*[1; (resids(end-(0:(p-1))).^4)./(ht(end-(0:(p-1))).^2); kt(end-(0:(q-1)))]; % 1-period ahead forecast
    if KF(1,1) < 3
        KF(1,1) = 3
    end
    for i = 2:max_forecast
       VF(i,1) = parameters(2+z:2+z+p+q)'*[1; (sqrt(VF(i-1,1)) - asym).^2; VF(i-1,1)];
       KF(i,1) = parameters(4+z+p+q) + ones(1,p+q)*parameters(5+z+p+q:4+z+2*p+2*q)*KF(i-1,1);
    end
clear i
elseif isequal(strcat(model), 'NAGARCH')
    asym=parameters(3+z+p+q:2+z+2*p+q);
    VF(1,1) = parameters(2+z:2+z+p+q)'*[1; (resids(end-(0:(p-1)))*sqrt(ht(end-(0:(p-1)))) - asym).^2; ht(end-(0:(p-1)))]; % 1-period ahead forecast
    KF(1,1) = parameters(4+z+p+q:4+z+2*p+2*q)'*[1; (resids(end-(0:(p-1))).^4)./(ht(end-(0:(p-1))).^2); kt(end-(0:(q-1)))]; % 1-period ahead forecast
    if KF(1,1) < 3
        KF(1,1) = 3
    end
    for i = 2:max_forecast
       VF(i,1) = parameters(2+z:2+z+p+q)'*[1; (sqrt(VF(i-1,1)) - asym).^2; VF(i-1,1)]
       KF(i,1) = parameters(4+z+p+q) + ones(1,p+q)*parameters(5+z+p+q:4+z+2*p+2*q)*KF(i-1,1);
    end
    clear i
else error('Invalid GARCH Model');
end

% Estimating cumulative measures
MC = cumsum(MF);
VC = sqrt(cumsum(VF));

end


function FBL = ffblasius(RE)
% FFBLASIUS Blasius skin friction factor
%  FFCW(RE) returns the skin friction loss coefficient
%  using the Blasius correlation.
%  CALLED FUNCTION: none
%   Required Inputs are:
%    RE  - Reynolds number (-)
% ---------------------------------------------------------------
% The MATLAB function was created by Tibor Balint, December 1998
% TBoreal Research Corporation, Toronto, Ont. Canada
% (tibor@netcom.ca) and also, University of Warwick, UK
% ---------------------------------------------------------------

format long g;                  % set the format of the calculations

if or((RE<4000),(RE>100000))
   error('The valid Reynolds number range is (4000 < RE < 100000)')
end

%set the correlation constants
a=0.3164;
b=-0.25;

%calculate the Blasius skin friction factor
FBL=a*RE^b;

return                          %end of the function
% -------------- end of the function ----------------------


function fem1d_bvp_quadratic_test01 ( )

%*****************************************************************************80
%
%% FEM1D_BVP_QUADRATIC_TEST01 carries out test case #1.
%
%  Discussion:
%
%    Use A1, C1, F1, EXACT1, EXACT_UX1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Dianne O'Leary,
%    Scientific Computing with Case Studies,
%    SIAM, 2008,
%    ISBN13: 978-0-898716-66-5,
%    LC: QA401.O44.
%
  n = 11;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'FEM1D_BVP_QUADRATIC_TEST01\n' );
  fprintf ( 1, '  Solve -( A(x) U''(x) )'' + C(x) U(x) = F(x)\n' );
  fprintf ( 1, '  for 0 < x < 1, with U(0) = U(1) = 0.\n' );
  fprintf ( 1, '  A1(X)  = 1.0\n' );
  fprintf ( 1, '  C1(X)  = 0.0\n' );
  fprintf ( 1, '  F1(X)  = X * ( X + 3 ) * exp ( X )\n' );
  fprintf ( 1, '  U1(X)  = X * ( 1 - X ) * exp ( X )\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of nodes = %d\n', n );
%
%  Geometry definitions.
%
  x_first = 0.0;
  x_last = 1.0;
  x = linspace ( x_first, x_last, n );
  x = x(:);

  u = fem1d_bvp_quadratic ( n, @a1, @c1, @f1, x );

  uexact = exact1 ( x );

  fprintf ( 1, '\n' );
  fprintf ( 1, '     I    X         U         Uexact    Error\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    fprintf ( 1, '  %4d  %8f  %8f  %8f  %8e\n', ...
      i, x(i), u(i), uexact(i), abs ( u(i) - uexact(i) ) );
  end

  e1 = l1_error ( n, x, u, @exact1 );
  e2 = l2_error_quadratic ( n, x, u, @exact1 );
  h1s = h1s_error_quadratic ( n, x, u, @exact_ux1 );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  l1 norm of error  = %g\n', e1 );
  fprintf ( 1, '  L2 norm of error  = %g\n', e2 );
  fprintf ( 1, '  Seminorm of error = %g\n', h1s );

  return
end
function value = a1 ( x )

%*****************************************************************************80
%
%% A1 evaluates A function #1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of A(X).
%
  value = 1.0;

  return
end
function value = c1 ( x )

%*****************************************************************************80
%
%% C1 evaluates C function #1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 March 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of C(X).
%
  value = 0.0;

  return
end
function value = exact1 ( x )

%*****************************************************************************80
%
%% EXACT1 evaluates exact solution #1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 March 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of U(X).
%
  value = x .* ( 1.0 - x ) .* exp ( x );

  return
end
function value = exact_ux1 ( x )

%*****************************************************************************80
%
%% EXACT_UX1 evaluates the derivative of exact solution #1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of dUdX(X).
%
  value = ( 1.0 - x - x .* x ) .* exp ( x );

  return
end
function value = f1 ( x )

%*****************************************************************************80
%
%% F1 evaluates right hand side function #1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 March 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of F(X).
%
  value = x .* ( x + 3.0 ) .* exp ( x );

  return
end


function x = tinv(p,v);
% TINV   Inverse of Student's T cumulative distribution function (cdf).
%   X=TINV(P,V) returns the inverse of Student's T cdf with V degrees
%   of freedom, at the values in P.
%
%   The size of X is the common size of P and V. A scalar input
%   functions as a constant matrix of the same size as the other input.
%
% This is an open source function that was assembled by Eric Maris using
% open source subfunctions found on the web.

if nargin < 2,
    error('Requires two input arguments.');
end

[errorcode p v] = distchck(2,p,v);

if errorcode > 0
    error('Requires non-scalar arguments to match in size.');
end

% Initialize X to zero.
x=zeros(size(p));

k = find(v < 0  | v ~= round(v));
if any(k)
    tmp  = NaN;
    x(k) = tmp(ones(size(k)));
end

k = find(v == 1);
if any(k)
  x(k) = tan(pi * (p(k) - 0.5));
end

% The inverse cdf of 0 is -Inf, and the inverse cdf of 1 is Inf.
k0 = find(p == 0);
if any(k0)
    tmp   = Inf;
    x(k0) = -tmp(ones(size(k0)));
end
k1 = find(p ==1);
if any(k1)
    tmp   = Inf;
    x(k1) = tmp(ones(size(k1)));
end

k = find(p >= 0.5 & p < 1);
if any(k)
    z = betainv(2*(1-p(k)),v(k)/2,0.5);
    x(k) = sqrt(v(k) ./ z - v(k));
end

k = find(p < 0.5 & p > 0);
if any(k)
    z = betainv(2*(p(k)),v(k)/2,0.5);
    x(k) = -sqrt(v(k) ./ z - v(k));
end

%%%%%%%%%%%%%%%%%%%%%%%%%
% SUBFUNCTION distchck
%%%%%%%%%%%%%%%%%%%%%%%%%

function [errorcode,varargout] = distchck(nparms,varargin)
%DISTCHCK Checks the argument list for the probability functions.

errorcode = 0;
varargout = varargin;

if nparms == 1
    return;
end

% Get size of each input, check for scalars, copy to output
isscalar = (cellfun('prodofsize',varargin) == 1);

% Done if all inputs are scalars.  Otherwise fetch their common size.
if (all(isscalar)), return; end

n = nparms;

for j=1:n
   sz{j} = size(varargin{j});
end
t = sz(~isscalar);
size1 = t{1};

% Scalars receive this size.  Other arrays must have the proper size.
for j=1:n
   sizej = sz{j};
   if (isscalar(j))
      t = zeros(size1);
      t(:) = varargin{j};
      varargout{j} = t;
   elseif (~isequal(sizej,size1))
      errorcode = 1;
      return;
   end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SUBFUNCTION betainv
%%%%%%%%%%%%%%%%%%%%%%%%%%%

function x = betainv(p,a,b);
%BETAINV Inverse of the beta cumulative distribution function (cdf).
%   X = BETAINV(P,A,B) returns the inverse of the beta cdf with
%   parameters A and B at the values in P.
%
%   The size of X is the common size of the input arguments. A scalar input
%   functions as a constant matrix of the same size as the other inputs.
%
%   BETAINV uses Newton's method to converge to the solution.

%   Reference:
%      [1]     M. Abramowitz and I. A. Stegun, "Handbook of Mathematical
%      Functions", Government Printing Office, 1964.

%   B.A. Jones 1-12-93

if nargin < 3,
    error('Requires three input arguments.');
end

[errorcode p a b] = distchck(3,p,a,b);

if errorcode > 0
    error('Requires non-scalar arguments to match in size.');
end

%   Initialize x to zero.
x = zeros(size(p));

%   Return NaN if the arguments are outside their respective limits.
k = find(p < 0 | p > 1 | a <= 0 | b <= 0);
if any(k),
   tmp = NaN;
   x(k) = tmp(ones(size(k)));
end

% The inverse cdf of 0 is 0, and the inverse cdf of 1 is 1.
k0 = find(p == 0 & a > 0 & b > 0);
if any(k0),
    x(k0) = zeros(size(k0));
end

k1 = find(p==1);
if any(k1),
    x(k1) = ones(size(k1));
end

% Newton's Method.
% Permit no more than count_limit interations.
count_limit = 100;
count = 0;

k = find(p > 0 & p < 1 & a > 0 & b > 0);
pk = p(k);

%   Use the mean as a starting guess.
xk = a(k) ./ (a(k) + b(k));

% Move starting values away from the boundaries.
if xk == 0,
    xk = sqrt(eps);
end
if xk == 1,
    xk = 1 - sqrt(eps);
end

h = ones(size(pk));
crit = sqrt(eps);

% Break out of the iteration loop for the following:
%  1) The last update is very small (compared to x).
%  2) The last update is very small (compared to 100*eps).
%  3) There are more than 100 iterations. This should NEVER happen.

while(any(abs(h) > crit * abs(xk)) & max(abs(h)) > crit    ...
                                 & count < count_limit),

    count = count+1;
    h = (betacdf(xk,a(k),b(k)) - pk) ./ betapdf(xk,a(k),b(k));
    xnew = xk - h;

% Make sure that the values stay inside the bounds.
% Initially, Newton's Method may take big steps.
    ksmall = find(xnew < 0);
    klarge = find(xnew > 1);
    if any(ksmall) | any(klarge)
        xnew(ksmall) = xk(ksmall) /10;
        xnew(klarge) = 1 - (1 - xk(klarge))/10;
    end

    xk = xnew;
end

% Return the converged value(s).
x(k) = xk;

if count==count_limit,
    fprintf('\nWarning: BETAINV did not converge.\n');
    str = 'The last step was:  ';
    outstr = sprintf([str,'%13.8f'],h);
    fprintf(outstr);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SUBFUNCTION betapdf
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function y = betapdf(x,a,b)
%BETAPDF Beta probability density function.
%   Y = BETAPDF(X,A,B) returns the beta probability density
%   function with parameters A and B at the values in X.
%
%   The size of Y is the common size of the input arguments. A scalar input
%   functions as a constant matrix of the same size as the other inputs.

%   References:
%      [1]  M. Abramowitz and I. A. Stegun, "Handbook of Mathematical
%      Functions", Government Printing Office, 1964, 26.1.33.

if nargin < 3,
   error('Requires three input arguments.');
end

[errorcode x a b] = distchck(3,x,a,b);

if errorcode > 0
    error('Requires non-scalar arguments to match in size.');
end

% Initialize Y to zero.
y = zeros(size(x));

% Return NaN for parameter values outside their respective limits.
k1 = find(a <= 0 | b <= 0 | x < 0 | x > 1);
if any(k1)
   tmp = NaN;
    y(k1) = tmp(ones(size(k1)));
end

% Return Inf for x = 0 and a < 1 or x = 1 and b < 1.
% Required for non-IEEE machines.
k2 = find((x == 0 & a < 1) | (x == 1 & b < 1));
if any(k2)
   tmp = Inf;
    y(k2) = tmp(ones(size(k2)));
end

% Return the beta density function for valid parameters.
k = find(~(a <= 0 | b <= 0 | x <= 0 | x >= 1));
if any(k)
    y(k) = x(k) .^ (a(k) - 1) .* (1 - x(k)) .^ (b(k) - 1) ./ beta(a(k),b(k));
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SUBFUNCTION betacdf
%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function p = betacdf(x,a,b);
%BETACDF Beta cumulative distribution function.
%   P = BETACDF(X,A,B) returns the beta cumulative distribution
%   function with parameters A and B at the values in X.
%
%   The size of P is the common size of the input arguments. A scalar input
%   functions as a constant matrix of the same size as the other inputs.
%
%   BETAINC does the computational work.

%   Reference:
%      [1]  M. Abramowitz and I. A. Stegun, "Handbook of Mathematical
%      Functions", Government Printing Office, 1964, 26.5.

if nargin < 3,
   error('Requires three input arguments.');
end

[errorcode x a b] = distchck(3,x,a,b);

if errorcode > 0
   error('Requires non-scalar arguments to match in size.');
end

% Initialize P to 0.
p = zeros(size(x));

k1 = find(a<=0 | b<=0);
if any(k1)
   tmp = NaN;
   p(k1) = tmp(ones(size(k1)));
end

% If is X >= 1 the cdf of X is 1.
k2 = find(x >= 1);
if any(k2)
   p(k2) = ones(size(k2));
end

k = find(x > 0 & x < 1 & a > 0 & b > 0);
if any(k)
   p(k) = betainc(x(k),a(k),b(k));
end

% Make sure that round-off errors never make P greater than 1.
k = find(p > 1);
p(k) = ones(size(k));