function [M, Minit, output] = cp_apr(X, R, varargin)
%CP_APR Compute nonnegative CP with alternating Poisson regression.
%
%   M = CP_APR(X, R) computes an estimate of the best rank-R CP model of a
%   nonnegative tensor X using an alternating Poisson regression. This is
%   most appropriate for sparse count data (i.e., nonnegative integer
%   values) because it uses Kullback-Liebler divergence.  The input X can
%   be a tensor or sptensor. The result M is a ktensor.  Input data must be
%   nonnegative, and the computed ktensor factors are all nonnegative.
%
%   Different algorithm variants are available (selected by the 'alg'
%   parameter):
%     'pqnr' - row subproblems by projected quasi-Newton (default)
%     'pdnr' - row subproblems by projected damped Hessian
%     'mu'   - multiplicative update (default in version 2.5)
%
%   M = CP_APR(X, R, 'param', value, ...) specifies optional parameters and
%   values. Some parameters work in all situations, others apply only for
%   a particular choice of algorithm.
%
%   Valid parameters and their default values are:
%      'alg'           - Algorithm ['mu'|'pdnr'|'pqnr'] {'pqnr'}
%      'stoptol'       - Tolerance on the overall KKT violation {1.0e-4}
%      'stoptime'      - Maximum number of seconds to run {1e6}
%      'maxiters'      - Maximum number of iterations {1000}
%      'init'          - Initial guess [{'random'}|ktensor]
%      'maxinneriters' - Maximum inner iterations per outer iteration {10}
%      'epsDivZero'    - Safeguard against divide by zero {1.0e-10}
%      'printitn'      - Print every n outer iterations; 0 for none {1}
%      'printinneritn' - Print every n inner iterations {0}
%
%   Additional input parameters for algorithm 'mu':
%      'kappa'         - Offset to fix complementary slackness {100}
%      'kappatol'      - Tolerance on complementary slackness {1.0e-10}
%
%   Additional input parameters for algorithm 'pdnr':
%      'epsActive'     - Bertsekas tolerance for active set {1.0e-8}
%      'mu0'           - Initial damping parameter {1.0e-5}
%      'precompinds'   - Precompute sparse tensor indices {true}
%      'inexact'       - Compute inexact Newton steps {true}
%
%   Additional input parameters for algorithm 'pqnr':
%      'epsActive'     - Bertsekas tolerance for active set {1.0e-8}
%      'lbfgsMem'      - Number vector pairs to store for L-BFGS {3}
%      'precompinds'   - Precompute sparse tensor indices {true}
%
%   [M,M0] = CP_APR(...) also returns the initial guess.
%
%   [M,M0,out] = CP_APR(...) also returns additional output:
%      out.kktViolations - maximum KKT violation per iteration
%      out.nInnerIters   - number of inner iterations per outer iteration
%      out.obj           - final negative log-likelihood objective
%      out.ttlTime       - time algorithm took to converge or reach max
%      out.times         - cumulative time through each outer iteration
%    If algorithm is 'mu':
%      out.nViolations   - number of factor matrices needing complementary
%                          slackness adjustment per iteration
%    If algorithm is 'pdnr' or 'pqnr':
%      out.nZeros        - number of zero factor entries per iteration
%
%   REFERENCES:
%   * E. C. Chi and T. G. Kolda. On Tensors, Sparsity, and Nonnegative
%     Factorizations, SIAM J. Matrix Analysis,  33(4):1272-1299, Dec. 2012,
%     http://dx.doi.org/10.1137/110859063
%   * S. Hansen, T. Plantenga and T. G. Kolda, Newton-Based Optimization
%     for Kullback-Leibler Nonnegative Tensor Factorizations,
%     Optimization Methods and Software, 2015,
%     http://dx.doi.org/10.1080/10556788.2015.1009977
%
%   See also CP_ALS, KTENSOR, TENSOR, SPTENSOR.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

%% Set the algorithm choice and initial guess from input or defaults.
params = inputParser;
params.addParameter('alg', 'pqnr', @(x) (ismember(x,{'mu','pdnr','pqnr'})) );
params.addParameter('init','random', @(x) (isa(x,'ktensor') || ismember(x,{'random'})) );
params.KeepUnmatched = true;
params.parse(varargin{:});

alg   = params.Results.alg;
Minit = params.Results.init;

% Extract the number of modes in tensor X.
N = ndims(X);

if (R <= 0)
    error('Number of components requested must be positive');
end

%% Check that the data is nonnegative.
tmp = find(X < 0.0);
if (size(tmp,1) > 0)
    error('Data tensor must be nonnegative for Poisson-based factorization');
end

%% Set up an initial guess for the factor matrices.
if isa(Minit,'ktensor')
    % User provided an initial ktensor; validate it.

    if (ndims(Minit) ~= N)
        error('Initial guess does not have the right number of modes');
    end
    if (ncomponents(Minit) ~= R)
        error('Initial guess does not have the right number of components');
    end

    for n = 1:N
        if (size(Minit,n) ~= size(X,n))
            error('Mode %d of the initial guess is the wrong size',n);
        end
        if (min(min(Minit.U{n})) < 0.0)
            error('Initial guess has negative element in mode %d',n);
        end
    end
    if (min(Minit.lambda) < 0.0)
        error('Initial guess has a negative ktensor weight');
    end

elseif strcmp(Minit,'random')
    % Choose random values for each element in the range (0,1).
    F = cell(N,1);
    for n = 1:N
        F{n} = rand(size(X,n),R);
    end
    Minit = ktensor(F);
end

%% Call a solver based on the choice of algorithm parameter, passing
%  all the other input parameters.
if strcmp(alg,'mu')
    [M, output] = tt_cp_apr_mu (X, R, Minit, params.Unmatched);
    output.params.alg = 'mu';

elseif strcmp(alg,'pdnr')
    [M, output] = tt_cp_apr_pdnr (X, R, Minit, params.Unmatched);
    output.params.alg = 'pdnr';

elseif strcmp(alg,'pqnr')
    [M, output] = tt_cp_apr_pqnr (X, R, Minit, params.Unmatched);
    output.params.alg = 'pqnr';
end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Main algorithm PQNR
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [M, out] = tt_cp_apr_pqnr(X, R, Minit, varargin)
%TT_CP_APR_PQNR Compute nonnegative CP with alternating Poisson regression.
%
%   tt_cp_apr_pqnr(X, R, ...) computes an estimate of the best rank-R
%   CP model of a tensor X using an alternating Poisson regression.
%   The algorithm solves "row subproblems" in each alternating subproblem,
%   using a quasi-Newton Hessian approximation.
%   The function is typically called by cp_apr.
%
%   The model is solved by nonlinear optimization, and the code literally
%   minimizes the negative of log-likelihood.  However, printouts to the
%   console reverse the sign to show maximization of log-likelihood.
%
%   The function call can specify optional parameters and values.
%   Valid parameters and their default values are:
%      'stoptol'       - Tolerance on the overall KKT violation {1.0e-4}
%      'stoptime'      - Maximum number of seconds to run {1e6}
%      'maxiters'      - Maximum number of iterations {1000}
%      'maxinneriters' - Maximum inner iterations per outer iteration {10}
%      'epsDivZero'    - Safeguard against divide by zero {1.0e-10}
%      'printitn'      - Print every n outer iterations; 0 for no printing {1}
%      'printinneritn' - Print every n inner iterations {0}
%      'epsActive'     - Bertsekas tolerance for active set {1.0e-8}
%      'lbfgsMem'      - Number vector pairs to store for L-BFGS {3}
%      'precompinds'   - Precompute sparse tensor indices to run faster {true}
%
%   Return values are:
%      M                 - ktensor model with R components
%      out.fnEvals       - number of row obj fn evaluations per outer iteration
%      out.kktViolations - maximum KKT violation per iteration
%      out.nInnerIters   - number of inner iterations per outer iteration
%      out.nZeros        - number of factor elements equal to zero per iteration
%      out.obj           - final log-likelihood objective
%                          (minimization objective is actually -1 times this)
%      out.ttlTime       - time algorithm took to converge or reach max
%      out.times         - cumulative time through each outer iteration
%
%   REFERENCE: Samantha Hansen, Todd Plantenga, Tamara G. Kolda.
%   Newton-Based Optimization for Nonnegative Tensor Factorizations,
%   arXiv:1304.4964 [math.NA], April 2013,
%   URL: http://arxiv.org/abs/1304.4964. Submitted for publication.
%
%   See also CP_APR, KTENSOR, TENSOR, SPTENSOR.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

%% Set algorithm parameters from input or by using defaults.
params = inputParser;
params.addParamValue('epsActive', 1e-8, @isscalar);
params.addParamValue('epsDivZero',1e-10,@isscalar);
params.addParamValue('lbfgsMem',3,@isscalar);
params.addParamValue('maxinneriters',10,@isscalar);
params.addParamValue('maxiters',1000,@(x) isscalar(x) & x > 0);
params.addParamValue('precompinds',true,@(x) isa(x,'logical'));
params.addParamValue('printinneritn',0,@isscalar);
params.addParamValue('printitn',1,@isscalar);
params.addParamValue('stoptime',1e6,@isscalar);
params.addParamValue('stoptol',1e-4,@isscalar);
params.parse(varargin{:});

%% Copy from params object.
epsActSet               = params.Results.epsActive;
epsDivZero              = params.Results.epsDivZero;
nSizeLBFGS              = params.Results.lbfgsMem;
maxInnerIters           = params.Results.maxinneriters;
maxOuterIters           = params.Results.maxiters;
precomputeSparseIndices = params.Results.precompinds;
printInnerItn           = params.Results.printinneritn;
printOuterItn           = params.Results.printitn;
stoptime                = params.Results.stoptime;
stoptol                 = params.Results.stoptol;

% Extract the number of modes in tensor X.
N = ndims(X);

% If the initial guess has any rows of all zero elements, then modify
% so the row subproblem is not taking log(0).  Values will be restored to
% zero later if the unfolded X for the row has no nonzeros.
for n = 1:N
  rowsum = sum(Minit{n},2);
  tmpIx = find(rowsum == 0);
  if (isempty(tmpIx) == false)
    Minit{n}(tmpIx,1) = 1.0e-8;
  end
end

% Start with the initial guess, normalized using the vector L1 norm.
M = normalize(Minit,[],1);

% Sparse tensor flag affects how Pi and Phi are computed.
if isa(X,'sptensor')
    isSparse = true;
else
    isSparse = false;
end

% Initialize output arrays.
fnEvals = zeros(maxOuterIters,1);
kktViolations = -ones(maxOuterIters,1);
nInnerIters = zeros(maxOuterIters,1);
nzeros = zeros(maxOuterIters,1);
times = zeros(maxOuterIters,1);

if (printOuterItn > 0)
    fprintf('\nCP_PQNR (alternating Poisson regression using quasi-Newton)\n');
end
dispLineWarn = (printInnerItn > 0);

% Start the wall clock timer.
tic;

if (isSparse && precomputeSparseIndices)
    % Precompute sparse index sets for all the row subproblems.
    % Takes more memory but can cut execution time significantly in some cases.
    if (printOuterItn > 0)
        fprintf('  Precomputing sparse index sets...');
    end
    sparseIx = cell(N);
    for n = 1:N
        num_rows = size(M{n},1);
        sparseIx{n} = cell(num_rows,1);
        for jj = 1:num_rows
            sparseIx{n}{jj} = find(X.subs(:,n) == jj);
        end
    end
    if (printOuterItn > 0)
        fprintf('done\n');
    end
end

