function [X, info] = IRhybrid_flsqr(A, b, varargin)
%IRhybrid_flsqr Hybrid version of FLSQR algorithm [...]
%
% options  = IRhybrid_flsqr('defaults')
% [X,info] = IRhybrid_flsqr(A,b)
% [X,info] = IRhybrid_flsqr(A,b,K)
% [X,info] = IRhybrid_flsqr(A,b,options)
% [X,info] = IRhybrid_flsqr(A,b,K,options)
%
% IRhybrid_flsqr is a hybrid iterative regularization method used for
% solving large-scale, ill-posed inverse problems of the form:
%               b = A*x + noise .
% The method combines FLSQR iteration (iterative regularization method)
% with a Tikhonov regularization method to stabilize the semiconvergence
% behavior that is characteristic of many iterative solvers applied to
% ill-posed problems.
%
% With 'defaults' as input returns the default options.  Otherwise outputs
% the iterates specified in K, using max(K) as MaxIter, and using all other
% default options.  With options as input: uses the user-specified options
% and all the other default options.
%
%
% Inputs:
%  A : either (a) a full or sparse matrix
%             (b) a matrix object that performs the matrix*vector operation
%             (c) user-defined function handle
%  b : right-hand side vector
%  K : (optional) integer vector that specifies which iterates are returned
%      in X; the maximum number of iterations is assumed to be max(K)
%      [ positive integer | vector of positive components ]
%  options : structure with the following fields (optional)
%      x0         - initial guess for the iterations; default = zero vector
%                   [ array | {'none'} ]
%      x_true     - true solution; allows us to returns error norms with
%                   respect to x_true at each iteration
%                   [ array | {'none'} ]
%      RegParam   - a value or a method to find the regularization
%                   parameter for the projected problems:
%                   [  non-neg. scalar | {'wgcv'} | 'gcv' | 'modgcv' |...
%                     'discrep' | 'discrepit' | 'optimal']
%                   This also determines which stopping rule is used
%                   If 'gcv', 'wgcv' or 'modgcv' is chosen, the iteration is
%                     stopped when the GCV function minimum stabilizes or
%                     increases within a certain window of iterations (see
%                     'stopGCV', 'FlatTol' and 'MinTol').
%                   If 'discrep' is chosen, and NoiseLevel is provided,
%                     then the discrepancy principle is used as stopping
%                     (see 'NoiseLevel' and 'eta').
%                   If 'discrepit' is chosen, and NoiseLevel is provided,
%                     then the discrepancy principle is used to set the
%                     parameter at each iteration, and the stabilization of
%                     successive parameters is used as stopping criterion
%                     (see 'NoiseLevel', 'eta' and 'regPflatTol').
%                   If 'optimal' is chosen, and x_true is provided,
%                     no stopping criterion is considered
%      stopGCV    - stopping criterion for the iterations when GCV is used
%                   [ 'GCVvalues' | {'resflat'} ]
%      resflatTol - tolerace for the stabilization of the residual
%                   (to be used if stopGCV is 'resflat')
%                   [ {0.05} | non-negative scalar ]
%      regPflatTol- tolerance for the stabilization of
%                   successive regularization parameters
%                   (to be used as stopping criterion)
%                   [ {0.9} | non-negative scalar ]
%      GCVflatTol - tolerance for detecting flatness (stabilization)
%                   in the GCV function as a stopping criterion
%                   [ {10^-6} | non-negative scalar ]
%      GCVminTol  - window of iterations - if the GCV minimum continues
%                   to increase over this window, then the iterations are
%                   stopped:
%                   [ {3} | positive integer ]
%      GCVweight  - weight to be used if RegParam is 'wgcv'
%                   [ 'adapt' | non-negative scalar ]
%      NoiseLevel - norm of noise in rhs divided by norm of rhs (must be
%                   assigned if RegParam is 'discrep')
%                   [ {'none'} | nonnegative scalar ]
%      eta        - safety factor for the discrepancy principle
%                   [ {1.01} | scalar greater than (and close to) 1 ]
%      RegParam0  - regularization parameter used in the first  projected
%                   problem (needed if RegParam is 'discrep')
%                   [ {1} | positive scalar ]
%      MaxIter    - maximum number of iterations
%                   [ {'none'} | positive integer ]
%      DecompOut  - returns the Golub-Kahan decomposition to the user
%                   [ 'on' | {'off'} ]
%      IterBar    - shows the progress of the iterations
%                   [ {'on'} | 'off' ]
%      NoStop     - specifies whether the iterations should proceed
%                   after a stopping criterion is satisfied
%                   [ 'on' | {'off'} ]
%   SparsityTrans - sparsity transform for the solution
%                   [ {'none'} | 'dwt' ]
%      wname      - discrete wavelet transform name (meaningful if
%                   SpartistyTrans is 'dwt')
%                   [ {'db1'} ]
%      wlevels    - discrete wavelet transform level (meaningful if
%                   SpartistyTrans is 'dwt')
%                   [ {2} | positive integer]
%   hybridvariant - kind of hybrid method to be implemented
%                   [ {'I'} | 'R' ]
%            tolX - tolerance for the weights (the modulus of the weights
%                   cannot be below this threshold
%                   [ {10^-10} | non-negative scalar ]
% Note: the options structure can be created using the function IRset.
%
% Outputs:
%   X : computed solutions, stored column-wise (at the iterations listed in K)
%   info: structure with the following fields:
%      its      - number of the last computed iteration
%      saved_iterations - iteration numbers of iterates stored in X
%      StopFlag - string that describes the output/stopping condition:
%                   * Flat GCV curve
%                   * Minimum of GCV function (within window of MinTol its)
%                   * Performed max number of iterations
%                   * Discrepancy principle satisfied
%                   * Breakdown of the Golub-Kahan bidiagonalization algorithm
%      StopReg  - structure with the following fields:
%                   * X: solution satisfying the stopping criterion
%                   * It: iteration satisfying the stopping criterion
%                   * RegP: regularization parameter at the iteration satisfying
%                     the stopping crierion
%                   * Xnrm: norm of the solution satisfying satisfying the
%                     stopping criterion
%                   * Rnrm: relative residual norm at the iteration
%                     satisfying the stopping criterion
%                   * Enrm: relative error norm at the iteration
%                     satisfying the stopping criterion (requires x_true)
%      Xnrm     - solution norms at each iteration
%      Rnrm     - relative residual norms at each iteration
%      Enrm     - relative error norms at each iteration (requires x_true)
%      RegP     - sequence of the regularization parameters
%      GCValues - GCV function evaluated at minimum point at each
%                 iteration (if RegParam is 'gcv', 'wgcv', 'modgcv')
%      V        - Golub-Kahan bidiagonalization basis vectors for the solution
%      U        - Golub-Kahan bidiagonalization basis vectors
%      B        - lower bidiagonal matrix computed by Golub-Kahan bidiagonalization
%
% See also: IRcgls, IRhybrid_fgmres, IRhybrid_gmres, IRhybrid_flsqr, IRget, IRset

% Julianne Chung, Virginia Tech
% Silvia Gazzola, University of Bath
% June, 2018.

% Initialization
defaultopt = struct('x0', 'none', 'MaxIter', 100 ,...
    'x_true', 'none', 'NoStop','off', 'IterBar', 'on',...
    'RegParam','wgcv',...
    'SparsityTrans', 'none', 'wname', 'db1', 'wlevels', 2,...
    'qnorm', 1, 'weight0', 'none',...
    'hybridvariant', 'I', 'tolX', 10^-10,...
    'GCVweight', 'adapt',...
    'GCVflatTol', 10^-6, 'GCVminTol', 3,...
    'stopGCV', 'GCVvalues', 'resflatTol', 0.05, 'regPflatTol', 0.9,...
    'NoiseLevel', 'none', 'eta', 1.01, 'RegParam0', 1, 'DecompOut', 'off');

if nargin == 0
    error('Not enough input arguments')
elseif nargin == 1
    % If input is 'defaults,' return the default options in X
    if nargout <= 1 && isequal(A,'defaults')
        X = defaultopt;
        return;
    else
        error('Not enough input arguments')
    end
end

defaultopt.restart = 'off';
defaultopt.verbosity = 'on';

% Check for acceptable number of optional input arguments
switch length(varargin)
    case 0
        K = []; options = [];
    case 1
        if isa(varargin{1}, 'double')
            K = varargin{1}; options = [];
        else
            % no matter the order of appearance
            K = []; options = varargin{1};
        end
    case 2
        if isa(varargin{1}, 'double')
            K = varargin{1}; options = varargin{2};
        else
            % again, no matter the order of appearance
            K = varargin{2}; options = varargin{1};
        end
        if isfield(options, 'MaxIter') && ~isempty(options.MaxIter) && (~isempty(K) && options.MaxIter ~= max(K))
            warning('The value of MaxIter is discarded; the maximum value in K is taken as MaxIter')
        end
    otherwise
        error('Too many input parameters')
end

if isempty(options)
    options = defaultopt;
end

options = IRset(defaultopt, options);

MaxIter    = IRget(options, 'MaxIter',    [], 'fast');
RegParam   = IRget(options, 'RegParam',   [], 'fast');
x_true     = IRget(options, 'x_true',     [], 'fast');
NoStop     = IRget(options, 'NoStop',     [], 'fast');
IterBar    = IRget(options, 'IterBar',    [], 'fast');
omega      = IRget(options, 'GCVweight',  [], 'fast');
stopGCV    = IRget(options, 'stopGCV',    [], 'fast');
resdegflat = IRget(options, 'resflatTol', [], 'fast');
degflat    = IRget(options, 'GCVflatTol', [], 'fast');
regPflat   = IRget(options, 'regPflatTol', [], 'fast');
mintol     = IRget(options, 'GCVminTol',  [], 'fast');
NoiseLevel = IRget(options, 'NoiseLevel', [], 'fast');
eta        = IRget(options, 'eta',        [], 'fast');
RegParamk  = IRget(options, 'RegParam0',  [], 'fast');
restart    = IRget(options, 'restart',    [], 'fast');
verbose    = IRget(options, 'verbosity',  [], 'fast');
DecompOut  = IRget(options, 'DecompOut', [], 'fast');
tolX       = IRget(options, 'tolX',      [], 'fast');
q          = IRget(options, 'qnorm', [], 'fast');
SparsityTrans = IRget(options, 'SparsityTrans', [], 'fast');
hybridvariant = IRget(options, 'hybridvariant', [], 'fast');

verbose = strcmp(verbose, 'on');

adaptWGCV = strcmp(RegParam, {'wgcv'}) && strcmp(omega, {'adapt'});

% setting K
if isempty(K)
    K = MaxIter;
end
% sorting the iterations (in case they are shuffled in input)
K = K(:); K = sort(K,'ascend'); K = unique(K);
if ~((isreal(K) && (all(K > 0)) && all(K == floor(K))))
    error('K must be a vector of positive real integers')
end
if K(end) ~= MaxIter
    MaxIter = K(end);
end
% note that there is no control on K, as it does not go through IRset

if (strcmp(RegParam,'discrep') || strcmp(RegParam,'discrepit')) && ischar(NoiseLevel)
    error('The noise level (NoiseLevel) must be assigned')
end

if strcmp(RegParam,'optimal') && ischar(x_true)
    error('The true solution (x_true) must be assigned')
end

StopIt = MaxIter;

restart = strcmp(restart, 'on');
if restart
    ktotcount  = IRget(options, 'ktotcount', [],'fast');
    TotIterMax = IRget(options, 'TotIterMax',[],'fast');
    if strcmp(TotIterMax, 'none') || TotIterMax < MaxIter
        TotIterMax = MaxIter;
    end
    if strcmp(ktotcount, 'none')
        error('the total iteration counter must be assigned')
    end
    Ktot = IRget(options, 'Ktot', [], 'fast');
    % no checks on Ktot, it should be given from IRrestart
end

d = Atransp_times_vec(A, b);
n = length(d);
m = length(b);

if strcmp(SparsityTrans, 'none')
    Trans = speye(n);
elseif strcmp(SparsityTrans, 'dwt')
    wname   = IRget(options, 'wname',   [], 'fast');
    wlevels = IRget(options, 'wlevels', [], 'fast');
    if wlevels > 1/2*(log2(n))
        error('The assigned wavelet levels are too high. Make sure that wlevels <= 1/2*(log2(n))')
    end
    Trans = FreqMatrix('dwt', [sqrt(n) sqrt(n)], wname, wlevels);
end

% setting x0

x0 = IRget(options, 'x0', [], 'fast');

if strcmp(x0, 'none')
    x0 = zeros(n,1);
    r = b(:);
    precX = ones(n,1);
else
    try
        x0 = Trans*x0;
    catch
        error('Check the length of x0')
    end
    if max(abs(x0(:))) == 0
        r = b;
        x0 = zeros(n,1);
        precX = ones(n,1);
    else
        x0 = Trans'*x0;
        Ax0 = A_times_vec(A, x0);
        r = b(:) - Ax0;
        precX = x0;
        precX(abs(precX) < tolX) = eps;
        precX = (precX).^((2-q)/2);
    end
end

x = x0; % useful in case we have an immediate breakdown of the algorithm
beta = norm(r(:));
nrmb = norm(b(:));

% means no true solution
notrue = strcmp(x_true,'none');
% means we do not want to stop when the stopping criterion is satisfied
NoStop = strcmp(NoStop,'on');

% assessing if we want inner Tikhonov regularization
if strcmp(RegParam,'off')
    RegParam = 0;
end

Rfactor = strcmpi(hybridvariant, 'R');

% Declare matrices.
X                = zeros(n,length(K));
Xnrm             = zeros(max(K),1);
Rnrm             = zeros(max(K),1);
RegParamVect     = zeros(max(K),1);
M = zeros(max(K)+1,max(K));
T = zeros(max(K)+1);
Z = zeros(n, max(K));
V = zeros(n, max(K)+1);
U = zeros(m, max(K)+1);
rhs              = zeros(max(K)+1,1); % projected right-hand side
if restart
    saved_iterations = zeros(1, length(Ktot));
else
    saved_iterations = zeros(1, length(K));
end
GCV   = zeros(max(K),1);
Omega = zeros(max(K), 1);
warningGCV          = 0;
if notrue
    errornorms = false;
else
    errornorms = true;
    Enrm       = zeros(max(K),1);
    nrmtrue = norm(x_true(:));
    BestReg.RegP = [];
    BestReg.It = [];
    BestReg.X =[];
    BestReg.Enrm = [];
    BestReg.Xnrm = [];
    BestReg.Rnrm = [];
    BestEnrm = 1e10;
end
% Main Code Begins Here
u = r;
U(:,1) = u/beta; % no matter the preconditioning (because it's on the right)
rhs(1) = beta;
% Iterate
noIterBar = strcmp(IterBar,{'off'});
if ~noIterBar
  h_wait = waitbar(0, 'Running iterations, please wait ...');
end
j = 0;
for k=1:MaxIter
    if ~noIterBar
        waitbar(k/MaxIter, h_wait)
    end
    if restart, ktotcount = ktotcount + 1; end
    v = Atransp_times_vec(A, U(:,k)); v = v(:);
    v = Trans*v;
    for i = 1:k-1
        T(i,k)=V(:,i)'*v;
        v = v - T(i,k)*V(:,i);
    end
    T(k,k) = norm(v);
    v = v / T(k,k);
    %
    z = precX.*v;
    u = Trans'*z;
    u = A_times_vec(A, u); u = u(:);  %%%%
    for i = 1:k
        M(i,k) = U(:,i)'*u;
        u = u - M(i,k)*U(:,i);
    end
    M(k+1,k) = norm(u);
    u = u / M(k+1,k);
    U(:,k+1) = u;
    V(:,k) = v;
    Z(:,k) = z;

    rhsk = rhs(1:k+1); % current projected rhs

    if abs(T(k,k)) <= eps || abs(M(k+1,k)) <= eps
        if verbose
            disp('Flexible Golub-Kahan algorithm breaks down')
        end
        M = M(1:k+1,1:k);
        T = T(1:k,1:k);
        Z = Z(:,1:k);
        V = V(:,1:k);
        U = U(:,1:k+1);
        X(:,j+1) = x;
        X = X(:,1:j+1);
        if restart
            saved_iterations(j+1) = ktotcount-1;
        else
            saved_iterations(j+1) = k-1;
        end
        saved_iterations = saved_iterations(1:j+1);
        if k>1
            Xnrm    = Xnrm(1:k-1);
            Rnrm    = Rnrm(1:k-1);
            RegParamVect    = RegParamVect(1:k-1);
            if errornorms, Enrm = Enrm(1:k-1); end
        end
        % stop because the bidiagonal matrix is (numerically) singular
        % No chioce: even if NoStop is 'on'...we simpy cannot compute the solution, anymore
        if StopIt == MaxIter
            StopFlag = 'Breakdown of the Golub-Kahan algorithm';
            StopReg.X = x;
            StopReg.It = k-1;
            StopReg.RegP = RegParamk;
            StopReg.Xnrm = Xnrm(k-1);
            StopReg.Rnrm = Rnrm(k-1);
            if errornorms, StopReg.Enrm = Enrm(k-1); end
        end
        break
    end
    Mk = M(1:k+1,1:k);
    [Uk, Sk, Vk] = svd(Mk);
    if k==1
        Sk = Sk(1,1);
    else
        Sk = diag(Sk);
    end
    rhskhat = Uk'*rhsk;
    flsqr_res = abs(rhskhat(k+1))/nrmb;

    if Rfactor
        % update the Householder-QR factorization of Lk
        if k == 1
            [ZUk, ZRk] = householderQR(Z(:,1:k));
        else
            [ZUk, ZRk] = upd_householderQR(Z(:,1:k-1),...
            Z(:,k), ZUk, ZRk);
        end
        ZRksq = ZRk(1:k,1:k);
        [Uk, Vk, ~, Ck, Sk] = gsvd(Mk, ZRksq);
        rhskhat = Uk'*rhsk;
        if k==1
            gammak = Ck(1)/Sk(1);
        else
            gammak = sqrt(diag(Ck'*Ck)./diag(Sk'*Sk));
        end
    else
        ZRksq = eye(k);
    end

    % if tik
        if isscalar(RegParam)
            RegParamk = RegParam;
            RegParamVect(k) = RegParamk;
        elseif strcmp(RegParam,'discrep')
            if k==1
                RegParamVect(k) = RegParamk;
            end
        elseif strcmp(RegParam, 'discrepit')
            if flsqr_res > eta*NoiseLevel
                RegParamk = 0;
                RegParamVect(k) = RegParamk;
            else
                RegParamk = fzero(@(l)discrfcn(l, Mk, ZRksq, rhsk, nrmb, eta*NoiseLevel), [0, 1e10]);
                RegParamVect(k) = RegParamk;
            end
        elseif strcmp(RegParam, 'optimal')
            optfun = @(l)TikOptParam(l, Mk, ZRksq, rhsk, x0, Z(:,1:k), Trans, x_true);
            RegParamk = fmincon(optfun,0,[],[],[],[],0,.1);
            RegParamVect(k) = RegParamk;
        elseif strcmp(RegParam,'wgcv')
            if k>1
            if ~Rfactor
                if adaptWGCV
                    %Use the adaptive, weighted GCV method
                    Omega(k) = min(1, findomega(rhskhat, Sk));
                    omega = mean(Omega(1:k));
                end
                RegParamk = fminbnd('TikGCV', 0, Sk(1), [], rhskhat, Sk, omega);
                GCValk = GCVstopfun(RegParamk, Uk(1,:)', Sk, nrmb, m, n);
            else
                if adaptWGCV
                    %Use the adaptive, weighted GCV method
                    Omega(k) = min(1, findomega(rhskhat, gammak));
                    omega = mean(Omega(1:k));
                end
                RegParamk = fminbnd('TikGCV', 0, gammak(k), [], rhskhat, gammak, omega);
                GCValk = GCVstopfun(RegParamk, Uk(1,:)', gammak, nrmb, m, n);
            end
            RegParamVect(k) = RegParamk;
            GCV(k) = GCValk;
            else
            RegParamk = 0; GCValk = 0;
            RegParamVect(k) = RegParamk; GCV(k) = GCValk;
            end
        elseif strcmp(RegParam,'gcv')
            if ~Rfactor
                RegParamk = fminbnd('TikGCV', 0, Sk(1), [], rhskhat, Sk);
                GCValk = GCVstopfun(RegParamk, Uk(1,:)', Sk, nrmb, m, n);
            else
                RegParamk = fminbnd('TikGCV', 0, gammak(k), [], rhskhat, gammak);
                GCValk = GCVstopfun(RegParamk, Uk(1,:)', gammak, nrmb, m, n);
            end
            RegParamVect(k) = RegParamk;
            GCV(k) = GCValk;
        elseif strcmp(RegParam,'modgcv')
            if ~Rfactor
                RegParamk = fminbnd('TikGCV', 0, Sk(1), [], rhskhat, Sk, m);
                GCValk = GCVstopfun(RegParamk, Uk(1,:)', Sk, nrmb, m, n);
            else
                RegParamk = fminbnd('TikGCV', 0, gammak(k), [], rhskhat, gammak, m);
                GCValk = GCVstopfun(RegParamk, Uk(1,:)', gammak, nrmb, m, n);
            end
            RegParamVect(k) = RegParamk;
            GCV(k) = GCValk;
        else
            error('Invalid parameter choice method')
        end
        if ~Rfactor
            Dk = Sk.^2 + RegParamk^2;
            % Dk = Sk.^2 + RegParamk;
            rhskhat = Sk .* rhskhat(1:k);
            yhat = rhskhat(1:k)./Dk;
            y = Vk * yhat;
%             MZk = [Mk; RegParamk*eye(k)];
%             rhsZk = [rhsk; zeros(k,1)];
%             y = MZk\rhsZk;
        else
            MZk = [Mk; RegParamk*ZRksq(1:k,:)];
            % MZk = [Mk; sqrt(RegParamk)*ZRksq(1:k,:)];
            rhsZk = [rhsk; zeros(k,1)];
            y = MZk\rhsZk;
        end
        Rnrm(k) = norm(rhsk - Mk*y)/nrmb;
        d = Z(:,1:k)*y;
        x = x0 + d;
        precX = abs(x);
        precX(precX < tolX) = eps;
        precX = precX.^((2-q)/2);
        x = Trans'*x;
        % Compute norms
        Xnrm(k) = norm(x(:));
        if errornorms
            Enrm(k) = norm(x_true(:) - x(:))/nrmtrue;
            if Enrm(k)<BestEnrm
                BestReg.RegP = RegParamk;
                BestReg.It = k;
                BestReg.X = x;
                BestEnrm = Enrm(k);
                BestReg.Enrm = BestEnrm;
                BestReg.Xnrm = Xnrm(k);
                BestReg.Rnrm = Rnrm(k);
            end
        end
        AlreadySaved = 0;
        if any(k==K)
            j = j+1;
            X(:,j) = x;
            saved_iterations(j) = k;
            % this is used to save the last iteration, in the case
            % K = MaxIterIn
            % (when performing restarts, and the inner stopping criterion is not satisfied)
            if restart, saved_iterations(j) = ktotcount; end
            AlreadySaved = 1;
        end
        if restart
            if any(ktotcount == Ktot) && ~ AlreadySaved
                j = j+1;
                X(:,j) = x;
                saved_iterations(j) = ktotcount;
                AlreadySaved = 1;
            end
            if ktotcount == TotIterMax
                if ~ AlreadySaved
                    j = j+1;
                    saved_iterations(j) = ktotcount;
                    X(:,j) = x;
                end
                StopIt = k;
                StopReg.X = x;
                StopReg.It = k;
                StopReg.RegP = RegParamk;
                StopReg.Xnrm = Xnrm(k);
                StopReg.Rnrm = Rnrm(k);
                if errornorms
                    Enrm = Enrm(1:k);
                    StopReg.Enrm = Enrm(k);
                end
                Xnrm    = Xnrm(1:k);
                Rnrm    = Rnrm(1:k);
                RegParamVect    = RegParamVect(1:k);
                M = M(1:k+1,1:k);
                T = T(1:k,1:k);
                Z = Z(:,1:k);
                V = V(:,1:k);
                U = U(:,1:k+1);
                X = X(:,1:j);
                saved_iterations = saved_iterations(1:j);
                if verbose
                    disp('reached maximum number of iterations')
                end
                StopFlag = 'reached maximum number of iterations';
                break
            end
        end
        % update parameters, check stopping criteria
        if isscalar(RegParam)
        % Purely iterative method case.
        if strcmp(NoiseLevel, 'none')
            if k>1
            if abs((Rnrm(k)-Rnrm(k-1)))/Rnrm(k-1) < resdegflat && ...
                Rnrm(k) == min(Rnrm(1:k)) && StopIt == MaxIter
                if verbose
                    disp('The stopping criterion for flsqr is satisfied')
                end
                % Stop because the residual stabilizes.
                StopFlag = 'The residual norm stabilizes';
                if ~AlreadySaved && ~NoStop
                    j = j+1;
                    X(:,j) = x;
                    if restart
                        saved_iterations(j) = ktotcount;
                    else
                        saved_iterations(j) = k;
                    end
                    AlreadySaved = 1;
                end
                StopIt = k;
                StopReg.RegP = RegParamk;
                StopReg.It = k;
                StopReg.X = x;
                if errornorms, StopReg.Enrm = Enrm(k); end
                if ~ NoStop
                    Xnrm    = Xnrm(1:k);
                    Rnrm    = Rnrm(1:k);
                    RegParamVect = RegParamVect(1:k);
                    M = M(1:k+1,1:k);
                    T = T(1:k,1:k);
                    Z = Z(:,1:k);
                    V = V(:,1:k);
                    U = U(:,1:k+1);
                    if errornorms, Enrm = Enrm(1:k); end
                    X = X(:,1:j);
                    saved_iterations = saved_iterations(1:j);
                    break
                end
            end
            end
        else
            if Rnrm(k) < eta*NoiseLevel
            % Stopping criterion.
            if StopIt == MaxIter
                if verbose
                    disp('The discrepancy principle is satisfied')
                end
                StopFlag = 'The discrepancy principle satisfied';
                if ~AlreadySaved && ~NoStop
                    j = j+1;
                    X(:,j) = x;
                    if restart
                        saved_iterations(j) = ktotcount;
                    else
                        saved_iterations(j) = k;
                    end
                    AlreadySaved = 1;
                end
                StopIt = k;
                StopReg.RegP = RegParamk;
                StopReg.It = k;
                StopReg.X = x;
                if errornorms, StopReg.Enrm = Enrm(k); end
                if ~ NoStop
                    Xnrm    = Xnrm(1:k);
                    Rnrm    = Rnrm(1:k);
                    RegParamVect    = RegParamVect(1:k);
                    M = M(1:k+1,1:k);
                    T = T(1:k,1:k);
                    Z = Z(:,1:k);
                    V = V(:,1:k);
                    U = U(:,1:k+1);
                    if errornorms, Enrm = Enrm(1:k); end
                    X = X(:,1:j);
                    saved_iterations = saved_iterations(1:j);
                    % Stop because the discrepancy principle is satisfied.
                    break
                end
            end
            end
        end
        elseif strcmp(RegParam,'discrep')
            if Rnrm(k) < eta*NoiseLevel
                % stopping criterion
                if StopIt == MaxIter % the method has not stopped, yet
                    if verbose
                        disp('The discrepancy principle is satisfied')
                    end
                    StopFlag = 'discrepancy principle (secant update method) satisfied';
                    if ~AlreadySaved && ~NoStop
                        j = j+1;
                        X(:,j) = x;
                        if restart
                            saved_iterations(j) = ktotcount;
                        else
                            saved_iterations(j) = k;
                        end
                        AlreadySaved = 1;
                    end
                    StopIt = k;
                    StopReg.X = x;
                    StopReg.It = k;
                    StopReg.RegP = RegParamk;
                    StopReg.Xnrm = Xnrm(k);
                    StopReg.Rnrm = Rnrm(k);
                    if errornorms, StopReg.Enrm = Enrm(k); end
                    if ~ NoStop
                        Xnrm    = Xnrm(1:k);
                        Rnrm    = Rnrm(1:k);
                        RegParamVect    = RegParamVect(1:k);
                        M = M(1:k+1,1:k);
                        T = T(1:k,1:k);
                        Z = Z(:,1:k);
                        V = V(:,1:k);
                        U = U(:,1:k+1);
                        if errornorms, Enrm = Enrm(1:k); end
                        X = X(:,1:j);
                        saved_iterations = saved_iterations(1:j);
                        % stop because the discrepancy principle is satisfied
                        break
                    else
                        RegParamk = abs((eta*NoiseLevel - flsqr_res)/(Rnrm(k) - flsqr_res))*(RegParamk^2);
                        % RegParamk = RegParamk^2;
                        % RegParamk = abs((eta*NoiseLevel - flsqr_res)/(Rnrm(k) - flsqr_res))^2*RegParamk;
                        % RegParamk = abs((eta*NoiseLevel - flsqr_res)/(Rnrm(k) - flsqr_res))*RegParamk;
                        RegParamk = sqrt(RegParamk);
                        if k~=MaxIter, RegParamVect(k+1) = RegParamk; end
                    end
                else
                    RegParamk = abs((eta*NoiseLevel - flsqr_res)/(Rnrm(k) - flsqr_res))*(RegParamk^2);
                    % RegParamk = RegParamk^2;
                    % RegParamk = abs((eta*NoiseLevel - flsqr_res)/(Rnrm(k) - flsqr_res))*RegParamk;
                    RegParamk = sqrt(RegParamk);
                    if k~=MaxIter, RegParamVect(k+1) = RegParamk; end
                end
            else
                RegParamk = abs((eta*NoiseLevel - flsqr_res)/(Rnrm(k) - flsqr_res))*(RegParamk^2);
                % RegParamk = RegParamk^2;
                % RegParamk = abs((eta*NoiseLevel - flsqr_res)/(Rnrm(k) - flsqr_res))*RegParamk;
                RegParamk = sqrt(RegParamk);
                if k~=MaxIter, RegParamVect(k+1) = RegParamk; end
            end
        elseif strcmp(RegParam,'discrepit')
            if k>2
                % stopping criterion
                if StopIt == MaxIter % the method has not stopped, yet
                    if abs(RegParamVect(k)-RegParamVect(k-1))/RegParamVect(k-1) < regPflat && abs(RegParamVect(k-1)-RegParamVect(k-2))/RegParamVect(k-2)<regPflat
                        if verbose
                            disp('The stopping criterion for the discrepancy principle is satisfied')
                        end
                        StopFlag = 'discrepancy principle (stopping criterion) satisfied';
                        if ~AlreadySaved && ~NoStop
                            j = j+1;
                            X(:,j) = x;
                            if restart
                                saved_iterations(j) = ktotcount;
                            else
                                saved_iterations(j) = k;
                            end
                            AlreadySaved = 1;
                        end
                        StopIt = k;
                        StopReg.X = x;
                        StopReg.It = k;
                        StopReg.RegP = RegParamk;
                        StopReg.Xnrm = Xnrm(k);
                        StopReg.Rnrm = Rnrm(k);
                        if errornorms, StopReg.Enrm = Enrm(k); end
                        if ~ NoStop
                            Xnrm    = Xnrm(1:k);
                            Rnrm    = Rnrm(1:k);
                            RegParamVect    = RegParamVect(1:k);
                            M = M(1:k+1,1:k);
                            T = T(1:k,1:k);
                            Z = Z(:,1:k);
                            V = V(:,1:k);
                            U = U(:,1:k+1);
                            if errornorms, Enrm = Enrm(1:k); end
                            X = X(:,1:j);
                            saved_iterations = saved_iterations(1:j);
                            % stop because the discrepancy principle is satisfied
                            break
                        end
                    end
                end
            end
        elseif strcmp(RegParam,'wgcv') || strcmp(RegParam,'gcv') || strcmp(RegParam,'modgcv')
            % check the stopping criterion (all the possibilities)
            if k > 1
            if strcmpi(stopGCV, 'GCVvalues')
                if StopIt == MaxIter % the method has not stopped, yet
                if abs((GCV(k)-GCV(k-1)))/GCV(2) < degflat && StopIt == MaxIter
                % the method has not stopped, yet
                        if verbose
                            disp('The stopping criterion for GCV principle is satisfied')
                        end
                        % stop because the GCV curve is too flat
                        StopFlag = 'GCV curve too flat';
                        if ~AlreadySaved && ~ NoStop
                            j = j+1;
                            X(:,j) = x;
                            if restart
                                saved_iterations(j) = ktotcount;
                            else
                                saved_iterations(j) = k;
                            end
                            AlreadySaved = 1;
                        end
                        StopIt = k;
                        StopReg.X = x;
                        StopReg.It = k;
                        StopReg.RegP = RegParamk;
                        StopReg.Xnrm = Xnrm(k);
                        StopReg.Rnrm = Rnrm(k);
                        if errornorms, StopReg.Enrm = Enrm(k); end
                        if ~ NoStop
                            Xnrm    = Xnrm(1:k);
                            Rnrm    = Rnrm(1:k);
                            RegParamVect    = RegParamVect(1:k);
                            M = M(1:k+1,1:k);
                            T = T(1:k,1:k);
                            Z = Z(:,1:k);
                            V = V(:,1:k);
                            U = U(:,1:k+1);
                            if errornorms, Enrm = Enrm(1:k); end
                            X = X(:,1:j);
                            saved_iterations = saved_iterations(1:j);
                            % stop because the GCV stopping criterion is satisfied
                            break
                        end
                elseif GCV(k-1) < GCV(k) && ~ warningGCV && StopIt == MaxIter % Potential minimum reached.
                    warningGCV = 1;
                    % Save data just in case.
                    x_save = x;
                    k_save = k; % for computing the GCV stopping criterion
                    j_save = j;
                    AlreadySaved_save = AlreadySaved;
                    RegParamk_save = RegParamk;
                    if restart, ktotcount_save = ktotcount; end
                elseif warningGCV && k > min(k_save + mintol, MaxIter) && StopIt == MaxIter % Passed window
                    if GCV(k_save) < GCV(k_save+1:min(k_save + mintol, MaxIter))
                        if verbose
                            disp('The stopping criterion for GCV principle is satisfied')
                        end
                        StopFlag = 'increasing GCV minima';
                        StopIt = k_save;
                        StopReg.It = k_save;
                        StopReg.X = x_save;
                        StopReg.RegP = RegParamk_save;
                        StopReg.Xnrm = Xnrm(k_save);
                        StopReg.Rnrm = Rnrm(k_save);
                        if errornorms
                            StopReg.Enrm = Enrm(k_save);
                        end
                        if ~ NoStop
                            j = j_save;
                            saved_iterations = saved_iterations(1:j);
                            X = X(:,1:j);
                            if ~AlreadySaved_save
                                j = j+1;
                                X(:,j) = x_save;
                                if restart
                                    saved_iterations(j) = ktotcount_save;
                                else
                                    saved_iterations(j) = k_save;
                                end
                            end
                            Xnrm    = Xnrm(1:k_save);
                            Rnrm    = Rnrm(1:k_save);
                            RegParamVect    = RegParamVect(1:k_save);
                            M = M(1:k_save+1,1:k_save);
                            T = T(1:k_save,1:k_save);
                            Z = Z(:,1:k_save);
                            V = V(:,1:k_save);
                            U = U(:,1:k_save+1);
                            if errornorms
                                Enrm = Enrm(1:k_save);
                                if BestReg.It > k_save
                                    [BestReg.Enrm, BestReg.It] = min(Enrm);
                                    BestReg.RegP = RegParamVect(BestReg.It);
                                    BestReg.Xnrm = Xnrm(BestReg.It);
                                    BestReg.Rnrm = Rnrm(BestReg.It);
                                    % recompute the best solution again
                                    ktemp = BestReg.It;
                                    Mtemp = M(1:ktemp+1,1:ktemp);
                                    Ztemp = Z(:,1:ktemp);
                                    rhsk = rhs(1:ktemp+1);
                                    [Uk, Sk, Vk] = svd(Mtemp);
                                    if ktemp==1
                                        Sk = Sk(1,1);
                                    else
                                        Sk = diag(Sk);
                                    end
                                    rhskhat = Uk'*rhsk;
                                    if Rfactor
                                        [~, ZRksq] = qr(Ztemp,0);
                                    end
                                    if ~Rfactor
                                        Dk = Sk.^2 + RegParamk^2;
                                        rhskhat = Sk .* rhskhat(1:ktemp);
                                        yhat = rhskhat(1:ktemp)./Dk;
                                        y = Vk * yhat;
                                    else
                                        MLk = [Mtemp; RegParamk*ZRksq];
                                        rhsLk = [rhsk; zeros(ktemp,1)];
                                        y = MLk\rhsLk;
                                    end
                                    dtemp = Ztemp*y;
%                                     if precond, dtemp = P_solve(L, dtemp); end
                                    xtemp = x0 + dtemp;
                                    BestReg.X = xtemp;
                                end
                            end
                            X = X(:,1:j);
                            saved_iterations = saved_iterations(1:j);
                            if restart, ktotcount = ktotcount_save; end
                            % stop because the GCV stopping criterion is satisfied
                            break
                        end
                    else
                        warningGCV = 0;
                    end
                end
            elseif strcmpi(stopGCV, 'resflat')
                if abs((Rnrm(k)-Rnrm(k-1)))/Rnrm(k-1) < resdegflat && ...
                    Rnrm(k) == min(Rnrm(1:k)) && StopIt == MaxIter
                    if verbose
                        disp('The stopping criterion for GCV principle is satisfied')
                    end
                    % stop because discrepancy (i.e., residual for the
                    % regularized problem) stabilizes
                    StopFlag = 'the residual norm stabilizes';
                    if ~AlreadySaved && ~NoStop
                        j = j+1;
                        X(:,j) = x;
                        if restart
                            saved_iterations(j) = ktotcount;
                        else
                            saved_iterations(j) = k;
                        end
                        AlreadySaved = 1;
                    end
                    StopIt = k;
                    StopReg.X = x;
                    StopReg.It = k;
                    StopReg.RegP = RegParamk;
                    StopReg.Xnrm = Xnrm(k);
                    StopReg.Rnrm = Rnrm(k);
                    if errornorms, StopReg.Enrm = Enrm(k); end
                    if ~ NoStop
                        Xnrm    = Xnrm(1:k);
                        Rnrm    = Rnrm(1:k);
                        RegParamVect    = RegParamVect(1:k);
                        M = M(1:k+1,1:k);
                        T = T(1:k,1:k);
                        Z = Z(:,1:k);
                        V = V(:,1:k);
                        U = U(:,1:k+1);
                        if errornorms, Enrm = Enrm(1:k); end
                        X = X(:,1:j);
                        saved_iterations = saved_iterations(1:j);
                        break
                    end
                end
                end
            end
            end
        end
end
if k == MaxIter
    if StopIt == MaxIter
        % Stop because max number of iterations reached
        if verbose
            disp('Reached maximum number of iterations')
        end
        StopFlag = 'reached maximum number of iterations';
        if ~AlreadySaved
            j = j+1;
            X(:,j) = x;
            if restart
                saved_iterations(j) = ktotcount;
            else
                saved_iterations(j) = k;
            end
        end
        StopReg.X = x;
        StopReg.It = k;
        StopReg.RegP = RegParamk;
        StopReg.Xnrm = Xnrm(k);
        StopReg.Rnrm = Rnrm(k);
        if errornorms, StopReg.Enrm = Enrm(k); end
        Xnrm    = Xnrm(1:k);
        Rnrm    = Rnrm(1:k);
        RegParamVect    = RegParamVect(1:k);
        M = M(1:k+1,1:k);
        T = T(1:k,1:k);
        Z = Z(:,1:k);
        V = V(:,1:k);
        U = U(:,1:k+1);
        if errornorms, Enrm = Enrm(1:k); end
        X = X(:,1:j);
        saved_iterations = saved_iterations(1:j);
    end
end
if ~noIterBar, close(h_wait), end
if nargout==2
  if NoStop
      info.its = k;
  else
      info.its = StopIt;
  end
  info.saved_iterations = saved_iterations(1:j);
  info.StopReg = StopReg;
  info.StopFlag = StopFlag;
  if errornorms
    info.Enrm = Enrm;
    info.BestReg = BestReg;
  end
  info.Xnrm = Xnrm;
  info.Rnrm = Rnrm;
  info.RegP = RegParamVect;
  if strcmp(RegParam,'wgcv') || strcmp(RegParam,'gcv') || strcmp(RegParam,'modgcv')
    info.GCValues = GCV(1:k);
  end
  if strcmp(DecompOut,'on')
      info.V = V(:,1:k);
      info.U = U(:,1:k+1);
      info.Z = Z(:,1:k);
      info.T = T(1:k,1:k);
      info.M = M(1:k+1,1:k);
  end
  if restart
      info.ktotcount = ktotcount;
  end
end

%% ---------------SUBFUNCTIONS ---------------------------------------
function [U,R] = householderQR(L)
%
%  [U,R] = householderQR(L)
%  This function computes the Householder-QR factorization of L
%  (a "projected" regularization matrix), that will be used to define a
%  "projected" regularization matrix R to employ within the LSQR iterates.
%

[m, n] = size(L);
R = L;
U = zeros(m, n);
for k = 1:n
    x = L(k:m,k);
    e = zeros(length(x),1); e(1) = 1;
    u = sign(x(1))*norm(x(:))*e + x;
    u = u./norm(u(:));
    R(k:m, k:n) = R(k:m, k:n) -2*u*(u'*R(k:m, k:n));
    U(k:m,k) = u;
end

function [U,R] = upd_householderQR(L,ll,U,R)
%
% [U,R] = upd_householderQR(L, ll, U, R)
% This function updates the Householder-QR factorization of [L, ll].
%
% Input:
%   L  - matrix whose QR factorization is defined by U and R
%   ll - column appended to L, i.e., [L, ll]
%    U - matrix defining the orthogonal matrix Q, such that L = QR
%    R - upper triangular factor of L = QR
%

[m,n] = size(L);
Unew = zeros(m, n+1);
Unew(:,1:n) = U;
w = ll;
for i = 1:n
    u = U(i:m,i);
    w(i:m) = w(i:m) - 2*u*(u'*w(i:m));
end
v = w(1:n); x = w(n+1:m);
e = zeros(length(x),1); e(1) = 1;
u = sign(x(1))*norm(x(:))*e + x;
u = u./norm(u(:));
x = x -2*u*(u'*x);
Unew(n+1:m,n+1)=u;
U = Unew;
rr = [v; x];
R = [R, rr];

function omega = findomega(bhat, s)
%
%   omega = findomega(bhat, s)
%
%  This function computes a value for the omega parameter used in wGCV.
%
%  The method: Assume the 'optimal' regularization parameter to be the
%  smallest singular value.  Then we take the derivative of the GCV
%  function with respect to alpha, evaluate it at alpha_opt, set the
%  derivative equal to zero and then solve for omega.
%
%  Input:   bhat -  vector U'*b, where U = left singular vectors
%              s -  vector containing the singular values
%
%  Output:     omega - computed value for the omega parameter.

%
%   First assume the 'optimal' regularization parameter to be the smallest
%   singular value.
%

%
% Compute the needed elements for the function.
%
m = length(bhat);
n = length(s);

t0 = sum(abs(bhat(n+1:m)).^2);
alpha = s(end);
s2 = abs(s) .^ 2;
alpha2 = alpha^2;

tt = 1 ./ (s2 + alpha2);

t1 = sum(s2 .* tt);
t2 = abs(bhat(1:n).*alpha.*s) .^2;
t3 = sum(t2 .* abs((tt.^3)));

t4 = sum((s.*tt) .^2);
t5 = sum((abs(alpha2*bhat(1:n).*tt)).^2);

v1 = abs(bhat(1:n).*s).^2;
v2 = sum(v1.* abs((tt.^3)));

%
% Now compute omega.
%
omega = (m*alpha2*v2)/(t1*t3 + t4*(t5 + t0));


%%
% \documentclass[12pt]{article}
%
% \title{ODEbox: A Toolbox for Ordinary Differential Equations\\
% Example 1}
%
% \author{Matthew Harker and Paul O'Leary\\
% Institute for Automation\\
% University of Leoben\\
% A-8700 Leoben,
% Austria\\
% URL: automation.unileoben.ac.at\\
% \\
% Original: January 9, 2013\\
% $\copyright$ 2013\\
% \\
% Last Modified: \today}
%%
%\section{Initial Value Problem}
%
% This file demonstrated the solution of initial value problems using the
% DOPBox and ODEBox toolbaxes. The example being considered here is:
%
% \begin{equation}
%    \ddot{y} + 6\,\dot{y} + 9 y = 0
%    \hspace{5mm}
%    \text{with}
%    \hspace{5mm}
%    y(0) = 10
%   \hspace{5mm}
%   \text{and}
%   \hspace{5mm}
%   \dot{y}(0) = -75
% \end{equation}
%
% The analytical solution to this problem is:
%
% \begin{equation}
%   y(x) = 10 \, e^{-3\,x} - 45 \,x {e^{-3\,x}}.
% \end{equation}
%
% This example has been taken from~\cite{adams}.
%
% This M-file demonstrated the new approach to solving this problem and
% compared the result with a classical runga-kutta solution as provided by
% MATLAB.
%
%%
% \section{Prepare the workspace}
%
close all;
clear all;
setUpGraphics(12) ;
%%
% Define the equation and solution as a strings for documentation
%
Eq = '$$\ddot{y} - 6\,\dot{y} - 9 y = 0$$, with, $$y(0) = 10$$ and $$\dot{y}(1) = -75$$' ;
Sol = '$$y(x) = 10 \, e^{-3\,x} - 45 \,x {e^{-3\,x}}$$';
%
%------------------------------------------------------------------
%%
% \section{Define the Matrix Linear Differential Operator}
%
% Define the number of points used in the solution
%
noPts = 85 ;
%
% Define the interval in which the problem is to be solved
%
xMin = 0 ;
xMax = 3 ;
%
% Use a nonlinear node placement. This ensures a higher density of nodes
% where the solution has a highed derivative.
%
x = linspace(0,1,noPts)';
x = 5 * x.^2;
%
% Setup the differentiating matrix, with support length 13.
%
ls = 13;
D = dopDiffLocal( x, ls, ls );
%
% Define the matrix C of constraints, i.e. the initial conditions
%
C = zeros(noPts,2) ;
%
C(1,1) = 1 ;
C(:,2) = D(1,:)' ;
%
d = [ 10 ; -75]  ;
%
% Compute the linear differential operator.
%
L = D*D + 6*D + 9*eye(noPts) ;
%%
%\section{Compute the Solutions}
%
% Solve the algebraic system of equations as a least squares problem. This
% is the actual computation of the solution of the differential equation.
%
y = odeLSE( L, [] , C, d );
%%
% For comparison solve using the MATLAB ode45 method.
% Runge-Kutta Solution:
%
[xm,Y] = ode45(@eulerODE01,[xMin xMax],d');
%
ym = Y(:,1);
%%
% Compute the Analytic Solution as an inline to compare the results
%
f = inline('10*exp(-3*t) - 45*t.*exp(-3*t)') ;
%
ya = f(x);
%%
% \section{Plot the Results}
%
% for plot purposes solve beyond the ends
%
xp = linspace(xMin-1/4,xMax+1/4,1000) ;
yp = f(xp);
%
% Present the solutions
%
setUpGraphics(12)
FigureSize=[1 1 10 6];
set(0,'DefaultFigureUnits','centimeters');
set(0,'DefaultFigurePosition',FigureSize);
set(0,'DefaultFigurePaperUnits','centimeters');
set(0,'DefaultFigurePaperPosition',FigureSize);
MyAxesPosition=[0.13 0.15 0.86 0.82];
set(0,'DefaultaxesPosition',MyAxesPosition);
%
fig1 = figure;
plot( xp, yp, 'k' ) ;
hold on
plot( x, y, 'ko', 'MarkerFaceColor', 'w' ) ;
%
plot( xm, ym,'kv');
legend('Analytical','New','Runge-Kutta','Location','NorthEast');
xlabel('$$x$$');
ylabel('$$y$$');
grid on;
range = axis;
plot( range(1:2), [0,0],'k');
axis([-0.5,3.5,-5,15]);
plot( x, -4.5*ones(size(x)), 'k.');
%
%\caption{Comparison of the analytical solution, the new numberical
% method and the Runga-Kutta solution.}
%%
%
setUpGraphics(12)
FigureSize=[1 1 10 6];
set(0,'DefaultFigureUnits','centimeters');
set(0,'DefaultFigurePosition',FigureSize);
set(0,'DefaultFigurePaperUnits','centimeters');
set(0,'DefaultFigurePaperPosition',FigureSize);
MyAxesPosition=[0.13 0.13 0.86 0.82];
set(0,'DefaultaxesPosition',MyAxesPosition);
%
fig2 = figure;
subplot(2,1,1)
plot( x, y - ya, 'k', 'MarkerFaceColor', 'w' ) ;
hold on
plot( x, y - ya, 'k.', 'MarkerFaceColor', 'w' ) ;
range = axis;
plot( range(1:2), [0,0],'k');
ylabel('$$e_{L}$$');
grid on;
%title(Sol);
%
subplot(2,1,2)
plot( xm, ym - f(xm), 'k', 'MarkerFaceColor', 'w' ) ;
hold on
plot( xm, ym - f(xm), 'k.', 'MarkerFaceColor', 'w' ) ;
hold on;
range = axis;
plot( range(1:2), [0,0],'k');
xlabel('$$x$$');
ylabel('$$e_{RK}$$');
grid on;
%
% \caption{Comparison the residual error for the new method (top)  (Bottom)and
% for the Runga-Kutta method. Note the residual is orders of magnitude smaller for the new method.}
%
%%
%\section{Save the figures to disk.}
%
fileType = 'eps';
printFigure( fig1, 'ivpExp3Sol', fileType);
printFigure( fig2, 'ivpExp3Err', fileType);
%
%% Define the Bibliography
%
% \bibliographystyle{IEEETran}
% \bibliography{odebib}


function [price, opt, L] = PROJ_GMDB_DCA_Fast(proj_params, S_0, gmdb_params, r, q, modelInput)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% About: Pricing Function for DCA-Style Garuanteed Minimum Withdraw Benefit (GMWB) using PROJ method
%        This version is based on a dollar cost average style investment account (see reference below)
%
% Terminal Payoff:  Payoff(tau) = L*exp(g*tau) + (Gam(tau) - L*exp(g*tau))^+
%                      Gam(tau) = S_M * sum_{m=0}^M(alpha*gamma / S_m)
%                          tau  = time of death (discrete periods)
%                            M  = number of periods until time of death (each period length dt)
%
% Models Supported: Levy Processes, including jump diffusions and Black-Scholes model
% Returns: price of contract
%
% Author: Justin Lars Kirkby
% References: 1) Equity-Linked  Guaranteed Minimum Death Benefits with Dollar Cost Averaging, J.L.Kirkby & D.Nguyen, 2021
%
% ----------------------
% Contract/Model Params
% ----------------------
% S_0 = initial stock price (e.g. 100)
% r   = interest rate (e.g. 0.05)
% q   = dividend yield (e.g. 0.05)
% M   = number of subintervals of [0,T] (total of M+1 monitoring points in time grid, including S_0)
% gmdb_params = container of GMDB contract params, see below
% modelInput =  model inputs, see below
%
% ----------------------
% Numerical (PROJ) Params
% ----------------------
% proj_params = numerical params
%   proj_params.N = number of basis elements, e.g. N = 2^10
%   proj_params.L1 = gridwidth param, e.g. L1 = 8
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% ------------------
% GMDB Contract Params
% ------------------
L = gmdb_params.L;   % Guarantee Level: Set L = -1 to use ATMF value for L
alpha = gmdb_params.alpha;  % Period premium payment, paid every dt time units
gamma = gmdb_params.gamma;  % Proportion of investment retained by policyholder (fee is 1-gamma)
contract_type = gmdb_params.contract_type;  % Contract type: 1 = GMDB, 2 = GMDB-RS (Ratchet strike)
p = gmdb_params.death_prob;  % death probability distribution, must be consistent with dt
g = gmdb_params.g;

% ------------------
% Model Inputs
% ------------------
dt = modelInput.dt;  % Time increment, premiums paid / underlying S is monitored every dt
phiR = modelInput.rnCHF;  % Risk neutral CHF for time period dt

call = 1;
ER = 0;

Z = gen_func(-r, dt, p);
if g == 0
    Zrg = Z;
else
    Zrg = gen_func(-(r-g), dt, p);
end

if L == -1
    MF = gen_func(r - q - g, dt, p);
    Zg = gen_func(-g, dt, p);
    L = alpha * gamma * (exp((r-q)*dt)*MF - Zg) / (exp((r-q)*dt) - 1);
end

Mmax = length(p); Tmax = Mmax*dt;
pr_alpha = getTruncationAlpha(Tmax, proj_params.L1, modelInput, modelInput.model);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N = proj_params.N;
dx = 2*pr_alpha/(N-1); a = 1/dx;
A    = 32*a^4;
C_aN = A/N;

%%% SHIFTS
x1    = zeros(1,Mmax);

if contract_type == 1
    strikes = S_0*L*exp(g*dt*(1:Mmax)) ./ (alpha*gamma*(2:Mmax + 1));
else
    strikes = S_0*ones(1,Mmax);
end

for m=1:Mmax
    if m == 1
        x1(m) = ER;
    else
        x1(m) = ER + log(1+exp(x1(m-1)));  %%BENHAMOU SHIFT
    end
    %x1(m) = log(m) + .5*(m+1)*ER;    %% LOWER BOUND SHIFT derived in APROJ paper
end

Nm   = floor(a*(x1-ER));
x1   = ER + (1-N/2)*dx + Nm*dx;
NNM  = N + Nm(Mmax-1);   %Number of columns of PSI

%%% Now check that we wont fall off the grid later
for m=1:Mmax
    ystar = log((m+1)*strikes(m)/S_0 -1);
    nbar  = floor((ystar-x1(m))*a+1);
    % fprintf('%.0f\n', nbar);
    if nbar + 1 > N
        proj_params.L1 = proj_params.L1*1.25;
        [price, opt, L] = PROJ_GMDB_DCA_Fast(proj_params, S_0, gmdb_params, r, q, modelInput);
        return;
    end
end

dxi   = 2*pi*a/N;
xi    = dxi*(1:(N-1))';
PhiR = [1; phiR(xi)];

b0    = 1208/2520; b1 = 1191/2520; b2 = 120/2520; b3 = 1/2520;
zeta  = (sin(xi/(2*a))./xi).^4./(b0 + b1*cos(xi/a) +b2*cos(2*xi/a) +b3*cos(3*xi/a));
AA = 1/A;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% PSI Matrix: 5-Point GAUSSIAN
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
PSI = make_PSI(N,NNM,x1(1),dx,dxi);

%%%%%%%%%%%%%%
% STEP 1) Value the European!!!
%%%%%%%%%%%%%%
beta  = [AA; zeta.*PhiR(2:N).*exp(-1i*x1(1)*xi)];   %grand(end)=.5*grand(end);
beta  = real(fft(beta));

s = european_price(zeta,PhiR,xi,dx, r, q, dt, strikes(1), S_0, ER, N, a, call);
s = p(1) * 2 * s;

%%%%%%%%%%%%%%
% STEP 2) Value the rest
%%%%%%%%%%%%%%

PhiR  = C_aN*PhiR;
beta  = PSI(:,1:N)*beta.*PhiR;  %Nm(1)=0

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%% Loop to find PSI_M
for n=2:Mmax
    opt_v = intermediate_asian_price(N, dx, dt, xi, zeta, beta, x1(n), n, r, q, strikes(n), S_0);
    % fprintf('%.12f \n',opt_v);

    beta(2:N) = zeta.*beta(2:N).*exp(-1i*x1(n)*xi); beta(1) = AA;
    beta      = real(fft(beta));
    if n < Mmax
        beta      = PSI(:,Nm(n)+1:Nm(n)+N)*beta.*PhiR;
    end

    s = s + p(n) * (n + 1) * opt_v;
end

opt = s * alpha * gamma / S_0;
price = L*Zrg - alpha * (exp(r*dt) - Z) / (exp(r*dt) - 1) + opt;

end

function PSI = make_PSI(N,NNM,x_1,dx,dxi)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% PSI Matrix: 5-Point GAUSSIAN
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
PSI     = zeros(N,NNM);    %The first row will remain ones
PSI(1,:) = ones(1,NNM);

%%%% Sample
Neta  = 5*(NNM) + 15;   %sample size
Neta5 = (NNM) + 3;
g2    = sqrt(5-2*sqrt(10/7))/6;
g3    = sqrt(5+2*sqrt(10/7))/6;
v1    = .5*128/225;
v2    = .5*(322+13*sqrt(70))/900;
v3    = .5*(322 - 13*sqrt(70))/900;

thet                 = zeros(1,Neta);   %sample initialized
thet(5*(1:Neta5)-2)  = x_1 -1.5*dx + dx*(0:Neta5-1);
thet(5*(1:Neta5)-4)  = x_1 -1.5*dx + dx*(0:Neta5-1) - dx*g3;
thet(5*(1:Neta5)-3)  = x_1 -1.5*dx + dx*(0:Neta5-1) - dx*g2;
thet(5*(1:Neta5)-1)  = x_1 -1.5*dx + dx*(0:Neta5-1) + dx*g2;
thet(5*(1:Neta5))    = x_1 -1.5*dx + dx*(0:Neta5-1) + dx*g3;

%%%% Weights
sig      = [-1.5-g3, -1.5-g2, -1.5, -1.5+g2, -1.5+g3, -.5-g3, -.5-g2, -.5, -.5+g2, -.5+g3,];
sig(1:5) = (sig(1:5) + 2).^3/6;
sig(6:10) = 2/3 - .5*(sig(6:10)).^3 - (sig(6:10)).^2;

sig([1 5 6 10]) = v3*sig([1 5 6 10]);
sig([2 4 7 9]) = v2*sig([2 4 7 9]);
sig([3 8]) = v1*sig([3 8]);

%%%% Fill Matrix
zz  = exp(1i*dxi*log(1+exp(thet)));
thet   = zz;

for j=2:N-1
    PSI(j,:) =  sig(1)*(thet(1:5:Neta-19) + thet(20:5:Neta)) ...
              + sig(2)*(thet(2:5:Neta-18) + thet(19:5:Neta-1)) ...
              + sig(3)*(thet(3:5:Neta-17)  + thet(18:5:Neta-2)) ...
              + sig(4)*(thet(4:5:Neta-16)  + thet( 17:5:Neta-3)) ...
              + sig(5)*(thet(5:5:Neta-15)  + thet( 16:5:Neta-4)) ...
              + sig(6)*(thet(6:5:Neta-14)  + thet( 15:5:Neta-5)) ...
              + sig(7)*(thet(7:5:Neta-13)  + thet( 14:5:Neta-6)) ...
              + sig(8)*(thet(8:5:Neta-12)  + thet( 13:5:Neta-7)) ...
              + sig(9)*(thet(9:5:Neta-11)  + thet( 12:5:Neta-8)) ...
              + sig(10)*(thet(10:5:Neta-10)  + thet( 11:5:Neta-9));

    thet = thet.*zz;
end
end

function Val = intermediate_asian_price(N, dx, dt, xi, zeta, chf, x_1, M, r, q, W, S_0)
call = 1;
a = 1/dx;
A    = 32*a^4;
AA = 1/A;
C_aN = A/N;
T = M*dt;

%%%%% FINAL VALUE
ystar = log((M+1)*W/S_0 -1);
nbar  = floor((ystar-x_1)*a+1);
C     = S_0/(M+1);
D     = W - C;
x_1   = ystar- (nbar-1)*dx;

beta(2:N) = zeta.*chf(2:N).*exp(-1i*x_1*xi); beta(1)=AA;
beta      = real(fft(beta)).';

Cc1 = C*( exp(-7/4*dx)/54 + exp(-1.5*dx)/18 + exp(-1.25*dx)/2 + 7*exp(-dx)/27 )/20;

Cc2 = C*.05*(28/27 + exp(-7/4*dx)/54 + exp(-1.5*dx)/18 + exp(-1.25*dx)/2 + 14*exp(-dx)/27 ...
              + 121/54*exp(-.75*dx) + 23/18*exp(-.5*dx) + 235/54*exp(-.25*dx));

Cc3 = C*( (28 + 7*exp(-dx))/3 ...
              + ( 14*exp(dx) + exp(-7/4*dx) + 242*cosh(.75*dx) + 470*cosh(.25*dx))/12 ...
              +.25*(exp(-1.5*dx) + 9*exp(-1.25*dx) + 46*cosh(.5*dx)))/90;

Cc4 = C*( 14/3*(2+cosh(dx)) ...
              + .5*(cosh(1.5*dx) + 9*cosh(1.25*dx) +23*cosh(.5*dx))...
              +  1/6*(cosh(7/4*dx) + 121*cosh(.75*dx) +235*cosh(.25*dx)))/90;

G           = zeros(nbar+1,1);
E           = exp(ystar-(nbar-1)*dx+dx*(0:nbar));

G(nbar+1)   = D/24    - Cc1*E(nbar+1);
G(nbar)     = .5*D    - Cc2*E(nbar);
G(nbar-1)   = 23*D/24 - Cc3*E(nbar-1);
G(1:nbar-2) = D       - Cc4*E(1:nbar-2);

Val = C_aN*exp(-r*T)*sum(beta(1:nbar+1).*G);
if call==1  %Call Option, use Put-Call-Parity
    if r - q == 0
        mult = M + 1;
    else
        mult = (exp((r-q)*T*(1+1/M))-1)/(exp((r-q)*dt)-1);
    end
    Val = Val + C*exp(-r*T)*mult - W*exp(-r*T);
end
Val = max(0, Val);

end

function price = european_price(zeta,PhiR,xi,dx, r, q, T, W, S_0, c1, N, a, call)

W = 2*W - S_0;  % we shift the strike then divide by 2 at the end, this is asian with M=2

lws = log(W/S_0);
lam = c1 -(N/2 -1)*dx;
nbar = floor(a*(lws-lam)+1);
if nbar>=N
    nbar = N-1;
end
xmin = lws - (nbar-1)*dx;

Cons = 32*a^4;

beta  = [1/Cons; zeta.*PhiR(2:N).*exp(-1i*xmin*xi)];   %grand(end)=.5*grand(end);
beta  = real(fft(beta)).';

G = zeros(1,nbar +1);
G(nbar +1) = W*(1/24 - 1/20*exp(dx)*(exp(-7/4*dx)/54 + exp(-1.5*dx)/18 + exp(-1.25*dx)/2 + 7*exp(-dx)/27));

G(nbar )   =  W*(.5 -.05*(28/27 + exp(-7/4*dx)/54 + exp(-1.5*dx)/18 + exp(-1.25*dx)/2 + 14*exp(-dx)/27 ...
            + 121/54*exp(-.75*dx) + 23/18*exp(-.5*dx) + 235/54*exp(-.25*dx)));

G(nbar -1) = W*( 23/24 - exp(-dx)/90*( (28 + 7*exp(-dx))/3 ...
            + ( 14*exp(dx) + exp(-7/4*dx) + 242*cosh(.75*dx) + 470*cosh(.25*dx))/12 ...
            +.25*(exp(-1.5*dx) + 9*exp(-1.25*dx) + 46*cosh(.5*dx))) );

G(1: nbar -2) = W - S_0*exp(xmin +dx*(0:nbar-3))/90*( 14/3*(2+cosh(dx)) ...
                + .5*(cosh(1.5*dx) + 9*cosh(1.25*dx) +23*cosh(.5*dx))...
                +  1/6*(cosh(7/4*dx) + 121*cosh(.75*dx) +235*cosh(.25*dx)));

if call == 1  % Use put-call parity
    price = Cons*exp(-r*T)/N*G*(beta(1:length(G))') + S_0*exp(-q*T) - W*exp(-r*T);
else
    price = Cons*exp(-r*T)/N*G*(beta(1:length(G))');
end

price = 0.5*max(price, 0);  % Protect against deep out of money case

end


function [T, DISTR, df, Ybar, S] = symNormalTest2(TEST_TYPE, Ybar1, S1, n1, Ybar2, S2, n2, COV_TYPE, COV_DIFF)

% Two-sample tests for symmetric matrices.
%
%   [T, DISTR, df, Ybar, S] = symNormalTest2(TEST_TYPE, Ybar1, S1, n1, Ybar2, [S2], [n2], [COV_TYPE], [COV_DIFF])
%
% Input:
%   TEST_TYPE   Controls the type of test (assuming equal variances
%               between the two groups):
%                   'full': H0: both groups have the same mean tensors.
%                   'val' : H0: both groups have the same eigenvalues,
%                           with possibly different unknown eigenvectors.
%                   'vec' : H0: both groups have the same eigenvectors,
%                           with common unknown eigenvalues.
%   Ybar1, Ybar2  pxpx[sz] arrays of mean matrices for each group
%   S1, S2        qxqx[sz] arrays of covariance matrices for each group, q=p(p+1)/2
%                      (default S2 = 0)
%   n1, n2        Number of subjects in each group (default n2 = 1).
%   COV_TYPE      Type of covariance: 'spherical', 'orth-inv' or 'full' (default).
%   COV_DIFF      Assume different covariances: 1 (yes - default) or 0 (no)
% Output:
%   T           [sz] array of test statistics
%   DISTR       'f' or 'gamma'
%   df          2x1 degrees of freedom of F, or 2x[sz] parameters of Gamma (nu/2, 2a)
%   Ybar        pxpx[sz] array of pooled mean matrices
%   S           qxqx[sz] array of pooled covariance matrices
%
% E.g.:
%   [Ybar1, S1, n1] = symNormalStats(Y(:,:,g1));
%   [Ybar2, S2, n2] = symNormalStats(Y(:,:,g2));
%   [T, DISTR, df] = symNormalTest2('vec', Ybar1, S1, n1, Ybar2, S2, n2);
%
% Copyright by Armin Schwartzman, 2009

% HISTORY:
%   2008.12.30 ASH (armins@hsph.harvard.edu) wrote it.
%

% Check inputs
if ~exist('S2'),
    S2 = 0;
end
if ~exist('n2'),
    n2 = 1;
end
if (size(Ybar1,1) ~= size(Ybar1,2) | size(S1,1) ~= size(S1,2) | ...
    size(Ybar2,1) ~= size(Ybar2,2) | size(S2,1) ~= size(S2,2)),
    error('Wrong input format');
end
if (size(Ybar1,1) ~= size(Ybar2,1) | size(S1,1) ~= size(S2,1)),
    error('Wrong input format');
end
if ~exist('COV_TYPE'), COV_TYPE = 'full'; end
if (~strmatch(COV_TYPE,'spherical') & ~strmatch(COV_TYPE,'rot-inv') & ~strmatch(COV_TYPE,'full')),
    error('Only spherical, rot-inv and full covariance types supported.')
end
if ~exist('COV_DIFF'), COV_DIFF = 1; end

% Constants
n = n1 + n2;
p = size(Ybar1, 1);
q = size(S1, 1);
if (q ~= p*(p+1)/2),
    error('Wrong input format');
end

% Pooled mean and covariance
Ybar = (n1*Ybar1 + n2*Ybar2)/n;
S = ((n1-1)*S1 + (n2-1)*S2)/(n-2);

% Test type
switch TEST_TYPE,
    case 'full',
        d = permute(vecd(Ybar1 - Ybar2), [1 ndims(Ybar1)+1 2:ndims(Ybar1)]);
        switch COV_TYPE,
        case 'spherical',
            T = n1*n2/n * ndfun('mult', permute(d, [2 1 3:ndims(d)]), d);
            DISTR = 'f';
            if COV_DIFF,
                error('Full test, different spherical cov., not implemented.')
            end
            s2 = S(1,1,:);
            df = [q; q*(n-2)];
            T = df(2)/df(1) * T./(q*(n-2)*s2);
        case 'orth-inv',
            error('Full test, orth-inv cov., not implemented.')
        case 'full',
            if COV_DIFF,
                S = S1/n1 + S2/n2;
                Sinv = ndfun('inv', S);
                T = ndfun('mult', permute(d, [2 1 3:ndims(d)]), ndfun('mult', Sinv, d));
                Sinv1 = ndfun('mult', Sinv, ndfun('mult', S1/n1, Sinv));
                T1 = ndfun('mult', permute(d, [2 1 3:ndims(d)]), ndfun('mult', Sinv1, d));
                Sinv2 = ndfun('mult', Sinv, ndfun('mult', S2/n2, Sinv));
                T2 = ndfun('mult', permute(d, [2 1 3:ndims(d)]), ndfun('mult', Sinv2, d));
                m = 1./((T1./T).^2/(n1-1) + (T2./T).^2/(n2-1));
                df(2,:) = m-q+1; df(1,:) = q;
                DISTR = 'f';
                T = (m-q+1)./(q*m) .* T;
                df = shiftdim(df);
            else
                Sinv = ndfun('inv', S);
                T = n1*n2/n * ndfun('mult', permute(d, [2 1 3:ndims(d)]), ndfun('mult', Sinv, d));
                df = [q; n-q-1];
                DISTR = 'f';
                T = df(2)./df(1) .* T./(n-2);
            end
        end

    case 'val',
        [V1,L1] = ndSymEig(Ybar1); % [V1,L1] = ndfunm('eig', Ybar1);
        [V2,L2] = ndSymEig(Ybar2); % [V2,L2] = ndfunm('eig', Ybar2);
        T = n1*n2/n * sum(sum((L1 - L2).^2, 1), 2);
        switch COV_TYPE,
        case 'spherical',
            DISTR = 'f';
            if COV_DIFF,
                error('Eigval test, different spherical cov., not implemented.')
            end
            s2 = S(1,1,:);
            df = [p; q*(n-2)];
            T = df(2)/df(1) * T./(q*(n-2)*s2);
        case 'orth-inv',
            error('Full test, orth-inv cov., not implemented.')
        case 'full',
            Omega = 0;
            for i=1:p,
                W = cat(1, v(i,i,V1), -v(i,i,V2));
                Omega = Omega + ndfun('mult', W, permute(W, [2 1 3:ndims(W)]));
            end
            Omega = (n1*n2)/n * Omega;
            SS = zeros(size(Omega));  % (2q)x(2q)x[]
            if COV_DIFF,
                SS(1:q,1:q,:) = S1(1:q,1:q,:)/n1; SS(q+1:2*q,q+1:2*q,:) = S2(1:q,1:q,:)/n2;
            else
                SS(1:q,1:q,:) = S(1:q,1:q,:)/n1; SS(q+1:2*q,q+1:2*q,:) = S(1:q,1:q,:)/n2;
            end
            [a,nu] = chi2approx(SS, Omega);
            DISTR = 'gamma';
            df = cat(1, nu/2, 2*a);  % shape parameter, scale parameter
        end

    case 'vec',
        [V1,L1] = ndSymEig(Ybar1); % [V1,L1] = ndfunm('eig', Ybar1);
        [V2,L2] = ndSymEig(Ybar2); % [V2,L2] = ndfunm('eig', Ybar2);
        T = 2*n1*n2/n * (sum(sum(L1.*L2, 1), 2) - sum(sum(Ybar1.*Ybar2, 1), 2));
        switch COV_TYPE,
        case 'spherical',
            DISTR = 'f';
            if COV_DIFF,
                error('Eigvec test, different spherical cov., not implemented.')
            end
            s2 = S(1,1,:);
            df = [q-p; q*(n-2)];
            T = df(2)/df(1) * T./(q*(n-2)*s2);
        case 'orth-inv',
            error('Eigvec test, orth-inv cov., not implemented.')
        case 'full',
            Omega = 0;
            for i=1:p,
                for j=1:p,
                    W = omega(i,j,n1,n2,V1,V2);
                    Omega = Omega + ndfun('mult', W, permute(W, [2 1 3:ndims(W)]));
                end
            end
            Omega = (n1*n2)/n * Omega;
            SS = zeros(size(Omega));  % (2q)x(2q)x[]
            if COV_DIFF,
                SS(1:q,1:q,:) = S1(1:q,1:q,:)/n1; SS(q+1:2*q,q+1:2*q,:) = S2(1:q,1:q,:)/n2;
            else
                SS(1:q,1:q,:) = S(1:q,1:q,:)/n1; SS(q+1:2*q,q+1:2*q,:) = S(1:q,1:q,:)/n2;
            end
            [a,nu] = chi2approx(SS, Omega);
            DISTR = 'gamma';
            df = cat(1, nu/2, 2*a);  % shape parameter, scale parameter
        end
end

% Adjust output
T = shiftdim(T, 2);

end

%------------------------------------------------------------------------
% Auxiliary functions

function v = v(i,j,U)
    Ui = zeros(size(U));
    Ui(:,1,:) = U(:,i,:);
    Uj = zeros(size(U));
    Uj(1,:,:) = U(:,j,:);
    v = ndfun('mult', Ui, Uj);
    v = vecd((v + permute(v, [2 1 3:ndims(v)]))/2);
    v = permute(v, [1 ndims(v)+1 2:ndims(v)]);
end

function W = omega(i,j,n1,n2,U1,U2)
    sz = size(U1);
    p = sz(1);
    v1 = v(j, i, permute(U1, [2 1 3:ndims(U1)]))/2;
    v2 = v(j, i, permute(U2, [2 1 3:ndims(U1)]))/2;
    b = zeros([p 1 sz(3:end)]);
    b(1:p,:) = (n1*v2(1:p,:) + n2*v1(1:p,:))/(n1+n2);
    Eji = zeros(size(U1)); Eji(j,i,:) = 1/2;  Eji(i,j,:) = 1/2;
    Eji = permute(vecd(Eji), [1 ndims(Eji)+1 2:ndims(Eji)]);
    w1 = Eji - ndfun('mult', B(U1), b);
    w2 = Eji - ndfun('mult', B(U2), b);
    W = cat(1, w1, -w2);
end

function B = B(U)
    sz = size(U);
    p = sz(1);
    q = p*(p+1)/2;
    B = zeros([q p sz(3:end)]);
    for i=1:p,
        vv = v(i,i,U);
        B(:,i,:) = vv(1:q,1,:);
    end
end

function [a,nu] = chi2approx(S,Q)
    SQ = ndfun('mult',S,Q);
    k1 = ndfunm('trace',SQ);
    k2 = ndfunm('trace',ndfun('mult',SQ,SQ));
    a = k2./k1; nu = k1.^2./k2;
end

%------------------------------------------------------------------------
% Debugging
% M = zeros(3);
% S = eye(6);
% Y = symNormalRnd(M, S, [100 4]);
% [Ybar1, S1, n1] = symNormalStats(Y(:,:,1:40,:), 'full');
% [Ybar2, S2, n2] = symNormalStats(Y(:,:,41:100,:), 'full');
% [T, DISTR, df, Ybar, S] = symNormalTest2('val', Ybar1, S1, n1, Ybar2, S2, n2)


function suborder_num = tetrahedron_nco_suborder_num ( rule )

%*****************************************************************************80
%
%% TETRAHEDRON_NCO_SUBORDER_NUM returns the number of suborders for an NCO rule.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 January 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Peter Silvester,
%    Symmetric Quadrature Formulae for Simplexes,
%    Mathematics of Computation,
%    Volume 24, Number 109, January 1970, pages 95-100.
%
%  Parameters:
%
%    Input, integer RULE, the index of the rule.
%
%    Output, integer SUBORDER_NUM, the number of suborders of the rule.
%
  if ( rule == 1 )
    suborder_num = 1;
  elseif ( rule == 2 )
    suborder_num = 1;
  elseif ( rule == 3 )
    suborder_num = 2;
  elseif ( rule == 4 )
    suborder_num = 3;
  elseif ( rule == 5 )
    suborder_num = 5;
  elseif ( rule == 6 )
    suborder_num = 6;
  elseif ( rule == 7 )
    suborder_num = 9;

  else

    suborder_num = -1;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TETRAHEDRON_NCO_SUBORDER_NUM - Fatal error!\n' );
    fprintf ( 1, '  Illegal RULE = %d\n', rule );
    error ( 'TETRAHEDRON_NCO_SUBORDER_NUM - Fatal error!\n' )

  end

  return
end


function spline_test23 ( )

%*****************************************************************************80
%
%% TEST23 tests SPLINE_OVERHAUSER_VAL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 February 2009
%
%  Author
%
%    John Burkardt
%
  ndata = 4;
  ndim = 2;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST23\n' );
  fprintf ( 1, '  SPLINE_OVERHAUSER_VAL evaluates the\n' );
  fprintf ( 1, '    Overhauser spline.\n' );
%
%  Set the data.
%
  tdata(1) = 1.0;
  ydata(1,1) =   0.0;
  ydata(2,1) =   0.0;

  tdata(2) = 2.0;
  ydata(1,2) =   1.0;
  ydata(2,2) =   1.0;

  tdata(3) = 3.0;
  ydata(1,3) =   2.0;
  ydata(2,3) = - 1.0;

  tdata(4) = 4.0;
  ydata(1,4) =   3.0;
  ydata(2,4) =   0.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The data to be interpolated:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of data values = %d\n', ndata );
  fprintf ( 1, '\n' );
  fprintf ( 1, '       T             Y\n' );
  fprintf ( 1, '\n' );

  for i = 1 : ndata
    fprintf ( 1, '%14f  ', tdata(i) );
    for j = 1 : ndim
      fprintf ( 1, '%14f  ',  ydata(j,i) );
    end
    fprintf ( 1, '\n' );
  end
%
%  Now evaluate the spline all over the place.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  T, Spline value\n' );
  fprintf ( 1, '\n' );

  for i = 0 : 6 * ndata + 3

    tval = ( i ) / 6.0;
    yval = spline_overhauser_val ( ndim, ndata, tdata, ydata, tval );

    fprintf ( 1, '%14f  ', tval );
    for j = 1 : ndim
      fprintf ( 1, '%14f  ',  yval(j) );
    end
    fprintf ( 1, '\n' );

  end

  return
end


function [delay,maxBitRate,energy] = dtiNeuronBitRate(radius, length, meanFiringRate, myelinated)
%
% [delay,maxBitRate,energy] = dtiNeuronBitRate(radius, length, meanFiringRate, [myelinated=true])
%
% Returns neural conduction delay in msec, the maximum bit-rate, and the energy consumption
% given the outer radius (in micrometers), the length (in millimeters), and the mean firing
% rate (in spikes/sec).
%
% Energy requirements are specified in amol glucose / action potential.
%
% See:
%   Brenner et. al. (2000). Synergy in a neural code. Neural Comput.
%   Wang et. al. (2008). Functional Trade-Offs in White Matter Axonal Scaling. J. Neurosci.
%   (The main reference that works out the bit-rate.)
%
% Also see:
%   Timing jitter vs. radius: Swadlow (2000). Time and the brain (R. Miller, ed.)
%   (We assume that timing jitter is +/-10% or the total conduction delay.)
%
% 2009.02.06 RFD wrote it.

if(~exist('myelinated','var') || isempty(myelinated))
    myelinated = true;
end

[speed,energyPerMm] = dtiNeuralConductionSpeed(radius, myelinated);

delay = 1 ./ (speed ./ length);
% delay is in ms- we need seconds, thus the 1000
energy = energyPerMm.*length;
maxBitRate = log2(1./(meanFiringRate.*0.2.*(delay./1000)));

return;


function specGlobal = doa_music(x,Param,nsrc)
if(size(x,2)<2)
    error('ERROR[MUSIC]:信号通道数必须大于等于2');
end
%% STFT
X = ssl_stft(x.',Param.window,Param.noverlap,Param.nfft,Param.fs);%nbin,nfram,nchan
X = X(2:end,:,:);
X = X(Param.freqBins,:,:);
[nbin,~,nmic] = size(X);
%% MUSIC
% linspace包含端点，保证插值时不会出现NaN
aziGrid = linspace(Param.azimuth(1),Param.azimuth(end),round((Param.azimuth(end)-Param.azimuth(1))/Param.alphaRes)+1);
eleGrid = linspace(Param.elevation(1),Param.elevation(end),round((Param.elevation(end)-Param.elevation(1))/Param.alphaRes)+1);
power = zeros(nbin, length(aziGrid), length(eleGrid));

for ibin = 1:nbin % 对于每个频点
    Rxx = (transpose(squeeze(X(ibin,:,:)))*conj(squeeze(X(ibin,:,:))));% 自相关矩阵
    [U,~,~] = svd(Rxx);    % SVD分解   Rxx = U * S * U^H
    En = U(:,nsrc+1:end);  % 噪声子空间
    fprintf('%d\n',ibin)
    for iaz = 1 :length(aziGrid)
        for iel = 1 :length(eleGrid)
            v = [cosd(eleGrid(iel))*cosd(aziGrid(iaz));cosd(eleGrid(iel))*sind(aziGrid(iaz));sind(eleGrid(iel))];% 3 x 1
            tau = v'*(Param.micPos-repmat(Param.micPos(:,1),[1,nmic]))./Param.c; % 1 * nmic  参考麦克为1：Param.micPos(:,1)
            a = exp(1i*2*pi*Param.f(ibin).*transpose(tau));%nmic x 1     SV = exp(-2*1i*pi*tau*Param.f.');  % nmic x nbin
            power(ibin,iaz,iel) = 1./(sum(abs( ctranspose(a) * En * ctranspose(En) * a )));
        end
    end
end

% 对所有频率的空间谱加在一起:
spec = squeeze(sum(power,1)); %nAzi x nEle
[az,el]=meshgrid(aziGrid,eleGrid);
[azi,eli]=meshgrid(Param.azimuth,Param.elevation);

specInterp = interp2(az,el,spec.',azi,eli);
% specInterp = interp2(azOri,elOri,spec.',azInterp,elInterp);
specGlobal = reshape(specInterp.',1,[]);
end

function X=ssl_stft(x,window,noverlap,nfft,fs)

% Inputs:x: nchan x nsampl  window = blackman(wlen);
% Output:X: nbin x nfram x nchan matrix

[nchan,~]=size(x);
[Xtemp,F,T,~] = spectrogram(x(1,:),window,noverlap,nfft,fs);%S nbinxnframe
nbin = length(F);
nframe = length(T);
X = zeros(nbin,nframe,nchan);
X(:,:,1) = Xtemp;
for ichan = 2:nchan
    X(:,:,ichan) = spectrogram(x(ichan,:),window,noverlap,nfft,fs);
end

end


function [rvts,rvtsSS,rvtsD,rvtsSSD,diagnostics] = realized_twoscale_variance(price,time,timeType,samplingType,samplingInterval,subsamples,options)
% Estimated quadratic variation using the Two-Scale estimator of Ait-Sahalia, Mykland and Zhang
%
% USAGE:
%   RVTS = realized_twoscale_variance(PRICE)
%   [RVTS,RVTSSS,RVTSD,RVTSSSD,DIAGNOSTICS] = realized_twoscale_variance(PRICE,TIME,TIMETYPE,SAMPLINGTYPE,SAMPLINGINTERVAL,SUBSAMPLES,OPTIONS)
%
% INPUTS:
%   PRICE            - m by 1 vector of high frequency prices
%   TIME             - [OPTIONAL] m by 1 vector of times where TIME(i) corresponds to PRICE(i).
%   TIMETYPE         - [OPTIONAL] String describing the way times are measured
%                       'wall'    24-hour clock of the form HHMMSS.mmm, e.g. 101543 or 153217
%                       'seconds' Time measured in seconds past midnight
%                       'unit'  Unit normalized date format, e.g. .1, .234, .9
%                         Unit normalized times are more general than the other types and can be
%                         applied to data from more than one calendar day
%   SAMPLINGTYPE     - [OPTIONAL] String describing the type of sampling to use when
%                        filtering PRICE for estimating the fast scale estimator.
%                        'CalendarTime' - Sample in calendar time using observations separated by
%                          SAMPLINGINTERVAL seconds
%                        'CalendarUniform' - Sample in calendar time using SAMPLINGINTERVAL
%                          observations spread uniformly between TIME(1) and TIME(m)
%                        'BusinessTime' - Sample in business (tick) time using observation separated
%                          by SAMPLINGINTERVAL ticks
%                        'BusinessUniform' - Sample in business (tick) time using observations
%                          uniformly spaced in business time.
%                        'Fixed' - Sample at specific points in time. When using fixed,
%                          SAMPLINGINTERVAL must be a n by 1 vector of times with the same TIMETYPE
%                          as TIME (i.e. seconds if TIME is in seconds)
%   SAMPLINGINTERVAL  - [OPTIONAL] Scalar integer or n by 1 vector whose meaning depends on the
%                         selected SAMPLINGTYPE
%   OPTIONS           - [OPTIONAL] Realized Two-Scale option structure initialized by calling
%                         realized_options('Multiscale'). See help realized_options for a description of
%                         available options.
%
% OUTPUTS:
%   RVTS        - Realized two-scale variance estimate
%   RVTSSS      - Realized two-scale variance estimate constructed by averaging RVTS across multiple
%                 initial observations.  If SAMPLINGTYPE is 'BusinessTime' and SAMPLINGINTERAL is 1
%                 then subsampling is not possible.
%   RVTSD       - Debiased version of realized two-scale variance estimate
%   RVTSSSD     - Debiased version of realized two-scale variance estimate constructed by averaging RVTS across multiple
%                 initial observations.
%   DIAGNOSTICS - Structure of useful diagnostic information.  Fields are
%                   BANDWIDTH             - Ratio of slow scale to fast scale where the fast scale
%                                           sampling is determined by SAMPLINGTYPE and SAMPLINGINTERVAL.
%                   NOISEVARIANCE         - Bandi-Russell noise variance estimate. Empty if user
%                                           supplies bandwidth.
%                   DEBIASEDNOISEVARIANCE - Bias adjusted estimate of the noise variance.
%                   IQESTIMATE            - Estimate of IQ used. Empty if not needed. Empty if user
%                                           supplies bandwidth.
%
% COMMENTS:
%   For best results:
%     - Use prices sampled close to the highest frequency available, if not using 'BusinessTime' and
%     - If using calendar time sampling, sample close to the limit of the data availability (e.g. 5-15
%       seconds for a stock that trades a few thousand times per day)
%     - When sampling less frequently (e.g. 5 - 30 minutes) standard realized variance is
%       probably more appropriate
%     - The debiased version should be preferred to the standard version
%
% EXAMPLE:
%  % Default usage with 'BusinessTime' sampling with interval 1 and automatic bandwidth selection
%  [RVTS,RVTSSS,RVTSD,RVTSSSD] = realized_twoscale_variance(PRICE)
%
%  % 5-tick two-scale variance with automatic bandwidth selection
%  [RVTS,RVTSSS,RVTSD,RVTSSSD] = realized_twoscale_variance(PRICE,TIME,'wall','BusinessTime',5)
%
%  % Two scale variance with a bandwidth of 30 sampling at every tick
%  options = realized_options('Twoscale');
%  options.bandwidth = 30;
%  [RVTS,RVTSSS,RVTSD,RVTSSSD] = realized_twoscale_variance(PRICE,TIME,'wall','BusinessTime',1,0,options)
%
%  % 5-tick two-scale variance with automatic bandwidth selection and dense subsampling
%  [RVTS,RVTSSS,RVTSD,RVTSSSD] = realized_twoscale_variance(PRICE,TIME,'wall','BusinessTime',5,4)
%
%  See also REALIZED_OPTIONS, REALIZED_NOISE_ESTIMATE, REALIZED_VARIANCE,
%  REALIZED_VARIANCE_OPTIMAL_SAMPLING, REALIZED_RANGE, REALIZED_QUANTILE_VARIANCE

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 1    Date: 5/1/2008

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
switch nargin
    case 1
        m = length(price);
        time = linspace(9.5*3600,16*3600,m)';
        timeType = 'seconds';
        samplingType = 'businesstime';
        samplingInterval = 1;
        subsamples = 1;
        options = realized_options('twoscale');
    case 5
        subsamples = 1;
        options = realized_options('twoscale');
    case 6
        options = realized_options('twoscale');
    case 7
        % Nothing
    otherwise
        error('One, five, six or seven inputs required.')
end
samplingType=lower(samplingType);
timeType=lower(timeType);
[errorMessage, options] = realized_twoscale_variance_parameter_check(price,time,timeType,samplingType,samplingInterval,subsamples,options);

if ~isempty(errorMessage )
    error(errorMessage)
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%

logPrice = log(price);
filteredLogPrice = realized_price_filter(logPrice,time,timeType,samplingType,samplingInterval);
%options.bandwidth= 60;
if isempty(options.bandwidth)
    % Estimate the optimal bandwidth if not provided
    [noiseVariance, debiasedNoiseVariance, IQEstimate] = realized_noise_estimate(price, time, timeType, options);
    diagnostics.noiseVariance=noiseVariance;
    diagnostics.debiasedNoiseVariance=debiasedNoiseVariance;
    diagnostics.IQEstimate=IQEstimate;
    % Select the correct noise variance estimate
    if options.useDebiasedNoise
        selectedNoiseVariance = debiasedNoiseVariance;
    else
        selectedNoiseVariance = noiseVariance;
    end
    % Compute the bandwidth
    n = length(filteredLogPrice) - 1;
    cOpt = ((12 * selectedNoiseVariance^2) / IQEstimate)^(1/3);
    options.bandwidth = ceil(cOpt * n^(2/3));
end

bandwidth = ceil(options.bandwidth);
if bandwidth<2
    warning('oxfordRealized:smallBandwidth','The selected bandwidth is less then 2, and RTVS is not well defined in this case.  Setting bandwidth to 2 and proceeding.')
    bandwidth = 2;
end
diagnostics.bandwidth = bandwidth;

n = length(filteredLogPrice) - 1;
K = bandwidth;
nbar = (n - K + 1) / K;
[RV,count,naturalCount] = overlap_realized_variance(filteredLogPrice,K);
overlapScale = count / naturalCount;
RVq = RV / overlapScale;
RV1 = overlap_realized_variance(filteredLogPrice,1);
% Eq. 55
rvts = RVq - nbar/n * RV1;
% Eq. 64
rvtsD = (1-nbar/n)^(-1)*rvts;

% Subsampled RVq and RV1
subsampledLogPrices = realized_subsample(logPrice,time,timeType,samplingType,samplingInterval,subsamples);
rvqs = zeros(subsamples,1);
rv1s = zeros(subsamples,1);
totalCount  = 0;
totalCount0 = 0;
for i=1:subsamples
    filteredLogPrice =  subsampledLogPrices{i};
    [RV,count] = overlap_realized_variance(filteredLogPrice,K);
    rvqs(i) = RV;
    totalCount = totalCount + count;
    if i==1
        baseCount = count;
    end
    [rv1s(i),count] = overlap_realized_variance(filteredLogPrice,1);
    totalCount0 = totalCount0 + count;
    if i==1
        baseCount0 = count;
    end
end
RVqSS = sum(rvqs) * (baseCount / totalCount);
% RVq is now based on all overlapping blcoks, so need to normalize to the
% non-overlapping case
RVqSS = RVqSS / overlapScale;
RV1SS = sum(rv1s) * (baseCount0 / totalCount0);
rvtsSS = RVqSS - nbar/n * RV1SS;
rvtsSSD = (1-nbar/n)^(-1)*rvtsSS;
end

function [rv,count,naturalCount] = overlap_realized_variance(price,skip)

naturalCount = (length(price)-1)/skip;
m = length(price);
returns = price(1+skip:m) - price(1:m-skip);
count = length(returns);
rv = returns' * returns;

end

function [errorMessage,options] = realized_twoscale_variance_parameter_check(price,time,timeType,samplingType,samplingInterval,subsamples,options)
% Support function for realized_twoscale_variance that does input validation
%
% USAGE:
%   [ERRORMESSAGE,OPTIONS] = realized_variance_optimal_sampling_parameter_check(PRICE,TIME,TIMETYPE,SAMPLINGTYPE,SAMPLINGINVERVAL,OPTIONS)
%
% INPUTS:
%   See realized_variance_optimal_sampling
%
% OUTPUT:
%   ERRORMESSAGE - String containing a description of the error if one is detected.  Empty if no error.
%
% COMMENTS:
%   See realized_options for a description of the other fields in OPTIONS
%
%  See also REALIZED_VARIANCE_OPTIMAL_SAMPLING

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 1    Date: 5/1/2008
errorMessage = [];

if size(price,2)>size(price,1)
    price=price';
end
if size(price,2)>1
    errorMessage = 'PRICE must be a m by 1 vector.';
    return
end
if size(time,2)>size(time,1)
    time=time';
end
if any(diff(time)<0)
    errorMessage = 'TIME must be sorted and increasing';
    return
end
if size(time,2)>1 || length(time)~=length(price)
    errorMessage = 'TIME must be a m by 1 vector.';
    return
end

timeType=lower(timeType);
if ~ismember(timeType,{'wall','seconds','unit'})
    errorMessage = 'TIMETYPE must be one of ''wall'', ''seconds'' or ''unit''.';
    return;
end

samplingType=lower(samplingType);
if ismember(samplingType,{'calendartime','calendaruniform','businesstime','businessuniform'})
    % Must be a scalar integer
    if ~isscalar(samplingInterval) || floor(samplingInterval)~=samplingInterval || samplingInterval<1
        errorMessage = 'SAMPLINGINTERVAL must be a positive integer for the SAMPLINGTYPE selected.';
        return
    end
else
    if size(samplingInterval,2)>size(samplingInterval,1)
        samplingInterval=samplingInterval';
    end
    if ~(any(samplingInterval>=t0) && any(samplingInterval<=tT))
        errorMessage = 'At least one sampling interval must be between min(TIME) and max(TIME) when using ''Fixed'' as SAMPLINGTYPE.';
        return
    end
    if any(diff(samplingInterval)<=0)
        errorMessage = 'When using ''Fixed'' as SAMPLINGTYPE the vector of sampling times in SAMPLINGINTERVAL must be sorted and strictly increasing.';
        return
    end
end

% Options

% List of flat top kernels
flatTopKernelList = {'bartlett','twoscale','2ndorder','epanechnikov',...
    'cubic','multiscale','5thorder','6thorder','7thorder','8thorder','parzen',...
    'th1','th2','th5','th16'};

% List of non flat top kernels
nonFlatTopKernelList = {'nonflatparzen','qs','fejer','thinf','bnhls'};

% Combined kernel list
kernelList = [flatTopKernelList nonFlatTopKernelList];

% Check fields for valid values
optionsFieldNames = fieldnames(options);
% Insert any missing fiedls
defaultOptions = realized_options('Optimal Sampling');
defaultFieldNames = fieldnames(defaultOptions);
missingFields = setdiff(defaultFieldNames,optionsFieldNames);
if ~isempty(missingFields)
    for i = 1:length(missingFields)
        options.(missingFields{i}) = defaultOptions.(missingFields{i});
    end
end

for i=1:length(optionsFieldNames)
    fieldName = optionsFieldNames{i};
    fieldValue = options.(fieldName);
    if ischar(fieldValue)
        fieldValue = lower(fieldValue);
        options.(fieldName) = fieldValue;
    end

    switch fieldName
        case {'medFrequencyKernel'}
            % Member of kernelList
            if ~ismember(fieldValue,kernelList)
                errorMessage = ['OPTIONS.' fieldName ' must be one of the listed types.'];
                return
            end
        case {'medFrequencyBandwidth'}
            % Non-negative scalar
            if ~isempty(fieldValue) && ~isnonnegativescalar(fieldValue)
                errorMessage = ['OPTIONS.' fieldName ' must a non-negative scalar.'];
                return
            end
        case {'useDebiasedNoise','useAdjustedNoiseCount'}
            % Logical or scalar
            if ~islogical(fieldValue) && ~ismember(fieldValue,[0 1])
                errorMessage = 'OPTIONS.useDebiasedNoise must be a logical value.';
                return
            end
        case {'IQEstimationSamplingType','medFrequencySamplingType','noiseVarianceSamplingType'}
            % One of the sampling types
            if ~ismember(fieldValue,{'calendartime','calendaruniform','businesstime','businessuniform','fixed'})
                errorMessage = ['OPTIONS.' fieldName ' must be one of ''CalendarTime'', ''CalendarUniform'', ''BusinessTime'', ''BusinessUniform'' or ''Fixed''.'];
                return
            end
        case {'medFrequencySamplingInterval','noiseVarianceSamplingInterval','IQEstimationSamplingInterval'}
            % Non-negative scalar, less that 1 if timeType is unit
            if isempty(fieldValue) || ~isnonnegativescalar(fieldValue)
                errorMessage = ['OPTIONS.' fieldName ' must be a non-negative scalar between 0 and 1.'];
                return
            end
            if strcmp(timeType,'unit') && fieldValue>1
                errorMessage = ['OPTIONS.' fieldName ' must be less than 1 if TIMETYPE when ''unit''.'];
                return
            end
    end
end

if ~isempty(subsamples)
    if ~isscalar(subsamples) || subsamples<0 || floor(subsamples)~=subsamples
        errorMessage = 'SUBSAMPLES must be a non-negative scalar.';
    end
end
end

function condition = isnonnegativescalar(x)
% Function that returns logical true if that input is a non-empty scalar >=0
condition = ~isempty(x) && isscalar(x) && x>=0;
end


function f = spm_fx_mountaincar(x,v,a,P)
% state equations for mountain car problem
% FORMAT f = spm_fx_mountaincar(x,v,P)
% FORMAT f = spm_fx_mountaincar(x,v,a,P)
% FORMAT f = spm_fx_mountaincar(x,v,P,M)
% x    - [x, x']
% v    - exogenous force
% a    - action
%
% P.a  - 0th order coefficients of force
% P.b  - 1st order coefficients of force
% P.c  - 2nd order coefficients of force
% P.d  - action coefficient
%
% M    - model structure
%
% f    - flow dx/dt
%
% see:
% Gaussian Processes in Reinforcement Learning
% Carl Edward Rasmussen and Malte Kuss
% Max Planck Institute for Biological Cybernetics
% Spemannstraße 38, 72076 T¨ubingen, Germany
% {carl,malte.kuss}@tuebingen.mpg.de
%__________________________________________________________________________
% Copyright (C) 2005 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_fx_mountaincar.m 7679 2019-10-24 15:54:07Z spm $

% determine controlled forces (a)
%==========================================================================

% spm_fx_mountaincar(x,v,P) - recognition model (no action)
%--------------------------------------------------------------------------
global eta
if isempty(eta), eta = 8; end
if nargin == 3

    P    = a;
    a    = 0;

elseif all(isfield(P,{'f','g'}))

    % spm_fx_mountaincar(x,v,P,M) - generative model (no action)
    %----------------------------------------------------------------------
    P.f;
    P.g;
    M    = P;
    P    = a;
    a    = 0;

end

% default parameters
%--------------------------------------------------------------------------
if isempty(P)
    P.a  = 0;
    P.b  = [0 0];
    P.c  = [0 0 0 0];
    P.d  = 1;
end

% acceleration = force:
%--------------------------------------------------------------------------
x     = x(1:2);
a     = tanh((P.d*a + P.a + P.b*x + P.c*kron(x,x))/2);

% f(x)
%--------------------------------------------------------------------------
dt    = 1/4;
if x(1) < 0                                  % gravity
    dHdx = 2*x(1) + 1;
else
    xx   = x(1)^2;
    dHdx = (5*xx + 1).^(-3/2) + (x(1)/2).^4;
end
f     = [x(2); a + v - dHdx - x(2)/eta]*dt;

return

% NOTES: Plots for figure
%--------------------------------------------------------------------------
dx    = 1/64;
x     = linspace(-2,2,1/dx);
xx    = x.^2;
dHdx  = (x < 0).*(2*x + 1);
dHdx  = (x > 0).*((5*xx + 1).^(-3/2) + (x/2).^4) + dHdx;
H     = cumsum(dHdx)*dx;
H     = H - min(H);

subplot(2,2,1)
plot(x,H,x.^0,H,':')
xlabel('position','FontSize',12)
ylabel('height','FontSize',12)
title('mountain car problem','FontSize',16)
axis square

subplot(2,2,2)
plot(x,-dHdx,x,x*0,'-.',x,-x.^0,'--')
xlabel('position','FontSize',12)
ylabel('force','FontSize',12)
title('forces','FontSize',16)
axis square


function [f] = elec_fit_ellipse_optim(r, X, Y, Z, xo, yo, zo)

% elec_fit_ellipse_optim - Optimization for elec_fit_ellipse.m
%
% Called from elec_fit_ellipse.m
%

% $Revision: 1.1 $ $Date: 2009-04-28 22:13:55 $

% Licence:  GNU GPL, no implied or express warranties
% History:  02/2002, Darren.Weber_at_radiology.ucsf.edu
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% r is a 3x1 vector of radius values for each x,y,z axis component of ellipse
%
% equation of ellipsoid with center (xo,yo,zo) and radius for each axis (x,y,z) = (a,b,c):
% (( x - xo )^2 / a^2) + (( y - yo )^2 / b^2) + (( z - zo )^2 / c^2) = 1
%
% This function below creates a scalar value to
% return to the fminsearch function in elec_fit_sphere.

E = ( (X-xo).^2 )/r(1).^2 + ((Y-yo).^2)/r(2).^2 + ((Z-zo).^2)/r(3).^2  - 1;

f = sum( E .* E );  % sum of squares returned

return