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


% This function normalize a set of vectors
% Parameters:
%   v     the set of vectors to be normalized (column stored)
%   nr    the norm for which the normalization is performed (Default: Euclidean)
%   rval  replace value in case the vector is 0-norm
%
% Output:
%   vout  the normalized vector
%   vnr   the norms of the input vectors
%
% Remark: the function return Nan for vectors of null norm
function [vout, vnr] = yael_vecs_normalize (v, nr, rval)

if nargin < 2, nr = 2; end

% norm of each column
vnr = (sum (v.^nr)) .^ (1 / nr);

% sparse multiplication to apply the norm
vout = bsxfun (@times, v, 1 ./ vnr);

if exist('rval')
  [~, ko] = find (isnan (vout));
  ko = unique (ko);
  vout (:, ko) = rval;
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 i4_modp_test ( )

%*****************************************************************************80
%
%% I4_MODP_TEST tests I4_MODP.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2009
%
%  Author:
%
%    John Burkardt
%
  test_num = 4;

  ndivid = [ 50, -50, 50, -50 ];
  number = [ 107, 107, -107, -107 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'I4_MODP_TEST\n' );
  fprintf ( 1, '  I4_MODP factors a number\n' );
  fprintf ( 1, '  into a multiple and a remainder.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    Number   Divisor  Multiple Remainder\n' );
  fprintf ( 1, '\n' );

  for test = 1 : test_num
    nrem = i4_modp ( number(test), ndivid(test) );
    nmult = floor ( ( number(test) - nrem ) / ndivid(test) );
    fprintf ( 1, '  %8d  %8d  %8d  %8d\n', ...
      number(test), ndivid(test), nmult, nrem );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Repeat using MOD:\n' );
  fprintf ( 1, '\n' );

  for test = 1 : test_num
    nrem = mod ( number(test), ndivid(test) );
    nmult = floor ( number(test) / ndivid(test) );
    fprintf ( 1, '  %8d  %8d  %8d  %8d\n', ...
      number(test), ndivid(test), nmult, nrem );
  end

  return
end


function linplus_test345 ( )

%*****************************************************************************80
%
%% TEST345 tests R8GE_FSS.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 June 2009
%
%  Author:
%
%    John Burkardt
%
  n = 10;
  nb = 3;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST345\n' );
  fprintf ( 1, '  For a matrix in general storage,\n' );
  fprintf ( 1, '  R8GE_FSS factors and solves multiple linear systems.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix order N = %d\n', n );
%
%  Set the matrix.
%
  [ a, seed ] = r8ge_random ( n, n, seed );
%
%  Set the desired solutions.
%
  x(1:n,1) = 1.0;
  x(1:n,2) = 1:n;
  x(1:n,3) = mod ( 0 : n - 1, 3 ) + 1;

  b(1:n,1:nb) = a(1:n,1:n) * x(1:n,1:nb);
%
%  Factor and solve the system.
%
  x = r8ge_fss ( n, a, nb, b );

  r8ge_print ( n, nb, x, '  Solution:' );

  return
end


function tests = test_spm_ncTcdf
% Unit Tests for spm_ncTcdf
%__________________________________________________________________________
% Copyright (C) 2018 Wellcome Trust Centre for Neuroimaging

% $Id: test_spm_ncTcdf.m 7258 2018-02-14 13:09:46Z guillaume $

tests = functiontests(localfunctions);

function test_spm_ncTcdf_1(testCase)
exp = 0.5; % nctcdf(0,1,0)
act = spm_ncTcdf(0,1,0);
tol = 1e-12;
testCase.verifyEqual(act, exp,'AbsTol',tol);

function test_spm_ncTcdf_2(testCase)
exp = 0.5; % nctcdf(0,4,0)
act = spm_ncTcdf(0,4,0);
tol = 1e-12;
testCase.verifyEqual(act, exp,'AbsTol',tol);

function test_spm_ncTcdf_3(testCase)
exp = 0.022750131948179; % nctcdf(0,4,2)
act = spm_ncTcdf(0,4,2);
tol = 1e-12;
testCase.verifyEqual(act, exp,'AbsTol',tol);

function test_spm_ncTcdf_4(testCase)
exp = 4.218150123125319e-05; % nctcdf(-4,4,2)
act = spm_ncTcdf(-4,4,2);
tol = 1e-12;
testCase.verifyEqual(act, exp,'AbsTol',tol);

