function stats = glmfit_multilevel(Y, X1, X2, varargin)
% :Usage:
% ::
%
%     stats = glmfit_multilevel(Y, X1, X2, varargin)
%
% Mixed-effects models differ in their assumptions and implementation
% details. glmfit_multilevel is a fast and simple option for running a
% two-level mixed effects model with participant as a random effect.
% It implements a random-intercept, random-slope model across 2nd-level units
% (e.g., participants). it fits regressions for individual 2nd-level
% units (e.g., participants), and then (optionally) uses a precision-weighted least
% squares approach to model group effects. It thus treats participants as a
% random effect. This is appropriate when 2nd-level units are participants
% and 1st-level units are observations (e.g., trials) within participants.
% glmfit_multilevel was designed with this use case in mind.
%
% Options:
% glmfit_multilevel includes some options that are not included in many
% mixed effects models, including:
% - bootstrapping or sign permutation for inference
% - robust regression (*needs code update*)
% - AR(p) autoregressive model (*needs code update*)
%
% Requirements: glmfit_multilevel requires enough 1st-level units to fit a
% separate model for each  2nd-level unit (participant). If this is not the
% case, other models (igls.m, LMER, etc.) are preferred.
%
% Degrees of freedom: glmfit_multilevel is conservative in the sense that the degrees of
% freedom in the group statistical test is always based on the number of
% subjects - 2nd-level parameters.
% The df is never higher than the sample size, which you would have with
% mixed effects models that estimate the df from the data. This causes
% problems in many other packages, particularly when there are many 1st-level
% observations and they are uncorrelated, resulting in large and undesirable
% estimated df.
%
% Correlated effects: The correlations across 1st-level observations are not measured, and
% 1st-level obs are assumed to be IID. This is valid when generalizing across
% 2nd-level units, but may not be fully efficient (powerful) if 1st-level
% units are correlated.
%
% :Inputs:
%
%   **Y:**
%        Is data in either:
%           -cell array, one cell per subject.
%            Column vector of subject outcome data in each cell.
%           -Matrix
%            One column per subject, with vector of subject outcome
%            data in that column
%
%   **X1 and X2:**
%        are first and 2nd level design matrices
%          - X1 in cell array, one cell per subject
%            design matrix for each subject in each cell.
%            *columns must code for the same variable for all subjects*
%
%         - X2 in rect. matrix
%         - can be empty (intercept only)
%
% E.g., with one 2nd-level predictor:
% stats = glmfit_multilevel(Y, X1, X2, ...
% 'names', {'Int' 'Temp'}, 'beta_names', {'2nd-level Intercept (overall group effect)' '2nd-lev predictor: Group membership'});
%
% :Output:
%
%   **stats:**
%        is structure with results.
%        Intercept is always added as first column!
%        (do not add intercept to input predictors)
%
% See glmfit_general.m for varargin variable input options.
%
% :Examples:
% ::
%
%    len = 200; sub = 20;
%    x = zeros(len,sub);
%    x(11:20,:) = 2;                   % create signal
%    x(111:120,:) = 2;
%    c = normrnd(0.5,0.1,sub,1);       % slope between-subjects variations
%    d = normrnd(3,0.2,sub,1);         % intercept between-subjects variations
%    % Create y: Add between-subjects error (random effects) and measurement noise
%    % (within-subjects error)
%    for i=1:sub, y(:,i) = d(i) + c(i).*x(:,i) + normrnd(0,0.5,len,1);
%    end;
%
%    for i = 1:size(y, 2), YY{i} = y(:, i); end
%    for i = 1:size(y, 2), XX{i} = x(:, i); end
%
%    % one-sample t-test, weighted by inv of btwn + within vars
%    stats = glmfit_multilevel(YY, XX, [], 'verbose', 'weighted');
%
%    statsg = glmfit_multilevel(y, x, covti, 'names', {'L1 Intercept' 'L1 Slope'},...
%    'beta_names', {'Group Average', 'L2_Covt'});
%
% :Input Options:
%
% General Defaults
%  - case 'names', Names of first-level predictors, starting
%    with 'Intercept', in cell array
%
%  - case 'analysisname', analysisname = varargin{i+1}; varargin{i+1} = [];
%  - case 'beta_names', beta_names = Names of 2nd-level predictors, starting
%    with 'Intercept', in cell array
%
% Estimation Defaults
%  - case 'robust', robust_option = 'yes';
%  - case {'weight', 'weighted', 'var', 's2'}, weight_option = 'unweighted';
%  - case {'nocenter'}, do not force centering of 2nd-level predictors
%
% Inference defaults
%  - case {'boot1', 'boot', 'bootstrap'}, inference_option = 'bootstrap';
%  - case {'sign perm', 'signperm', 'sign'}, inference_option = 'signperm';
%  - case {'t-test', 'ttest'}, inference_option = 't-test';
%
% Display control defaults
%  - case 'plots', doplots = 1; plotstr = 'plots';
%  - case 'noplots', doplots = 0; plotstr = 'noplots';
%
%  - case {'dosave', 'save', 'saveplots'}, dosave = 1; savestr = 'save';
%  - case 'verbose', verbose = 1; verbstr = 'verbose';
%  - case 'noverbose', verbose = 0; verbstr = 'noverbose';
%
%  - case {'savefile', 'savefilename'}, savefilename = varargin{i + 1}; varargin{i+1} = [];
%
% Bootstrap defaults
%  - case 'nresample', nresample = varargin{i+1};
%  - case {'pvals', 'whpvals_for_boot'}, whpvals_for_boot = varargin{i+1};
%
% Sign perm defaults
%  - case {'permsign'}, permsign = varargin{i+1};
%

%    Programmer's notes:
%    9/2/09: Tor and Lauren: Edited to drop NaNs within-subject, and drop
%    subject only if there are too few observations to estimate.
% ..

 % Convert from matrix form to cells
 % Matrix: Y is a col vector, X is one predictor column per subject
 % -------------------------------------------------------------------

  if ~iscell(Y)
    N = size(Y, 2);
    for i = 1:N
      YY{i} = Y(:, i);
    end
    Y = YY;
    clear YY;
  end

  if ~iscell(X1)
    N2 = size(X1, 2);
    if N ~= N2, error('Sizes of X and Y do not match'); end
    for i = 1:N
      XX{i} = X1(:, i);
    end
    X1 = XX;
    clear XX
  end

  N = length(Y);
  if N ~= length(X1)
    error('Enter one cell per subject for each of X and Y');
  end

 % first level: SETUP
 % -------------------------------------------------------------------

 % Check variances and exclude subjects with no variance in any Y or X
 [Y, X1, X2] = check_variances_and_exclude(Y, X1, X2);
 N = length(Y);

 % set up first-level X matrix (sample)
  if any(strcmp(varargin, 'noint')) % no-intercept version
    X1tmp = X1{1};
  else
    X1tmp = setup_X_matrix(X1{1}); % intercept first
  end

  k = size(X1tmp, 2);             % num predictors; assumed to be the same!!

 % Second level: SETUP
 % Need to remove 2nd-level units with NaN data at first level
 % -------------------------------------------------------------------
  wh_omit = false(1, N);
  for i = 1:N
     %if any(isnan(Y{i})) || any(isnan(X1{i}(:))), wh_omit(i) = 1; end

    can_be_nans = length(Y{i}) - k - 1;  % up to this many can be NaN, still leaving 1 degree of freedom
    if can_be_nans < 0, warning('Warning: you might be overparameterized!  Seems like you have more predictors than observations'); end
    if sum(isnan(Y{i}) | any(isnan(X1{i}), 2)) > can_be_nans
      wh_omit(i) = 1;
    end
  end

  if any(wh_omit)
    if isempty(X2), X2 = ones(N, 1); end

    Y(wh_omit) = [];
    X1(wh_omit) = [];
    X2(wh_omit, :) = [];
    N = length(Y);
  end

  beta = zeros(k, N);
  sterr = zeros(k, N);
  t = zeros(k, N);
  p = zeros(k, N);
  dfe = zeros(1, N);