%% Main Loop: Iterate until convergence or a max threshold is reached.
for iter = 1:maxOuterIters

    isConverged = true;
    kktModeViolations = zeros(N,1);
    countInnerIters = zeros(1,N);

    % Alternate thru each factor matrix, A_1, A_2, ... , A_N.
    for n = 1:N

        % Shift the weight from lambda to mode n.
        M = redistribute(M,n);

        % Calculate Khatri-Rhao product of all matrices but the n-th.
        if (isSparse == false)
            % Data is not a sparse tensor.
            Pi = tt_calcpi_prowsubprob(X, isSparse, M, R, n, N, []);
            X_mat = double(tenmat(X,n));
        end

        num_rows = size(M{n},1);
        isRowNOTconverged = zeros(1,num_rows);

        % Loop over the row subproblems in mode n.
        for jj = 1:num_rows

            % Get data values for row jj of matricized mode n.
            if (isSparse)
                % Data is a sparse tensor.
                if (precomputeSparseIndices == false)
                    sparse_indices = find(X.subs(:,n) == jj);
                else
                    sparse_indices = sparseIx{n}{jj};
                end
                if (isempty(sparse_indices))
                    % The row jj of matricized tensor X in mode n is empty.
                    M{n}(jj,:) = 0;
                    continue
                end
                x_row = X.vals(sparse_indices);

                % Calculate just the columns of Pi needed for this row.
                Pi = tt_calcpi_prowsubprob(X, isSparse, M, ...
                                           R, n, N, sparse_indices);
            else
                x_row = X_mat(jj,:);
            end

            % Get current values of the row subproblem variables.
            m_row = M{n}(jj,:);

            % Initialize L-BFGS storage for the row subproblem.
            delm = zeros(R, nSizeLBFGS);
            delg = zeros(R, nSizeLBFGS);
            rho = zeros(nSizeLBFGS, 1);
            lbfgsPos = 1;
            m_rowOLD = [];
            gradOLD = [];

            % Iteratively solve the row subproblem with projected qNewton steps.
            for i = 1:maxInnerIters
                % Calculate the gradient.
                [gradM, phi_row] = calc_grad(isSparse, Pi, epsDivZero, ...
                                             x_row, m_row);

                if (i == 1)
                    % Original cp_aprPQN_row code (and plb_row) does a gradient
                    % step to prime the L-BFGS approximation.  However, it means
                    % a row subproblem that already converged wastes time
                    % doing a gradient step before checking KKT conditions.
                    % TODO: fix in a future release.
                    m_rowOLD = m_row;
                    gradOLD = gradM;
                    [m_row, f, f_unit, f_new, num_evals] ...
                        = tt_linesearch_prowsubprob(-gradM', gradM', ...
                                                    m_rowOLD, ...
                                                    1, 1/2, 10, 1.0e-4, ...
                                                    isSparse, x_row, Pi, ...
                                                    phi_row, dispLineWarn);
                    fnEvals(iter) = fnEvals(iter) + num_evals;
                    [gradM, phi_row] = calc_grad(isSparse, Pi, epsDivZero, ...
                                                 x_row, m_row);
                end

                % Compute the row subproblem kkt_violation.
                % Experiments in the original paper used this:
                %kkt_violation = norm(abs(min(m_row,gradM')),2);
                % Now we use | KKT |_inf:
                kkt_violation = max(abs(min(m_row,gradM')));

                % Report largest row subproblem initial violation.
                if ((i == 1) && (kkt_violation > kktModeViolations(n)))
                     kktModeViolations(n) = kkt_violation;
                end

                if (mod(i, printInnerItn) == 0)
                    fprintf('    Mode = %1d, Row = %d, InnerIt = %d', ...
                            n, jj, i);
                    if (i == 1)
                        fprintf(', RowKKT = %.2e\n', kkt_violation);
                    else
                        fprintf(', RowKKT = %.2e, RowObj = %.4e\n', ...
                                kkt_violation, -f_new);
                    end
                end

                % Check for row subproblem convergence.
                if (kkt_violation < stoptol)
                    break;
                else
                    % Not converged, so m_row will be modified.
                    isRowNOTconverged(jj) = 1;
                end

                % Update the L-BFGS approximation.
                tmp_delm = m_row - m_rowOLD;
                tmp_delg = gradM - gradOLD;
                tmp_rho = 1 / (tmp_delm * tmp_delg);
                if ((tmp_rho > 0.0) && (isinf(tmp_rho) == false))
                    delm(:,lbfgsPos) = tmp_delm;
                    delg(:,lbfgsPos) = tmp_delg;
                    rho(lbfgsPos) = tmp_rho;
                else
                    % Rho is required to be positive; if not, then skip
                    % the L-BFGS update pair.  The recommended safeguard for
                    % full BFGS is Powell damping, but not clear how to damp
                    % in 2-loop L-BFGS.
                    if (dispLineWarn)
                        fprintf('WARNING: skipping L-BFGS update, rho would be 1 / %.2e\n', ...
                                (tmp_delm * tmp_delg));
                    end
                    % Roll back lbfgsPos since it will increment later.
                    if (lbfgsPos == 1)
                        if (rho(nSizeLBFGS) > 0)
                            lbfgsPos = nSizeLBFGS;
                        else
                            % Fatal error, should not happen.
                            fprintf('ERROR: L-BFGS first iterate is bad\n');
                            return;
                        end
                    else
                        lbfgsPos = lbfgsPos - 1;
                    end
                end

                % Calculate the search direction.
                search_dir = getSearchDirPqnr(m_row, gradM, epsActSet, ...
                                              delm, delg, rho, lbfgsPos, ...
                                              i, dispLineWarn);
                lbfgsPos = mod(lbfgsPos, nSizeLBFGS) + 1;

                m_rowOLD = m_row;
                gradOLD = gradM;

                % Perform a projected linesearch and update variables.
                % Start from a unit step length, decrease by 1/2, stop with
                % sufficient decrease of 1.0e-4 or at most 10 steps.
                [m_row, f, f_unit, f_new, num_evals] ...
                    = tt_linesearch_prowsubprob(search_dir', gradOLD', m_rowOLD, ...
                                                1, 1/2, 10, 1.0e-4, ...
                                                isSparse, x_row, Pi, ...
                                                phi_row, dispLineWarn);
                fnEvals(iter) = fnEvals(iter) + num_evals;
            end

            M{n}(jj,:) = m_row;
            countInnerIters(n) = countInnerIters(n) + i;

        end

        % Test if all row subproblems have converged, which means that
        % no variables in this mode were changed.
        if (sum(isRowNOTconverged) ~= 0)
            isConverged = false;
        end

        % Shift weight from mode n back to lambda.
        M = normalize(M,[],1,n);

        % Total number of inner iterations for a given outer iteration,
        % totalled across all modes and all row subproblems in each mode.
        nInnerIters(iter) = nInnerIters(iter) + countInnerIters(n);
    end

    % Save output items for the outer iteration.
    num_zero = 0;
    for n = 1:N
        num_zero = num_zero + nnz(find(M{n} == 0.0));
    end
    nzeros(iter) = num_zero;
    kktViolations(iter) = max(kktModeViolations);

    % Print outer iteration status.
    if (mod(iter,printOuterItn) == 0)
        fprintf('%4d. Ttl Inner Its: %d, KKT viol = %.2e, obj = %.8e, nz: %d\n', ...
        iter, nInnerIters(iter), kktViolations(iter), tt_loglikelihood(X,M), ...
        num_zero);
    end

    times(iter) = toc;

    % Check for convergence
    if (isConverged)
        break;
    end
    if (times(iter) > stoptime)
        fprintf('Exiting because time limit exceeded\n');
        break;
    end

end

t_stop = toc;

%% Clean up final result and set output items.
M = normalize(M,'sort',1);
loglike = tt_loglikelihood(X,M);

if (printOuterItn > 0)
    % For legacy reasons, compute "fit", the fraction explained by the model.
    % Fit is in the range [0,1], with 1 being the best fit.
    normX = norm(X);
    normresidual = sqrt( normX^2 + norm(M)^2 - 2 * innerprod(X,M) );
    fit = 1 - (normresidual / normX);

    fprintf('===========================================\n');
    fprintf(' Final log-likelihood = %e \n', loglike);
    fprintf(' Final least squares fit = %e \n', fit);
    fprintf(' Final KKT violation = %7.7e\n', kktViolations(iter));
    fprintf(' Total inner iterations = %d\n', sum(nInnerIters));
    fprintf(' Total execution time = %.2f secs\n', t_stop);
end

out = struct;
out.params = params.Results;
out.obj = loglike;
out.kktViolations = kktViolations(1:iter);
out.fnEvals = fnEvals(1:iter);
out.nInnerIters = nInnerIters(1:iter);
out.nZeros = nzeros(1:iter);
out.times = times(1:iter);
out.ttlTime = t_stop;

end

%----------------------------------------------------------------------

function [grad_row, phi_row] = calc_grad(isSparse, Pi, eps_div_zero, x_row, m_row)
%function grad_row = calc_grad(isSparse, Pi, eps_div_zero, x_row, m_row)
% Compute the gradient for a PQNR row subproblem.
%
%   isSparse     - true if x_row is sparse, false if dense
%   Pi           - matrix
%   eps_div_zero - safeguard value to prevent division by zero
%   x_row        - row vector of data values for the row subproblem
%   m_row        - vector of variables for the row subproblem
%
%   Returns the gradient vector for a row subproblem.

    if (isSparse)
        v = m_row * Pi';
        w = x_row' ./ max(v, eps_div_zero);
        phi_row = w * Pi;

    else
        v = m_row * Pi';
        w = x_row ./ max(v, eps_div_zero);
        phi_row = w * Pi;
    end

    grad_row = (ones(size(phi_row)) - phi_row)';
end

%----------------------------------------------------------------------

function [d] = getSearchDirPqnr (m_row, grad, epsActSet, ...
                                 delta_m, delta_g, rho, lbfgs_pos, ...
                                 iters, disp_warn)
% Compute the search direction by projecting with L-BFGS.
%
%   m_row     - current variable values
%   grad      - gradient at m_row
%   epsActSet - Bertsekas tolerance for active set determination
%   delta_m   - L-BFGS array of vector variable deltas
%   delta_g   - L-BFGS array of gradient deltas
%   lbfgs_pos - pointer into L-BFGS arrays
%
%   Returns
%     d       - search direction based on current L-BFGS and grad
%
%   Adapted from MATLAB code of Dongmin Kim and Suvrit Sra written in 2008.
%   Modified extensively to solve row subproblems and use a better linesearch;
%   see the reference at the top of this file for details.

    lbfgsSize = size(delta_m,2);

    % Determine active and free variables.
    % If epsActSet is zero, then the following works:
    %   fixedVars = find((m_row == 0) & (grad' > 0));
    % For the general case this works but is less clear and assumes m_row > 0:
    %   fixedVars = find((grad' > 0) & (m_row <= min(epsActSet,grad')));
    projGradStep = (m_row - grad') .* (m_row - grad' > 0);
    wk = norm(m_row - projGradStep);
    fixedVars = find((grad' > 0) & (m_row <= min(epsActSet,wk)));

    d = -grad;
    d(fixedVars) = 0;

    if ((delta_m(:,lbfgs_pos)' * delta_g(:,lbfgs_pos)) == 0.0)
        % Cannot proceed with this L-BFGS data; most likely the iteration
        % has converged, so this is rarely seen.
        if (disp_warn)
            fprintf('WARNING: L-BFGS update is orthogonal, using gradient\n');
        end
        return;
    end

    alpha = ones(lbfgsSize,1);
    k = lbfgs_pos;

    % Perform an L-BFGS two-loop recursion to compute the search direction.

    for i = 1 : min(iters, lbfgsSize)
        alpha(k) = rho(k) * delta_m(:, k)' * d;
        d = d - alpha(k) * delta_g(:, k);
        k = lbfgsSize - mod(1 - k, lbfgsSize);
    end

    coef = 1 / rho(lbfgs_pos) / (delta_g(:, lbfgs_pos)' * delta_g(:, lbfgs_pos));
    d = coef * d;

    for i = 1 : min(iters, lbfgsSize)
        k = mod(k, lbfgsSize) + 1;
        b = rho(k) * delta_g(:, k)' * d;
        d = d + (alpha(k) - b) * delta_m(:, k);
    end

    d(fixedVars) = 0;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Main algorithm PDNR
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [M, out] = tt_cp_apr_pdnr(X, R, Minit, varargin)
%TT_CP_APR_PDNR Compute nonnegative CP with alternating Poisson regression.
%
%   tt_cp_apr_pdnr(X, R, ...) computes an estimate of the best rank-R
%   CP model of a tensor X using an alternating Poisson regression.
%   The algorithm solves "row subproblems" in each alternating subproblem,
%   using a Hessian of size R^2.
%   The function is typically called by cp_apr.
%
%   The model is solved by nonlinear optimization, and the code literally
%   minimizes the negative of log-likelihood.  However, printouts to the
%   console reverse the sign to show maximization of log-likelihood.
%
%   The function call can specify optional parameters and values.
%   Valid parameters and their default values are:
%      'stoptol'       - Tolerance on the overall KKT violation {1.0e-4}
%      'stoptime'      - Maximum number of seconds to run {1e6}
%      'maxiters'      - Maximum number of iterations {1000}
%      'maxinneriters' - Maximum inner iterations per outer iteration {10}
%      'epsDivZero'    - Safeguard against divide by zero {1.0e-10}
%      'printitn'      - Print every n outer iterations; 0 for no printing {1}
%      'printinneritn' - Print every n inner iterations {0}
%      'epsActive'     - Bertsekas tolerance for active set {1.0e-8}
%      'mu0'           - Initial damping parameter {1.0e-5}
%      'precompinds'   - Precompute sparse tensor indices to run faster {true}
%      'inexact'       - Compute inexact Newton steps {true}
%
%   Return values are:
%      M                 - ktensor model with R components
%      out.fnEvals       - number of row obj fn evaluations per outer iteration
%      out.kktViolations - maximum KKT violation per iteration
%      out.nInnerIters   - number of inner iterations per outer iteration
%      out.nZeros        - number of factor elements equal to zero per iteration
%      out.obj           - final log-likelihood objective
%                          (minimization objective is actually -1 times this)
%      out.ttlTime       - time algorithm took to converge or reach max
%      out.times         - cumulative time through each outer iteration
%
%   REFERENCE: Samantha Hansen, Todd Plantenga, Tamara G. Kolda.
%   Newton-Based Optimization for Nonnegative Tensor Factorizations,
%   arXiv:1304.4964 [math.NA], April 2013,
%   URL: http://arxiv.org/abs/1304.4964. Submitted for publication.
%
%   See also CP_APR, KTENSOR, TENSOR, SPTENSOR.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

%% Set algorithm parameters from input or by using defaults.
params = inputParser;
params.addParamValue('epsActive', 1e-8, @isscalar);
params.addParamValue('epsDivZero',1e-10,@isscalar);
params.addParamValue('maxinneriters',10,@isscalar);
params.addParamValue('maxiters',1000,@(x) isscalar(x) & x > 0);
params.addParamValue('precompinds',true,@(x) isa(x,'logical'));
params.addParamValue('inexact',true,@(x) isa(x,'logical'));
params.addParamValue('mu0',1e-5,@isscalar);
params.addParamValue('printinneritn',0,@isscalar);
params.addParamValue('printitn',1,@isscalar);
params.addParamValue('stoptime',1e6,@isscalar);
params.addParamValue('stoptol',1e-4,@isscalar);
params.parse(varargin{:});

%% Copy from params object.
epsActSet               = params.Results.epsActive;
epsDivZero              = params.Results.epsDivZero;
maxInnerIters           = params.Results.maxinneriters;
maxOuterIters           = params.Results.maxiters;
mu0                     = params.Results.mu0;
precomputeSparseIndices = params.Results.precompinds;
inexactNewton           = params.Results.inexact;
printInnerItn           = params.Results.printinneritn;
printOuterItn           = params.Results.printitn;
stoptime                = params.Results.stoptime;
stoptol                 = params.Results.stoptol;

% Extract the number of modes in tensor X.
N = ndims(X);

% If the initial guess has any rows of all zero elements, then modify
% so the row subproblem is not taking log(0).  Values will be restored to
% zero later if the unfolded X for the row has no nonzeros.
for n = 1:N
  rowsum = sum(Minit{n},2);
  tmpIx = find(rowsum == 0);
  if (isempty(tmpIx) == false)
    Minit{n}(tmpIx,1) = 1.0e-8;
  end
end

% Start with the initial guess, normalized using the vector L1 norm.
M = normalize(Minit,[],1);

% Sparse tensor flag affects how Pi and Phi are computed.
if isa(X,'sptensor')
    isSparse = true;
else
    isSparse = false;
end

% Initialize output arrays.
fnEvals = zeros(maxOuterIters,1);
kktViolations = -ones(maxOuterIters,1);
nInnerIters = zeros(maxOuterIters,1);
nzeros = zeros(maxOuterIters,1);
times = zeros(maxOuterIters,1);

if (printOuterItn > 0)
    fprintf('\nCP_PDNR (alternating Poisson regression using damped Newton)\n');
end
dispLineWarn = (printInnerItn > 0);

% Start the wall clock timer.
tic;

if (isSparse && precomputeSparseIndices)
    % Precompute sparse index sets for all the row subproblems.
    % Takes more memory but can cut execution time significantly in some cases.
    if (printOuterItn > 0)
        fprintf('  Precomputing sparse index sets...');
    end
    sparseIx = cell(N);
    for n = 1:N
        num_rows = size(M{n},1);
        sparseIx{n} = cell(num_rows,1);
        for jj = 1:num_rows
            sparseIx{n}{jj} = find(X.subs(:,n) == jj);
        end
    end
    if (printOuterItn > 0)
        fprintf('done\n');
    end
end

e_vec = ones(1,R);

rowsubprobStopTol = stoptol;

%% Main Loop: Iterate until convergence or a max threshold is reached.
for iter = 1:maxOuterIters

    isConverged = true;
    kktModeViolations = zeros(N,1);
    countInnerIters = zeros(1,N);

    % Alternate thru each factor matrix, A_1, A_2, ... , A_N.
    for n = 1:N

        % Shift the weight from lambda to mode n.
        M = redistribute(M,n);

        % Calculate Khatri-Rhao product of all matrices but the n-th.
        if (isSparse == false)
            % Data is not a sparse tensor.
            Pi = tt_calcpi_prowsubprob(X, isSparse, M, R, n, N, []);
            X_mat = double(tenmat(X,n));
        end

        num_rows = size(M{n},1);
        isRowNOTconverged = zeros(1,num_rows);

        % Loop over the row subproblems in mode n.
        for jj = 1:num_rows
            % Initialize the damped Hessian parameter for the row subproblem.
            mu = mu0;

            % Get data values for row jj of matricized mode n.
            if (isSparse)
                % Data is a sparse tensor.
                if (precomputeSparseIndices == false)
                    sparse_indices = find(X.subs(:,n) == jj);
                else
                    sparse_indices = sparseIx{n}{jj};
                end
                if (isempty(sparse_indices))
                    % The row jj of matricized tensor X in mode n is empty.
                    M{n}(jj,:) = 0;
                    continue
                end
                x_row = X.vals(sparse_indices);

                % Calculate just the columns of Pi needed for this row.
                Pi = tt_calcpi_prowsubprob(X, isSparse, M, ...
                                           R, n, N, sparse_indices);
            else
                x_row = X_mat(jj,:);
            end

            % Get current values of the row subproblem variables.
            m_row = M{n}(jj,:);

            % Iteratively solve the row subproblem with projected Newton steps.
            innerIterMaximum = maxInnerIters;
            if (inexactNewton && (iter == 1))
                innerIterMaximum = 2;
            end
            for i = 1:innerIterMaximum
                % Calculate the gradient.
                [phi_row, ups_row] ...
                    = calc_partials(isSparse, Pi, epsDivZero, x_row, m_row);
                gradM = (e_vec - phi_row)';

                % Compute the row subproblem kkt_violation.
                % Experiments in the original paper used this:
                %kkt_violation = norm(abs(min(m_row,gradM')),2);
                % Now we use | KKT |_inf:
                kkt_violation = max(abs(min(m_row,gradM')));

                % Report largest row subproblem initial violation.
                if ((i == 1) && (kkt_violation > kktModeViolations(n)))
                     kktModeViolations(n) = kkt_violation;
                end

                if (mod(i, printInnerItn) == 0)
                    fprintf('    Mode = %1d, Row = %d, InnerIt = %d', ...
                            n, jj, i);
                    if (i == 1)
                        fprintf(', RowKKT = %.2e\n', kkt_violation);
                    else
                        fprintf(', RowKKT = %.2e, RowObj = %.4e\n', ...
                                kkt_violation, -f_new);
                    end
                end

                % Check for row subproblem convergence.
                if (kkt_violation < rowsubprobStopTol)
                    break;
                else
                    % Not converged, so m_row will be modified.
                    isRowNOTconverged(jj) = 1;
                end

                % Calculate the search direction.
                [search_dir, predicted_red] ...
                    = getSearchDirPdnr(Pi, ups_row, R, gradM, m_row, mu, epsActSet);

                % Perform a projected linesearch and update variables.
                % Start from a unit step length, decrease by 1/2, stop with
                % sufficient decrease of 1.0e-4 or at most 10 steps.
                [m_rowNEW, f_old, f_unit, f_new, num_evals] ...
                    = tt_linesearch_prowsubprob(search_dir', gradM', m_row, ...
                                                1, 1/2, 10, 1.0e-4, ...
                                                isSparse, x_row, Pi, ...
                                                phi_row, dispLineWarn);
                fnEvals(iter) = fnEvals(iter) + num_evals;
                m_row = m_rowNEW;

                % Update damping parameter mu based on the unit step length,
                % which is returned in f_unit.
                actual_red = f_old - f_unit;
                rho = actual_red / (-predicted_red);
                if (predicted_red == 0)
                    mu = 10 * mu;
                elseif (rho < 1/4)
                    mu = (7/2) * mu;
                elseif (rho > 3/4)
                    mu = (2/7) * mu;
                end
            end

            M{n}(jj,:) = m_row;
            countInnerIters(n) = countInnerIters(n) + i;

        end

        % Test if all row subproblems have converged, which means that
        % no variables in this mode were changed.
        if (sum(isRowNOTconverged) ~= 0)
            isConverged = false;
        end

        % Shift weight from mode n back to lambda.
        M = normalize(M,[],1,n);

        % Total number of inner iterations for a given outer iteration,
        % totalled across all modes and all row subproblems in each mode.
        nInnerIters(iter) = nInnerIters(iter) + countInnerIters(n);
    end

    % Save output items for the outer iteration.
    num_zero = 0;
    for n = 1:N
        num_zero = num_zero + nnz(find(M{n} == 0.0));
    end
    nzeros(iter) = num_zero;
    kktViolations(iter) = max(kktModeViolations);
    if (inexactNewton)
        rowsubprobStopTol = max(stoptol, kktViolations(iter) / 100.0);
    end

    % Print outer iteration status.
    if (mod(iter,printOuterItn) == 0)
        fprintf('%4d. Ttl Inner Its: %d, KKT viol = %.2e, obj = %.8e, nz: %d\n', ...
        iter, nInnerIters(iter), kktViolations(iter), tt_loglikelihood(X,M), ...
        num_zero);
    end

    times(iter) = toc;

    % Check for convergence
    if (isConverged && (inexactNewton == false))
        break;
    end
    if (isConverged && (inexactNewton == true) && (rowsubprobStopTol <= stoptol))
        break;
    end
    if (times(iter) > stoptime)
        fprintf('Exiting because time limit exceeded\n');
        break;
    end

end

t_stop = toc;

%% Clean up final result and set output items.
M = normalize(M,'sort',1);
loglike = tt_loglikelihood(X,M);

if (printOuterItn > 0)
    % For legacy reasons, compute "fit", the fraction explained by the model.
    % Fit is in the range [0,1], with 1 being the best fit.
    normX = norm(X);
    normresidual = sqrt( normX^2 + norm(M)^2 - 2 * innerprod(X,M) );
    fit = 1 - (normresidual / normX);

    fprintf('===========================================\n');
    fprintf(' Final log-likelihood = %e \n', loglike);
    fprintf(' Final least squares fit = %e \n', fit);
    fprintf(' Final KKT violation = %7.7e\n', kktViolations(iter));
    fprintf(' Total inner iterations = %d\n', sum(nInnerIters));
    fprintf(' Total execution time = %.2f secs\n', t_stop);
end

out = struct;
out.params = params.Results;
out.obj = loglike;
out.kktViolations = kktViolations(1:iter);
out.fnEvals = fnEvals(1:iter);
out.nInnerIters = nInnerIters(1:iter);
out.nZeros = nzeros(1:iter);
out.times = times(1:iter);
out.ttlTime = t_stop;

end

%----------------------------------------------------------------------

function [phi_row, ups_row] ...
    = calc_partials(isSparse, Pi, eps_div_zero, x_row, m_row)
% Compute derivative quantities for a PDNR row subproblem.
%
%   isSparse     - true if x_row is sparse, false if dense
%   Pi           - matrix
%   eps_div_zero - safeguard value to prevent division by zero
%   x_row        - row vector of data values for the row subproblem
%   m_row        - vector of variables for the row subproblem
%
%   Returns two vectors for a row subproblem:
%     phi_row - gradient of row subproblem, except for a constant
%     ups_row - intermediate quantity (upsilon) used for second derivatives

    if (isSparse)
        v = m_row * Pi';
        w = x_row' ./ max(v, eps_div_zero);
        phi_row = w * Pi;
        u = v .^ 2;
        ups_row = x_row' ./ max(u, eps_div_zero);

    else
        v = m_row * Pi';
        w = x_row ./ max(v, eps_div_zero);
        phi_row = w * Pi;
        u = v .^ 2;
        ups_row = x_row ./ max(u, eps_div_zero);
    end

end

%----------------------------------------------------------------------

function H = getHessian(upsilon, Pi, free_indices)
% Return the Hessian for one PDNR row subproblem of M{n}, for just the rows and
% columns corresponding to the free variables.

    num_free = length(free_indices);
    H = zeros(num_free,num_free);
    for i = 1:num_free
        for j = i:num_free
            c = free_indices(i);
            d = free_indices(j);
            val = sum(upsilon' .* Pi(:,c) .* Pi(:,d));
            H(i,j) = val;
            H(j,i) = val;
        end
    end

end

%----------------------------------------------------------------------

function [search_dir, pred_red] ...
    = getSearchDirPdnr (Pi, ups_row, R, gradM, m_row, mu, epsActSet)
% Compute the search direction for PDNR using a two-metric projection
% with damped Hessian.
%
%   Pi        - matrix
%   ups_row   - intermediate quantity (upsilon) used for second derivatives
%   R         - number of variables for the row subproblem
%   gradM     - gradient vector for the row subproblem
%   m_row     - vector of variables for the row subproblem
%   mu        - damping parameter
%   epsActSet - Bertsekas tolerance for active set determination
%
%   Returns:
%     search_dir - search direction vector
%     pred_red   - predicted reduction in quadratic model

    search_dir = zeros(R,1);
    projGradStep = (m_row - gradM') .* (m_row - gradM' > 0);
    wk = norm(m_row - projGradStep);

    % Determine active and free variables.
    num_free = 0;
    free_indices_tmp = zeros(R,1);
    for r = 1:R
        if ((m_row(r) <= min(epsActSet,wk)) && (gradM(r) > 0) )
            % Variable is not free (belongs to set A or G).
            if (m_row(r) ~= 0)
                % Variable moves according to the gradient (set G).
                search_dir(r) = -gradM(r);
            end
        else
            % Variable is free (set F).
            num_free = num_free + 1;
            free_indices_tmp(num_free) = r;
        end
    end
    free_indices = free_indices_tmp(1:num_free);

    % Compute the Hessian for free variables.
    Hessian_free = getHessian(ups_row, Pi, free_indices);
    grad_free = -gradM(free_indices);

    % Compute the damped Newton search direction over free variables.
    search_dir(free_indices) ...
        = linsolve(Hessian_free + (mu * eye(num_free)), grad_free);

    % If the Hessian is too ill-conditioned, use gradient descent.
    [~, msgid] = lastwarn('MATLAB:noWarning');
    if (strcmp(msgid,'MATLAB:nearlySingularMatrix'))
        fprintf('WARNING: damped Hessian is nearly singular\n');
        search_dir = -gradM;
    end

    % Calculate expected reduction in the quadratic model of the objective.
    q = search_dir(free_indices)' ...
        * (Hessian_free + (mu * eye(num_free))) ...
        * search_dir(free_indices);
    pred_red = (search_dir(free_indices)' * gradM(free_indices)) + (0.5 * q);
    if (pred_red > 0)
        fprintf('ERROR: expected decrease is positive\n');
        search_dir = -gradM;
    end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Main algorithm MU
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [M, output] = tt_cp_apr_mu(X, R, Minit, varargin)
%TT_CP_APR_MU Compute nonnegative CP with alternating Poisson regression.
%
%   tt_cp_apr_mu(X, R, ...) computes an estimate of the best rank-R
%   CP model of a tensor X using an alternating Poisson regression.
%   The algorithm solves each alternating subproblem using multiplicative
%   updates with adjustments for values near zero.
%   The function is typically called by cp_apr.
%
%   The function call can specify optional parameters and values.
%   Valid parameters and their default values are:
%      'stoptol'       - Tolerance on the overall KKT violation {1.0e-4}
%      'stoptime'      - Maximum number of seconds to run {1e6}
%      'maxiters'      - Maximum number of iterations {1000}
%      'maxinneriters' - Maximum inner iterations per outer iteration {10}
%      'epsDivZero'    - Safeguard against divide by zero {1.0e-10}
%      'printitn'      - Print every n outer iterations; 0 for no printing {1}
%      'printinneritn' - Print every n inner iterations {0}
%      'kappatol'      - Tolerance on complementary slackness {1.0e-10}
%      'kappa'         - Offset to fix complementary slackness {100}
%
%   Return values are:
%      M                 - ktensor model with R components
%      out.kktViolations - maximum KKT violation per iteration
%      out.nInnerIters   - number of inner iterations per outer iteration
%      out.nViolations   - number of factor matrices needing complementary
%                          slackness adjustment per iteration
%      out.obj           - final log-likelihood objective
%      out.ttlTime       - time algorithm took to converge or reach max
%      out.times         - cumulative time through each outer iteration
%
%   REFERENCE: E. C. Chi and T. G. Kolda. On Tensors, Sparsity, and
%   Nonnegative Factorizations, arXiv:1112.2414 [math.NA], December 2011,
%   URL: http://arxiv.org/abs/1112.2414. Submitted for publication.
%
%   See also CP_APR, KTENSOR, TENSOR, SPTENSOR.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

%% Set algorithm parameters from input or by using defaults.
params = inputParser;
params.addParamValue('epsDivZero',1e-10,@isscalar);
params.addParamValue('kappa',1e-2,@isscalar);
params.addParamValue('kappatol',1e-10,@isscalar);
params.addParamValue('maxinneriters',10,@isscalar);
params.addParamValue('maxiters',1000,@(x) isscalar(x) & x > 0);
params.addParamValue('printinneritn',0,@isscalar);
params.addParamValue('printitn',1,@isscalar);
params.addParamValue('stoptime',1e6,@isscalar);
params.addParamValue('stoptol',1e-4,@isscalar);
params.parse(varargin{:});

%% Extract dimensions of X and number of dimensions of X.
N = ndims(X);

%% Copy from params object.
epsilon       = params.Results.epsDivZero;
tol           = params.Results.stoptol;
stoptime      = params.Results.stoptime;
maxOuterIters = params.Results.maxiters;
kappa         = params.Results.kappa;
kappaTol      = params.Results.kappatol;
maxInnerIters = params.Results.maxinneriters;
printOuterItn = params.Results.printitn;
printInnerItn = params.Results.printinneritn;
kktViolations = -ones(maxOuterIters,1);
nInnerIters   = zeros(maxOuterIters,1);
times         = zeros(maxOuterIters,1);

%% Set up and error checking on initial guess for U.
if isa(Minit,'ktensor')
    if ndims(Minit) ~= N
        error('Initial guess does not have the right number of dimensions');
    end

    if ncomponents(Minit) ~= R
        error('Initial guess does not have the right number of components');
    end

    for n = 1:N
        if size(Minit,n) ~= size(X,n)
            error('Dimension %d of the initial guess is the wrong size',n);
        end
    end
elseif strcmp(Minit,'random')
    F = cell(N,1);
    for n = 1:N
        F{n} = rand(size(X,n),R);
    end
    Minit = ktensor(F);
else
    error('The selected initialization method is not supported');
end

%% Set up for iterations - initializing M and Phi.
M = normalize(Minit,[],1);
Phi = cell(N,1);
kktModeViolations = zeros(N,1);

if printOuterItn > 0
  fprintf('\nCP_APR:\n');
end

nViolations = zeros(maxOuterIters,1);

% Start the wall clock timer.
tic;

% PDN-R and PQN-R benefit from precomputing sparse indices of X for each
% mode subproblem.  However, MU execution time barely changes, so the
% precompute option is not offered.

%% Main Loop: Iterate until convergence.
for iter = 1:maxOuterIters

    isConverged = true;
    for n = 1:N

        % Make adjustments to entries of M{n} that are violating
        % complementary slackness conditions.
        if (iter > 1)
            V = (Phi{n} > 1) & (M{n} < kappaTol);
            if any(V(:))
                nViolations(iter) = nViolations(iter) + 1;
                M{n}(V>0) = M{n}(V>0) + kappa;
            end
        end

        % Shift the weight from lambda to mode n
        M = redistribute(M,n);

        % Calculate product of all matrices but the n-th
        % (Sparse case only calculates entries corresponding to nonzeros in X)
        Pi = calculatePi(X, M, R, n, N);

        % Do the multiplicative updates
        for i = 1:maxInnerIters

            % Count the inner iterations
            nInnerIters(iter) = nInnerIters(iter) + 1;

            % Calculate matrix for multiplicative update
            Phi{n} = calculatePhi(X, M, R, n, Pi, epsilon);

            % Check for convergence
            kktModeViolations(n) = max(abs(vectorizeForMu(min(M.U{n},1-Phi{n}))));
            if (kktModeViolations(n) < tol)
                break;
            else
                isConverged = false;
            end

            % Do the multiplicative update
            M{n} = M{n} .* Phi{n};

            % Print status
             if mod(i, printInnerItn)==0
                 fprintf('    Mode = %1d, Inner Iter = %2d, KKT violation = %.6e\n', n, i, kktModeViolations(n));
             end
        end

        % Shift weight from mode n back to lambda
        M = normalize(M,[],1,n);

    end

    kktViolations(iter) = max(kktModeViolations);

    if (mod(iter,printOuterItn)==0)
        fprintf(' Iter %4d: Inner Its = %2d KKT violation = %.6e, nViolations = %2d\n', ...
        iter, nInnerIters(iter), kktViolations(iter), nViolations(iter));
    end
    times(iter) = toc;

    % Check for convergence
    if (isConverged)
        if printOuterItn>0
            fprintf('Exiting because all subproblems reached KKT tol.\n');
        end
        break;
    end
    if (times(iter) > stoptime)
        if printOuterItn>0
            fprintf('Exiting because time limit exceeded.\n');
        end
        break;
    end
end
t_stop = toc;

%% Clean up final result
M = normalize(M,'sort',1);

obj = tt_loglikelihood(X,M);
if printOuterItn>0
    normX = norm(X);
    normresidual = sqrt( normX^2 + norm(M)^2 - 2 * innerprod(X,M) );
    fit = 1 - (normresidual / normX); %fraction explained by model
    fprintf('===========================================\n');
    fprintf(' Final log-likelihood = %e \n', obj);
    fprintf(' Final least squares fit = %e \n', fit);
    fprintf(' Final KKT violation = %7.7e\n', kktViolations(iter));
    fprintf(' Total inner iterations = %d\n', sum(nInnerIters));
    fprintf(' Total execution time = %.2f secs\n', t_stop);
end

output = struct;
output.params = params.Results;
output.kktViolations = kktViolations(1:iter);
output.nInnerIters = nInnerIters(1:iter);
output.nViolations = nViolations(1:iter);
output.nTotalIters = sum(nInnerIters);
output.times = times(1:iter);
output.ttlTime = t_stop;
output.obj = obj;

end

function Pi = calculatePi(X, M, R, n, N)

if (isa(X,'sptensor'))
    Pi = ones(nnz(X), R);
    for nn = [1:n-1,n+1:N]
        Pi = M{nn}(X.subs(:,nn),:) .* Pi;
    end
else
    U = M.U;
    Pi = khatrirao(U{[1:n-1,n+1:N]},'r');
end

end

function Phi = calculatePhi(X, M, R, n, Pi, epsilon)

if (isa(X,'sptensor'))
    Phi = -ones(size(X,n),R);
    xsubs = X.subs(:,n);
    v = sum(M.U{n}(xsubs,:).*Pi,2);
    wvals = X.vals ./ max(v, epsilon);
    for r = 1:R
        Yr = accumarray(xsubs, wvals .* Pi(:,r), [size(X,n) 1]);
        Phi(:,r) = Yr;
    end
else
    Xn = double(tenmat(X,n));
    V = M.U{n}*Pi';
    W = Xn ./ max(V, epsilon);
    Y = W * Pi;
    Phi = Y;
end

end

%----------------------------------------------------------------------

function y = vectorizeForMu(x)
y = x(:);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Shared Internal Functions
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function Pi = tt_calcpi_prowsubprob(X, isSparse, M, R, n, N, sparse_indices)
% TT_CALCPI_PROWSUBPROB Compute Pi for a row subproblem.
%
%   X              - data tensor
%   isSparse       - true if X is sparse, false if dense
%   M              - current factor matrices
%   R              - number of columns in each factor matrix
%   n              - mode
%   N              - number of modes (equals the number of factor matrices)
%   sparse_indices - indices of row subproblem nonzero elements
%
%   Returns Pi matrix.
%
%   Intended for use by CP_PDN and CP_PQN.
%   Based on calculatePi() in CP_APR, which computes for an entire mode
%   instead of a single row subproblem.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

    if (isSparse)
        % X is a sparse tensor.  Compute Pi for the row subproblem specified
        % by sparse_indices.
        num_row_nnz = length(sparse_indices);

        Pi = ones(num_row_nnz, R);
        for nn = [1:n-1,n+1:N]
            Pi = M{nn}(X.subs(sparse_indices,nn),:) .* Pi;
        end
    else
        % X is a dense tensor.  Compute Pi for all rows in the mode.
        U = M.U;
        Pi = khatrirao(U{[1:n-1,n+1:N]},'r');
    end

end

%----------------------------------------------------------------------

function [m_new, f_old, f_1, f_new, num_evals] ...
    = tt_linesearch_prowsubprob(d, grad, m_old, step_len, step_red, ...
                                max_steps, suff_decr, isSparse, x_row, Pi, ...
                                phi_row, disp_warn)
% TT_LINESEARCH_PROWSUBPROB Perform a line search on a row subproblem.
%
%   d         - search direction
%   grad      - gradient vector at m_old
%   m_old     - current variable values
%   step_len  - initial step length, which is the maximum possible step length
%   step_red  - step reduction factor (suggest 1/2)
%   max_steps - maximum number of steps to try (suggest 10)
%   suff_decr - sufficient decrease for convergence (suggest 1.0e-4)
%   isSparse  - sparsity flag for computing the objective
%   x_row     - row subproblem data, for computing the objective
%   Pi        - Pi matrix, for computing the objective
%   phi_row   - 1-grad, more accurate if failing over to multiplicative update
%   disp_warn - true means warning messages are displayed
%
%   Returns
%     m_new     - new (improved) variable values
%     num_evals - number of times objective was evaluated
%     f_old     - objective value at m_old
%     f_1       - objective value at m_old + step_len * d
%     f_new     - objective value at m_new

    minDescentTol = 1.0e-7;
    smallStepTol = 1.0e-7;

    stepSize = step_len;

    % Evaluate the current objective value.
    f_old = -1 * tt_loglikelihood_row(isSparse, x_row, m_old, Pi);
    num_evals = 1;
    count = 1;

    while (count <= max_steps)
        % Compute a new step and project it onto the positive orthant.
        m_new = m_old + (stepSize .* d);
        m_new = m_new .* (m_new > 0);

        % Check that it is a descent direction.
        gDotd = sum(grad .* (m_new - m_old));
        if (gDotd > 0) || (sum(m_new) < minDescentTol)
            % Don't evaluate the objective if not a descent direction
            % or if all of the elements of m_new are close to zero.
            f_new = Inf;
            if (count == 1)
               f_1 = f_new;
            end

            stepSize = stepSize * step_red;
            count = count + 1;
        else
            % Evaluate objective function at new iterate.
            f_new = -1 * tt_loglikelihood_row(isSparse, x_row, m_new, Pi);
            num_evals = num_evals + 1;
            if (count == 1)
               f_1 = f_new;
            end

            % Check for sufficient decrease.
            if (f_new <= f_old + suff_decr * gDotd)
                break;
            else
                stepSize = stepSize * step_red;
                count = count + 1;
            end
        end
    end

    % Check if the line search failed.
    if (isinf(f_1) == 1)
        % Unit step failed; return a value that yields ared = 0.
        f_1 = f_old;
    end
    if (   ((count >= max_steps) && (f_new > f_old)) ...
        || (sum(m_new) < smallStepTol) )

        % Fall back on a multiplicative update step (scaled steepest descent).
        % Experiments indicate it works better than a unit step in the direction
        % of steepest descent, which would be the following:
        % m_new = m_old - (step_len * grad);     % steepest descent
        % A simple update formula follows, but suffers from round-off error
        % when phi_row is tiny:
        % m_new = m_old - (m_old .* grad);
        % Use this for best accuracy:
        m_new = m_old .* phi_row;                % multiplicative update

        % Project to the constraints and reevaluate the subproblem objective.
        m_new = m_new .* (m_new > 0);
        f_new = -1 * tt_loglikelihood_row(isSparse, x_row, m_new, Pi);
        num_evals = num_evals + 1;

        % Let the caller know the search direction made no progress.
        f_1 = f_old;

        if (disp_warn)
            fprintf('WARNING: line search failed, using multiplicative update step\n');
        end
    end

end

%----------------------------------------------------------------------

function f = tt_loglikelihood_row(isSparse, x, m, Pi)
%TT_LOGLIKELIHOOD_ROW Compute log-likelihood of one row subproblem.
%
%    The row subproblem for a given mode includes one row of matricized tensor
%    data (x) and one row of the model (m) in the same matricized mode.
%    Then
%       (dense case)
%          m:  R-length vector
%          x:  J-length vector
%          Pi: R x J matrix
%       (sparse case)
%          m:  R-length vector
%          x:  p-length vector, where p = nnz in row of matricized data tensor
%          Pi: R x p matrix
%       F = - (sum_r m_r - sum_j x_j * log (m * Pi_j)
%           where Pi_j denotes the j^th column of Pi
%           NOTE: Rows of Pi' must sum to one
%
%   isSparse - true if x is sparse, false if dense
%   x        - vector of data values
%   m        - vector of model values
%   Pi       - matrix
%
%   Returns the log-likelihood probability f.
%
%   Intended for use by CP_PDN and CP_PQN.
%   Similar to tt_loglikelihood() in CP_APR, which computes log likelihood
%   for the entire tensor instead of a single row subproblem.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

    term1 = -sum(m);

    if (isSparse)
        term2 = sum(x' .* log(m * Pi'));
    else
        b_pi = m * Pi';
        term2 = 0;
        for i = 1:length(x)
            if (x(i) == 0)
                % Define zero times log(anything) to be zero.
            else
                term2 = term2 + x(i) .* log(b_pi(i));
            end
        end
    end

    f = term1 + term2;

end

%----------------------------------------------------------------------

function f = tt_loglikelihood(X,M)
%TT_LOGLIKELIHOOD Compute log-likelihood of data X with model M.
%
%   F = TT_LOGLIKELIHOOD(X,M) computes the log-likelihood of model M given
%   data X, where M is a ktensor and X is a tensor or sptensor.
%   Specifically, F = - (sum_i m_i - x_i * log_i) where i is a multiindex
%   across all tensor dimensions.
%
%   See also cp_apr, tensor, sptensor, ktensor.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

N = ndims(X);

if ~isa(M, 'ktensor')
    error('M must be a ktensor');
end

M = normalize(M,1,1);

if isa(X, 'sptensor')
    xsubs = X.subs;
    A = M.U{1}(xsubs(:,1),:);
    for n = 2:N
       A = A .* M.U{n}(xsubs(:,n),:);
    end
    f = sum(X.vals .* log(sum(A,2))) - sum(sum(M.U{1}));
else
%{
% Old code is probably faster, but returns NaN if X and M are both zero
% for some element.
    f = sum(sum(double(tenmat(X,1)) .* log(double(tenmat(M,1))))) - sum(sum(M.U{1}));
%}
    % The check for x==0 is also in tt_loglikelihood_row.
    dX = double(tenmat(X,1));
    dM = double(tenmat(M,1));
    f = 0;
    for i = 1:size(dX,1)
      for j = 1:size(dX,2)
        if (dX(i,j) == 0.0)
          % Define zero times log(anything) to be zero.
        else
          f = f + dX(i,j) * log(dM(i,j));
        end
      end
    end
    f = f - sum(sum(M.U{1}));

end

end


function X = sampleimages( winsize, samples )
% sampleimages - gathers image patches from Olshausens images.
%

% Start by loading the images
fprintf('Loading images...\n');
load ../data/IMAGES;
num_images=size(IMAGES,2);
image_size=sqrt(size(IMAGES,1));

% This will hold the patches
X=zeros(winsize^2,samples);
totalsamples = 0;

for i=1:num_images

  % Choose an image for this batch
  this_image=reshape(IMAGES(:,i),image_size,image_size)';
  BUFF=4;

  % Determine how many patches to take
  getsample = floor(samples/num_images);
  if i==num_images, getsample = samples-totalsamples; end

  % Extract patches at random from this image to make data vector X
  for j=1:getsample
    r=BUFF+ceil((image_size-winsize-2*BUFF)*rand);
    c=BUFF+ceil((image_size-winsize-2*BUFF)*rand);
    totalsamples = totalsamples + 1;
    X(:,totalsamples) = ...
	reshape( this_image(r:r+winsize-1,c:c+winsize-1),winsize^2,1);
  end

end


% TRUNCATE_FILTER Truncates the Fourier transform of a filter
%
% Usage
%    filter = TRUNCATE_FILTER(filter_f, threshold, lowpass)
%
% Input
%    filter_f (numeric): The Fourier representation of the filter.
%
% Output
%    filter (struct): The truncated representation of the filter. See descrip-
%       tion for more details.
%
% Description
%    By extracting and storing only the Fourier transform coefficients whose
%    are above a certain threshold relative to the maximum value, storage
%    requirements for the filters are lessened and computation is sped up
%    since only non-zero coefficients are multiplied during convolution.
%
%    The support of the Fourier transform is defined so that all coefficients
%    with a magnitude above threshold*fmax are kept, where fmax is the maxi-
%    mum magnitude, and so that length(filter_f) divided by the size of the
%    support is a power of 2.
%
%    The output filter contains the fields:
%       filter.type (char): Fixed to 'fourier_truncated'.
%       filter.N (int): The original size of the filter.
%       filter.recenter (boolean): Indicates whether the Fourier transform
%          should be recentered after convolution. This is always true.
%       filter.start (int): The frequency index where the support of the
%          Fourier transform starts.
%       filter.coefft (numeric): The values of the Fourier coefficients on the
%          support.
%
% See also
%    OPTIMIZE_FILTER, PERIODIZE_FILTER

function filter = truncate_filter(filter_f, threshold, lowpass)
	N = length(filter_f);

	filter.type = 'fourier_truncated';
	filter.N = N;

	% Could have filter.recenter = lowpass, but since we don't know if we're
	% taking the modulus or not, we always need to recenter.
	filter.recenter = 1;

	[temp,ind_max] = max(filter_f);
	filter_f = circshift(filter_f,N/2-ind_max);
	ind1 = find(abs(filter_f)>(max(abs(filter_f))*threshold),1);
	ind2 = find(abs(filter_f)>(max(abs(filter_f))*threshold),1,'last');

	len = ind2-ind1+1;
	len = filter.N/2^(floor(log2(filter.N/len)));

	ind1 = round(round((ind1+ind2)/2)-len/2);
	ind2 = ind1+len-1;

	filter_f = filter_f(mod([ind1:ind2]-1,filter.N)+1);

	filter.coefft = filter_f;
	filter.start = ind1-(N/2-ind_max);
end


function img=ndimfilter(im,kernel,varargin)
%
% img=ndimfilter(im,kernel,r,sigma)
%
% filter an ND array using a specified filter using convolution
%
% author: Qianqian Fang (q.fang at neu.edu)
%
% input:
%    im: input ND array
%    kernel: can be an ND array, or a string. if string, the below filters
%        are supported:
%        'box': box filter (need r)
%        'gaussian': Gaussian filter (need r,sigma input)
%    r: kernel half-width, the output is 2*r+1 in each dimension; if
%       missing, use 1
%    sigma: the standard deviation of the Gaussian; if not given, use 1; if
%       set to inf, output box filter
%
% output:
%    img: the filtered ND array
%
% -- this function is part of the Iso2Mesh Toolbox (http://iso2mesh.sf.net)
%    License: GPL v3 or later, see LICENSE.txt for details
%

if(nargin<2)
    kernel='box';
end
if(ischar(kernel))
    switch(kernel)
        case 'box'
            kernel=ndgaussian(varargin{1},inf,ndims(im));
        case 'gaussian'
            kernel=ndgaussian(varargin{1},varargin{2},ndims(im));
        otherwise
            error('filter type %s is not supported',type);
    end
end

img=convn(im,kernel,'same');


function [ value, ifault ] = ppnd ( p )

%*****************************************************************************80
%
%% PPND produces the normal deviate value corresponding to lower tail area = P.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 January 2008
%
%  Author:
%
%    Original FORTRAN77 version by J Beasley, S Springer.
%    FORTRAN90 version by John Burkardt.
%
%  Reference:
%
%    J Beasley, S Springer,
%    Algorithm AS 111:
%    The Percentage Points of the Normal Distribution,
%    Applied Statistics,
%    Volume 26, Number 1, 1977, pages 118-121.
%
%  Parameters:
%
%    Input, real P, the value of the cumulative probability
%    densitity function.  0 < P < 1.
%
%    Output, real VALUE, the normal deviate value with the property that
%    the probability of a standard normal deviate being less than or
%    equal to PPND is P.
%
%    Output, integer IFAULT, error flag.
%    0, no error.
%    1, P <= 0 or P >= 1.  PPND is returned as 0.
%
  a0 = 2.50662823884;
  a1 = -18.61500062529;
  a2 = 41.39119773534;
  a3 = -25.44106049637;
  b1 = -8.47351093090;
  b2 = 23.08336743743;
  b3 = -21.06224101826;
  b4 = 3.13082909833;
  c0 = -2.78718931138;
  c1 = -2.29796479134;
  c2 = 4.85014127135;
  c3 = 2.32121276858;
  d1 = 3.54388924762;
  d2 = 1.63706781897;
  split = 0.42;

  ifault = 0;
%
%  0.08 < P < 0.92
%
  if ( abs ( p - 0.5 ) <= split )

    r = ( p - 0.5 ) * ( p - 0.5 );

    value = ( p - 0.5 ) * ( ( ( ...
        a3   * r ...
      + a2 ) * r ...
      + a1 ) * r ...
      + a0 ) / ( ( ( ( ...
        b4   * r ...
      + b3 ) * r ...
      + b2 ) * r ...
      + b1 ) * r ...
      + 1.0 );
%
%  P < 0.08 or P > 0.92,
%  R = min ( P, 1-P )
%
  elseif ( 0.0 < p & p < 1.0 )

    if ( 0.5 < p )
      r = sqrt ( - log ( 1.0 - p ) );
    else
      r = sqrt ( - log ( p ) );
    end

    value = ( ( ( ...
        c3   * r ...
      + c2 ) * r ...
      + c1 ) * r ...
      + c0 ) / ( ( ...
        d2   * r ...
      + d1 ) * r ...
      + 1.0 );

    if ( p < 0.5 )
      value = - value;
    end
%
%  P <= 0.0 or 1.0 <= P
%
  else

    ifault = 1;
    value = 0.0;

  end

  return
end


function [elem,HB] = uniformcoarsen3red(elem)
%% UNIFORMCOARSEN3RED uniform coarsening of red refinement
%
% [elem,HB] = uniformcoarsen3red(elem) remove grid points added by uniform
% refinement in 3-D.
%
% It is mainly used to get multilevel decomposition in multigrid methods.
% The input matrix elem stands for the fine mesh and the output one for the
% coarse mesh. The HB records the hierarchical structure of added points
% going from the coarse to the fine mesh such that HB(:,2:3) are two parent
% nodes of HB(:,1).
%
%   See also: uniformrefine3, mg, uniformcoarsen, uniformcoarsen3
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

HB = [];
NT = size(elem,1);
if mod(NT,8)==0
    NTc = NT/8; % number of triangles in the coarse grid
else
%     display('Not from red refinement');
    return
end

%% Find points
t1 = 1:NTc; t2 = t1+NTc; t3 = t2+NTc; t4 = t3+NTc;
if any(elem(t1,2)~=elem(t2,1)) || any(elem(t1,3)~=elem(t3,1)) || ...
   any(elem(t4,1)~=elem(t1,4)) || any(elem(t4,2)~=elem(t2,4))
%     display('Not from red refinement');
    return
end
p1 = elem(t1,1);
p2 = elem(t2,2);
p3 = elem(t3,3);
p4 = elem(t4,4);
p5 = elem(t1,2);
p6 = elem(t1,3);
p7 = elem(t1,4);
p8 = elem(t2,3);
p9 = elem(t2,4);
p10 = elem(t3,4);

%% Remove tetrahedron
elem(t1,:) = [p1 p2 p3 p4];
elem = elem(t1,:);

%% Record HB
HB(p5,:) = [p5 p1 p2];
HB(p6,:) = [p6 p1 p3];
HB(p7,:) = [p7 p1 p4];
HB(p8,:) = [p8 p2 p3];
HB(p9,:) = [p9 p2 p4];
HB(p10,:) = [p10 p3 p4];
Nc = max(elem(:));
HB = HB(Nc+1:end,:);


function b = wraprad(a)
%WRAPRAD Map angles measured in radians to the interval [-pi,pi).
%
%   B = WRAPRAD(A) maps the angles in A to their equivalent in the interval
%   [-pi,pi) by adding or subtracting the appropriate multiple of 2*pi.
%
%   See also WRAPDEG, WRAPGRAD, UNWRAPDEG, UNWRAP.

%   Author:      Peter J. Acklam
%   Time-stamp:  2003-10-13 14:27:25 +0200
%   E-mail:      pjacklam@online.no
%   URL:         http://home.online.no/~pjacklam

   % check number of input arguments
   error(nargchk(1, 1, nargin));

   PI = pi;
   TWOPI = 2*PI;

   b = a - TWOPI * floor((a + PI) / TWOPI);


function [configure, obj] = Estimate_Weight(configure, Seqs)

% initialization
configure.weight = rand(length(configure.id),1);
tau = configure.tau;
obj = zeros(configure.epoch * length(Seqs), 1);

tic
for n = 1:configure.epoch
    ind = randperm(length(Seqs));
    lr = configure.lr * (0.9)^(n-1);
    for m = 1:length(Seqs)
        X = [Seqs(ind(m)).Time; Seqs(ind(m)).Mark];
        [Prob, Delta] = ChosenProbability(X, configure);

        grad = 0;
        for t1 = 1:size(Prob,1)-tau
            for t2 = 1:size(Prob,2)-tau
                if t1~=t2
                    obj((n-1)*length(Seqs)+m) = obj((n-1)*length(Seqs)+m) +...
                       Prob(t1, t2)*Prob(t1+tau, t2+tau);
                    grad = grad + ...
                        2*Prob(t1, t2)*Prob(t1+tau, t2+tau)*...
                        (Delta(:,t1,t2)+Delta(:,t1+tau,t2+tau));
                end
            end
        end
        configure.weight = configure.weight - lr * grad;

        fprintf('epoch=%d, #seq=%d/%d, obj=%f, ||grad||=%.4f, time=%.2fsec\n',...
            n, m, length(Seqs), obj((n-1)*length(Seqs)+m), norm(grad), toc);
    end

end


clear all

Nsamples = 41500;
EulerSaved = zeros(Nsamples, 3);

dt = 0.01;

for k=1:Nsamples
  [p q r] = GetGyro();
  [ax ay] = GetAccel();

  [phi_a theta_a] = EulerAccel(ax, ay);

  [phi theta psi] = EulerUKF([phi_a theta_a]', [p q r], dt);

  EulerSaved(k, :) = [ phi theta psi ];
end

PhiSaved   = EulerSaved(:, 1) * 180/pi;
ThetaSaved = EulerSaved(:, 2) * 180/pi;
PsiSaved   = EulerSaved(:, 3) * 180/pi;

t = 0:dt:Nsamples*dt-dt;

figure
plot(t, PhiSaved)

figure
plot(t, ThetaSaved)

figure
plot(t, PsiSaved)


% This example shows how to calculate the even and odd modes
% of two coupled waveguides, using the semivectorial modesolver

% Refractive indices:
n1 = 3.34;          % Lower cladding
n2 = 3.44;          % Core
n3 = 1.00;          % Upper cladding (air)

% Layer heights:
h1 = 2.0;           % Lower cladding
h2 = 1.3;           % Core thickness
h3 = 0.5;           % Upper cladding
rh = 1.1;           % Ridge height

% Horizontal dimensions:
rw = 1.0;           % Ridge half-width
d = 2.5;            % center-to-center separation
side = 2.5;         % Space on side

% Grid size:
dx = 0.0125;        % grid size (horizontal)
dy = 0.0125;        % grid size (vertical)

lambda = 1.55;      % wavelength
nmodes = 1;         % number of modes to compute

[x,y,xc,yc,nx,ny,eps,edges] = waveguidemeshfull([n1,n2,n3],[h1,h2,h3],...
                                          rh,rw,[(d/2-rw),side],dx,dy);

% First, we calculate the symmetric mode

[Ex1,neff1] = svmodes(lambda,n2,nmodes,dx,dy,eps,'000S','EX');
fprintf(1,'neff(1) = %.6f\n',neff1);

% Next, we calculate the symmetric mode

[Ex2,neff2] = svmodes(lambda,n2,nmodes,dx,dy,eps,'000A','EX');
fprintf(1,'neff(2) = %.6f\n',neff2);

subplot(211);
contourmode(x,y,Ex1);
title('Ex (Symmetric TE Mode)'); xlabel('x'); ylabel('y');
for v = edges, line(v{:}); end

subplot(212);
contourmode(x,y,Ex2);
title('Ex (Antisymmetric TE Mode)'); xlabel('x'); ylabel('y');
for v = edges, line(v{:}); end


function    g = lfmGradientSigmaH3(gamma1, gamma2, sigma2, t1, t2, preFactor, mode, term)

% LFMGRADIENTSIGMAH3 Gradient of the function h_i(z) with respect \sigma.
% FORMAT
% DESC Computes the gradient of the function h_i(z) with respect to the
% length-scale of the input "force", \sigma.
% ARG gamma1 : Gamma value for first system.
% ARG gamma2 : Gamma value for second system.
% ARG sigma2 : length scale of latent process.
% ARG t1 : first time input (number of time points x 1).
% ARG t2 : second time input (number of time points x 1).
% ARG mode: indicates in which way the vectors t1 and t2 must be transposed
% RETURN g : Gradient of the function with respect to \sigma.
%
% COPYRIGHT : David Luengo, 2007, 2008, Mauricio Alvarez, 2008
%
% MODIFICATIONS : Mauricio Alvarez, 2008

% SEEALSO : lfmKernGradient, lfmXlfmKernGradient, lfmGradientSigmaUpsilon

% KERN

% Gradient

if nargin<8
    term = [];
end

if ~mode
    if ~term
        g = preFactor*lfmGradientSigmaUpsilonMatrix(gamma1,sigma2, t1,t2);
    else
        gradupsilon = lfmGradientSigmaUpsilonMatrix(gamma1,sigma2, t1,t2);
        g = -preFactor(1)*gradupsilon + preFactor(2)*conj(gradupsilon);
    end
else
    g =  preFactor(1)*lfmGradientSigmaUpsilonMatrix(gamma1,sigma2,t1,t2) + ...
        preFactor(2)*lfmGradientSigmaUpsilonMatrix(gamma2,sigma2,t1,t2);
end


function v= visutil_goodContourValues(mi, ma, ct)
% to be optimized

if ct<0,
  spacing= (ma-mi)/(-ct+1);
  gro= floor(log10(spacing));
  res= spacing*10^-gro;
  if res<1.5,
    resi= 1;
  elseif res<3.5,
    resi= 2;
  elseif res<7,
    resi= 5;
  else
    resi= 1;
    gro= gro+1;
  end
  ct= resi*10^gro;
end

v= ct*ceil(mi/ct):ct:ct*floor(ma/ct);


%%Ex. 8 Extracting an individual element of an array

a = [3 6 7];
b = [1 9 4 5];
c = a(2) + b(4)

%Output:  c = 11


function y = ttsv(A,x,n)
%TTSV Tensor times same vector in multiple modes.
%
%   Y = TTSV(A,X) multiples the tensor A by the vector X in all modes.
%
%   Y = TTSV(A,X,-1) multiplies the tensor A by the vector X in all modes
%   but the first. Returns the answer as a normal MATLAB array (not a
%   tensor).
%
%   Y = TTSV(A,X,-2) multiplies the tensor A by the vector X in all modes
%   but the first two. Returns the answer as a normal MATLAB matrix (not a
%   tensor).
%
%   Y = TTSV(A,X,-N) multiplies the tensor A by the vector X is all modes
%   but the first N.
%
%   See also TENSOR, TENSOR/TTV.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

%% Process inputs (only two simple cases are supported)
if ~exist('n','var')
    n = 0;
elseif (n > 0)
    error('Invalid usage');
end

%% Create X.
P = ndims(A);
[X{1:P}] = deal(x);

%% Calculate
if (n == 0)
    y = ttv(A,X);
elseif (n == -1) || (n == -2)
    y = double(ttv(A,X,-(1:-n)));
else
    y = ttv(A,X,-(1:-n));
end


%% Parameter setting %%

param.nx = 64;
param.ny = 64;
param.nz = 64;

param.sx = 64; % mm
param.sy = 64; % mm
param.sz = 64; % mm

% Detector setting, according to Varian Trilogy OBI (real size)
param.su = 256;	% mm
param.sv = 200;	% mm

%The real detector panel pixel density (number of pixels)
param.nu = 128;
param.nv = 100;

% X-ray source and detector setting
param.DSD = 900;    %  Distance source to detector
param.DSO = 400;	%  X-ray source to object axis distance

% angle setting
dir = -1;   % gantry rotating direction
param.deg = 0:4.3:360;
param.deg = param.deg*dir;
param.nProj = length(param.deg);

% filter='ram-lak','cosine', 'hamming', 'hann'
param.filter='hamming'; % high pass for sintetic images

param.dx = param.sx/param.nx;
param.dy = param.sy/param.ny;
param.dz = param.sz/param.nz;
param.du = param.su/param.nu;
param.dv = param.sv/param.nv;

param.off_u = 0; param.off_v = 0; % detector rotation shift (real size)

% % % For fast CPU calculation % % %
param.xs = [-(param.nx-1)/2:1:(param.nx-1)/2]*param.dx;
param.ys = [-(param.ny-1)/2:1:(param.ny-1)/2]*param.dy;
param.zs = [-(param.nz-1)/2:1:(param.nz-1)/2]*param.dz;

param.us = (-(param.nu-1)/2:1:(param.nu-1)/2)*param.du + param.off_u;
param.vs = (-(param.nv-1)/2:1:(param.nv-1)/2)*param.dv + param.off_v;

%% projection
load Phantom64.mat % img

proj = CTprojection(img,param);

for i=1:param.nProj
    figure(1); imagesc(max(proj(:,:,i)',0)); axis off; axis equal; colormap gray; colorbar;
    title(num2str(i));
    pause(0.01);
end

%% filtered backprojection
% filter='ram-lak','shepp-logan','cosine', 'hamming', 'hann'
param.filter='hamming';
% param.filter='ram-lak';

Reconimg = CTbackprojection(proj, param);

for i=1:param.nz
    figure(2); imagesc(max(Reconimg(:,:,i),0)); axis off; axis equal; colormap gray; colorbar;
    title(num2str(i));
    pause(0.01);
end

%% MLEM
param.filter = 'none';

img = ones(param.nx, param.ny, param.nz,'single');
Norimg = CTbackprojection(ones(param.nu, param.nv, param.nProj, 'single'), param);

for iter = 1:50

    proj_ratio = proj./CTprojection(img,param);
    proj_ratio(isnan(proj_ratio)) = 0;
    proj_ratio(isinf(proj_ratio)) = 0;

    img_ratio = CTbackprojection(proj_ratio, param)./Norimg;
    img_ratio(isnan(img_ratio)) = 0;
    img_ratio(isinf(img_ratio)) = 0;

    img = img.*img_ratio;

    figure(3); imagesc(max(img(:,:,round(end/2)),0)); axis off; axis equal; colormap gray; colorbar;
    title(['Iteration - ',num2str(iter)]);
    pause(0.1);
end


function D=bwdistsc(bw,aspect)
% D=BWDISTSC(BW,ASPECT)
% BWDISTSC computes Euclidean distance transform of a binary 3D image BW.
% Distance transform assigns to each pixel in BW a number that is the
% distance from that pixel to the nearest nonzero pixel in BW. BWDISTSC
% can accept a regular 2D image, a 3D array, and a cell array of 2D slices.
% ASPECT is a 3-component vector defining the voxel-aspect-ratio for BW.
% If ASPECT is not given, [1 1 1] isotropic aspect ratio is assumed.
%
% BWDISTSC uses fast optimized scan algorithm and cell-arrays to
% represent internal data, and is less demanding to physical memory as
% well as in many cases up to 10 times faster than MATLAB's native bwdist.
%
% Example:
% bw=zeros(100,100,100);
% bw(40:60,40:60,40:60)=1;
% tic;D=bwdist(bw);toc
% tic;D=bwdistsc(bw);toc
%
% BWDISTSC tries to use MATLAB bwdist from image processing toolbox for 2D
% scans if possible, which is faster, otherwise BWDISTSC will use its own
% algorithm to also perform 2D scans. Own algorithm is also used if x- and
% y-anisotropy scales are not equal; therefore, if your data has only one
% axis that is anisotropic, it is always advantageous to feed it to
% BWDISTSC so that the anisotropic axis is z.
%
%(c) Yuriy Mishchenko HHMI JFRC Chklovskii Lab JUL 2007
% Updated Yuriy Mishchenko (Toros University) SEP 2013

% This implementation uses optimized forward-backward scan version of the
% algorithm of the original bwdistsc (2007), which substantially improves
% its speed and simplifies the code. The improvement is described in the
% part on the selection initial point in the SIVP paper below. The original
% implementation is still used in bwdistsc1, since forward-backward scan
% does not allow limiting computation to a fixed distance value MAXVAL.

% This code is free for use or modifications, just please give credit
% where appropriate. And if you modify code or fix bugs, please drop
% me a message at gmyuriy@hotmail.com.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Scan algorithms below use following Lema:                     %
% LEMA: let F(X,z) be lower envelope of a family of parabola:   %
% F(X,z)=min_{i} [G_i(X)+(z-k_i)^2];                            %
% and let H_k(X,z)=A(X)+(z-k)^2 be a parabola.                  %
% Then for H_k(X,z)==F(X,z) at each X there exist at most       %
% two solutions k1<k2 such that H_k12(X,z)=F(X,z), and          %
% H_k(X,z)<F(X,z) is restricted to at most k1<k2.               %
% Here X is any-dimensional coordinate.                         %
%                                                               %
% Thus, simply scan away from any z such that H_k(X,z)<F(X,z)   %
% in either direction as long as H_k(X,z)<F(X,z) and update     %
% F(X,z). Note that need to properly choose starting point;     %
% starting point is any z such that H_k(X,z)<F(X,z); z==k is    %
% usually, but not always the starting point!!!                 %
% usually, but not always the starting point!                   %
%                                                               %
% Citation:                                                     %
% Mishchenko Y. (2013) A function for fastcomputation of large  %
% discrete Euclidean distance transforms in three or more       %
% dimensions in Matlab. Signal, Image and Video Processing      %
% DOI: 10.1007/s11760-012-0419-9.                               %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% parse inputs
if(nargin<2 || isempty(aspect)) aspect=[1 1 1]; end

% determine geometry of the data
if(iscell(bw)) shape=[size(bw{1}),length(bw)]; else shape=size(bw); end

% correct this for 2D data
if(length(shape)==2) shape=[shape,1]; end
if(length(aspect)==2) aspect=[aspect,1]; end

% allocate internal memory
D=cell(1,shape(3)); for k=1:shape(3) D{k}=zeros(shape(1:2)); end

%%%%%%%%%%%%% scan along XY %%%%%%%%%%%%%%%%
for k=1:shape(3)
    if(iscell(bw)) bwXY=bw{k}; else bwXY=bw(:,:,k); end

    % initialize arrays
    DXY=zeros(shape(1:2));
    D1=zeros(shape(1:2));

    % if can, use 2D bwdist from image processing toolbox
    if(exist('bwdist') && aspect(1)==aspect(2))
        D1=aspect(1)^2*bwdist(bwXY).^2;
    else    % if not, use full XY-scan
        %%%%%%%%%%%%%%% X-SCAN %%%%%%%%%%%%%%%
        % reference for nearest "on"-pixel in bw in x direction down

        %  scan bottow-up (for all y), copy x-reference from previous row
        %  unless there is "on"-pixel in that point in current row, then
        %  that's the nearest pixel now
        xlower=repmat(Inf,shape(1:2));

        xlower(1,find(bwXY(1,:)))=1;    % fill in first row
        for i=2:shape(1)
            xlower(i,:)=xlower(i-1,:);  % copy previous row
            xlower(i,find(bwXY(i,:)))=i;% unless there is pixel
        end

        % reference for nearest "on"-pixel in bw in x direction up
        xupper=repmat(Inf,shape(1:2));

        xupper(end,find(bwXY(end,:)))=shape(1);
        for i=shape(1)-1:-1:1
            xupper(i,:)=xupper(i+1,:);
            xupper(i,find(bwXY(i,:)))=i;
        end

        % build (X,Y) for points for which distance needs to be calculated
        idx=find(~bwXY); [x,y]=ind2sub(shape(1:2),idx);

        % update distances as shortest to "on" pixels up/down in the above
        DXY(idx)=aspect(1)^2*min((x-xlower(idx)).^2,(x-xupper(idx)).^2);

        %%%%%%%%%%%%%%% Y-SCAN %%%%%%%%%%%%%%%
        % this will be the envelop of parabolas at different y
        D1=repmat(Inf,shape(1:2));

        p=shape(2);
        for i=1:shape(2)
            % some auxiliary datasets
            d0=DXY(:,i);

            % selecting starting point for x:
            % * if parabolas are incremented in increasing order of y,
            %   then all below-envelop intervals are necessarily right-
            %   open, which means starting point can always be chosen
            %   at the right end of y-axis
            % * if starting point exists it should be below existing
            %   current envelop at the right end of y-axis
            dtmp=d0+aspect(2)^2*(p-i)^2;
            L=D1(:,p)>dtmp;
            idx=find(L);
            D1(idx,p)=dtmp(L);

            % these will keep track along which X should
            % keep updating distances
            map_lower=L;
            idx_lower=idx;

            % scan from starting points down in increments of 1
            for ii=p-1:-1:1
                % new values for D
                dtmp=d0(idx_lower)+aspect(2)^2*(ii-i)^2;

                % these pixels are to be updated
                L=D1(idx_lower,ii)>dtmp;
                D1(idx_lower(L),ii)=dtmp(L);

                % other pixels are removed from scan
                map_lower(idx_lower)=L;
                idx_lower=idx_lower(L);

                if(isempty(idx_lower)) break; end
            end
        end
    end
    D{k}=D1;
end

%%%%%%%%%%%%% scan along Z %%%%%%%%%%%%%%%%
D1=cell(size(D));
for k=1:shape(3)
  D1{k}=repmat(Inf,shape(1:2));
end

% start building the envelope
p=shape(3);
for k=1:shape(3)
    % if there are no objects in this slice, nothing to do
    if(isinf(D{k}(1,1)))
      continue;
    end

    % selecting starting point for (x,y):
    % * if parabolas are incremented in increasing order of k, then all
    %   intersections are necessarily at the right end of the envelop,
    %   and so the starting point can be always chosen as the right end
    %   of the axis

    % check which points are valid starting points, & update the envelop
    dtmp=D{k}+aspect(3)^2*(p-k)^2;
    L=D1{p}>dtmp;
    D1{p}(L)=dtmp(L);

    % map_lower keeps track of which pixels can be yet updated with the
    % new distance, i.e. all such XY that had been under the envelop for
    % all Deltak up to now, for Deltak<0
    map_lower=L;

    % these are maintained to keep fast track of whether map is empty
    idx_lower=find(map_lower);

    % scan away from the starting points in increments of -1
    for kk=p-1:-1:1
        % new values for D
        dtmp=D{k}(idx_lower)+aspect(3)^2*(kk-k)^2;

        % these pixels are to be updated
        L=D1{kk}(idx_lower)>dtmp;
        map_lower(idx_lower)=L;
        D1{kk}(idx_lower(L))=dtmp(L);

        % other pixels are removed from scan
        idx_lower=idx_lower(L);

        if(isempty(idx_lower)) break; end
    end
end

% prepare the answer
if(iscell(bw))
    D=cell(size(bw));
    for k=1:shape(3) D{k}=sqrt(D1{k}); end
else
    D=zeros(shape);
    for k=1:shape(3) D(:,:,k)=sqrt(D1{k}); end
end

end


function result=RS_PF_Rationality(m,forecasterror,x,pdate)
%%   INPUT:
%%   m = size of rolling window
%%	forecastserror = actual - forecast
%%	x is the variable you want to test coefficients equal to zero
%%   pdate is the vector of dates

T=length(forecasterror);
if m>=T
    err_dlg = errordlg('The window size of the Rossi-Sekhposyan (JAE,2016) test cannot be equal to or larger than the forecasted sample.');
    waitfor(err_dlg);
elseif m==0
    err_dlg = errordlg('The window size of the Rossi-Sekhposyan (JAE,2016) test cannot be equal to zero.');
    waitfor(err_dlg);
end

cvtable = [
      NaN     0.1000    0.2000    0.3000    0.4000    0.5000    0.6000    0.7000    0.8000    0.9000
    1.0000   10.5066    9.0503    8.0245    7.1035    6.3957    5.6112    5.1113    4.6141    3.9748
    2.0000   21.2392   18.0544   15.8290   13.9122   13.0720   11.1526   10.4549    9.0570    7.8723
    3.0000   31.4497   26.8866   23.7832   21.4577   19.6097   17.4180   15.3225   13.5010   11.4381
    4.0000   43.5150   36.9028   32.8187   28.4075   25.1774   23.3645   20.5785   17.6700   15.5384
    5.0000   52.4148   45.7998   39.6896   35.7848   32.0200   28.4850   26.2204   23.1738   19.1090
    6.0000   62.6771   54.3749   47.4711   42.4503   38.4920   34.9394   30.4063   27.9807   23.8787
    7.0000   74.8406   62.3659   56.2449   49.0721   44.4213   39.6189   36.4280   33.0852   26.9654
    8.0000   84.5728   72.8813   63.2267   56.8973   51.5069   45.7856   41.3975   36.7853   31.2008
    9.0000  109.6177   95.3818   87.2701   77.9691   72.5004   63.5162   60.0533   51.9429   47.0975
   10.0000  122.4825  107.9759   94.2844   88.3139   80.2026   71.6698   67.4220   58.3127   54.7817 ];

P     = size(forecasterror,1);
mu    = m/P;
cvcol = round(mu*10)+1;

if cvcol==1;
    cvcol = 2;
end;

if cvcol>10;
    cvcol = 10;
end;

nreg = size(x,2);
cv   = cvtable(nreg+1,cvcol);

resultt=[];
for t=m:P
    % Calculate OLS Wald-Test
    out     = bear.RS_PF_OLS_Wald(forecasterror(t-m+1:t,:),x(t-m+1:t,:));
    resultt = [resultt;out];
end;

ptruncdate = pdate(m:end,1);

result.MZ         = max(resultt);
result.cv         = cv;
result.ptruncdate = ptruncdate;
result.resultt    = resultt(:,1);
result.cvones     = cv*ones(size(resultt,1),1);


clear
clc

fitnessfcn = @my_first_multi;   % Function handle to the fitness function
nvars = 2;                      % Number of decision variables
lb = [-5,-5];                   % Lower bound
ub = [5,5];                     % Upper bound
A = []; b = [];                 % No linear inequality constraints
Aeq = []; beq = [];             % No linear equality constraints
options = gaoptimset('ParetoFraction',0.3,'PopulationSize',100,'Generations',200,'StallGenLimit',200,'TolFun',1e-100,'PlotFcns',@gaplotpareto);

[x,fval] = gamultiobj(fitnessfcn,nvars, A,b,Aeq,beq,lb,ub,options);


function r8lib_test127 ( )

%*****************************************************************************80
%
%% R8LIB_TEST127 tests R8VEC_MAX_INDEX and R8VEC_MIN_INDEX.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2009
%
%  Author:
%
%    John Burkardt
%
  n = 10;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8LIB_TEST127\n' );
  fprintf ( 1, '  For an R8VEC:\n' );
  fprintf ( 1, '  R8VEC_MAX_INDEX: index of maximum entry;\n' );
  fprintf ( 1, '  R8VEC_MIN_INDEX: index of minimum entry;\n' );

  b = -n;
  c = n;

  seed = 123456789;

  [ a, seed ] = r8vec_uniform_ab ( n, b, c, seed );

  r8vec_print ( n, a, '  Input vector:' );

  fprintf ( 1, '\n' );

  ival = r8vec_max_index ( n, a );
  fprintf ( 1, '  Maximum index: %d\n', ival );

  ival = r8vec_min_index ( n, a );
  fprintf ( 1, '  Minimum index: %d\n', ival );

  return
end


function test_old_preproc_resample

% MEM 1gb
% WALLTIME 00:10:00
% DEPENDENCY

%this script tests the anti-aliasing filter in the resample function as used by ft_preproc_resample

fs    = 1000;
nsec  = 100;
nsmp  = fs*nsec;
tim   = ([1:nsmp]-1)./fs;
foi   = [0:(fs-1)];

datin       = randn(1,nsmp);
datin2      = reshape(datin, [fs nsec]);
datin2pow   = abs(fft(datin2, [], 1)).^2;

dathigh     = ft_preproc_highpassfilter(datin, fs, 300, 10, 'but', 'twopass');
dathigh     = dathigh + 0.1.*sin(2.*pi.* 400 .* tim);
dathigh2    = reshape(dathigh, [fs nsec]);
dathigh2pow = abs(fft(dathigh2, [], 1)).^2;

datlow     = ft_preproc_lowpassfilter(datin, fs, 300, 10, 'but', 'twopass');
datlow     = datlow + 0.1.*sin(2.*pi.* 400 .* tim);
datlow2    = reshape(datlow, [fs nsec]);
datlow2pow = abs(fft(datlow2, [], 1)).^2;

figure;hold on;
plot(foi, mean(datin2pow,2));
plot(foi, mean(dathigh2pow,2),'r');
plot(foi, mean(datlow2pow,2), 'm');

dathigh     = ft_preproc_highpassfilter(datin, fs, 400, 10, 'fir', 'twopass');
dathigh     = dathigh + 0.1.*sin(2.*pi.* 350 .* tim) + 0.1.*randn(1,nsmp);
dathigh2    = reshape(dathigh, [fs nsec]);
dathigh2pow = abs(fft(dathigh2, [], 1)).^2;

datlow     = ft_preproc_lowpassfilter(datin, fs, 400, 10, 'fir', 'twopass');
datlow     = datlow + 0.1.*sin(2.*pi.* 350 .* tim);
datlow2    = reshape(datlow, [fs nsec]);
datlow2pow = abs(fft(datlow2, [], 1)).^2;

figure;hold on;
plot(foi, mean(datin2pow,2));
plot(foi, mean(dathigh2pow,2),'r');
plot(foi, mean(datlow2pow,2), 'm');

newfs    = 500;
newfoi   = [0:(newfs-1)];
dathighr = ft_preproc_resample(dathigh, fs, newfs, 'resample');
dathighx = reshape(dathighr, [newfs nsec]);
dathighxpow = abs(fft(dathighx, [], 1)).^2;

dathighlow = ft_preproc_resample(ft_preproc_lowpassfilter(dathigh, fs, 150, 2, 'but', 'twopass'), fs, newfs, 'resample');
dathighlow = reshape(dathighlow, [newfs nsec]);
dathighlowpow = abs(fft(dathighlow, [], 1)).^2;

figure;hold on;
plot(foi(1:250),    (mean(dathigh2pow(1:250,2:end-1),2)));
plot(newfoi(1:250), (mean(dathighxpow(1:250,2:end-1),2)),'r');
plot(newfoi(1:250), (mean(dathighlowpow(1:250,2:end-1),2)),'m');

figure;hold on;
plot(foi(1:250), mean(dathighxpow(1:250,2:end-1),2)./mean(dathigh2pow(1:250,2:end-1),2));


function [ U, S, V ] = SubspaceIter( U1, V1, U0, V0, spa, bi, R, maxIter )

if(size(U1, 1) > size(V1, 1))
    V1 = V1*(1 + bi);
else
    U1 = U1*(1 + bi);
end

if(size(U0, 1) > size(V0, 1))
    V0 = V0*(-bi);
else
    U0 = U0*(-bi);
end

V = R;
for i = 1:maxIter
    U = AfuncAcc( U1, V1, U0, V0, spa, V);
    [U, ~] = qr(U, 0);

    V = AtfuncAcc( U1, V1, U0, V0, spa, U);
    [V, ~] = qr(V, 0);
end

S = AfuncAcc( U1, V1, U0, V0, spa, V);
S = U'*S;

[Us, S, Vs] = svd(S, 'econ');

U = U*Us;
V = V*Vs;

end