function test_spm_ncTcdf_5(testCase)
exp = 0.920822617476195; % nctcdf(5,4,2)
act = spm_ncTcdf(5,4,2);
tol = 1e-12;
testCase.verifyEqual(act, exp,'AbsTol',tol);

function test_spm_ncTcdf_6(testCase)
exp = 0.992600181884681; % nctcdf(10,4,2)
act = spm_ncTcdf(10,4,2);
tol = 1e-12;
testCase.verifyEqual(act, exp,'AbsTol',tol);

function test_spm_ncTcdf_7(testCase)
exp = [0.000000011096548  % nctcdf(-4:10,4,4)
   0.000000032879594
   0.000000140813044
   0.000001174173412
   0.000031671241833
   0.001992604459483
   0.042091256308006
   0.202851164067265
   0.429784541497594
   0.622224194923879
   0.754499466251885
   0.838999243587669
   0.892202382905940
   0.926042984950848
   0.948002154097108]';
act = spm_ncTcdf(-4:10,4,4);
tol = 1e-12;
testCase.verifyEqual(act, exp,'AbsTol',tol);


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 r8row_variance_test ( )

%*****************************************************************************80
%
%% R8ROW_VARIANCE_TEST tests R8ROW_VARIANCE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2009
%
%  Author:
%
%    John Burkardt
%
  m = 3;
  n = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8ROW_VARIANCE_TEST\n' );
  fprintf ( 1, '  For a R8ROW (a matrix regarded as rows):\n' );
  fprintf ( 1, '  R8ROW_VARIANCE computes variances;\n' );

  k = 0;
  for i = 1 : m
    for j = 1 : n
      k = k + 1;
      a(i,j) = k;
    end
  end

  r8mat_print ( m, n, a, '  The original matrix:' );

  variance = r8row_variance ( m, n, a );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Row variances:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : m
    fprintf ( 1, '  %3d  %10f\n', i, variance(i) );
  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 f21=f21(x)
Bound=[0 10];

if nargin==0
    f21 = Bound;
else
aij=[4 4 4 4;
     1 1 1 1;
     8 8 8 8;
     6 6 6 6;
     3 7 3 7];
ci=[.1 .2 .2 .4 .4];

f21=0;