%phi = zeros(arorder, N);

 % first level: ESTIMATE
 % -------------------------------------------------------------------
  for i = 1:N
    [beta(:, i), sterr(:, i), t(:, i), p(:, i), dfe(:, i), phi(:,i), V{i}] = first_level_model(Y{i}, X1{i}, varargin{:});
       % V{i} is var/cov matrix (xtxi)*sigmasq
  end

  varnames = {'beta' 't' 'p' 'dfe' 'phi'};
  first_level = create_struct(varnames);
  first_level.ste = sterr;

 % second level: Finish SETUP and ESTIMATE
 % -------------------------------------------------------------------
 % set up second-level X matrix: intercept first
  X2 = setup_X_matrix(X2, beta(1,:)');

% set up second-level options
% names of outcomes become beta_names here b/c second level test on 1st
% level betas
  [beta_names1, analysisname, beta_names2, robust_option, weight_option, inference_option, ...
   verbose, dosave, doplots, ...
   verbstr, savestr, plotstr, ...
   targetu, nresample, whpvals_for_boot, ...
   permsign] = ...
  setup_inputs(beta', X2, varargin{:});

  switch weight_option
    case 'weighted'
  % Note: R & B-style : replaced sterr' with V

      stats = glmfit_general( ...
	      beta', X2, ...
	      'analysisname', analysisname, 'names', beta_names1, 'beta_names', beta_names2, ...
	      verbstr, savestr, plotstr, ...
	      weight_option, V, inference_option, 'dfwithin', dfe', ...
	      'targetu', targetu, 'nresample', nresample, ...
	      'whpvals_for_boot', whpvals_for_boot, 'permsign', permsign);

    case 'unweighted'

      stats = glmfit_general( ...
	      beta', X2, ...
	      'analysisname', analysisname, 'names', beta_names1, 'beta_names', beta_names2, ...
	      verbstr, savestr, plotstr, ...
	      weight_option, inference_option, ...
	      'targetu', targetu, 'nresample', nresample, ...
	      'whpvals_for_boot', whpvals_for_boot, 'permsign', permsign);

    otherwise
      error('Problem with weight_option. Please select either weighted or unweighted.')
  end

  stats.first_level = first_level;

  if doplots
    scn_stats_helper_functions('xyplot', X1, Y, weight_option, 'names', beta_names1, 'nostats');
    xlabel('X'); ylabel('Y');
  end

% _________________________________________________________________________
%
%
%
% * Inline functions
%
%
%
%__________________________________________________________________________

  function newstruct = create_struct(varnames)
    newstruct = struct();
    for i = 1:length(varnames)
      eval(['newstruct.' varnames{i} ' = ' varnames{i} ';']);
    end
  end

end %End of glmfit_multilevel

function [b, sterr, t, p, dfe, phi, V] = first_level_model(y, X, varargin)

% defaults
% -------------------------------------------------------------------
verbose = 0;
verbstr = 'noverbose';
arorder = 0;                    % or Zero for no AR
interceptstr = 'intercept';

% optional inputs
% -------------------------------------------------------------------
for varg = 1:length(varargin)
    if ischar(varargin{varg})
        switch varargin{varg}

            % reserved keywords
            case 'verbose all', verbose = 1; verbstr = 'verbose';
            case 'verbose', % do nothing
            case {'ar', 'arorder'} , arorder = varargin{varg+1};
                %otherwise, disp(['Unknown input string option: ' varargin{varg}]);

            case 'noint', interceptstr = 'noint';
        end
    end
end

k = size(X, 2) + 1;

[whnan X y] = nanremove(X, y);

if isempty(X)
    % no data

    [b, t, p, sterr] = deal(NaN * ones(k, 1));
    dfe = deal(NaN);
    if arorder, phi = NaN * ones(arorder, 1); else phi = NaN; end
    V = [];

    return

end

% set up X matrix: intercept first
if ~strcmp(interceptstr, 'noint')
    X = setup_X_matrix(X, y);
end

if arorder
    % if we have missing observations or redundant columns, let's
    % regularize a bit so we can still estimate this, using a ridge prior
    % The degree of regularization is arbitrary.
    % V is used to estimate variance components and re-weight.
    if rank(X) < size(X, 2)
        disp('WARNING! RANK DEFICIENT.  THIS FUNCTION WILL RETURN AN ERROR.')
        X = [X; eye(size(X, 2)) ./ size(X, 1)];
        if ~strcmp(interceptstr, 'noint'), X(:, 1) = 1; end
        y = [y; ones(size(X, 2), 1) .* nanmean(y)];
    end

    [t, dfe, b, phi, sigma, sterr] = fit_gls(y, X, [], arorder);
    p = 2 * (1 - tcdf(abs(t), dfe));  % two-tailed

    V = inv(X' * X) * sigma .^ 2; % Var/Cov mtx, Precision^-1, used in weighted est. and empirical bayes

else
    % if we have missing observations or redundant columns, let's
    % regularize a bit so we can still estimate this, using a ridge prior
    % The degree of regularization is arbitrary.
    if rank(X) < size(X, 2)
        disp('WARNING! RANK DEFICIENT.  THIS FUNCTION WILL RETURN AN ERROR.')
        X = [X; eye(size(X, 2)) ./ size(X, 1)];
        if ~strcmp(interceptstr, 'noint'), X(:, 1) = 1; end
        y = [y; ones(size(X, 2), 1) .* nanmean(y)];
    end

    stats = glmfit_general(y, X, verbstr, interceptstr);
    t = stats.t; dfe = stats.dfe; b = stats.beta; phi = NaN; sterr = stats.ste; p = stats.p;

    V = inv(X' * X) * stats.var; % Var/Cov mtx, Precision^-1, used in weighted est. and empirical bayes
end

end

function X = setup_X_matrix(X, y)
  % set up X matrix: intercept first
  [n, k] = size(X);
  if n == 0
    n = size(y, 1);
  end
  equal_x = false(1, k);
  for i = 1:k
    if all(X(:, i) == X(1, i))
    % weights are equal
      equal_x(i) = 1;
    end
  end
  if any(equal_x)
    error('Warning: some columns of X have no variance.  Do not enter intercept in X; it will be added automatically as the first predictor.');
    X(:, equal_x) = [];
  end
  X = [ones(n, 1) X];
end

% -------------------------------------------------------------------------
% Setup inputs, print info to screen if verbose
% THIS IS FOR SECOND-LEVEL MODEL -- NOT FIRST
% -------------------------------------------------------------------------
function [names, analysisname, beta_names, robust_option, weight_option, inference_option, ...
    verbose, dosave, doplots, ...
    verbstr, savestr, plotstr, ...
    targetu, nresample, whpvals_for_boot, ...
    permsign] = ...
    setup_inputs(Y, X, varargin)

% Initial compliance checks
% ----------------------------------------------------------------
[n, k] = size(X);
nvars = size(Y, 2);
nobs_tmp = size(Y, 1);

% Check sizes
if nobs_tmp ~= n, error('Y and X must have same number of rows!'); end

% Check intercept
if ~(all(X(:, 1) == X(1)))
    error('First column of X must be an intercept column. (e.g., all ones)');
end

beta_names = cell(1, k);
for i = 1:k
    beta_names{i} = sprintf('2nd-level B%02d', i);
end

% Defaults
% ----------------------------------------------------------------

% General Defaults
names = cell(1, nvars);     % variable names, columns of Y
for i = 1:nvars, names{i} = ['1st-level B' num2str(i)]; end

analysisname = 'Second Level of Multilevel Model ';

% Estimation Defaults
robust_option = 'no';               % robust IRLS; 'no' 'yes'
weight_option = 'unweighted';       % 'weighted' 'unweighted'
force_centering = true;             % force 2nd-level predictor centering

% Inference defaults
inference_option = 't-test';        % 't-test' 'bootstrap' 'signperm'

% Display control defaults
verbose = 1;                % verbose output
dosave = 0;                 % save figures at end
doplots = 0;                % make plots
plotstr = 'noplots';
savestr = 'nosave';
verbstr = 'verbose';

savefilename = 'glmfit_general_output.txt';

% Bootstrap defaults
targetu = .20;              % proportion contribution of boot procedure to p-value
nresample = 1000;         % initial bootstrap samples
whpvals_for_boot = 1:size(Y,2);   % indices of p-values, the min of which is used to determine boot samples needed
% lower p-values require more boot samples
% for the p-vals to be meaningful.

% Sign perm defaults
permsign = [];              % empty: setup new sign permutation indices
% if entered: keep same permutation matrix
% across repeated calls (much faster! but
% reduces accuracy in simulations!)

% Inputs
% ----------------------------------------------------------------
for i = 1:length(varargin)
    if ischar(varargin{i})
        switch varargin{i}
            % General Defaults
            case 'names', names = varargin{i+1};  varargin{i+1} = [];
            case 'analysisname', analysisname = varargin{i+1}; varargin{i+1} = [];
            case 'beta_names', beta_names = varargin{i+1}; varargin{i+1} = [];

                % Estimation Defaults
            case 'robust', robust_option = 'yes';
            case {'weight', 'weighted', 'var', 's2'}, weight_option = 'weighted';
            case {'nocenter', 'nocentering'}, force_centering = false;

                % Inference defaults
            case {'boot1', 'boot', 'bootstrap'}, inference_option = 'bootstrap';
            case {'sign perm', 'signperm', 'sign'}, inference_option = 'signperm';
            case {'t-test', 'ttest'}, inference_option = 't-test';

                % Display control defaults
            case {'plot', 'plots'}, doplots = 1; plotstr = 'plots';
            case {'noplots', 'noplot'}, doplots = 0; plotstr = 'noplots';

            case {'dosave', 'save', 'saveplots'}, dosave = 1; savestr = 'save';
            case 'verbose', verbose = 1; verbstr = 'verbose';
            case 'noverbose', verbose = 0; verbstr = 'noverbose';

            case {'savefile', 'savefilename'}, savefilename = varargin{i + 1}; varargin{i+1} = [];

                % Bootstrap defaults
            case 'nresample', nresample = varargin{i+1};
            case {'pvals', 'whpvals_for_boot'}, whpvals_for_boot = varargin{i+1};

                % Sign perm defaults
            case {'permsign'}, permsign = varargin{i+1};

            case 'intercept'

            otherwise
                fprintf('Warning! Unknown input string option: %s', varargin{i});

        end
    end
end

% all the other checking, etc. is done in glmfit_general

is_intercept = all(abs(diff(X, 1, 1)) < 100 * eps);

if force_centering
    X(:, ~is_intercept) = X(:, ~is_intercept) - mean(X(:, ~is_intercept));
end

is_centered = abs(mean(X)) < 100*eps;

any_noncentered = any(~is_intercept & ~is_centered);
intercept_beta_name = 'Within-person average effects';

if any_noncentered
    disp('WARNING!!! Some 2nd-level predictors are not mean-centered.')
    disp('Within-person effects and P-values are NOT interpretable as the')
    disp('average within-person effect');

    intercept_beta_name = 'Within-person effects when all 2nd-level predictors are zero';

end

% fix names by adding intercept if needed
% This is over-rided by user input
if length(beta_names) == k - 1
    if ~isrow(beta_names), beta_names = beta_names'; end
    beta_names = {intercept_beta_name beta_names{:}};
end

end

% -------------------------------------------------------------------------
% Check variances
% -------------------------------------------------------------------------

function [Y, X1, X2] = check_variances_and_exclude(Y, X1, X2)
xvar = cellfun(@var, X1, 'UniformOutput', false);
xvar = cat(1, xvar{:});
wh = any(xvar < 100 * eps, 2);
if any(wh)
    disp('Warning! Some participants have no variance in X column(s) and will be excluded')
    fprintf('Participant numbers:');
    fprintf('%d ', find(wh));
    fprintf('\n');
end

wh_out = wh;

yvar = cellfun(@var, Y, 'UniformOutput', false);
yvar = cat(1, yvar{:});
wh = any(yvar < 100 * eps, 2);
if any(wh)
    disp('Warning! Some participants have no variance in Y and will be excluded')
    fprintf('Participant numbers:');
    fprintf('%d ', find(wh));
    fprintf('\n');
end

wh_out = wh_out | wh;

X1(wh_out) = [];
Y(wh_out) = [];
if ~isempty(X2), X2(wh_out,  :) = []; end

end


function [ craft_vel craft_theta a e EoverM GMsun] = predictor(craft_v,angle1, source_planet,target_planet )
%PREDICTOR - ellipse determination function
% the equations in this procedure is based on the
%  This will determine the critical ellipse parametres - mainly a and e,
%  based on the trajectory characteristics of the craft.
% the procedure for determining a and e from craft trajectory is based on:
%'Satellite Orbits and Gravitational Assist for Planets' (2008)-Larry Bogan
%http://www.bogan.ca/astrpages.html

%target planets are indexed , as below.
%1 = sun
%2 = earth
%3 = jupiter
%4 = saturn
%5 = uranus
%6 =neptune

mass = [1.9891e+30,5.9736e+24,1.8983e+27,5.68462313752e+26,8.6810e25,1.0243e26, 750];
m_sun = mass(1);
% planet masses
m_craft = mass(7);
G = 6.67428e-11;
tp = target_planet;
sp = source_planet;
AU=149597870691;
radius = [ (6.955e8/AU) 1 5.2 9.582 20.083 30.1036];

%####################################################################
% Step 1  Determine GMsun
% This determines GM with respect to the sun. This is necesscary as all of
% the tjrajectory ellipses are with respect to the sun as a focus.

GMsun_metres = m_sun * G;

GMsun = GMsun_metres / AU;  % this GMsun in m/s)^2 - being converted to AU

GMsun = GMsun / 1e6; %to go from square metres to square kilometres
% Should be 887 AU/(km/s)^2

%####################################################################
%Step 2 Energy over mass relationship

KE = (m_craft * craft_v^2)/2;

GPE = GMsun * m_craft / radius(sp);
E = (KE - GPE);
EoverM = (KE - GPE)/ m_craft;

% if KE - GPE is negative, it means the orbit is still elliptical around
% the sun - if not it is a hyperbolic orbit and it will leave the solar
% system.

%####################################################################
%Step 3 determine semimajor axis

a = -0.5 * GMsun / EoverM;

%####################################################################
%Step 4 - work out circular velocity, based on using the semi major axis as
%a radius. The radius is converted to metres from AU, and period is
%converted to seconds from years.

P = a^(3/2);

Vc = 2 * pi * a * AU   / P;

Vc3 = Vc / (365 * 86164);
Vcirc = Vc3 / 1000;

%####################################################################
% Step 5 from semimajor axis determine the period
% using Kepeler's third law - already been done above

P = a^(3/2);

%####################################################################
%Step 6 work out the areal velocity -

V_wrt_planet = craft_v * cosd(angle1);

V_wrt_planet_AU = (V_wrt_planet / (AU / 1000)) * 86164 * 365;

A =  radius(sp) * V_wrt_planet_AU /2;

%A = area of ellipse / priod - areal velocity

%####################################################################
%Step 7
% determines eccentricity from Areal Velocity and period of Ellipse

KK = A * P / (pi * a^2);

e = sqrt(1 - KK^2);

%####################################################################
%Step 8 - from semimajor axis and eccentricity, we get apophelion
%and periphelion

r_a = a*(1 + e);
r_p = a*(1 - e);

%####################################################################
%Step 9 determine true anomoly - the angle between periphelion and the
%spacecraft

%1 = sun
%2 = earth
%3 = jupiter
%4 = saturn
%5 = uranus
%6 =neptune
%7 = our spacecraft

V_per = Vcirc * sqrt((1+e) /(1-e));
V_aps = Vcirc * sqrt((1-e) /(1+e));

GSI  = radius(tp) * (( mass(tp)/mass(1))^(2/5));
%
craft_vel = sqrt( (2* EoverM) + (2 * GMsun ./ (radius(tp)- GSI)));

%its reduced by tp.
craft_theta = acosd((a*(1 - e^2)/(radius(tp)- GSI) - 1)/e);
end


function x = dge_sl ( n, a_lu, pivot, b, job )

%*****************************************************************************80
%
%% DGE_SL solves a system factored by DGE_FA.
%
%  Discussion:
%
%    The DGE storage format is used for a general M by N matrix.  A storage
%    space is made for each logical entry.  The two dimensional logical
%    array is mapped to a vector, in which storage is by columns.
%
%    DGE_SL is a simplified version of the LINPACK routine DGESL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 March 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Input, real A_LU(N,N), the LU factors from DGE_FA.
%
%    Input, integer PIVOT(N), the pivot vector from DGE_FA.
%
%    Input, real B(N), the right hand side vector.
%
%    Input, integer JOB, specifies the operation.
%    0, solve A * x = b.
%    nonzero, solve A' * x = b.
%
%    Output, real X(N), the solution vector.
%
  x(1:n) = b(1:n);
%
%  Solve A * x = b.
%
  if ( job == 0 )
%
%  Solve PL * Y = B.
%
    for k = 1 : n-1

      l = pivot(k);

      if ( l ~= k )
        t    = x(l);
        x(l) = x(k);
        x(k) = t;
      end

      x(k+1:n) = x(k+1:n) + a_lu(k+1:n,k)' * x(k);

    end
%
%  Solve U * X = Y.
%
    for k = n : -1 : 1
      x(k) = x(k) / a_lu(k,k);
      x(1:k-1) = x(1:k-1) - a_lu(1:k-1,k)' * x(k);
    end
%
%  Solve A' * X = B.
%
  else
%
%  Solve U' * Y = B.
%
    for k = 1 : n
      x(k) = ( x(k) - x(1:k-1) * a_lu(1:k-1,k) ) / a_lu(k,k);
    end
%
%  Solve ( PL )' * X = Y.
%
    for k = n-1 : -1 : 1

      x(k) = x(k) + x(k+1:n) * a_lu(k+1:n,k);

      l = pivot(k);

      if ( l ~= k )
        t    = x(l);
        x(l) = x(k);
        x(k) = t;
      end

    end

  end

  return
end


function e = year_to_epact_julian ( y )

%*****************************************************************************80
%
%% YEAR_TO_EPACT_JULIAN returns the epact of a Julian year.
%
%  Discussion:
%
%    The epact of a year is the age in days of the notional moon on
%    the first day of the year.  If the year begins with a new moon,
%    the epact is zero.  If the new moon occurred the day before,
%    the epact is 1.  There is a unique epact for every golden number.
%
%    Bear in mind that the notional moon is not the one in the sky,
%    but a theoretical one that satisfactorily approximates the behavior
%    of the real one, but which is tame enough to be described by a formula.
%
%  Example:
%
%    Year  Golden Number  Epact
%
%      1 BC     1           8
%      1 AD     2          19
%      2 AD     3           0
%      3 AD     4          11
%      4 AD     5          22
%      5 AD     6           3
%      6 AD     7          14
%      7 AD     8          25
%      8 AD     9           6
%      9 AD    10          17
%     10 AD    11          28
%     11 AD    12           9
%     12 AD    13          20
%     13 AD    14           1
%     14 AD    15          12
%     15 AD    16          23
%     16 AD    17           4
%     17 AD    18          15
%     18 AD    19          26
%     19 AD     1           8
%     20 AD     2          19
%   1066 AD     3           0
%   1900 AD     1           8
%   1919 AD     1           8
%   1938 AD     1           8
%   1957 AD     1           8
%   1976 AD     1           8
%   1995 AD     1           8
%   2014 AD     1           8
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 June 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Edward Richards,
%    Mapping Time, The Calendar and Its History,
%    Oxford, 1999.
%
%  Parameters:
%
%    Input, integer Y, the year.  The year 0 is illegal input.
%
%    Output, integer E, the epact, between 0 and 28.
%
  if ( y == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'YEAR_TO_EPACT_JULIAN - Fatal error!\n' );
    fprintf ( 1, '  Illegal input Y = 0.\n' );
    error ( 'YEAR_TO_EPACT_JULIAN - Fatal error!' );
  end

  g = year_to_golden_number ( y );

  e = i4_wrap ( 11 * g - 3, 0, 29 );

  return
end


function lines2d(varargin)
%LINES2D  Description of functions operating on planar lines.
%
%   The term 'line' refers to a planar straight line, which is an unbounded
%   curve. Line segments defined between 2 points, which are bounded, are
%   called 'edge', and are presented in file 'edges2d'.
%
%   A straight line is defined by a point (its origin), and a vector (its
%   direction). The parameters are bundled into a 1-by-4 row vector:
%   LINE = [x0 y0 dx dy];
%
%   A line contains all points (x,y) such that:
%       x = x0 + t*dx
%       y = y0 + t*dy;
%   for all t between -infinity and +infinity.
%
%   See also
%   points2d, vectors2d, edges2d, rays2d
%   createLine, cartesianLine, medianLine, edgeToLine, lineToEdge
%   orthogonalLine, parallelLine, bisector, radicalAxis
%   lineAngle, linePosition, projPointOnLine
%   isPointOnLine, distancePointLine, isLeftOriented
%   intersectLines, intersectLineEdge, clipLine
%   reverseLine, transformLine, drawLine
%   lineFit

% ------
% Author: David Legland
% E-mail: david.legland@inra.fr
% Created: 2008-10-13, using Matlab 7.4.0.287 (R2007a)
% Copyright 2008-2022 INRA - BIA PV Nantes - MIAJ Jouy-en-Josas

help('lines2d');


function S = L0Restoration(Im, kernel, lambda, kappa)
%%
% Image restoration with L0 prior
% The objective function:
% S^* = argmin ||I*k - B||^2 + lambda |\nabla I|_0
%% Input:
% @Im: Blurred image
% @kernel: blur kernel
% @lambda: weight for the L0 prior
% @kappa: Update ratio in the ADM
%% Output:
% @S: Latent image
%
% The Code is created based on the method described in the following paper
%   [1] Jinshan Pan, Zhe Hu, Zhixun Su, and Ming-Hsuan Yang,
%        Deblurring Text Images via L0-Regularized Intensity and Gradient
%        Prior, CVPR, 2014.
%   [2] Li Xu, Cewu Lu, Yi Xu, and Jiaya Jia. Image smoothing via l0 gradient minimization.
%        ACM Trans. Graph., 30(6):174, 2011.
%
%   Author: Jinshan Pan (sdluran@gmail.com)
%   Date  : 05/18/2014

if ~exist('kappa','var')
    kappa = 2.0;
end
%% pad image
H = size(Im,1);    W = size(Im,2);
Im = wrap_boundary_liu(Im, opt_fft_size([H W]+size(kernel)-1));
%%
S = Im;
betamax = 1e5;
fx = [1, -1];
fy = [1; -1];
[N,M,D] = size(Im);
sizeI2D = [N,M];
otfFx = psf2otf(fx,sizeI2D);
otfFy = psf2otf(fy,sizeI2D);
%%
KER = psf2otf(kernel,sizeI2D);
Den_KER = abs(KER).^2;
%%
Denormin2 = abs(otfFx).^2 + abs(otfFy ).^2;
if D>1
    Denormin2 = repmat(Denormin2,[1,1,D]);
    KER = repmat(KER,[1,1,D]);
    Den_KER = repmat(Den_KER,[1,1,D]);
end
Normin1 = conj(KER).*fft2(S);
%%
beta = 2*lambda;
while beta < betamax
    Denormin   = Den_KER + beta*Denormin2;
    h = [diff(S,1,2), S(:,1,:) - S(:,end,:)];
    v = [diff(S,1,1); S(1,:,:) - S(end,:,:)];
    if D==1
        t = (h.^2+v.^2)<lambda/beta;
    else
        t = sum((h.^2+v.^2),3)<lambda/beta;
        t = repmat(t,[1,1,D]);
    end
    h(t)=0; v(t)=0;
    Normin2 = [h(:,end,:) - h(:, 1,:), -diff(h,1,2)];
    Normin2 = Normin2 + [v(end,:,:) - v(1, :,:); -diff(v,1,1)];
    FS = (Normin1 + beta*fft2(Normin2))./Denormin;
    S = real(ifft2(FS));
    beta = beta*kappa;
end
S = S(1:H, 1:W, :);
end


function [c,r,k]=pdfmoments(t,m,b,a)
%PDFMOMENTS convert between central moments, raw moments and cumulants [C,R,K]=(T,M,B,A)
%  Inputs: t  text string containing:
%               'm','r','k'  Imput is central moments, raw moments, cumulants [default 'm']
%               'M','R','K'  Ouptut c is central moments, raw moments, cumulants [default 'M']
%          m    vector of input moments; m(r) is moment r, m(1) is always the mean
%          a,b  If input moments are for x, output moments are for a*x+b [defaulta=1, b=0]
%
% Outputs: c    central moments (or as determined by 'R' or 'K' options)
%          r    raw moments
%          k    cumulants
%
% (a) For all formats, the first element is the mean (i.e. not the first central moment or cumulant which equal zero).
% (b) pdfmoments('k',[m,s^2,0,0,0])=pdfmoments('k',[0,1,0,0,0],m,s) gives a Gaussian with mean m and std dev s

%   Copyright (c) 1998 Mike Brookes,  mike.brookes@ic.ac.uk
%      Version: $Id: pdfmoments.m 9678 2017-04-04 14:28:37Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
persistent n0 bc mk fa
if isempty(n0)
    n0=3;  % order 3
    bc=[1 1 0 0; 1 2 1 0; 1 3 3 1]; % binomial coefficients
    mk={[1 1 1];[0 1 1 1]};  %  cumulant coefficients: one row per term, powers of k(2:i+1), coef k->m, coef m->k
    mn=[1 0; 1 1]; % [i,j] = number of terms in m(i+1) whose lowest moment is >= j+1
    fa=1; % factorial list
end
% check arguments
if nargin<4 || isempty(a)
    a=1;
end
if nargin<3 || isempty(b)
    b=0;
end
if isempty(t)
    t='';
end
n=length(m);                                    % number of moments required
if n>n0                                         % check if need to update coefficient arrays
    if fix(n/2-1)>length(fa)                    % we need factorials up to fix(n/2-1)
        fal=length(fa);
        fa(fix(n/2-1),1)=0;                     % enlarge factorial vector
        for i=fal+1:fix(n/2-1)
            fa(i)=i*fa(i-1);                    % create new factorials
        end
    end
    bc(n,n+1)=0;                                % enlarge binomial coefficient array
    mk{n-1,1}=[];                               % enlarge cumulant coefficient array
    mn(n-1,n-1)=0;                              % enlarge cumulant coefficient counts
    for i=n0+1:n
        bc(i,1:i+1)=[1 bc(i-1,1:i)+bc(i-1,2:i+1)];                      % update binomial coefficients
        j=fix((i+1)/2);                                                 % first coefficient row to sum
        nr=1+sum(mn(((j-1:i-3)+(n-1)*(i-j-2:-1:0))));                   % number of terms
        mki=zeros(nr,i+1);                                              % coefficient matrix
        ix=1;
        mki(1,i-1:i+1)=1;                                               % first term always has a coefficient of 1
        for r=j:i-2                                                     % previous coefficients to use
            nk=mn(r-1+(n-1)*(i-r-2));                                   % number of new coefficients for this value of r
            mkk=mk{r-1};                                                % old coefficients for this value of r
            mkik=mkk(1:nk,1:r-1);                                       % extract just the list of powers for each term
            mkik(:,i-r-1)=mkik(:,i-r-1)+1;                              % increment the power of moment  i-r
            mki(ix+1:ix+nk,1:r-1)=mkik;                                 % and save as new terms
            mki(ix+1:ix+nk,i)=mkk(1:nk,r)*bc(i,i-r+1)./mkik(:,i-r-1);   % calculate coefficient for r->m
            rho=sum(mkik,2)-1;                                          % rho is one less than the sum of the moment powers
            mki(ix+1:ix+nk,i+1)= mki(ix+1:ix+nk,i).*fa(rho).*(-1).^rho; % calculate coefficient for m->r
            ix=ix+nk;                                                   % update the number of terms so far
        end
        mki=sortrows(mki);                                              % sort according to the lowest moment that is used
        mn(i-1,1:i-1)=[nr sum(cumprod(mki(:,1:i-2)==0,2),1)];           % update count of terms with lowest moment >= j+1
        mk{i-1}=mki;                                                    % save in persistent cell array
    end
    n0=n;                                       % coefficients are now calculated up to order n
end
% apply scaling if input type is 'c' or 'k'
mu=a*m(1)+b; % calculate new mean
c=m; % initialize output shapes
r=m;
k=m;
m=m(:)'; % now force the input to be a row vector
if any(t=='k')
    tin=3; % set input type
    k(:)=k(:)'.*a.^(1:n);
    k(1)=0; % first cumulant is actually zero
elseif any(t=='r')
    tin=2;
else
    tin=1;
    c(:)=c(:)'.*a.^(1:n);
    c(1)=0;  % first cenral moment is actually zero
end
tout=[(~any(t=='K') && ~any(t=='R')) (nargout>=2 || any(t=='R')) (nargout>=3 || any(t=='K'))]; % outputs required
for il=1:2 % loop through conversion routines twice
    % first convert between moments
    if il==1 % convert unscaled R -> C
        v=[1 m.*a.^(1:n)];
        bb=b-mu;
        doit=tin==2 && (tout(1) || tout(3));
    else  % convert C -> R or unscaled R -> R
        if tin==2 % input type was 'r' (v is OK from previous iteration)
            bb=b;
        else % input type was 'c' or 'k'
            v=[1 c(:)'];
            bb=mu;
        end
        doit=tout(2); % convert if 'R' output required
    end
    if doit
        y=v(2:end);
        if bb~=0 % don't bother if the constant term is zero
            for i=1:n
                y(i)=polyval(bc(i,1:i+1).*v(1:i+1),bb);
            end
        end
        if il==1 % convert unscaled R -> C
            c(:)=y;
        else  % convert C -> R or unscaled R -> R
            r(:)=y;
        end
    end
    % now convert cumulants to/from moments
    if il==1 % convert K -> C
        x=k(:)';
        doit=tin==3 && (tout(1) || tout(2));
    else  % convert C -> K
        x=c(:)';
        doit=(tin<3) && tout(3);
    end
    if doit
        y=x;
        for i=4:n
            mki=mk{i-1}; % get coefficient matrix
            y(i)=mki(:,i-1+il)'*prod(repmat(x(2:i),size(mki,1),1).^mki(:,1:i-1),2); % calculate moment/cumulant (neat but not efficient)
        end
        if il==1 % converted K -> C
            c(:)=y;
        else % converted C -> K
            k(:)=y;
        end
    end
end
c(1)=mu; % restore the means
k(1)=mu;
if any(t=='R')
    c=r;
elseif any(t=='K')
    c=k;
end


function [coef]=comp_dgt_fb(f,g,a,M)
%COMP_DGT_FB  Filter bank DGT
%   Usage:  c=comp_dgt_fb(f,g,a,M);
%
%   This is a computational routine. Do not call it directly.
%
%   See help on DGT.

%   AUTHOR : Peter L. Søndergaard.

% Calculate the parameters that was not specified.
L=size(f,1);
N=L/a;
gl=length(g);
W=size(f,2);      % Number of columns to apply the transform to.
glh=floor(gl/2);  % gl-half

% Conjugate the window here.
g=conj(fftshift(g));

coef=zeros(M,N,W,assert_classname(f,g));

% ----- Handle the first boundary using periodic boundary conditions. ---
for n=0:ceil(glh/a)-1

    % Periodic boundary condition.
    fpart=[f(L-(glh-n*a)+1:L,:);...
           f(1:gl-(glh-n*a),:)];

    fg=bsxfun(@times,fpart,g);

    % Do the sum (decimation in frequency, Poisson summation)
    coef(:,n+1,:)=sum(reshape(fg,M,gl/M,W),2);

end;

% ----- Handle the middle case. ---------------------
for n=ceil(glh/a):floor((L-ceil(gl/2))/a)

  fg=bsxfun(@times,f(n*a-glh+1:n*a-glh+gl,:),g);

  % Do the sum (decimation in frequency, Poisson summation)
  coef(:,n+1,:)=sum(reshape(fg,M,gl/M,W),2);
end;

% ----- Handle the last boundary using periodic boundary conditions. ---
for n=floor((L-ceil(gl/2))/a)+1:N-1

    % Periodic boundary condition.
    fpart=[f((n*a-glh)+1:L,:);... %   L-n*a+glh elements
           f(1:n*a-glh+gl-L,:)];  %  gl-L+n*a-glh elements

    fg=bsxfun(@times,fpart,g);

    % Do the sum (decimation in frequency, Poisson summation)
    coef(:,n+1,:)=sum(reshape(fg,M,gl/M,W),2);
end;

% --- Shift back again to make it a frequency-invariant system. ---
for n=0:N-1
  coef(:,n+1,:)=circshift(coef(:,n+1,:),n*a-glh);
end;

coef=fft(coef);

% Simple code using a lot of circshifts.
% Move f initially so it lines up with the initial fftshift of the
% window
%f=circshift(f,glh);
%for n=0:N-1
  % Do the inner product.
  %fg=circshift(f,-n*a)(1:gl,:).*gw;

  % Periodize it.
  %fpp=zeros(M,W);
  %for ii=0:gl/M-1
    %  fpp=fpp+fg(ii*M+1:(ii+1)*M,:);
    %end;
%  fpp=sum(reshape(fg,M,gl/M,W),2);

  % Shift back again.
%  coef(:,n+1,:)=circshift(fpp,n*a-glh); %),M,1,W);

%end;


function [mad_similarity cos_similarity r_similarity] = outliers_xval(obj)
% Similarity metrics comparing each image to the mean of others in the set using repeated k-fold cross-validation
%
% :Usage:
% ::
%
%     [mad_similarity cos_similarity r_similarity] = outliers_xval(obj)
%
% :Background:
% ::
% Assessing image outliers is crucial for detecting artifacts and other
% violations of assumptions underlying image analysis.
%
% Many methods are available, but they often use measures of variance defined relative to a sample,
% which creates problems. For example, Mahalanobis distance uses squared
% multivariate distances among images. This is useful, but detects only
% relative outliers. If there are many corrupted images, all images will
% look normal by comparison to others.  Inter-image correlations are also
% commonly used. These are also useful, but they are also sensitive to the
% overall distribution of artifacts across the set. If there are many
% corrupted images, correlations will tend to be low overall, and the bad
% images will be harder to detect. In cases where comparing individual
% images to a gold standard is desired, but there is no external standard
% image (perhaps because images look somewhat different in each sample),
% comparing images to a mean image derived from others in the set may be useful.
% In this case, however, it's desirable for the mean to be derived
% independent of the test image itself. This can be accomplished with
% cross-validation, bootstrapping, or jackknife analyses.
%
% Here, we compare each of a set of images to a mean derived from other
% images in the set using k-fold cross-validation. To average over errors
% related to the particular fold splits chosen, we average over a number of
% repeated cross-valiations.
%
% We return two error metrics, which have different desirable properties
% depending on the type of image and use case.
% - Pearson's correlation (r): correlation is insensitive to mean shift and scale
%   it is most useful when only the pattern in the image is meaningful
%   and/or images will be rescaled in analysis, and the zero-point and
%   scale are not important.
%
% - cosine similarity is sensitive to mean shift, not scale
%   This is useful for images with a meaningful zero-point that should match across images,
%   but where the scale is irrelevant (or will be removed, e.g., via
%   normalization)
%
% - Absolute agreement is most relevant when mean and scale are both relevant
%   This is the case with many images, e.g., those subjected to group statistical analysis of intensity
%   values. e.g., when testing task - control contrast images across a group,
%   We are interested in whether the group mean is 0. The zero-point is meaningful and
%   it should agree if the individual test images are replicates of the same effect.
%   The scale is also meaningful, as deviations in the values determine the variance estimate
%   of the test statistic. Any deviation is treated as error, even if it results from
%   mean shift or scale varation in the image as a whole.
%
%
% :Inputs:
%
%   **obj:**
%        an image_vector object (e.g., fmri_data) with >=5 images
%
% :Outputs:
%
%   **mad_similarity:**
%        [nimages x 1] vector of absolute agreement between each image and the mean,
%        1/(1+MAD), averaged across repeated k-fold
%
%   **cos_similarity:**
%        [nimages x 1] vector of cosine similarity between each image and the mean,
%        averaged across repeated k-fold
%
%   **r_similarity:**
%        [nimages x 1] vector of correlation between each image and the mean,
%        averaged across repeated k-fold
%
% ----------------------------------------------------------------------
% Examples:
% ----------------------------------------------------------------------
% test_images = load_image_set('emotionreg');
%
% [mad_similarity cos_similarity r_similarity ] = outliers_xval(test_images);
% figure; plot(mad_similarity)
% hold on;
% plot(cos_similarity)
% plot(r_similarity)
% legend({'Abs agreement' 'Cos sim' 'r'})
%
% % Compare with outliers based on Mahalanobis and other metrics
% figure;
% [est_outliers_uncorr, est_outliers_corr, outlier_tables] = outliers(test_images, 'notimeseries');
%
% Another, larger sample dataset
% test_images = load_image_set('kragel18_alldata');
% [mad_similarity cos_similarity r_similarity ] = outliers_xval(test_images);

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

% ----------------------------------------------------------------------
% Default parameters
% ----------------------------------------------------------------------

nfolds = 5;
nrepeats = 100;  % number of cross-validation repeats

% ----------------------------------------------------------------------
% Set up variables and functions
% ----------------------------------------------------------------------

% Analyze gray matter, where we expect meaningful variation
%obj_orig = obj;
obj = apply_mask(obj, fmri_data(which('gray_matter_mask.nii'), 'noverbose'));

nimages = size(obj.dat, 2);

if nimages < 5, error('Object must contain at least 5 images'), end

Yid = ones(nimages, 1);

[r_test, cos_sim_test, mad_test] = deal(NaN .* zeros(nimages, nrepeats));

cvpart = cvpartition(Yid, 'k', nfolds);
% [S.trIdx, S.teIdx] = xval_stratified_holdout_leave_whole_subject_out(S.Y, S.id, 'doverbose', doverbose, 'doplot', doplot, 'nfolds', nfolds);

cos_sim =  @(x, y) x' * y ./ (norm(x) * norm(y));

% ----------------------------------------------------------------------
% Run
% ----------------------------------------------------------------------

for p = 1:nrepeats

    % Draw a new k-fold cross-validation partition
    cvpart = cvpart.repartition;

    for i = 1:nfolds

        train_set = get_wh_image(obj, cvpart.training(i));
        test_set = get_wh_image(obj, cvpart.test(i));

        m = mean(train_set);

        % correlation is insensitive to mean shift and scale
        % cosine sim is sensitive to mean shift, not scale
        % for images with a meaningful zero-point that should match across images,
        % cosine sim is more meaningful.
        % if mean and scale are both relevant, as with contrast images, absolute
        % agreement may be the best metric, e.g. MAD = median absolute deviation

        r_test(cvpart.test(i), p) = corr(m.dat, test_set.dat)';

        cos_sim_test(cvpart.test(i), p) = cos_sim(m.dat, test_set.dat)';

        mad_test(cvpart.test(i), p) = median(abs(m.dat - test_set.dat))';

    end % k-fold

end % cv repeats

% ----------------------------------------------------------------------
% Calculate final similarity measures
% ----------------------------------------------------------------------

r_similarity = mean(r_test')';
cos_similarity = mean(cos_sim_test')';

mad_dissim = mean(mad_test')'; % this is actually still deviation here
mad_similarity = 1./(1+mad_dissim); % convert to similarity, scale from [0 1]
% +1 scales so that zero error (perfect agreement) will have a value of 1.

end % function


function C = EstimateUnitNormalCorr(Y_X, exp_window_size, shrink_factor)
%C = EstimateUnitNormalCorr(Y_X, exp_window_size, shrink_factor)
%  Estimate the correlation matrix of a joint normal sample, where each
%  element is assumed to be unit normal. Each column of Y_F is a random
%  variable and each row is a sample. Exponential weighting is applied in
%  time, and RMT filtering is applied using the "PG" algorithm.
%
% To do!!:
%   - review implementation of exponential weighting and RMT
%   - add choice for rectangular averaging?
%   - add choice for other RMT schemes?
% Code by S. Gollamudi. This version December 2009.

[T,N] = size(Y_X);
use_rmt_cleaning = 0;

%% Estimate sample correlation matrix with exponential averaging

% compute exponential weights
lambda = 2/exp_window_size;
exp_wts = (1-lambda).^(T-1:-1:0)*(lambda/(1-(1-lambda)^T));
m_exp_wts = repmat(exp_wts',1,N);

% compute the exponentially weighted sample mean
meanYX = sum(m_exp_wts.*Y_X, 1);
% remove mean from the time series Y_X
Y_X = Y_X - repmat(meanYX,T,1);

% compute the exponentially weighted standard deviation
stdYX = sqrt(sum(m_exp_wts.*(Y_X.^2), 1));

% compute the exponentially weighted correlation matrix
C = ((m_exp_wts.*Y_X)'*Y_X)./(stdYX'*stdYX);
diag_indx = 1:(N+1):(N*N);

%% Clean the correlation matrix estimate using Random Matrix Theory

if use_rmt_cleaning,

    % ratio of the effective series length and the number of variables
    Q = exp_window_size/N;

    % maximum random eigenvalue
    e_max = rmtEigenLim(Q,1);

    [eigVec,eigVal] = eig(C);
    [eigVal,Idx] = sort(diag(eigVal),'descend');
    eigVec=eigVec(:,Idx);
    % K is the # of non-random eigenvalues
    K=length(find(eigVal>e_max));
    % PG filter
    eigVal(K+1:end)=mean(eigVal(K+1:end));
    C = eigVec*diag(eigVal)*eigVec';
    % make the diagonal elements equal to one
    C(diag_indx) = 1;

end

%% Shrink to average correlation

% compute the average pairwise correlation coefficient
rho = (sum(sum(C)) - N)/(N*(N-1));
C_rho = rho*ones(N,N);
C_rho(diag_indx) = 1;

% shrink C towards C_rho
if nargin < 3, shrink_factor = 1/3; end
C = (1-shrink_factor)*C + shrink_factor*C_rho;


function simpletrans
% 1D transport - modelling with extensions for decay and linear sorption
%    using mixing cell method
%
%   $Ekkehard Holzbecher  $Date: 2006/02/08 $
%--------------------------------------------------------------------------
T = 2;                     % maximum time [s]
L = 1;                     % length [m]
D = 0.1;                   % dispersivity [m*m/s]
v = 1;                     % velocity [m/s]
lambda = 1.2;              % decay constant [1/s]
R = 1;                     % retardation [1]
c0 = 0;                    % initial concentration [kg/m*m*m]
cin = 1;                   % inflow concentration [kg/m*m*m]

dtout = 0.05;              % output-timestep [s]
dxmax = 0.02;              % maximum grid spacing [m]
%------------------------ output parameters
gplot = 2;                 % =1: breakthrough curves; =2: profiles
gsurf = 0;                 % surface
gcont = 0;                 % =1: contours; =2: filled contours
ganim = 2;                 % animation

%------------------------ execution----------------------------------------

dtout = dtout/R;           % timestep reduction for retardation case
dx = dtout*v;              % grid spacing
K = 1;                     % K = reduction factor for grid spacing
if (dx>dxmax) K = ceil(dx/dxmax); end
dx = dx/K;                 % reduced grid spacing
dtadv=dtout/K;             % advection-timestep
N = ceil(L/dx);            % N = number of cells
x = linspace(0,(N-1)*dx,N);% nodes on x-axis
Neumann = D*dtadv/dx/dx;   % Neumann-number for dispersion
M = max (1,ceil(3*Neumann)); % M = reduction factor to fulfill Neumann-condition
Neumann = Neumann/M/R;     % reduced Neumann-number
dtdiff = dtadv/M;          % diffusion timestep
t = dtadv;

clear c c1 c2;
c(1:N) = c0; c1 = c;
k = 1; kanim = 1;
while (t < T/R)
    for i=1:M
        kinetics;          % decay (1. order kinetics)
        diffusion;         % diffusion
    end
    advection;             % advection
    if k >= K
        c = [c;c1]; k=0;
    end
    t = t + dtadv; k = k+1;
end
xlabel ('space'); ylabel ('concentration');

%-------------------- graphical output-------------------------------------

switch gplot
    case 1
        plot (c)        % breakthrough curves
        xlabel ('time'); ylabel ('concentration');
    case 2
        plot (x,c','--')% profiles
        xlabel ('space'); ylabel ('concentration');
end
if gsurf                % surface
    figure; surf (x,[0 t],c);
    xlabel ('space'); ylabel ('time'); zlabel('concentration');
end
if gcont figure; end
switch gcont
    case 1
        contour (c)     % contours
        grid on; xlabel ('space'); ylabel ('time');
    case 2
        contourf(c)     % filled contours
        colorbar; xlabel ('space'); ylabel ('time');
end
if (ganim)
    [FileName,PathName] = uiputfile('*.mpg');
    figure; if (ganim > 1) hold on; end
    for j = 1:size(c,1)
        axis manual;  plot (x,c(j,:),'r','LineWidth',2);
        YLim = [min(c0,cin) max(c0,cin)];
        legend (['t=' num2str(dtout*(j-1))]);
        Anim(j) = getframe;
        plot (x,c(j,:),'b','LineWidth',2);
    end
    mpgwrite (Anim,colormap,[PathName '/' FileName]);     % mgwrite not standard MATLAB
    movie (Anim,0);   % play animation
end


% this script compares hedging based on Black-Scholes deltas with Factors on Demand hedging
% see Meucci, A. (2010) "Factors on Demand", Risk, 23, 7, p. 84-89
% available at http://ssrn.com/abstract=1565134

clc; clear; close all;
%%%%%%%%%%%%%%%%%%%
% inputs
tau_tilde=5;    % estimation step (days)
tau=40;         % time to horizon (days)
Time2Mats=[100 150 200 250 300];    % current time to maturity of call options in days
Strikes = [850 880 910 940 970];    % strikes of call options, same dimension as Time2Mat

r_free=0.04;    % risk-free rate
J=10000;        % number of simulations

%%%%%%%%%%%%%%%%%%%
% load underlying and volatility surface
load('DB_ImplVol');
numCalls = length(Time2Mats);
timeLength = length(spot);
numSurfPoints = length(days2Maturity)*length(moneyness);

%%%%%%%%%%%%%%%%%%%
% estimate invariant distribution assuming normality
% variables in X are changes in log(spot) and changes in log(imp.vol)
% evaluated at the 'numSurfPoints' points on the vol surface (vectorized).
X = zeros(timeLength-1,numSurfPoints+1);
% log-changes of underlying spot
X(:,1) = diff(log(spot));

% log-changes of implied vol for different maturities
impVolSeries = reshape(impVol,timeLength,numSurfPoints);
for i = 1:numSurfPoints,
    X(:,i+1) = diff(log(impVolSeries(:,i)));
end
muX = mean(X);
SigmaX = cov(X,1);

%%%%%%%%%%%%%%%%%%%
% project distribution to investment horizon
muX = muX*tau/tau_tilde;
SigmaX = SigmaX*tau/tau_tilde;

%%%%%%%%%%%%%%%%%%%
% linearly interpolate the vol surface at the current time to obtain
% implied vol for the given calls today, and price the calls
spot_T = spot(end);
volSurf_T = squeeze(impVol(end,:,:));
time2Mat_T = Time2Mats;
moneyness_T = Strikes/spot_T;
impVol_T = interpne(volSurf_T,[time2Mat_T',moneyness_T'],{days2Maturity,moneyness})'; % function by John D'Errico
callPrice_T = BlackScholesCall(spot_T,Strikes,r_free,impVol_T,Time2Mats/252);

%%%%%%%%%%%%%%%%%%%
% generate simulations at horizon
X_ = mvnrnd(muX,SigmaX,J);

% interpolate vol surface at horizon for the given calls
spot_ = spot_T*exp(X_(:,1));
impVol_ = zeros(J,numCalls);
for j = 1:J,
    volSurf = volSurf_T.*exp(reshape(X_(j,2:end),length(days2Maturity),length(moneyness)));
    time2Mat_ = Time2Mats-tau;
    moneyness_ = Strikes/spot_(j);
    impVol_(j,:) = interpne(volSurf,[time2Mat_',moneyness_'],{days2Maturity,moneyness})';  % function by John D'Errico
end

% price the calls
callPrice_ = zeros(J,numCalls);
for i = 1:numCalls,
    callPrice_(:,i) = BlackScholesCall(spot_,Strikes(i),r_free,impVol_(:,i),time2Mat_(i)/252);
end

% linear returns of the calls
Rc = callPrice_./repmat(callPrice_T,J,1) - 1;
% linear returns of the underlying
Rsp = spot_./spot_T - 1;

%%%%%%%%%%%%%%%%%%%
% compute the OLS linear (affine) model: Rc = a + b*Rsp + U
Z = [ones(J,1), Rsp];
olsLoadings = (Rc'*Z)/(Z'*Z);
a = olsLoadings(:,1);
b = olsLoadings(:,2);

%%%%%%%%%%%%%%%%%%%
% compute Black-Scholes delta and cash held in replicating portfolio
[callPrice_T,delta_T,cash_T] = BlackScholesCall(spot_T,Strikes,r_free,impVol_T,Time2Mats/252);
a_bs = cash_T./callPrice_T*r_free*tau/252;
b_bs = (delta_T./callPrice_T.*spot_T)';
fprintf('OLS: a = [%s\t]\n',sprintf('\t%7.4f',a'))
fprintf('B-S: a = [%s\t]\n',sprintf('\t%7.4f',a_bs'))
fprintf('OLS: b = [%s\t]\n',sprintf('\t%7.4f',b'))
fprintf('B-S: b = [%s\t]\n',sprintf('\t%7.4f',b_bs'))

for i = 1:numCalls
    figure
    plot(Rsp,Rc(:,i),'.')
    xlabel('return underlying')
    ylabel('return call option')
end


function normalizedU = hyperConvexHullRemoval(U,wavelengths)
%HYPERCONVEXHULLREMOVAL Performs spectral normalization via convex hull removal
%
% Usage
%   [ normalizedU ] = hyperConvexHullRemoval( U, wavelengths )
%
% Inputs
%   U - 2D HSI data (p x q)
%   wavelengths - Wavelength of each band (p x 1)
%
% Outputs
%   normalizedU - Data with convex hull removed (p x q)
%
% Author
%   Luca Innocenti
%
% References
%   Clark, R.N. and T.L. Roush (1984) Reflectance Spectroscopy: Quantitative
% Analysis Techniques for Remote Sensing Applications, J. Geophys. Res., 89,
% 6329-6340.

% Metadata and formatting
wavelengths = wavelengths(:);
p = length(wavelengths);
q = size(U,2);
U = U.';

U(:,1) = 0;
U(:,420) = 0;

normalizedU = zeros(q,420);

% The algorithm
for s = 1:q,
    rifl = U(s,:);
    k = convhull(wavelengths,rifl');
    c = [rifl(k); wavelengths(k)'];
    d = sortrows(c',2);

    xs = d(:,2);
    ys = d(:,1);
    [xsp, idx] = unique(xs);
    ysp = ys(idx);
    rifl_i = interp1(xsp,ysp,wavelengths');

    for t = 1:420,
        if rifl_i(t) ~= 0
            normalizedU(s,t) = rifl(t)/rifl_i(t);
        else
            normalizedU(s,t) = 1;
        end
    end
end

normalizedU = normalizedU.';


function out=prox_Huber(x,mu,alpha)
%PROX_HUBER computes the proximal operator of the function alpha*H_(mu)
%                      where H_(mu)=huber function with parameter mu
%
%  Usage:
%  out = PROX_HUBER(x,mu,alpha)
%  ===========================================
%  INPUT:
%  x - point to be projected (vector/matrix)
%  mu - positive scalar
%  alpha - positive scalar
%  ===========================================
%  Assumptions:
%  mu is positive
%  ===========================================
%  Output:
%  out - proximal operator at x

% This file is part of the FOM package - a collection of first order methods for solving convex optimization problems
% Copyright (C) 2017 Amir and Nili Beck
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

if (nargin < 3)
    error ('usage: prox_huber(x,mu,alpha)') ;
end

if (alpha < 0)
    error('usage: prox_huber(x,mu,alpha) - alpha should be positive')
end

if (mu < 0)
    error('usage: prox_huber(x,mu,alpha) - mu should be positive')
end

out = x*( 1  - alpha/max(norm(x,'fro'),mu+alpha))   ;

end


% HMPD: Estimate the short-term deviation of Phase Distortion
%
% Inputs
%  PD   : [NxM rad] A matrix of Phase Distortion to measure the deviation from.
%         N is the number of frames, M is the order of the PD (either the
%         maximum number of harmonics or the number of bins).
%  nbat : The number of frames to consider in the smoothing window, i.e. the
%         window size.
%
% Outputs
%  PDD  : [NxM rad] The Phase Distortion Deviation (PDD)
%
% Copyright (c) 2013 University of Crete - Computer Science Department(UOC-CSD)/
%                    Foundation for Research and Technology-Hellas - Institute
%                    of Computer Science (FORTH-ICS)
%
% License
%  This file is under the LGPL license,  you can
%  redistribute it and/or modify it under the terms of the GNU Lesser General
%  Public License as published by the Free Software Foundation, either version 3
%  of the License, or (at your option) any later version. This file is
%  distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
%  without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
%  PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
%  details.
%
% This function is part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  Gilles Degottex <degottex@csd.uoc.gr>
%

function PDD = hmpd_phase_deviation(PD, nbat)

    winlen = round(nbat/2)*2+1;
    % win = hann(winlen);
    win = ones(winlen,1); % Rectangular window is better than smooth window
                          % It better discriminates voiced/unvoiced segments
    win = win./sum(win);

    % Compute the std in polar coordinates
    % Compute first the center of gravity of the exp(i*angles)
    PDc = filtfilt(win, 1, cos(PD));
    PDs = filtfilt(win, 1, sin(PD));

    z = abs(PDc + 1j*PDs); % For the variance, we need only the magnitude

    PDD = zeros(size(PDc));
    idx = find(z<1); % To avoid neg sqrt or sqrt(-log(0))
    PDD(idx) = sqrt(-2*log(z(idx))); % Fisher's standard-deviation

return


function ecop = ecopula(x)
%ECOPULA Empirical copula based on sample X.
%   ECOP = ECOPULA(X) returns bivariate empirical copula. Extension to
%   n dimensional empirical copula is straightforward.
%
%   Written by Robert Kopocinski, Wroclaw University of Technology,
%   for Master Thesis: "Simulating dependent random variables using copulas.
%   Applications to Finance and Insurance".
%   Date: 2007/05/12
%
%   Reference:
%      [1]  Durrleman, V. and Nikeghbali, A. and Roncalli, T. (2000) Copulas approximation and
%           new families, Groupe de Recherche Operationnelle Credit Lyonnais

[m n] = size(x);

y = sort(x);

for i=1:m
    for j=1:m
        ecop(i,j) = sum( (x(:,1)<=y(i,1)).*(x(:,2)<=y(j,2)) )/m;
    end
end


function Q = IsentropicVortexIC2D(x, y, time)

% function Q = IsentropicVortexIC2D(x, y)
% Purpose: compute flow configuration given by
%     Y.C. Zhou, G.W. Wei / Journal of Computational Physics 189 (2003) 159

% based flow parameters
xo = 5; yo = 0; beta = 5; gamma = 1.4;
rho = 1; u = 1; v = 0; p = 1;

xmut = x-u*time; ymvt = y-v*time;
r = sqrt((xmut-xo).^2 + (ymvt-yo).^2);

% perturbed density
u   = u - beta*exp(1-r.^2).*(ymvt-yo)/(2*pi);
v   = v + beta*exp(1-r.^2).*(xmut-xo)/(2*pi);
rho1 = (1 - ((gamma-1)*beta^2*exp(2*(1-r.^2))/(16*gamma*pi*pi))).^(1/(gamma-1));
p1   = rho1.^gamma;

Q(:,:,1) = rho1; Q(:,:,2) = rho1.*u; Q(:,:,3) = rho1.*v;
Q(:,:,4) = p1/(gamma-1) + 0.5*rho1.*(u.^2 + v.^2);
return;


%   y: dimensionanlity-reduced data
%	eigVector: eigen-vector obtained in kPCA
%   X: data matrix
%   para: parameter of Gaussian kernel
%	z: pre-image of y

%   Copyright by Quan Wang, 2011/05/10
%   Please cite: Quan Wang. Kernel Principal Component Analysis and its
%   Applications in Face Recognition and Active Shape Models.
%   arXiv:1207.3538 [cs.CV], 2012.

function z=kPCA_PreImage(y,eigVector,X,para)

iter=1000;
N=size(X,1);
d=max(size(y));

gamma=zeros(1,N);
for i=1:N
    gamma(i)=eigVector(i,1:d)*y;
end

z=mean(X)'; % initialization
fprintf('\nReconstruction: \n');
for count=1:iter
    fprintf('%d ', count);
    if mod(count,10)==0
        fprintf('\n');
    end

    pre_z=z;
    xx=bsxfun(@minus,X',z);
    xx=xx.^2;
    xx=-sum(xx)/(2*para.^2);
    xx=exp(xx);
    xx=xx.^gamma;

    z=xx*X/sum(xx);
    z=z';
    if norm(pre_z-z)/norm(z)<0.0001
        break;
    end
end
fprintf('\n');


function [L,S] = PCP(M,lam,tol)
%
% [L,S] = PCP(M,lam,opts)

% This code solves the following model
%
% min_A { lam*||S(:)||_1 + ||L||_* }
% s.t. M = S+L

% where M is the data matrix, which will be decomposed into
% S sparse matrix S and low-rank matrix L.
%
% lam -- S small positive parameter

%% parameter setting
beta = .25/mean(abs(M(:))); %
maxit = 1000;

%% initialization
[m,n] = size(M);
S = zeros(m,n);
L = zeros(m,n);
Lambda = zeros(m,n); % the dual variable

% main
for iter = 1:maxit

    nrmLS = norm([S,L],'fro');
    % dS, dL record the change of S and L, only used for stopping criterion

    %% S - subproblem
    % S = argmin_A  lam*||S||_1 - <Lambda, S+L-M> + (beta/2) * ||S+L-M||.^2
    % Define element wise softshinkage operator as
    %     softshrink(z; gamma) = sign(z).* max(abs(z)-gamma, 0);
    % S has closed form solution: S=softshrink(Lambda/beta + M - L; lam/beta)
    % (see my slide page 42 Equation (66).
    X = Lambda / beta + M;
    Y = X - L;
    dS = S;
    S = sign(Y) .* max(abs(Y) - lam/beta, 0); % softshinkage operator
    dS = S - dS;

    %% L - subproblem
    % L = argmin_B ||L||_* -<Lambda, S+L-M> + + (beta/2) * ||S+L-M||.^2
    % L has closed form solution (singular value thresholding)
    % see my slide page 42, Equation (65).
    Y = X - S;
    dL = L;

    %[U,D,V] = svd(Y,'econ'); % use 'econ' is more efficient especially when Y is large
    [U,D,V] = svdecon(Y); % fastest

    VT=V';
    D = diag(D);
    ind = find(D > 1/beta);
    D = diag(D(ind) - 1/beta);
    L = U(:,ind) * D * VT(ind,:);
    dL = L - dL;

    %% stopping criterion
    RelChg = norm([dS,dL],'fro') / (1 + nrmLS);
    %fprintf('Iter %d, RelChg %4.2e \n',iter,RelChg);
    if RelChg < tol, break; end

    %% Update Lambda (dual variable)
    Lambda = Lambda - beta * (S + L - M);
end
end


classdef svd

methods(Static)

    function [rank, max_gap] = mahdi_rank(singular_values)
        % rank accordingly to mahdi 2013 heuristic
        [ min_val , ind_min ] = min( diff( singular_values(1:end-1) ) ) ;
        rank = ind_min;
        max_gap = -min_val;
    end

    function rank = vidal_rank(singular_values,kappa)
        if nargin < 2
            kappa = .1;
        end
        % Rank used in Vidal papers
        n = length(singular_values);
        % we will try rank values between 1 to n-1.
        % an array to store criterion value
        criterion = zeros(1, n-1);
        for(r=1:n-1)
            num = singular_values(r+1)^2;
            den = sum(singular_values(1:r).^2);
            criterion(r)=num/den + kappa*r;
        end
        %criterion
        % find the rank with minimum value of the criterion
        [min_value, index]=min(criterion);
        rank = index;
    end

    function result = low_rank_approx(X, r)
        % return low rank approximation of X
        [U S V] = svd(X);
        % keep the first r left singular vectors
        U = U(:, 1:r);
        % keep the first r singular values
        S = S(1:r, 1:r);
        % keep the first r right singular vectors
        V = V(:, 1:r);
        % Return the approximation
        result = U * S * V';
    end

    function [result, basis] = low_rank_projection(X, r)
        % Projects X to a low dimensional space
        % X is NxS
        % result is RxS
        % basis is [NxR] orthonormal basis

        % Compute the SVD
        [U, ~, ~] = svd(X, 0);
        % Choose the low rank basis
        basis = U(:, 1:r);
        % Compute coefficients in this basis
        result = basis' * X;
    end

    function result = low_rank_basis(X, r)
        % Returns the ON basis for low rank approximation
        [U S V] = svd(X, 'econ');
        result = U(:, 1:r);
    end

    function result = low_rank_bases(X, counts, r)
        % low rank bases for individual subspaces
        K = length(counts);
        % bases cell array
        result = cell(1, K);
        [start_indices, end_indices] = spx.cluster.start_end_indices(counts);
        for k=1:K
            ss = start_indices(k);
            ee = end_indices(k);
            XX = X(:, ss:ee);
            basis = spx.la.svd.low_rank_basis(XX, r);
            result{k} = basis;
        end
    end

    function [result, r] = mahdi_rank_basis(X)
        [U S V] = svd(X, 'econ');
        sv = diag(S);
        r = spx.la.svd.mahdi_rank(sv);
        % r = r + 1;
        result = U(:, 1:r);
    end

    function [result, ranks] = mahdi_rank_bases(X, counts)
        % low rank bases for individual subspaces
        K = length(counts);
        % bases cell array
        result = cell(1, K);
        [start_indices, end_indices] = spx.cluster.start_end_indices(counts);
        ranks = zeros(1, K);
        for k=1:K
            ss = start_indices(k);
            ee = end_indices(k);
            XX = X(:, ss:ee);
            [basis,r] = spx.la.svd.mahdi_rank_basis(XX);
            result{k} = basis;
            ranks(k) = r;
        end
    end

    function [result, r] = vidal_rank_basis(X, kappa)
        if nargin < 3
            kappa = 0.1;
        end
        [U S V] = svd(X, 'econ');
        sv = diag(S)';
        r = spx.la.svd.vidal_rank(sv, kappa);
        % r = r + 1;
        result = U(:, 1:r);
    end

    function [result, ranks] = vidal_rank_bases(X, counts, kappa)
        if nargin < 3
            kappa = 0.1;
        end
        % low rank bases for individual subspaces
        K = length(counts);
        % bases cell array
        result = cell(1, K);
        [start_indices, end_indices] = spx.cluster.start_end_indices(counts);
        ranks = zeros(1, K);
        for k=1:K
            ss = start_indices(k);
            ee = end_indices(k);
            XX = X(:, ss:ee);
            [basis,r] = spx.la.svd.vidal_rank_basis(XX, kappa);
            result{k} = basis;
            ranks(k) = r;
        end
    end

end

end


function val=calcGAE(xTrue,xEst,is3D)
%%CALCGAE Compute the scalar geometric average error (GAE). Unlike the
%         RMSE, it is not dominated by large individual terms.
%
%INPUTS: xTrue The truth data. This is either an xDimXNumSamples matrix or
%              an xDimXNXnumSamples matrix. The latter formulation is
%              useful when the MSE over multiple Monte Carlo runs of an
%              entire length-N track is desired. In the first formulation,
%              we take N=1. Alternatively, if the same true value is used
%              for all numSamples, then xTrue can just be an xDimXN matrix.
%              Alternatively, if xTrue is the same for all numSamples and
%              all N, then just an xDimX1 matrix can be passed. N and
%              numSamples are inferred from xEst.
%         xEst An xDimXnumSamples set of estimates or an xDimXNXnumSamples
%              set of estimates (if values at N times are desired).
%         is3D An optional indicating that xEst is 3D. This is only used if
%              xEst is a matrix. In such an instance, there is an ambiguity
%              whether xEst is truly 2D, so N=1, or whether numSamples=1
%              and xEst is 3D with the third dimension being 1. If this
%              parameter is omitted or an empty matrix is passed, it is
%              assumed that N=1.
%
%OUTPUTS: val The 1XN set of scalar GAE values.
%
%The GAE is given in Equation 3 in [1].
%
%EXAMPLE:
%For a Gaussian random vector, the root-trace of the covariance matrix is
%the RMSE. The RMSE is larger than the average Euclidean error, which is
%also larger than the  geometric average error
% R=[28,   4, 10;
%     4,  22, 16;
%     10, 16, 16];%The covariance matrix.
% xTrue=[10;-20;30];
% numRuns=100000;
% xEst=GaussianD.rand(numRuns,xTrue,R);
% valRMSE=rootTrace=sqrt(trace(R))
% valAEE=calcAEE(xTrue,xEst)
% valGAE=calcGAE(xTrue,xEst)
%
%REFERENCES:
%[1] X. R. Li and Z. Zhao, "Measures of performance for evaluation of
%    estimators and filters," in Proceedings of SPIE: Conference on Signal
%    and Data processing of Small Targets, vol. 4473, San Diego, CA, 29
%    Jul. 2001, pp. 530-541.
%
%February 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<3||isempty(is3D))
    is3D=false;
end

xDim=size(xEst,1);
if(ismatrix(xEst)&&is3D==false)
    N=1;
    numSamples=size(xEst,2);
    xEst=reshape(xEst,[xDim,1,numSamples]);

    if(size(xTrue,2)==1)
        %If the true values are the same for all samples.
        xTrue=repmat(xTrue,[1,1,numSamples]);
    else
        xTrue=reshape(xTrue,[xDim,1,numSamples]);
    end
else
    N=size(xEst,2);
    numSamples=size(xEst,3);

    if(ismatrix(xTrue))
        if(size(xTrue,2)==1)
            %If the true values are the same for all samples and for all N.
            xTrue=repmat(xTrue,[1,N,numSamples]);
        else
            %If the true values are the same for all samples.
            xTrue=repmat(xTrue,[1,1,numSamples]);
        end
    end
end

val=zeros(1,N);
for k=1:N
    val(k)=exp((1/(2*numSamples))*sum(log(sum((xEst(:,k,:)-xTrue(:,k,:)).^2,1))));
end
end

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


function value = error_f ( x )

%*****************************************************************************80
%
%% ERROR_F computes the error function.
%
%  Discussion:
%
%    This function was renamed "ERROR_F" from "ERF", to avoid a conflict
%    with the name of a corresponding routine often, but not always,
%    supplied as part of the math support library.
%
%    The definition of the error function is:
%
%      ERF(X) = ( 2 / SQRT ( PI ) ) * Integral ( 0 <= T <= X ) EXP ( -T**2 ) dT
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 March 2007
%
%  Author:
%
%    FORTRAN77 original version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    David Kahaner, Cleve Moler, Steven Nash,
%    Numerical Methods and Software,
%    Prentice Hall, 1989,
%    ISBN: 0-13-627258-4,
%    LC: TA345.K34.
%
%  Parameters:
%
%    Input, real X, the argument of the error function.
%
%    Output, real VALUE, the value of the error function at X.
%
  persistent nterf;
  persistent sqeps;
  persistent xbig;

  erfcs = [ ...
    -0.049046121234691808, -0.14226120510371364, ...
     0.010035582187599796, -0.000576876469976748, ...
     0.000027419931252196, -0.000001104317550734, ...
     0.000000038488755420, -0.000000001180858253, ...
     0.000000000032334215, -0.000000000000799101, ...
     0.000000000000017990, -0.000000000000000371, ...
     0.000000000000000007 ];

  sqrtpi = 1.7724538509055160;
%
%  Initialize the Chebyshev series.
%
  if ( size ( nterf ) == 0 )
    nterf = inits ( erfcs, 13, 0.1 * eps );
    xbig = sqrt ( - log ( sqrtpi * eps ) );
    sqeps = sqrt ( 2.0 * eps );
  end

  y = abs ( x );

  if ( y <= sqeps )
    value = 2.0 * x / sqrtpi;
  elseif ( y <= 1.0 )
    value = x * ( 1.0 + csevl ( 2.0 * x * x - 1.0, erfcs, nterf ) );
  elseif ( y <= xbig )
    value = r8_sign ( x ) * ( 1.0 - error_fc ( y ) );
  else
    value = r8_sign ( x );
  end

  return
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Gaussian Process Regression 2D Example  %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%             1) Load 2D Regression Datasets                 %%
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Example of Gaussian Process Regression (2D)
%% Generate Random Gaussian distribution
clear all; close all; clc;
K = 90;
a =-50;
b = 50;
x = a + (b-a).*rand(K,1);
y = a + (b-a).*rand(K,1);

a    = 40;
b    = 100;
vars = a + (b-a).*rand(K,1);

Mu    = [x,y]';
Sigma = zeros(2,2,K);

for k=1:K
    Sigma(:,:,k) = eye(2,2) .* vars(k);
end

gmm_x.Priors    = ones(1,K)./K;
gmm_x.Mu        = Mu;
gmm_x.Sigma     = Sigma;

% Generate some training Data
X = ml_gmm_sample(500,gmm_x.Priors,gmm_x.Mu,gmm_x.Sigma )';
f = @(X)ml_gmm_pdf(X',gmm_x.Priors,gmm_x.Mu,gmm_x.Sigma );
y = f(X);
y = y(:);

% Plot Training Data

% Plot Real Function
options           = [];
options.title     = 'Training data';
options.surf_type = 'surf';

if exist('h1','var') && isvalid(h1), delete(h1);end
h1 = ml_plot_value_func(X,f,[1 2],options);hold on

% Plot Training Data
options.bFigure     = false;
options.surf_type   = 'scatter';
options.points_size = 20;
ml_plot_value_func(X,f,[1 2],options);hold on

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%                 2) "Train" GPR Model                       %%
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Train GP (no real training, just give parameters)
epsilon         = 0.1;
dims            = 2;
model.X_train   = X;
model.y_train   = y;
rbf_var         = 25;
gp_f            = @(X)ml_gpr(X,[],model,epsilon,rbf_var);

% Plot Estimated Function
options           = [];
options.title     = 'Estimated y=f(x) from Gaussian Process Regression';
options.surf_type = 'surf';
if exist('h2','var') && isvalid(h2), delete(h2);end
h2 = ml_plot_value_func(X,gp_f,[1 2],options);hold on

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%           3) Test GPR Model on Train points                %%
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Test GPR
options           = [];
options.bFigure   = true;
options.title     = 'Test data';
options.surf_type = 'pcolor';
dims              = [1,2];

if exist('h2','var') && isvalid(h2), delete(h2);end
h2 = gp_plot(model.X_train,gp_f,dims,options);
colorbar

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%           4) Grid Search for GPR with RBF Kernel           %%
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% K-fold cross validation
Kfold = 20;

disp('Parameter grid search GP');

rbf_vars = [5,10,20,40,50,100,10000];

test  = cell(length(rbf_var),1);
train = cell(length(rbf_var),1);

for i=1:length(rbf_vars)
    disp(['[' num2str(i) '/' num2str(length(rbf_vars)) ']']);

    f                       = @(X,y,model)ml_gpr(X,y,model,epsilon,rbf_vars(i));
    [test_eval,train_eval]  = ml_kcv(X,y,Kfold,f,'regression');

    test{i}                 = test_eval;
    train{i}                = train_eval;
    disp(' ');
end

%% Get Statistics

[ stats ] = ml_get_cv_grid_states_regression(test,train);

%% Plot Statistics

options             = [];
options.title       = 'GPR k-CV';
options.metrics     = {'nrmse','r'};     % <- you can add many other metrics, see list in next cell box
options.para_name   = 'variance rbf';

if exist('handle','var'), delete(handle); end
[handle,handle_test,handle_train] = ml_plot_cv_grid_states_regression(stats,rbf_vars,options);

%% Full list of evaluation metrics for regression methods

options.metric = {'mse','nmse','rmse','nrmse','mae','mare','r','d','e','me','mre'};

%   '1'  - mean squared error                               (mse)
%   '2'  - normalised mean squared error                    (nmse)
%   '3'  - root mean squared error                          (rmse)
%   '4'  - normalised root mean squared error               (nrmse)
%   '5'  - mean absolute error                              (mae)
%   '6'  - mean  absolute relative error                    (mare)
%   '7'  - coefficient of correlation                       (r)
%   '8'  - coefficient of determination                     (d)
%   '9'  - coefficient of efficiency                        (e)
%  '10'  - maximum absolute error                           (me)
%  '11'  - maximum absolute relative error                  (mre)


function J=polAzEquiDistRefChangeCrossGrad(pAzPt,latLonRefOld,latLonRefNew,rE)
%%POLAZEQUIDISTREFCHANGECROSSGRAD Given a point in polar azimuthal
%      coordinates with respect to a reference position on a spherical
%      Earth, find the gradient of the point in a polar azimuthal
%      coordinate system with a difference reference point taken with
%      respect to the point in the original system. This is the gradient of
%      the polAzEquidistRefChange function.
%
%INPUTS: pAzPts A 2XN set of the [ground distance; heading] points, with
%              the heading given in radians East of North, to convert.
%              Alternatively, if heights are also given, this can be a 3XN
%              set of points with the height being the third dimension.
% latLonRefOld A 2X1 [latitude;longitude] reference point in radians about
%              which pAzPts was computed.
% latLonRefNew A 2X1 [latitude;longitude] reference point in radians with
%              respect to which the function to be differentied is defined.
%           rE The radius of the reference sphere. If this argument is
%              omitted or an empty matrix is passed, the value in
%              Constants.WGS84MeanRadius is used.
%
%OUTPUTS: J A 2X2XN matrix holding the gradient of the transformation
%           evaluated at each point in pAzPts (or a 3X3XN matrix if height
%           is also provided). If one says that a point in pAzPts=[p;az;h]
%           and a converted point is [p1;az1;h1], the gradients in 3D are
%           ordered:
%           [dp1dp,   dp1dAz,  dp1dh;
%            dAz1dp, dAz1dAz, dAz1dh;
%            dh1dp,   dh1dAz,  dh1dh];
%
%The gradient is the analytic gradient of the spherical Earth conversion
%given in the function polAzEquidistRefChange. Due to a singularity, the
%gradient is not valid if the target is exactly on the opposite side of the
%Earth from the first sensor.
%
%EXAMPLE:
%Given three random points, the accuracy of this function is compared to
%numeric differentiation. The relative error is on the order of what one
%might expect due to finite precision limitations.
% rE=Constants.WGS84MeanRadius;
% latLonRef1Old=[UniformD.rand(1,[-pi/2;pi/2]);
%                UniformD.rand(1,[-pi;pi])];
% latLonRefNew=[UniformD.rand(1,[-pi/2;pi/2]);
%               UniformD.rand(1,[-pi;pi])];
% latLonPt=[UniformD.rand(1,[-pi/2;pi/2]);
%           UniformD.rand(1,[-pi;pi])];
% pAzPt=ellips2PolarAzEquidistProj(latLonPt,latLonRef1Old,rE,0);
% J=polAzEquiDistRefChangeCrossGrad(pAzPt,latLonRef1Old,latLonRefNew,rE);
% f=@(x)polAzEquidistRefChange(x,latLonRef1Old,latLonRefNew,rE,0);
% JNumDiff=numDiff(pAzPt,f,2);
% RelErr=max(max(abs((J-JNumDiff)./JNumDiff)))
%
%December 2021 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<3||isempty(rE))
    rE=Constants.WGS84MeanRadius;
end

numPoints=size(pAzPt,2);
if(all(latLonRefOld==latLonRefNew))
    %For the special case of no change, force the result to be exact (avoid
    %finite precision issues).
    numDim=size(pAzPt,1);%2 or 3.
    J=repmat(eye(numDim,numDim),[1,1,numPoints]);
    return;
end

azStart=greatCircleAzimuth(latLonRefOld,latLonRefNew);
azOffset1=pi/2-azStart;
lambda0=greatCircleDistance(latLonRefOld,latLonRefNew,1);

if(size(pAzPt,1)==3)
    J=zeros(3,3,numPoints);
    J(3,3,:)=1;
else
    J=zeros(2,2,numPoints);
end

%Transform into the system with both sensors on the equator.
pAzPt(2,:)=pAzPt(2,:)+azOffset1;

p=pAzPt(1,:);
az=pAzPt(2,:);

cosPre=cos(p/rE);
sinPre=sin(p/rE);

cosLambda0=cos(lambda0);
sinLambda0=sin(lambda0);
sinAz=sin(az);
cosAz=cos(az);

denom2=1-(cosPre.*cosLambda0+sinPre.*sinAz.*sinLambda0).^2;
denom1=sqrt(denom2);

dp0dp=(cosLambda0.*sinPre-cosPre.*sinAz.*sinLambda0)./denom1;
dp0dAz=-rE.*sinPre.*cosAz.*sinLambda0./denom1;

dAz0dp=cosAz.*sinLambda0./(rE.*denom2);
dAz0dAz=sinPre.*(cosLambda0.*sinPre-cosPre.*sinAz.*sinLambda0)./denom2;

J(1,1,:)=dp0dp;
J(2,1,:)=dAz0dp;
J(1,2,:)=dp0dAz;
J(2,2,:)=dAz0dAz;

end

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


% a=0.005, x^2
% a=0.0002, abs(x)^3
% a=0.0001, x^4

a = 0.005;
b = 0.00025;
xs = 0:50;
ys2 = zeros(length(xs), 1);
ys3 = zeros(length(xs), 1);
for i = 1:length(xs)
    ys2(i) = exp(-a*abs(xs(i))^2);
    ys3(i) = exp(-b*abs(xs(i))^3);
end
plot(xs, ys2);
hold on;
plot(xs, ys3);
hold off;


%DEMGPARD Demonstrate ARD using a Gaussian Process.
%
%	Description
%	The data consists of three input variables X1, X2 and X3, and one
%	target variable  T. The  target data is generated by computing
%	SIN(2*PI*X1) and adding Gaussian  noise, x2 is a copy of x1 with a
%	higher level of added noise, and x3 is sampled randomly from a
%	Gaussian distribution. A Gaussian Process, is trained by optimising
%	the hyperparameters  using the scaled conjugate gradient algorithm.
%	The final values of the hyperparameters show that the model
%	successfully identifies the importance of each input.
%
%	See also
%	DEMGP, GP, GPERR, GPFWD, GPGRAD, GPINIT, SCG
%

%	Copyright (c) Ian T Nabney (1996-2001)

clc;
randn('state', 1729);
rand('state', 1729);
disp('This demonstration illustrates the technique of automatic relevance')
disp('determination (ARD) using a Gaussian Process.')
disp(' ');
disp('First, we set up a synthetic data set involving three input variables:')
disp('x1 is sampled uniformly from the range (0,1) and has a low level of')
disp('added Gaussian noise, x2 is a copy of x1 with a higher level of added')
disp('noise, and x3 is sampled randomly from a Gaussian distribution. The')
disp('single target variable is given by t = sin(2*pi*x1) with additive')
disp('Gaussian noise. Thus x1 is very relevant for determining the target')
disp('value, x2 is of some relevance, while x3 should in principle be')
disp('irrelevant.')
disp(' ');
disp('Press any key to see a plot of t against x1.')
pause;

ndata = 100;
x1 = rand(ndata, 1);
x2 = x1 + 0.05*randn(ndata, 1);
x3 = 0.5 + 0.5*randn(ndata, 1);
x = [x1, x2, x3];
t = sin(2*pi*x1) + 0.1*randn(ndata, 1);

% Plot the data and the original function.
h = figure;
plotvals = linspace(0, 1, 200)';
plot(x1, t, 'ob')
hold on
xlabel('Input x1')
ylabel('Target')
axis([0 1 -1.5 1.5])
[fx, fy] = fplot('sin(2*pi*x)', [0 1]);
plot(fx, fy, '-g', 'LineWidth', 2);
legend('data', 'function');

disp(' ');
disp('Press any key to continue')
pause; clc;

disp('The Gaussian Process has a separate hyperparameter for each input.')
disp('The hyperparameters are trained by error minimisation using the scaled.')
disp('conjugate gradient optimiser.')
disp(' ');
disp('Press any key to create and train the model.')
disp(' ');
pause;

net = gp(3, 'sqexp');
% Initialise the parameters.
prior.pr_mean = 0;
prior.pr_var = 0.1;
net = gpinit(net, x, t, prior);

% Now train to find the hyperparameters.
options = foptions;
options(1) = 1;
options(14) = 30;

[net, options] = netopt(net, options, x, t, 'scg');

rel = exp(net.inweights);

fprintf(1, ...
  '\nFinal hyperparameters:\n\n  bias:\t\t%10.6f\n  noise:\t%10.6f\n', ...
  exp(net.bias), exp(net.noise));
fprintf(1, '  Vertical scale: %8.6f\n', exp(net.fpar(1)));
fprintf(1, '  Input 1:\t%10.6f\n  Input 2:\t%10.6f\n', ...
  rel(1), rel(2));
fprintf(1, '  Input 3:\t%10.6f\n\n', rel(3));
disp(' ');
disp('We see that the inverse lengthscale associated with')
disp('input x1 is large, that of x2 has an intermediate value and the variance')
disp('of weights associated with x3 is small.')
disp(' ');
disp('This implies that the Gaussian Process is giving greatest emphasis')
disp('to x1 and least emphasis to x3, with intermediate emphasis on')
disp('x2 in the covariance function.')
disp(' ')
disp('Since the target t is statistically independent of x3 we might')
disp('expect the weights associated with this input would go to')
disp('zero. However, for any finite data set there may be some chance')
disp('correlation between x3 and t, and so the corresponding hyperparameter remains')
disp('finite.')
disp('Press any key to continue.')
pause

disp('Finally, we plot the output of the Gaussian Process along the line')
disp('x1 = x2 = x3, together with the true underlying function.')
xt = linspace(0, 1, 50);
xtest = [xt', xt', xt'];

cn = gpcovar(net, x);
cninv = inv(cn);
[ytest, sigsq] = gpfwd(net, xtest, cninv);
sig = sqrt(sigsq);

figure(h); hold on;
plot(xt, ytest, '-k');
plot(xt, ytest+(2*sig), '-b', xt, ytest-(2*sig), '-b');
axis([0 1 -1.5 1.5]);
fplot('sin(2*pi*x)', [0 1], '--m');

disp(' ');
disp('Press any key to end.')
pause; clc; close(h); clear all


function g = sphf2cartf(f, lam, th, coord)
%SPHF2CARTF    Wrapper for evaluating a function defined in Cartesian
%
%  G = SPHF2CARTF(F, LAM, TH) evaluates the function handle F = F(x,y,z)
%  at x = cos(lam).*sin(th), y = sin(lam).*sin(th), z = cos(th). This is
%  the co-latitude spherical coordinate system.
%
%  G = SPHF2CARTF(F, LAM, TH, 0) same as SPHF2CARTF(F, LAM, TH).
%
%  G = SPHF2CARTF(F, LAM, TH, 1) evaluates F = F(x,y,z) at
%  at x = cos(lam).*cos(th), y = sin(lam).*cos(th), z = sin(th). This is
%  the latitude spherical coordinate system.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% For Developers, recall that:
%   coord - Type of spherical coordinate system:
%          coord = 0 (co-latitude) --> -pi <= lam < pi, 0 <= th <= pi
%          coord = 1 (latitude)    --> -pi <= lam < pi, -pi/2 <= th <= pi/2
% (Default above is co-latitude.)

% TODO: Create separate wrapper functions for latitude/co-latitude so that
% this if can be removed (thus improving performance.

if ( nargin == 3 )
    coord = 0;  % Default is to use co-latitude.
end

if ( coord == 0 )
    % Latitude: 0 <= theta < pi
    x = cos(lam).*sin(th);
    y = sin(lam).*sin(th);
    z = cos(th);
elseif ( coord == 1 )
    % Latitude: -pi/2 <= theta < pi/2
    x = cos(lam).*cos(th);
    y = sin(lam).*cos(th);
    z = sin(th);
else
    error('SPHEREFUN:sphf2cartf:CoordSysUnknown', ['Unknown coordinate '...
        'system for the sphere.']);
end

g = feval(f, x, y, z);

end


function obj = setRotationMatrix(obj,r)
    % set the rotation matrix (R) of the frame w.r.t to the
    % reference frame
    %
    % Parameters:
    % r: the rotation matrix or Euler angles (in radian) @type
    % rowvec|matrix

    if isvector(r) && length(r) == 3
        %         rpy = rad2deg(r);
        obj.R = rotz(r(3)) * roty(r(2)) * rotx(r(1));
    elseif all(size(r)==[3,3])
        if isnumeric(r)
            assert(abs(det(r))-1 <= 1e-6,...
                'The determinant of the rotation matrix must equal 1.');
        end
        obj.R = r;
    end
    % remove small numbers generated from the rotation matrix
    if isnumeric(r)
        obj.R = roundn(obj.R,-6);
    end
    % update the homogeneous transformation matrix
    obj.computeHomogeneousTransform();
end

function R = rotx(alpha)
%rotz  rotate around X by ALPHA
%
%	R = rotx(alpha)
%
% See also: roty, rotz
% Author: Jake Reher: jreher@caltech.edu

R = [1 0 0; ...
     0 cos(alpha) -sin(alpha); ...
     0 sin(alpha)  cos(alpha)];

end

function R = roty(alpha)
%roty  rotate around Y by ALPHA
%
%	R = roty(alpha)
%
% See also: rotx, rotz
% Author: Jake Reher: jreher@caltech.edu

R = [cos(alpha)  0 sin(alpha); ...
             0   1          0; ...
    -sin(alpha)  0 cos(alpha)];

end

function R = rotz(alpha)
%rotz  rotate around Z by ALPHA
%
%	R = rotz(alpha)
%
% See also: ROTX, ROTY, ROT, POS.
% Author: Jake Reher: jreher@caltech.edus

R = [cos(alpha) -sin(alpha) 0; ...
     sin(alpha)  cos(alpha) 0; ...
              0           0 1];

end


function [pos, tri] = mesh_tetrahedron

% MESH_TETRAHEDRON returns the vertices and triangles of a tetrahedron.
%
% Use as
%   [pos, tri] = mesh_tetrahedron;
%
% See also MESH_ICOSAHEDRON, MESH_OCTAHEDRON, MESH_SPHERE

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

v1 = [ 0, 0, 1 ];
v2 = [  sqrt(8/9),          0, -1/3 ];
v3 = [ -sqrt(2/9),  sqrt(2/3), -1/3 ];
v4 = [ -sqrt(2/9), -sqrt(2/3), -1/3 ];

pos = [
  v1
  v2
  v3
  v4
  ];

tri = [
  1 2 3
  1 3 4
  1 4 2
  2 4 3
  ];

  % scale all vertices to the unit sphere
  pos = pos ./ repmat(sqrt(sum(pos.^2,2)), 1,3);


function [repairedData, coefficients, grossErrors] = repair_corrupted_data(dictionary, corruptedData)
% repair_missing_data.m
%
%   Given a set of complete data vectors, and another set of vectors with
%   missing entries, use L1-minimization to repair the incomplete vectors.
%   This function required the CVX package for semidefinite programming.
%
%
% Inputs:
%   dictionary           - a matrix whose columns are data vectors that are
%                          used as overcomplete basis to represent other
%                          vectors. note that some or all of these columns
%                          may contain gross errors.
%   corruptedData        - a matrix whose columns are data vectors with
%                          some elements that may have gross errors. these
%                          vectors will be repaired using the dictionary.
%
% Outputs:
%   repairedData        - a matrix whose columns are repaired versions of
%                         the given incomplete data vectors
%   coefficients        - a matrix whose ith column is  a list of
%                         coefficients used to represent the ith vector in
%                         in corrupted data as a linear combination of
%                         vectors in the dictionary
%   grossErrors         - a matrix whose ij-th entry is true if the ij-th
%                         entry of corruptedData was corrupted by gross
%                         errors.
% Dependencies:
%   CVX package
%
% Nov. '07  Shankar Rao -- srrao@uiuc.edu

% Copyright 2007, University of Illinois. All rights reserved.

VERBOSE = false;
GROSS_ERROR_THRESHOLD = 0.5;

[dimensionCount, sampleCount] = size(dictionary);
corruptedSampleCount = size(corruptedData,2);

repairedData = corruptedData;
grossErrors = false(size(corruptedData));
coefficients = zeros(sampleCount+dimensionCount, corruptedSampleCount);
normalizedData = dictionary ./ repmat(sqrt(sum(dictionary.^2, 1)), dimensionCount, 1);
I = eye(dimensionCount);
X = [ normalizedData I];
for sampleIndex = 1:corruptedSampleCount
    y = corruptedData(:, sampleIndex);

    if VERBOSE,
        disp(sprintf('Repairing sample %d of %d', sampleIndex, corruptedSampleCount));
    end

    state = cvx_quiet(true);
    cvx_begin
        variable c(sampleCount+dimensionCount);
        minimize(norm(c,1));
            subject to
                X * c == y;
    cvx_end
    cvx_quiet(state);

    yhat = X(:, 1:sampleCount)*c(1:sampleCount);
    coefficients(:, sampleIndex) = c;
    errors = c(sampleCount+1:end);
    grossErrors(:, sampleIndex) = abs(errors) > GROSS_ERROR_THRESHOLD;
    repairedData(grossErrors(:, sampleIndex), sampleIndex) = yhat(grossErrors(:, sampleIndex));
end


function h = p08_h ( n, x )

%*****************************************************************************80
%
%% P08_H evaluates the Hessian for problem 8.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 October 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the values of the variables.
%
%    Output, real H(N,N), the N by N Hessian matrix.
%
  ap = 0.00001;

  t1 = - 0.25 + sum ( x(1:n).^2 );

  d1 = 2.0 * ap;
  th = 4.0 * t1;

  for i = 1 : n
    h(i,i) = d1 + th + 8.0 * x(i)^2;
    for j = 1 : i - 1
      h(i,j) = 8.0 * x(i) * x(j);
    end
  end

  for i = 1 : n
    for j = i + 1 : n
      h(i,j) = h(j,i);
    end
  end

  return
end