for i=1:5
    f21=f21+(norm(x-aij(i,:)').^2+ci(:,i))^-1;
end

f21=-f21;

end


function [y,deriv] = affineTrans(w,affineMap,linMap,transMap)
% This is an MV2DF. See MV2DF_API_DEFINITION.readme.
%
% Applies affine transform y = affineMap(w). It needs also needs
% linMap, the linear part of the mapping, as well as transMap, the
% transpose of linMap. All of affineMap, linMap and transMap are function
% handles.
%
% Note, linMap(x) =  J*x where J is the Jacobian of affineMap; and
% transMap(y) = J'y.

if nargin==0
    test_this();
    return;
end

if isempty(w)
    y = @(w)affineTrans(w,affineMap,linMap,transMap);
    return;
end

if isa(w,'function_handle')
    outer = affineTrans([],affineMap,linMap,transMap);
    y = compose_mv(outer,w,[]);
    return;
end

y = affineMap(w);
y = y(:);

deriv = @(g2) deriv_this(g2,linMap,transMap);

function [g,hess,linear] = deriv_this(g2,linMap,transMap)
g = transMap(g2);
g = g(:);
%linear = false;  % use this to test linearity of affineMap, if in doubt
linear = true;
hess = @(d) hess_this(linMap,d);

function [h,Jd] = hess_this(linMap,d)
%h=zeros(size(d)); % use this to test linearity of affineMap, if in doubt
h = [];
if nargout>1
    Jd = linMap(d);
    Jd = Jd(:);
end

function test_this()
A = randn(4,5);
k = randn(4,1);
affineMap = @(w) A*w+k;
linMap = @(w) A*w;
transMap = @(y) (y.'*A).'; % faster than A'*y, if A is big
f = affineTrans([],affineMap,linMap,transMap);
test_MV2DF(f,randn(5,1));


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 polys = readPolygonSet(filename)
%READPOLYGONSET Read a set of simple polygons stored in a file
%
%   POLY = readPolygonSet(FILENAME);
%   Returns the polygon stored in the file FILENAME.
%   Polygons are assumed to be stored in text files, without headers, with
%   x and y coordinates packed in two separate lines:
%     X11 X12 X13 ... X1N
%     Y11 Y12 Y13 ... Y1N
%     X21 X22 X23 ... X2N
%     Y21 Y22 Y23 ... Y2N
%
%   Each polygon may have a different number of vertices. The result is a
%   cell array of polygon, each cell containing a N-by-2 array representing
%   the vertex coordinates.
%
%   See also:
%   polygons2d
%
%   ---------
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 11/04/2004.
%

% the set of polygons (no pre-allocation, as we do not know how many
% polygons are stored)
polys = {};

% index of polygon
p = 0;

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

% use an infinite loop, terminated in case of EOF
while true
    % set of X, and Y coordinates
    line1 = fgetl(fid);
    line2 = fgetl(fid);

    % break loop if end of file is reached
    if line1 == -1
        break;
    end

    % create a new polygon by concatenating vertex coordinates
    p = p + 1;
    polys{p} = [str2num(line1)' str2num(line2)']; %#ok<AGROW,ST2NM>
end

% close file
fclose(fid);


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


function out = shuffledMatrix(x)

    N = length(x(:,1));
    L = length(x(1,:));

    out = zeros(N,L);
    for i=1:L
        q = randperm(N);
        out(:,i) = x(q,i);
    end


function [y,deriv] = bsxtimes(w,m,n)
% This is an MV2DF
%
% w = [vec(A); vec(b) ] --> vec(bsxfun(@times,A,b)),
%
%      where A is an m-by-n matrix and
%            b is a 1-by-n row.
%

if nargin==0
    test_this();
    return;
end

if isempty(w)
    y = @(w) bsxtimes(w,m,n);
    return;
end

if isa(w,'function_handle')
    f = bsxtimes([],m,n);
    y = compose_mv(f,w,[]);
    return;
end

[A,b] = extract(w,m,n);
y = bsxfun(@times,A,b);
y = y(:);

deriv = @(Dy) deriv_this(Dy,A,b);

function  [g,hess,linear] = deriv_this(Dy,A,b)
g = gradient(Dy,A,b);
linear = false;

hess = @(v) hess_this(v,Dy,A,b);

function [h,Jv] = hess_this(v,Dy,A,b)
[m,n] = size(A);
[vA,vb] = extract(v,m,n);
h = gradient(Dy,vA,vb);
if nargout>1
  Jv = bsxfun(@times,vA,b);
  Jv = Jv + bsxfun(@times,A,vb);
  Jv = Jv(:);
end

function [A,b] = extract(w,m,n)
A = reshape(w(1:m*n),m,n);
b = w(m*n+1:end).';

function g = gradient(Dy,A,b)
Dy = reshape(Dy,size(A));
gA = bsxfun(@times,Dy,b);
gb = sum(Dy.*A,1);
g = [gA(:);gb(:)];

function test_this()
m = 5;
n = 10;
A = randn(m,n);
b = randn(1,n);
w = [A(:);b(:)];

f = bsxtimes([],m,n);
test_MV2DF(f,w);


function y = vl_nnbilinearpool(x, varargin)
% VL_NNBILINEARPOOL computes self outer product of a feature x and pool the features across
% all locations
%
% Author: Subhransu Maji, Aruni RoyChowdhury, Tsung-Yu Lin

%
% This file is part of the BCNN and is made available under
% the terms of the BSD license (see the COPYING file).

% input:
% forward pass:
% x: input featre of size [hight, width, channels, batches]
% backward pass:
% x: input featre of size [hight, width, channels, batches]
% dzdy: the gradient with respect to output y.

% output:
% forward pass:
% y: self outer product of x.Features are pooled across locations
%    The output size if [1, 1, channels*channels, batches]
% backward pass:
% y: graident with respect to x. y will have the same size as input x.

backMode = numel(varargin) > 0 && ~isstr(varargin{1}) ;
if backMode
  dzdy = varargin{1};
end

gpuMode = isa(x, 'gpuArray');
[h, w, ch, bs] = size(x);

if backMode
    if gpuMode
        y = gpuArray(zeros(size(x), 'single'));
    else
        y = zeros(size(x), 'single');
    end
    for b=1:bs
        dzdy_b = reshape(dzdy(1,1,:,b), [ch, ch]);
        a = reshape(x(:,:,:,b), [h*w, ch]);
        y(:, :, :, b) = reshape(a*dzdy_b, [h, w, ch])/(h*w);
    end
else
    if gpuMode
        y = gpuArray(zeros([1, 1, ch*ch, bs], 'single'));
    else
        y = zeros([1, 1, ch*ch, bs], 'single');
    end
    for b = 1:bs,
        a = reshape(x(:,:,:,b), [h*w, ch]);
        y(1,1,:, b) = reshape(a'*a, [1 ch*ch])/(h*w);
    end
end


function [fmat ortirfcell]=tvcfsim1(beta,D,ybarT,data_exo_p,Fperiods,n,m,p,k)

% first recover the set of At matrices, Cbart matrices, and xbart vectors
% initiate At, Cbart and xt
At=cell(Fperiods,1);
Cbart=cell(Fperiods,1);
xbart=cell(Fperiods,1);
% reshape beta to obtain one equation per column
beta=reshape(beta,k,Fperiods*n);
% separate the coefficients related to exogenous variables (the last m rows of each equation)
% from the ones related to endogenous variables (the other entries) in order to be able to obtain At and Cbart
beta_endo=reshape(beta(1:end-m,:),[k-m n Fperiods]);
beta_exo=reshape(beta(end-m+1:end,:),[m n Fperiods]);
% then loop over forecast periods to recover the series of matrices
for ii=1:Fperiods
At{ii,1}=sparse([beta_endo(:,:,ii)';speye(n*(p-1)) sparse(n*(p-1),n)]);
Cbart{ii,1}=sparse([beta_exo(:,:,ii)';sparse(n*(p-1),m)]);
xbart{ii,1}=data_exo_p(ii,:)';
end

% then produce the series of forecasts and impulse response functions
% preliminary element: generate the selection matrix J
J=[speye(n) sparse(n,n*(p-1))];
% then create the cells storing the IRFs and the forecasts
ortirfcell=cell(Fperiods,Fperiods);
fmat=[];

% estimate first the forecasts
% loop over forecasts periods
for hh=1:Fperiods
% compute the first product term
prod1=speye(n*p);
   for ii=1:hh
   prod1=prod1*At{1+hh-ii,1};
   end
% multiply by ybarT to obtain the first term
term1=prod1*ybarT;
% obtain the second term
% initiate the summation
summ=sparse(n*p,1);
   % loop over summation periods
   for ii=1:hh
   % initiate the product
   prod2=speye(n*p);
      % loop over product periods
      for jj=ii:hh-1
      prod2=prod2*At{hh+ii-jj,1};
      end
   % multiply by Cbart*xt and add to summation
   summ=summ+prod2*Cbart{ii,1}*xbart{ii,1};
   end
term2=summ;
% obtain the forecast for the period
fmat(:,hh)=J*(term1+term2);
end
fmat=fmat';

% then compute the period-specific IRFs
% loop over forecast periods
for hh=1:Fperiods
% the first row of the cell represents the term ii=hh in the summation in 273: it is always equal to identity ( and then becomes D once multiplied by the structural matrix)
ortirfcell{1,hh}=D;
   % loop over IRF periods (the summation term in 272)
   for ii=1:hh-1
   % initiate the product
   prod3=speye(n*p);
      % loop over the periods involved into the product and calculate it
      for jj=ii:hh-1
      prod3=prod3*At{hh+ii-jj,1};
      end
   % recover the matrix of interest from the selection matrix J
   irfmat=full(J*prod3*J');
   % obtain orthogonalised IRFs
   ortirfmat=irfmat*D;
   % record in IRFcell
   ortirfcell{hh-ii+1,hh}=ortirfmat;
   end
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 p_xy = cvt_circle ( r, np, p_xy, p_type )

%*****************************************************************************80
%
%% CVT_CIRCLE applies the CVT algorithm to points on a circle.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 December 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real R, the radius of the circle.
%
%    Input, integer NP, the number of points.
%
%    Input, real P_XY(2,NP), the point coordinates.
%
%    Input, integer P_TYPE(NP),
%    1, the point is constrained to the circle.
%    2, the point is constrainted to the disk.
%
%    Output, real P_XY(2,NP), the updated point coordinates.
%
  for i = 1 : 30
    p_xy = cvt_circle_step ( r, np, p_xy, p_type );
  end

  return
end


function pass = test_partitionCombine( )
% Test the partition and combine methods.

tol = 1e3*chebfunpref().cheb2Prefs.chebfun2eps;

% Check that partitioning an empty diskfun gives two empty diskfuns
f = diskfun;
[feven,fodd] = partition(f);
pass(1) = isempty(feven) & isempty(fodd);

feven = diskfun;
fodd = diskfun;
f = combine(feven,fodd);
pass(2) = isempty(f);

% Check that an even diskfun is partitioned correctly.
f = diskfun(@(x,y) sin(pi*x.*y));  % Strictly even/pi-periodic
[feven,fodd] = partition(f);
pass(3) = isequal(feven,f);
pass(4) = isempty(fodd);

% Check that odd diskfun is partitioned correctly.
f = diskfun(@(x,y) sin(pi*x));  % Strictly odd/anti-periodic
[feven,fodd] = partition(f);
pass(5) = isequal(fodd,f);
pass(6) = isempty(feven);

% Check that the even and odd terms are partitioned correctly
fe = @(x,y) sin(pi*x.*y);  % Strictly even/pi-periodic
fo = @(x,y) sin(pi*x);  % Strictly odd/anti-periodic
f = diskfun(@(x,y) fe(x,y) + fo(x,y));
[feven,fodd] = partition(f);
pass(7) = norm(diskfun(fe)-feven) < tol;
pass(8) = norm(diskfun(fo)-fodd) < tol;

% Check that combine puts an even diskfun and empty spherfun back
% together.
feven = diskfun(@(x,y) sin(pi*x.*y));  % Strictly even/pi-periodic
fodd = diskfun;
f = combine(feven,fodd);
pass(9) = norm(feven-f) < tol;

% Check that combine puts an even diskfun and empty spherfun back
% together.
feven = diskfun;
fodd = diskfun(@(x,y) sin(pi*x));  % Strictly odd/anti-periodic
f = combine(feven,fodd);
pass(10) = norm(fodd-f) < tol;

% Check that combine puts non-empty odd and even diskfuns back together.
fe = @(x,y) sin(pi*x.*y);  % Strictly even/pi-periodic
fo = @(x,y) sin(pi*x);  % Strictly odd/anti-periodic
fcombine = diskfun(@(x,y) fe(x,y) + fo(x,y));
f = combine(diskfun(fe),diskfun(fo));
pass(11) = norm(fcombine-f) < tol;

% Check that combine cannot put together diskfuns that are not strictly
% odd or even.
try
    f = diskfun(@(x,y) sin(pi*x.*y) + sin(pi*x));
    g = combine(f,f);
    pass(12) = false;
catch ME
    pass(12) = strcmp(ME.identifier, 'CHEBFUN:DISKFUN:combine:parity');
end

end


function indx = multigrid_index_cfn ( dim_num, order_1d, order_nd )

%*****************************************************************************80
%
%% MULTIGRID_INDEX_CFN indexes a sparse grid based on CFN 1D rules.
%
%  Discussion:
%
%    The sparse grid is presumed to have been created from products
%    of CLOSED FULLY NESTED 1D quadrature rules.
%
%    CFN rules include Clenshaw Curtis rules.
%
%    For dimension DIM, the second index of INDX may vary from
%    0 to ORDER_1D(DIM)-1.
%
%  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 of the points.
%
%    Input, integer ORDER_1D(DIM_NUM), the order of the
%    rule in each dimension.
%
%    Input, integer ORDER_ND, the product of the entries of ORDER_1D.
%
%    Output, integer INDX(DIM_NUM,ORDER_ND), the indices of the points in
%    the grid.  The second dimension of this array is equal to the
%    product of the entries of ORDER_1D.
%
  p = 0;
  indx = zeros(dim_num,order_nd);

  a = [];
  more = 0;

  while ( 1 )

    [ a, more ] = vec_colex_next2 ( dim_num, order_1d, a, more );

    if ( ~more )
      break
    end

    p = p + 1;

    indx(1:dim_num,p) = a(1:dim_num)';

  end

  return
end


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


% DECIMATE_LIBIGL Decimate a closed manifold mesh (V,F)
%
% [W,G] = decimate_libigl(V,F,ratio)
% [W,G,J,I] = decimate_libigl(V,F,ratio,'ParameterName',ParameterValue, ...)
%
% Inputs:
%   V  #V by 3 list of vertex positions
%   F  #F by 3 list of triangle indices into V
%   ratio   either a 1<number<#F  of max faces, or a 0<ratio<1 to be multiplied
%     against #F to get max faces in output
%   Optional:
%     'Method' followed by one of:
%        {'naive'}  simply collapse small edges and place vertices at midpoint
%        'qslim'  Quadric error metric
%        'progressive-hulls'  Progressive hulls
% Outputs:
%   W  #W by 3 list of vertex positions
%   G  #G by 3 list of triangle indices into W
%   J  #G list of indices into F of birth face
%   I  #U list of indices into V of birth vertices
%


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


% 2D Example for Parametric Multilevel Mass-Preserving Image Registration using VAMPIRE
%
% (c) Fabian Gigengack and Lars Ruthotto 2011/02/04, see FAIR.2 and FAIRcopyright.m.
% http://www.uni-muenster.de/EIMI/
% http://www.mic.uni-luebeck.de/
%
%   - data                 synthetic 2D Gaussian blobs (level 3:8, full
%                          resolution: 256x256)
%   - viewer               viewImage2D
%   - interpolation        splineInter
%   - distance             SSD
%   - pre-registration     none
%   - transformation       splineTransformation2Dsparse
%   - regularizer          parametric FAIR regularizer
%   - optimizer            Gauss-Newton with ArmijoBacktrack linesearch

setup2DGaussianData;

% prepare the plot
FAIRplots('clear')
Dshow = @(T,R,omega,m) viewImage2D(128+(T-R)/2,omega,m,'colormap',gray(256));
FAIRplots('set','Dshow',Dshow);

% initialize the regularizer for the non-parametric part
alpha       = 100;
alphaLength = 1;
alphaArea   = 0;
alphaVolume = 1;
[reg,regOptn] = regularizer('reset', 'regularizer', 'mfHyperElastic', ...
    'alpha',alpha, 'alphaLength', alphaLength, 'alphaArea', alphaArea, ...
    'alphaVolume', alphaVolume);

% set the parametric transformation
p = [18 18];
trafo('reset','trafo','splineTransformation2Dsparse','omega',omega,'m',m,'p',p);

% finally: run MLPIR
PIRpara            = optPara('PIR-GN');
PIRpara.solver     = @VAMPIREsolveGN_PCG;

[wc,his] = MLPIR(ML, 'PIRobj', @VAMPIREPIRobjFctn, ...
    'getGrid', @getNodalGrid,'PIRpara',PIRpara);

%% Plot results
yc = trafo(wc,getNodalGrid(omega, m));
% Compute resulting image: dataT(yc) * det(D(yc))
Topt = reshape(linearInter(dataT,omega,center(yc,m)) ...
    .* geometry(yc,m,'Jac','omega',omega), m);
figure;
subplot(2,2,1);
viewImage2D(dataT,omega,m,'colormap', 'gray(256)'); hold on;
plotGrid(center(yc, m), omega, m, 'spacing', [5 5]); axis off; hold off;
title('Template (T) & Grid')
subplot(2,2,2);
viewImage2D(dataR,omega,m,'colormap', 'gray(256)'); axis off;
title('Reference (R)')
subplot(2,2,3);
viewImage2D(Topt,omega,m,'colormap', 'gray(256)'); axis off;
title('VAMPIRE result (Topt)')
subplot(2,2,4);
viewImage2D(abs(Topt-dataR),omega,m,'colormap', 'gray(256)'); axis off;
title('Absolute difference image of R and Topt')


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 data = clusterincluster(N, r1, r2, w1, w2, arms)

    if nargin < 1
        N = 1000;
    end
    if nargin < 2
        r1 = 1;
    end
    if nargin < 3
        r2 = 5*r1;
    end
    if nargin < 4
        w1 = 0.8;
    end
    if nargin < 5
        w2 = 1/3;
    end
    if nargin < 6
        arms = 64;
    end

    data = [];

    N1 = floor(N/2);
    N2 = N-N1;

    phi1 = rand(N1,1) * 2 * pi;
    dist1 = r1 + randint(N1,1,3)/3 * r1 * w1;
    d1 = [dist1 .* cos(phi1) dist1 .* sin(phi1) zeros(N1,1)];

    perarm = round(N2/arms);
    N2 = perarm * arms;
    radperarm = (2*pi)/arms;
    phi2 = ((1:N2) - mod(1:N2, perarm))/perarm * (radperarm);
    phi2 = phi2';
    dist2 = r2 * (1 - w2/2) + r2 * w2 * mod(1:N2, perarm)'/perarm;
    d2 = [dist2 .* cos(phi2) dist2 .* sin(phi2) ones(N2,1)];

    data = [d1;d2];

    scatter(data(:,1), data(:,2), 20, data(:,3)); axis square;
end


function [Adraw]=sampleA_H(yData,Psi,B,Hvars,T,priorValues,dataValues)
% This function takes a draw from the conditional posterior distribution of
% A. The algorithm is based on Cogley and Sargent (2005).

%% Initialize
M=size(yData,2);
p = size(B,1)/M;
Hscaling=Hvars(p+1:T,:).^0.5; %sqrt of the diagonal elements of the H matrix

% priors
priorVarAscaling=priorValues.priorVarAscaling_H;  %prior variance for the below diagonal elements of A
priorMeanAscaling=priorValues.priorMeanAscaling_H; %prior mean for the below diagonal elements of A

%% Prepare data
Y_Psi=yData-Psi; %remove local mean
Y_Psi(1:p,:)=yData(1:p,:)-ones(p,1)*mean(yData(1:p,:));
X_Psi = lagmatrix(Y_Psi,1:p);
X_Psi = X_Psi(p+1:end,:);
Y_Psi=Y_Psi(p+1:end,:);

V=Y_Psi-X_Psi*B; %generate residuals

%% Sample row-by-row
Atemp=eye(M);

for i=2:M %the first row of A is by construction [1 0 ...M]

    Hscaling_i=Hscaling(:,i)*ones(1,M);
    Vscaled_i=V./Hscaling_i;

    % row data
    zi=Vscaled_i(:,i);  %rescaled residuals
    Zi=-Vscaled_i(:,1:i-1);

    % make row priors
    priorVar_ai=priorVarAscaling*eye(i-1);
    priorMean_ai=priorMeanAscaling*ones(i-1,1);

    % determine posterior
    postVar_ai=(Zi'*Zi+(priorVar_ai\eye(i-1)))\eye(i-1); %posterior variance implied by the normality assumption on prior and likelihood
    postMean_ai=postVar_ai*(Zi'*zi+(priorVar_ai\eye(i-1))*priorMean_ai); %posterior mean implied by the normality assumption

    % obtain cholvar
    [Cvar,testPD]=chol(postVar_ai);
    if testPD>0
        Cvar= cholred(postVar_ai);
        disp('NPD!')
    end
    Cvar=Cvar'; % transpose to obtain lower triangular matrix

    % sample from posterior
    aiDraw=postMean_ai+Cvar*randn(i-1,1);

    % save aiDraw
    Atemp(i,1:i-1)=aiDraw';

end

%% Save the draw of A
Adraw=Atemp;

end


Network vgg16 {
Layer CONV1 {
Type: CONV
Dimensions { K 64,C 3,R 3,S 3,Y 224,X 224 }
Dataflow {
TemporalMap (1,1) K;
TemporalMap (1,1) C;
TemporalMap (3,1) Y;
SpatialMap (3,1) X;
TemporalMap (3,3) R;
TemporalMap (3,3) S;
}
}

Layer CONV2 {
Type: CONV
Dimensions { K 64,C 64,R 3,S 3,Y 224,X 224 }
Dataflow {
TemporalMap (1,1) K;
TemporalMap (1,1) C;
TemporalMap (3,1) Y;
SpatialMap (3,1) X;
TemporalMap (3,3) R;
TemporalMap (3,3) S;
}
}

Layer CONV3 {
Type: CONV
Dimensions { K 128,C 64,R 3,S 3,Y 112,X 112 }
Dataflow {
TemporalMap (1,1) K;
TemporalMap (1,1) C;
TemporalMap (3,1) Y;
SpatialMap (3,1) X;
TemporalMap (3,3) R;
TemporalMap (3,3) S;
}
}

Layer CONV4 {
Type: CONV
Dimensions { K 128,C 128,R 3,S 3,Y 112,X 112 }
Dataflow {
TemporalMap (1,1) K;
TemporalMap (1,1) C;
TemporalMap (3,1) Y;
SpatialMap (3,1) X;
TemporalMap (3,3) R;
TemporalMap (3,3) S;
}
}

Layer CONV5 {
Type: CONV
Dimensions { K 256,C 128,R 3,S 3,Y 56,X 56 }
Dataflow {
TemporalMap (1,1) K;
TemporalMap (1,1) C;
TemporalMap (3,1) Y;
SpatialMap (3,1) X;
TemporalMap (3,3) R;
TemporalMap (3,3) S;
}
}

Layer CONV6 {
Type: CONV
Dimensions { K 256,C 256,R 3,S 3,Y 56,X 56 }
Dataflow {
TemporalMap (1,1) K;
TemporalMap (1,1) C;
TemporalMap (3,1) Y;
SpatialMap (3,1) X;
TemporalMap (3,3) R;
TemporalMap (3,3) S;
}
}

Layer CONV7 {
Type: CONV
Dimensions { K 256,C 256,R 3,S 3,Y 56,X 56 }
Dataflow {
TemporalMap (1,1) K;
TemporalMap (1,1) C;
TemporalMap (3,1) Y;
SpatialMap (3,1) X;
TemporalMap (3,3) R;
TemporalMap (3,3) S;
}
}

Layer CONV8 {
Type: CONV
Dimensions { K 512,C 256,R 3,S 3,Y 28,X 28 }
Dataflow {
TemporalMap (1,1) K;
TemporalMap (1,1) C;
TemporalMap (3,1) Y;
SpatialMap (3,1) X;
TemporalMap (3,3) R;
TemporalMap (3,3) S;
}
}

Layer CONV9 {
Type: CONV
Dimensions { K 512,C 512,R 3,S 3,Y 28,X 28 }
Dataflow {
TemporalMap (1,1) K;
TemporalMap (1,1) C;
TemporalMap (3,1) Y;
SpatialMap (3,1) X;
TemporalMap (3,3) R;
TemporalMap (3,3) S;
}
}

Layer CONV10 {
Type: CONV
Dimensions { K 512,C 512,R 3,S 3,Y 28,X 28 }
Dataflow {
TemporalMap (1,1) K;
TemporalMap (1,1) C;
TemporalMap (3,1) Y;
SpatialMap (3,1) X;
TemporalMap (3,3) R;
TemporalMap (3,3) S;
}
}

Layer CONV11 {
Type: CONV
Dimensions { K 512,C 512,R 3,S 3,Y 14,X 14 }
Dataflow {
TemporalMap (1,1) K;
TemporalMap (1,1) C;
TemporalMap (3,1) Y;
SpatialMap (3,1) X;
TemporalMap (3,3) R;
TemporalMap (3,3) S;
}
}

Layer CONV12 {
Type: CONV
Dimensions { K 512,C 512,R 3,S 3,Y 14,X 14 }
Dataflow {
TemporalMap (1,1) K;
TemporalMap (1,1) C;
TemporalMap (3,1) Y;
SpatialMap (3,1) X;
TemporalMap (3,3) R;
TemporalMap (3,3) S;
}
}

Layer CONV13 {
Type: CONV
Dimensions { K 512,C 512,R 3,S 3,Y 14,X 14 }
Dataflow {
TemporalMap (1,1) K;
TemporalMap (1,1) C;
TemporalMap (3,1) Y;
SpatialMap (3,1) X;
TemporalMap (3,3) R;
TemporalMap (3,3) S;
}
}

}


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 [cal] = kcal2cal(kcal)
% Convert energy or work from kilocalories to calories.
% Note: kilocalories are the same thing as nutritional Calories.
% Chad A. Greene 2012
cal = kcal*1000;


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 hrfs = getcanonicalhrflibrary(duration,tr)

% function hrfs = getcanonicalhrflibrary(duration,tr)
%
% <duration> is the duration of the stimulus in seconds.
%   should be a multiple of 0.1 (if not, we round to the nearest 0.1).
%   0 is automatically treated as 0.1.
% <tr> is the TR in seconds.
%
% generate a library of 20 predicted HRFs to a stimulus of
% duration <duration>, with data sampled at a TR of <tr>.
%
% the resulting HRFs are returned as 20 x time. the first point is
% coincident with stimulus onset. each HRF is normalized such
% that the maximum value is one.
%
% example:
% hrfs = getcanonicalhrflibrary(4,1);
% figure; plot(0:size(hrfs,2)-1,hrfs,'o-');

% inputs
if duration == 0
  duration = 0.1;
end

% load the library
file0 = strrep(which('getcanonicalhrflibrary'),'getcanonicalhrflibrary.m','getcanonicalhrflibrary.tsv');
hrfs = load(file0)';  % 20 HRFs x 501 time points

% convolve to get the predicted response to the desired stimulus duration
trold = 0.1;
hrfs = conv2(hrfs,ones(1,max(1,round(duration/trold))));

% resample to desired TR
hrfs = interp1((0:size(hrfs,2)-1)*trold,hrfs',0:tr:(size(hrfs,2)-1)*trold,'pchip')';  % 20 HRFs x time

% make the peak equal to one
hrfs = hrfs ./ repmat(max(hrfs,[],2),[1 size(hrfs,2)]);

%%%%%%%%%%%%%%%%%%% FOR OUR RECORDS BELOW:

% % params taken from the Natural Scenes Dataset
% a1 = load('~/nsd/nsddata/templates/hrfparams.mat');
%
% % obtain canonical response to a 0.1-s stimulus
% hrfs = [];
% for p=1:size(a1.params,1)
%   hrfs(p,:) = spm_hrf(0.1,a1.params(p,:));
% end
%
% dlmwrite('getcanonicalhrflibrary.tsv',hrfs','delimiter','\t','precision',5);
% test=load('getcanonicalhrflibrary.tsv');
% figure; plot(hrfs');
% figure; plot(test);


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));