function [s, lambda_k] = tr_subsolver(problem, w, grad, tr_radius, sub_hess_indices, ...
                                                successful_flag, lambda_k, subproblem_solver,...
                                                exact_tol, krylov_tol)
    if strcmp(subproblem_solver, 'cauchy_point')
        %Hg = Hv_f(w, new_X, new_Y, grad,alpha);
        Hg = problem.hess_vec(w, grad, sub_hess_indices);
        gBg = grad'*Hg;
        tau = 1;
        if gBg > 0  % if model is convex quadratic the unconstrained minimizer may be inside the TR
            tau = min([(norm(grad))^3 / (tr_radius * gBg), 1]);
        end
        pc = - tau * tr_radius * (grad./norm(grad));
        s = pc;
        lamda_k = 0;
    elseif strcmp(subproblem_solver, 'dog_leg')
        %H = hessian_f(w, new_X, new_Y, alpha);
        H = problem.hess(w, sub_hess_indices);
        gBg = grad'*(H'*grad);
        if gBg <= 0
            error('dog_leg requires H to be positive definite in all steps!') ;
        end

        % Compute the Newton Point and return it if inside the TR
        L = chol(H,'lower');
        y = L\grad;
        pn = -L'\y;
%         cholesky_B = chol(H,'lower');
%         pn = - (cholesky_B\grad);
        if (norm(pn) < tr_radius)
            s = pn;
            lambda_k = 0;
            return;
        end

        % Compute the 'unconstrained Cauchy Point'
        pc = -((grad'*grad)/gBg) * grad;
        pc_norm = norm(pc);

        % if it is outside the TR, return the point where the path intersects the boundary
        if pc_norm >= tr_radius
            p_boundary = pc * (tr_radius / pc_norm);
            s = p_boundary;
            lambda_k = 0;
            return;
        end

        % else, give intersection of path from pc to pn with tr_radius.
        [t_lower, t_upper] = solve_quadratic_equation(pc, pn, tr_radius);
        p_boundary = pc + t_upper * (pn - pc);
        s = p_boundary;
        lambda_k = 0;

    elseif strcmp(subproblem_solver, 'cg')
        grad_norm = norm(grad);
        p_start = zeros(size(grad,1),size(grad,2));

        if grad_norm < min([sqrt(norm(grad)) * norm(grad), krylov_tol])
            s = p_start;
            lambda_k = 0;
            return;
        end

        % initialize
        z = p_start;
        r = grad;
        d = -r;
        k = 0;

        while true
            %Bd = Hv_f(w, new_X, new_Y, d, alpha);
            Bd = problem.hess_vec(w, d, sub_hess_indices);
            dBd = d'*Bd;
            % terminate when encountering a direction of negative curvature with lowest boundary point along current search direction
            if dBd <= 0
                [t_lower, t_upper] = solve_quadratic_equation(z, d, tr_radius);
                p_low = z + t_lower * d;
                p_up = z + t_upper * d;
                %m_p_low = loss_f(w + p_low, X, Y, alpha) + grad'*p_low + 0.5 * p_low'*(H'*p_low);
                %m_p_up = loss_f(w + p_up, X, Y, alpha) + grad'*p_up + 0.5 * p_up'*(H'*p_up);

                H = problem.hess(w, sub_hess_indices); % Added by HK (Need to be checked.)
                m_p_low = problem.cost(w+p_low) + grad'*p_low + 0.5 * p_low'*(H'*p_low);
                m_p_up = problem.cost(w+p_up) + grad'*p_up + 0.5 * p_up'*(H'*p_up);
                problem.cost(w);
                if m_p_low < m_p_up
                    s = p_low;
                    lambda_k = 0;
                    return;
                else
                    s = p_up;
                    lambda_k = 0;
                    return;
                end
            end

            alpha = (r'*r) / dBd;
            z_next = z + alpha * d;
            % terminate if z_next violates TR bound
            if norm(z_next) >= tr_radius
                % return intersect of current search direction w/ boud
                [t_lower, t_upper] = solve_quadratic_equation(z, d, tr_radius);
                s = z + t_upper * d;
                lambda_k = 0;
                return;
            end

            r_next = r + alpha * Bd;
            if norm(r_next) < min([sqrt(norm(grad)) * norm(grad),krylov_tol])
                s = z_next;
                lambda_k = 0;
                return;
            end

            beta_next = (r_next'*r_next) / (r'*r);
            d_next = -r_next + beta_next * d;
            % update iterates
            z = z_next;
            r = r_next;
            d = d_next;
            k = k + 1;
        end
    elseif strcmp(subproblem_solver, 'GLTR')
        g_norm = norm(grad);
        s = zeros(size(grad,1), size(grad,2));

        if g_norm == 0
            % escape along the direction of the leftmost eigenvector as far as tr_radius permits
            fprintf('zero gradient encountered');
            %H = hessian_f(w, new_X, new_Y, alpha);
            H = problem.hess(w, sub_hess_indices);
            [s, lambda_k] = exact_TR_suproblem_solver(grad, H, tr_radius, exact_tol, successful_flag, lambda_k);
        else
            % initialize
            g = grad;
            p = -g;
            gamma = g_norm;
            T = zeros(1, 1);
            alpha_k = [];
            beta_k = [];
            interior_flag = true;
            k = 0;

            while true
                %Hp = Hv_f(w, new_X, new_Y, p, alpha);
                Hp = problem.hess_vec(w, p, sub_hess_indices);
                pHp = p'*Hp;
                alpha = g'*g / pHp;

                alpha_k = [alpha_k; alpha];

                %Lanczos Step 1: Build up subspace
                % a) Create g_lanczos = gamma*e_1

                e_1 = zeros(k + 1, 1);
                e_1(1) = 1.0;
                g_lanczos = gamma .* e_1;

                % b) Create T for Lanczos Model
                T_new = zeros(k + 1, k + 1);
                if k == 0
                    T(k+1, k+1) = 1.0/alpha;
                    T_new(1:k+1, 1:k+1) = T;
                else
                    T_new(1:size(T,1), 1:size(T,2)) = T;
                    T_new(k+1, k+1) = 1.0 / alpha + beta_/ alpha_k(k);
                    T_new(k, k+1) = sqrt(beta_) / abs(alpha_k(k));
                    T_new(k+1, k) = sqrt(beta_) / abs(alpha_k(k));
                    T = T_new;
                end

                if (interior_flag == true && alpha < 0) || norm(s + alpha * p) >= tr_radius
                    interior_flag = false;
                end

                if interior_flag == true
                    s = s + alpha * p;
                else
                    % Lanczos Step 2: solve problem in subspace
                    [h, lambda_k] = exact_TR_suproblem_solver(g_lanczos, T, tr_radius, exact_tol, successful_flag, lambda_k);
                end
                g_next = g + alpha * Hp;

                % test for convergence
                e_k = zeros(k + 1,1);
                e_k(k+1) = 1.0;

                if interior_flag == true && norm(g_next) < min([sqrt(norm(grad)) * norm(grad), krylov_tol])
                    break;
                end
                if interior_flag == false && norm(g_next) * abs(h'*e_k) < min([sqrt(norm(grad)) * norm(grad), krylov_tol])
                    break;
                end

                if k == problem.d
                    fprintf('Krylov dimensionality reach full space! Breaking out..\n');
                    break;
                end
                beta_ = (g_next'*g_next) / (g'*g);
                beta_k = [beta_k; beta_];
                p = -g_next + beta_ * p;
                g = g_next;
                k = k + 1;
            end

            if interior_flag == false
                % Recover Q by building up the lanczos space, TBD: keep storable Qs in memory
                n = size(grad,1);
                Q1 = zeros(n, k + 1);

                g = grad;
                p = -g;

                for j = 0 : k
                    gn = norm(g);
                    if j == 0
                        sigma = 1;
                    else
                        sigma = -sign(alpha_k(j)) * sigma;
                    end
                    Q1(:, j+1) = sigma * g / gn;

                    if ~ (j == k)
                        %Hp = Hv_f(w, new_X, new_Y, p, alpha);
                        Hp = problem.hess_vec(w, p, sub_hess_indices);
                        g = g + alpha_k(j+1) * Hp;
                        p = -g + beta_k(j+1) * p;
                    end
                end

                % compute final step in R^n
                s = Q1*h;
            end
        end
    elseif strcmp(subproblem_solver, 'exact')
        %H = hessian_f(w, new_X, new_Y, alpha);
        Hw = problem.hess(w, sub_hess_indices);
        [s, lambda_k] = exact_TR_suproblem_solver(grad, H, tr_radius, exact_tol, successful_flag, lambda_k);
    else
    	error('solver unknown\n');
    end
end

function [s, lambda_j] = exact_TR_suproblem_solver(grad, H, tr_radius, exact_tol, successful_flag, lambda_k)

    s = zeros(size(grad,1),size(grad,2));
    % Step 0: initialize safeguards
    H_ii_min = min(diag(H));
    absH = abs(H);
    H_max_norm = sqrt((size(H,1))^2) * max(absH(:));
    H_fro_norm = norm(H, 'fro');
    list_l = [];
    list_u = [];
    for i = 1 : length(H)
        l = H(i, i) + sum(abs(H(i, :))) - abs(H(i, i));
        u = -H(i, i) + sum(abs(H(i, :))) - abs(H(i, i));
        list_l = [list_l l];
        list_u = [list_u u];
    end
    gerschgorin_l = max(list_l);
    gerschgorin_u = max(list_u);

    lambda_lower = max([0, -H_ii_min, norm(grad) / tr_radius - min([H_fro_norm, H_max_norm, gerschgorin_l])]);
    lambda_upper = max([0, norm(grad) / tr_radius + min([H_fro_norm, H_max_norm, gerschgorin_u])]);

    if successful_flag == false && (lambda_lower <= lambda_k) && (lambda_k <= lambda_upper) % reinitialize at previous lambda in case of unscuccesful iterations
        lambda_j = lambda_k;
    elseif lambda_lower == 0  % allow for fast convergence in case of inner solution
        lambda_j = lambda_lower;
    else
        lambda_j = lambda_lower + (lambda_upper-lambda_lower)*rand(1,1);
    end

    i = 0;
    % Root Finding
    while true
        i = i + 1;
        lambda_in_N = false;
        lambda_plus_in_N = false;
        B = H + lambda_j * eye(size(H,1), size(H,2));
        try
            % 1 Factorize B
            L = chol(B, 'lower');
            % 2 Solve LL^Ts=-g
            Li = inv(L);
            s = - (Li'*Li)*grad;
            sn = norm(s);
            % 2.1 Termination: Lambda in F, if q(s(lamda))<eps_opt q(s*) and sn<eps_tr tr_radius -> stop. By Conn: Lemma 7.3.5:
            phi_lambda = 1.0 / sn - 1.0 / tr_radius;
            %if (abs(sn - tr_radius) <= exact_tol * tr_radius):
            if (abs(phi_lambda)<=exact_tol) %
                break;
            end

            % 3 Solve Lw=s
                w = Li*s;
                wn = norm(w);

            % Step 1: Lambda in L
            if lambda_j > 0 && (phi_lambda) < 0
                % print ('lambda: ',lambda_j, ' in L')
                lambda_plus = lambda_j + ((sn - tr_radius) / tr_radius) * ((sn^2) / (wn^2));
                lambda_j = lambda_plus;

            % Step 2: Lambda in G    (sn<tr_radius)
            elseif (phi_lambda) > 0 && lambda_j > 0 && any(grad ~= 0) % TBD: remove grad
                % print ('lambda: ',lambda_j, ' in G')
                lambda_upper = lambda_j;
                lambda_plus = lambda_j + ((sn - tr_radius) / tr_radius) * ((sn^2) / (wn^2));

                % Step 2a: If factorization succeeds: lambda_plus in L
                if lambda_plus > 0
                    try
                        % 1 Factorize B
                        B_plus = H + lambda_plus * eye(size(H,1), size(H,2));
                        L = chol(B_plus, 'lower');
                        lambda_j = lambda_plus;
                        % print ('lambda+', lambda_plus, 'in L')
                    catch
                        lambda_plus_in_N = true;
                    end
                end

                % Step 2b/c: If not: Lambda_plus in N
                if lambda_plus <= 0 || lambda_plus_in_N == true
                    % 1. Check for interior convergence (H pd, phi(lambda)>=0, lambda_l=0)
                    try
                        U = chol(H, 'upper');
                        H_pd = true;
                    catch
                        H_pd = false;
                    end

                    if lambda_lower == 0 && H_pd == true && phi_lambda >= 0 %cannot happen in ARC!
                        lambda_j = 0;
                        % print ('inner solution found');
                        break;
                    % 2. Else, choose a lambda within the safeguard interval
                    else
                        % print ('lambda_plus', lambda_plus, 'in N')
                        lambda_lower = max([lambda_lower, lambda_plus]);  % reset lower safeguard
                        lambda_j = max([sqrt(lambda_lower * lambda_upper),lambda_lower + 0.01 * (lambda_upper - lambda_lower)]);
                        lambda_upper = single(lambda_upper);

                        if lambda_lower == lambda_upper
                            lambda_j = lambda_lower;
                            % Hard case
                            [ev, ew] = eig(H);
                            d = ev(:, 1);
                            dn = norm(d);
                            assert((ew == -lambda_j), 'Ackward: in hard case but lambda_j != -lambda_1');
                            [tao_lower, tao_upper] = mitternachtsformel(1, 2*(s'*d), (s'*s)-tr_radius^2);
                            s = s + tao_lower * d;
                            fprintf('hard case resolved inside');
                        end
                    end
                end

            elseif (phi_lambda) == 0
                break;
            else % TBD:  move into if lambda+ column #this only happens for Hg=0 -> s=(0,..,0)->phi=inf -> lambda_plus=nan -> hard case (e.g. at saddle)
                lambda_in_N = true;
            end
        % Step 3: Lambda in N
        catch
            lambda_in_N = true;
        end

        if lambda_in_N == true
            % print ('lambda: ',lambda_j, ' in N')
            try
                U = chol(H, 'upper');
                H_pd = true;
            catch
                H_pd = false;
            end

            % 1. Check for interior convergence (H pd, phi(lambda)>=0, lambda_l=0)
            if lambda_lower == 0 && H_pd == true && phi_lambda >= 0
                lambda_j = 0;
                %print ('inner solution found')
                break;
            % 2. Else, choose a lambda within the safeguard interval
            else
                lambda_lower = max([lambda_lower, lambda_j]);  % reset lower safeguard
                lambda_j = max([sqrt(lambda_lower * lambda_upper),lambda_lower + 0.01 * (lambda_upper - lambda_lower)]);  % eq 7.3.14
                lambda_upper = single(lambda_upper);
                % Check for Hard Case:
                if lambda_lower == lambda_upper
                    lambda_j = lambda_lower;
                    [ev, ew] = eig(H);
                    d = ev(:, 1);
                    dn = norm(d);
                    assert((ew == -lambda_j), 'Ackward: in hard case but lambda_j != -lambda_1');
                    [tao_lower, tao_upper] = mitternachtsformel(1, 2*(s'*d), (s'*s)-tr_radius^2);
                    s = s + tao_lower * d;

                    fprintf('hard case resolved outside');
                end
            end
        end
    end

    % compute final step
    B = H + lambda_j * eye(size(H,1), size(H,2));
    % 1 Factorize B
    L = chol(B, 'lower');
    % 2 Solve LL^Ts=-g
    Li = inv(L);
    s = - Li'*Li*grad;
    %print (i,' exact solver iterations')

end

% Auxiliary Functions
function [t_lower, t_upper] = mitternachtsformel(a, b, c)
    sqrt_discriminant = sqrt(b * b - 4 * a * c);
    t_lower = (-b - sqrt_discriminant) / (2 * a);
    t_upper = (-b + sqrt_discriminant) / (2 * a);
end

function [t_lower, t_upper] = solve_quadratic_equation(pc, pn, tr_radius)
    % solves ax^2+bx+c=0
    a = (pn - pc)'*(pn - pc);
    b = 2 * (pc'*(pn - pc));
    c = (pc'*pc) - tr_radius^2;
    sqrt_discriminant = sqrt(b * b - 4 * a * c);
    t_lower = (-b - sqrt_discriminant) / (2 * a);
    t_upper = (-b + sqrt_discriminant) / (2 * a);
end


function [reconstruction]=gsp_pyramid_synthesis_old(coarsest_approximation,prediction_errors,Gs,varargin)
%GSP_PYRAMID_SYNTHESIS Synthesizes a signal from its graph pyramid transform coefficients
%   Usage:  reconstruction=gsp_pyramid_synthesis(coarsest_approximation,prediction_errors,Gs);
%           reconstruction=gsp_pyramid_synthesis(coarsest_approximation,prediction_errors,Gs,param);
%
%   Input parameters:
%         coarsest_approximation    : The coarsest approximation of the original signal.
%         prediction_errors         : Cell array with the prediction errors at each level.
%         Gs                        : A multiresolution sequence of graph structures, including the idx parameters tracking the subsampling pattern.
%   Output parameters:
%         reconstruction            : The synthesized signal.
%   Additional parameters:
%         param.use_exact           : To use exact graph spectral filtering instead of the Chebyshev approximation.
%         param.order               : Degree of the Chebyshev approximation (default=30).
%         param.least_squares       : Set to 1 to use the least squares synthesis (default=0)
%         param.h_filters           : The filters used in the analysis operator. These are required for least squares synthesis, but not for the direct synthesis method
%
%   'gsp_pyramid_synthesis(coarsest_approximation,prediction_errors,Gs)'
%   synthesizes a signal from its graph pyramid transform coefficients.
%
%   See also:
%
%   Demos:
%
%   References:

%   AUTHOR : David I Shuman.
%   TESTING:
%   REFERENCE:

% Read input parameters
if nargin>3
    param=varargin{1};
else
    param=0;
end

num_levels=length(prediction_errors);

% Compute the pyramid transform
current_coarse_approximation=coarsest_approximation;

for i=num_levels:-1:1
    if isfield(param, 'least_squares')
        if param.least_squares
            if ~isfield(param, 'h_filters')
                error('h-filter not provided');
            else
                param.h_filter=param.h_filters{i};
            end
        end
    end
    current_coarse_approximation=gsp_pyramid_synthesis_single_interpolation_old(current_coarse_approximation,prediction_errors{i},Gs{i},Gs{i+1}.idx,param);
end
reconstruction=current_coarse_approximation;


% ASINH  Inverse hyperbolic sine.
%    ASINH(X) is the inverse hyperbolic sine of the elements of X.
%
%    See also SINH.
%
%    Reference page in Doc Center
%       doc asinh
%
%    Other functions named asinh
%
%       codistributed/asinh    gpuArray/asinh    sym/asinh    ts/asinh
%


function legendre_poly_coef_test ( )

%*****************************************************************************80
%
%% LEGENDRE_POLY_COEF_TEST tests LEGENDRE_POLY_COEF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 October 2008
%
%  Author:
%
%    John Burkardt
%
  n = 5;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'LEGENDRE_POLY_COEF_TEST\n' );
  fprintf ( 1, '  LEGENDRE_POLY_COEF determines the Legendre \n' );
  fprintf ( 1, '  P(N) polynomial coefficients.\n' );

  c = legendre_poly_coef ( n );

  for i = 0 : n
    fprintf ( 1, '\n' );
    fprintf ( 1, '  P(%d)', i );
    fprintf ( 1, '\n' );
    for j = i : -1 : 0
      if ( j == 0 )
        fprintf ( 1, '  %f\n', c(i+1,j+1) );
      elseif ( j == 1 )
        fprintf ( 1, '  %f * x\n', c(i+1,j+1) );
      else
        fprintf ( 1, '  %f * x^%d\n', c(i+1,j+1), j );
      end
    end
  end

  return
end


function y = logmvn(X, Mu, Sigma, options)
%LOGMVN Evaluate the logarithms of normal density functions.
%The function evaluates the logarithm of Gaussian density function given
%data at X(n,:) with center at Mu(n,:) and covariance matrix Sigma(:,:,n).
%The code can handle different means and covariance matrices for each
%sample. It is much faster than the native function mvnpdf.
%
% Input:
%   X - N by D data. N is the number of samples. D is the number of
%       dimensions.
%   Mu - N by D (multiple) or 1 by D (single) mean.
%   Sigma - D by D by N (multiple) or D by D (single) covariance matrice.
%   Options - structure containing indices for transforming the Sigma to a
%       sparse matrix. Usually no need to set.
% Output:
%   y - N by 1 results that contain the logarithm of the Gaussians.
%
% Example:
%
% N = 1000;
% D = 2;
% Mu = [1 -1];
% Mu = repmat(Mu, 1000, 1) + randn(N,D);
% SN = repmat(reshape(abs(randn(N,1)), [1,1,N]), D, D) .* repmat(eye(D), [1,1,N]);
% Sigma = [.9 .4; .4 .3];
% Sigma = repmat(Sigma, [1,1,N]) + SN;
% X = mvnrnd(Mu,Sigma,1000);
%
% tic
% y = logmvn(X, Mu, Sigma);
% y = exp(y);
% toc
%
% tic
% y1 = mvnpdf(X, Mu, Sigma);
% toc
%
% norm(y-y1)
%
% Author: Yuan Zhou (zhouyuanzxcv@gmail.com)

% Copyright (C) 2015
if nargin < 4
    options = [];
end

if size(Mu,1) == 1 && ndims(Sigma) == 2 % single
    y = loggausspdf(X, Mu, Sigma);
elseif size(Mu,1) == size(X,1) && size(Sigma,3) == size(X,1) % multiple means
    y = logmvn_multiple(X, Mu, Sigma, options);
end

end

function y = logmvn_multiple(X, Mu, Sigma, options)
[N,B] = size(X);

% Transform the sigma matrix to a sparse block diagonal matrix
if ~isempty(options) && isfield(options,'sigma_update_in_logmvn')
    sparse_update_inplace(options.sigma_update_in_logmvn, Sigma(:));
    Sigma1 = options.sigma_update_in_logmvn;
else
    if ~isempty(options) && isfield(options,'Is') && isfield(options,'Js')
        Is = options.Is;
        Js = options.Js;
    else
        Is = (1:N*B);
        Is = repmat(reshape(Is, [B,1,N]), 1, B);
        Is = Is(:);

        Js = (1:N*B);
        Js = repmat(reshape(Js, [1,B,N]), B, 1);
        Js = Js(:);
    end

    Sigma1 = sparse(Is,Js,reshape(Sigma,N*B*B,1),N*B,N*B);
end

% Compute the Cholesky decomposition
[R,err] = chol(Sigma1);
if err ~= 0
    error('A Covariance matrix is not positive definite.');
end

% Final calculation
Y1 = X - Mu;

x1 = reshape(Y1', 1, N*B) / R;
x1 = sum(reshape(x1, B, N).^2, 1)';
x2 = sum(reshape(log(diag(R)),B,N), 1)';

y = -0.5 * x1 - x2 - B * log(2*pi) / 2;
end

function y = loggausspdf(X, mu, Sigma)
X = X';
mu = mu';
d = size(X,1);
X = bsxfun(@minus,X,mu);
[U,p]= cholcov(Sigma);
if p ~= 0
    error('ERROR: Sigma is not PD.');
end
Q = U'\X;
q = dot(Q,Q,1);  % quadratic term (M distance)
c = d*log(2*pi)+2*sum(log(diag(U)));   % normalization constant
y = -(c+q)/2;

y = y';
end


% fig_error_J.m
% plot maximum normalized error vs J using QR method
% for min-max NUFFT with uniform scaling factors
% This is Fig. 3 in the 2003 IEEE T-SP paper.
% Jeff Fessler, The University of Michigan

if ~isvar('emax')
%	Nlist = 1;
	Nlist = 2^10;
	Mlist = [1.5 2 2.5 3 4 5];	% over-sampling factors
%	Jlist = [2:11];
	Jlist = [2:20];	% neighborhood size

	[JJ, MM, NN] = ndgrid(Jlist, Mlist, Nlist);

	emax = zeros(size(NN));

	alpha = [1]; beta = [];
	for ii=1:numel(NN)
		N = NN(ii);
		J = JJ(ii);
		M = MM(ii);
		printf('M=%d J=%d', M, J)
		K = M * N;
		gam = 2*pi/K;
		om = gam * [0:20]'/40;
		emax(ii) = max(nufft1_err_mm(om, N, J, K, 'qr', alpha, beta));
	end
end

%
% plot min-max errors
%
if 1
	semilogy(Jlist, emax(:,1), '-o', ...
		Jlist, emax(:,2), '-d', ...
		Jlist, emax(:,3), '-^', ...
		Jlist, emax(:,4), '-s', ...
		Jlist, emax(:,5), '-p', ...
		Jlist, emax(:,6), '-+')
%		Jlist(Jlist <= 99), emax(Jlist <= 99, 2), '-d', ...
%		Jlist(Jlist <= 98), emax(Jlist <= 98, 3), '-^', ...
%		Jlist(Jlist <= 96), emax(Jlist <= 96, 4), '-s', ...
%		Jlist(Jlist <= 95), emax(Jlist <= 95, 5), '-p')
%	axis([minmax(Jlist)'+[-0.2 0.2] 1e-5 1e-1])
%	axis tight
%	axisx([minmax(Jlist)'+[-0.2 0.2]])
	axis([minmax(Jlist)'+[-0.2 0.2] 1e-10 1e-0])
	xtick([2:3:20])
	ytick(10.^[-10:2:0])
%	text(2.2, 2e-4, '(for unit-norm signal)')
	xlabel J, ylabel 'E_{max}'
	title(sprintf('Maximum error for \\alpha = (%g)', alpha))
	leg = {};
	for ii=1:length(Mlist)
		leg{ii} = sprintf('K/N=%g', Mlist(ii));
	end
	legend(leg)

%	ir_savefig c 'fig_error_J'
end


%
% Author: Marius Drulea
% http://www.cv.utcluj.ro/optical-flow.html

% References
% M. Drulea and S. Nedevschi, "Total variation regularization of
% local-global optical flow," in Intelligent Transportation Systems (ITSC),
% 2011 14th International IEEE Conference on, 2011, pp. 318-323.

% Copyright (C) 2011 Technical University of Cluj-Napoca

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

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

function [u, v, pu, pv] = solve_clg_tv_equation(u, v, u0, v0, pu, pv, ...
    I1, Ix, Iy, It, D, settings)
%%
lambda = settings.lambda;

theta = 0.5;
tau = 0.5;
% tau = 1.0/(4.0*theta + epsilon);

der_mask = [0 -1 1];
adjoint_der_mask = [-1 1 0];

if settings.use_bilateral == 1
     r0 = It - u0.*Ix - v0.*Iy;
    % apply the bilateral filter to the data terms
    [W_Ix_2, W_Ixy, W_Iy_2, W_Ix_r0, W_Iy_r0] = ...
    applyBilateralFilterToDataTerms(I1, Ix.*Ix, Ix.*Iy, Iy.*Iy, Ix.*r0, Iy.*r0, ...
        settings.wSize, settings.sigma_d, settings.sigma_r);
else
    gauss = fspecial('gaussian', [settings.wSize settings.wSize], settings.wSize/6);

    W_Ix_2 = imfilter(Ix.^2, gauss, 'replicate');
    W_Ixy = imfilter(Ix.*Iy, gauss, 'replicate');
    W_Iy_2 = imfilter(Iy.^2, gauss, 'replicate');

    r0 = It - u0.*Ix - v0.*Iy;
    W_Ix_r0 = imfilter(Ix.*r0, gauss, 'replicate');
    W_Iy_r0 = imfilter(Iy.*r0, gauss, 'replicate');
end

a11 = 1 + 2*lambda*theta*W_Ix_2;
a12 = 2*lambda*theta*W_Ixy;
a21 = a12;
a22 = 1 + 2*lambda*theta*W_Iy_2;
l_t_2_W_Ix_r0 = 2*lambda*theta*W_Ix_r0;
l_t_2_W_Iy_r0 = 2*lambda*theta*W_Iy_r0;

delta = a11.*a22 - a21.*a12;
%%
for k = 1:settings.its
    %%
    % 1. update the coupling variable

    b1 = u - l_t_2_W_Ix_r0;
    b2 = v - l_t_2_W_Iy_r0;

    % update u_ and v_
    deltaU_ = b1.*a22 - b2.*a12;
    deltaV_ = b2.*a11 - b1.*a21;

    u_ = deltaU_./delta;
    v_ = deltaV_./delta;

    %%
  % compute the divergence of the dual variable
  % the adjoint of the nabla (derivative) operator = (- divergence) operator

  div_u = imfilter(pu(:, :, 1), adjoint_der_mask, 'replicate') + ...
          imfilter(pu(:, :, 2), adjoint_der_mask', 'replicate');
  div_v = imfilter(pv(:, :, 1), adjoint_der_mask, 'replicate') + ...
          imfilter(pv(:, :, 2), adjoint_der_mask', 'replicate');

  % update primal variable u
  u = u_ + theta*div_u;

  % update primal variable v
  v = v_ + theta*div_v;

  % compute nabla(u); the derivative operator
  ux = imfilter(u, der_mask, 'replicate');
  uy = imfilter(u, der_mask', 'replicate');
  vx = imfilter(v, der_mask, 'replicate');
  vy = imfilter(v, der_mask', 'replicate');

  % update dual variable; gradient descent
  % p = p_k + tau*nabla(u);
  pu(:, :, 1) = pu(:, :, 1) + tau * ux;
  pu(:, :, 2) = pu(:, :, 2) + tau * uy;
  pv(:, :, 1) = pv(:, :, 1) + tau * vx;
  pv(:, :, 2) = pv(:, :, 2) + tau * vy;

  % project the dual variable to ensure the inequality |p| <= D
  % p = p./max(|p|, D) .* D;
  pu(:, :, 1) = pu(:, :, 1)./max(abs(pu(:, :, 1)), D) .* D;
  pu(:, :, 2) = pu(:, :, 2)./max(abs(pu(:, :, 2)), D) .* D;
  pv(:, :, 1) = pv(:, :, 1)./max(abs(pv(:, :, 1)), D) .* D;
  pv(:, :, 2) = pv(:, :, 2)./max(abs(pv(:, :, 2)), D) .* D;
end


function [month, day, year] = gdate (jdate)

% convert Julian date to Gregorian (calendar) date

% input

%  jdate = julian day

% output

%  month = calendar month [1 - 12]
%  day   = calendar day [1 - 31]
%  year  = calendar year [yyyy]

%  note: day may include fractional part

% Orbital Mechanics with Matlab

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

jd = jdate;

z = fix(jd + .5);
fday = jd + .5 - z;

if (fday < 0)
   fday = fday + 1;
   z = z - 1;
end

if (z < 2299161)
   a = z;
else
   alpha = floor((z - 1867216.25) / 36524.25);
   a = z + 1 + alpha - floor(alpha / 4);
end

b = a + 1524;
c = fix((b - 122.1) / 365.25);
d = fix(365.25 * c);
e = fix((b - d) / 30.6001);
day = b - d - fix(30.6001 * e) + fday;

if (e < 14)
   month = e - 1;
else
   month = e - 13;
end

if (month > 2)
   year = c - 4716;
else
   year = c - 4715;
end


function result = blurOnGraph(signal,laplacian,time,steps)

h = time/steps;
nVertices = size(laplacian,1);

timeStep = @(x) (speye(nVertices) - h*laplacian) \ x;

result = signal;
for i=1:steps
    result = timeStep(result);

    % help fix numerical issues
    result(result<0) = 0;
    result = result / sum(result) * sum(signal);
end


function [ loss ] = path_NLOS( d )
%     d=d/1000;
%     loss=147.4 +43.3*log10(d);
%     loss=max(15.3 +37.6*log10(d), path_LOS( d ))+20;
%     loss=max(2.7 +42.8*log10(d), path_LOS( d ))+20;
    loss=32.6 +36.7*log10(d);
end


function i = binary_to_i4 ( s )

%*****************************************************************************80
%
%% BINARY_TO_I4 converts a binary representation into an integer value.
%
%  Example:
%
%        S        I
%
%      '101'      5
%    '-1000'     -8
%        '1'      1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 May 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string S, the binary representation.
%
%    Output, integer I, the integer whose representation was input.
%
  nchar = s_len_trim ( s );

  i = 0;
  ichr = 1;
  istate = 0;
  isgn = 1;

  while ( ichr <= nchar )

    c = s(ichr);
%
%  Blank.
%
    if ( c == ' ' )

      if ( istate == 2 )
        istate = 3;
      end
%
%  Sign, + or -.
%
    elseif ( c == '-' )

      if ( istate == 0 )
        istate = 1;
        isgn = - 1;
      else
        istate = - 1;
      end

    elseif ( c == '+' )

      if ( istate == 0 )
        istate = 1;
      else
        istate = - 1;
      end
%
%  Digit, 0 or 1.
%
    elseif ( c == '1' )

      i = 2 * i;
      i = i + 1;
      istate = 2;

    elseif ( c == '0' )

      i = 2 * i;
      istate = 2;
%
%  Illegal or unknown sign.
%
    else

      fprintf ( 1, '\n' );
      fprintf ( 1, 'BINARY_TO_I4 - Serious error!\n' );
      fprintf ( 1, '  Illegal digit = "%c"\n', c );
      fprintf ( 1, '  Conversion halted prematurely!\n' );
      error ( 'BINARY_TO_I4 - Serious error!' );

    end

    if ( istate == -1 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'BINARY_TO_I4 - Serious error!\n' );
      fprintf ( 1, '  Unable to decipher input!\n' );
      error ( 'BINARY_TO_I4 - Serious error!' );
    end

    if ( 3 <= istate )
      break;
    end

    ichr = ichr + 1;

  end
%
%  Apply the sign.
%
  i = isgn * i;

  return
end


function [err, XYZ] = AfniIndex2AfniXYZ (Indx, Nxx, Nyy)
%
%    [err, XYZ] = AfniIndex2AfniXYZ (Indx, Nxx, Nyy);
%
% Returns  the XYZ coordinates of an AFNI voxel with
%  an AFNI index of Indx
%
% Indx : Nx1 vector containing the AFNI indices
%   Indx must be an integer vector. If it is not, it's values
%   are rounded to the nearest integer
%
% Nxx, Nyy  : Number of pixels in the slice in the X and Y directions
%
% err = 0 No problem
% err = 1 input Matrix size problems
%
% XYZ : is the XYZ triplets matrix Nx3
%
%      Ziad Saad   Sun Mar 15 19:24:39 CST 1998

	[n1,m1] = size(Indx);

	if (m1 ~= 1),
		fprintf (1,'\a\nError in AfniIndex2AfniXYZ : Bad size for Indx\n\n');
		err = 1;
		return;
	end

	Indx = round (Indx);

	NxxNyy = Nxx .* Nyy;

	Z = floor (Indx ./ NxxNyy);
	Y = floor ((Indx - Z .* NxxNyy) ./ Nxx);
	X = Indx - ( Y .* Nxx) - (Z .* NxxNyy) ;

	XYZ = [X Y Z];

err = 0;
return;


function J = amedfilt2_calc(I) %#codegen
% 2-D Adaptive Median Filter
% This filter ignores edge effects and boundary conditions, as such, the
% output is a cropped version of the original image, where the amount
% cropped is equal to the maximum window size vertically and horizontally.

% Define smax as a constant
smax = 9;

% Initialize Output Image (J)
J = I;

% Calculate valid region limits for filter
[nrows ncols] = size(I);
ll = ceil(smax/2);
ul = floor(smax/2);

% Loop over the entire image ignoring edge effects
for rows = ll:nrows-ul
    for cols = ll:ncols-ul

        window_ind = -ul:ul;
        region = I(rows+window_ind,cols+window_ind);
        centerpixel = region(ll,ll);

        for s = 3:2:smax

            % We can collapse the ROI calculations into a single function
            [rmin,rmax,rmed] = roi_stats(region,smax,s);

            % adapt region size
            if rmed > rmin && rmed < rmax
                if centerpixel <= rmin || centerpixel >= rmax
                    J(rows,cols) = rmed;
                end

                % stop adapting
                break;
            end
        end
    end
end

function [rmin,rmax,rmed] = roi_stats(region,smax,s)
% Limits for ROI
ll = ceil(smax/2)-floor(s/2);
ul = ceil(smax/2)+floor(s/2);

v = ones(smax*smax,1);
count = 1;

for i = ll:ul
    for j = ll:ul
        v(count) = region(i,j);
        count = count+1;
    end
end

v = visort(v,s*s);
rmed = v(ceil(s*s/2));
rmin = v(1);
rmax = v(s*s);


function showgraph(node,edge)
%% SHOWGRAPH displays a planar graph
%
%    showgraph(node,edge) displays a planar undirected graph.
%
%   See also showmesh, findedge
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

line([node(edge(:,1),1)'; node(edge(:,2),1)'],...
     [node(edge(:,1),2)'; node(edge(:,2),2)'],...
     'LineWidth',1,'Color',[0.125 0.5 0.125]);
hold on
plot(node(:,1),node(:,2),'k.', 'MarkerSize', 12);
axis equal; axis off


%Copyright (c) October,15 2008 by Varsha Hedau, UIUC.  All rights reserved.
function [vp Pout]=ordervp(vp,h,w,Pin)
%Orders vanishing points as vertical, farther horizontal and closer
%horizontal vanishing points.
%Note this takes VP=[x1 y1; x2 y2; x3 y3 ]as input
%if P is given P is also shuffled
if nargin < 4
Pin=[];
Pout=[];
end

vptemp=vp;
dists = ((vp(:,1)-w/2).^2 + (vp(:,2)-h/2).^2).^0.5;
[vv,ii] = sort(dists,'descend');
vp = vp(ii,:);
dot1 = dot(vp(1,:)-[w/2,h/2],[1 0])/norm(vp(1,:)-[w/2,h/2]);
dot2 = dot(vp(2,:)-[w/2,h/2],[1 0])/norm(vp(2,:)-[w/2,h/2]);
if abs(dot1)>abs(dot2)
    tempvar = vp(1,:);
    vp(1,:) = vp(2,:);
    vp(2,:) = tempvar;
end

if numel(Pin)>0

ind=find(vptemp(:,1)==vp(1,1) & vptemp(:,2)==vp(1,2));
Pout=Pin(:,ind);
ind=find(vptemp(:,1)==vp(2,1) & vptemp(:,2)==vp(2,2));
Pout=[Pout Pin(:,ind)];
ind=find(vptemp(:,1)==vp(3,1) & vptemp(:,2)==vp(3,2));
Pout=[Pout Pin(:,ind)];
Pout=[Pout Pin(:,4)];% 4th is outlier
end

 return


function [x, infos] = als_nmf(V, rank, in_options)
% Alternative least squares (ALS) for non-negative matrix factorization (NMF).
%
% The problem of interest is defined as
%
%       min || V - WH ||_F^2,
%       where
%       {V, W, H} > 0.
%
% Given a non-negative matrix V, factorized non-negative matrices {W, H} are calculated.
%
%
% Inputs:
%       V           : (m x n) non-negative matrix to factorize
%       rank        : rank
%       in_options
%           alg     : als: Alternative least squares (ALS)
%
%                   : hals: Hierarchical alternative least squares (Hierarchical ALS)
%                       Reference:
%                           Andrzej Cichocki and PHAN Anh-Huy,
%                           "Fast local algorithms for large scale nonnegative matrix and tensor factorizations,"
%                           IEICE Transactions on Fundamentals of Electronics, Communications and Computer Sciences,
%                           vol. 92, no. 3, pp. 708-721, 2009.
%
%                   : acc_hals: Accelerated hierarchical alternative least squares (Accelerated HALS)
%                       Reference:
%                           N. Gillis and F. Glineur,
%                           "Accelerated Multiplicative Updates and hierarchical ALS Algorithms for Nonnegative
%                           Matrix Factorization,",
%                           Neural Computation 24 (4), pp. 1085-1105, 2012.
%                           See http://sites.google.com/site/nicolasgillis/.
%                           The corresponding code is originally created by the authors,
%                           Then, it is modifided by H.Kasai.
%
%
% Outputs:
%       x           : non-negative matrix solution, i.e., x.W: (m x rank), x.H: (rank x n)
%       infos       : log information
%           epoch   : iteration nuber
%           cost    : objective function value
%           optgap  : optimality gap
%           time    : elapsed time
%           grad_calc_count : number of sampled data elements (gradient calculations)
%
%
% This file is part of NMFLibrary
%
% Created by H.Kasai on Mar. 24, 2017
%
% Change log:
%
%       Oct. 27, 2017 (Hiroyuki Kasai): Fixed algorithm.
%
%       Apr. 22, 2019 (Hiroyuki Kasai): Fixed bugs.
%
%       May. 20, 2019 (Hiroyuki Kasai): Added initialization module.
%
%       Jun. 24, 2022 (Hiroyuki Kasai): Added momentum acceleration mode and mofified.
%
%       Jul. 12, 2022 (Hiroyuki Kasai): Modified code structures.
%

    % set dimensions and samples
    [m, n] = size(V);

    % set local options
    local_options = [];
    local_options.alg   = 'hals';
    local_options.sub_mode = 'std';
    local_options.alpha = 2;
    local_options.delta = 0.1;
    local_options.inner_max_epoch = 500;
    local_options.inner_max_epoch_parameter = 0.5;
    local_options.beta0 = 0.5;
    local_options.eta = 1.5;
    local_options.gammabeta = 1.01;
    local_options.gammabetabar = 1.005;
    local_options.momentum_h = 0;
    local_options.momentum_w = 0;
    local_options.scaling = true;
    local_options.warm_restart = false;

    % check input options
    if ~exist('in_options', 'var') || isempty(in_options)
        in_options = struct();
    end
    % merge options
    options = mergeOptions(get_nmf_default_options(), local_options);
    options = mergeOptions(options, in_options);

    % set paramters
    if ~strcmp(options.alg, 'als') && ~strcmp(options.alg, 'hals') ...
       && ~strcmp(options.alg, 'acc_hals')
        fprintf('Invalid algorithm: %s. Therfore, we use hals (i.e., Hierarchical ALS).\n', options.alg);
        options.alg = 'hals';
    end

    % initialize factors
    init_options = options;
    [init_factors, ~] = generate_init_factors(V, rank, init_options);
    W = init_factors.W;
    H = init_factors.H;

    % initialize
    method_name = sprintf('ALS (%s:%s)', options.alg, options.sub_mode);
    epoch = 0;
    grad_calc_count = 0;

    if options.verbose > 0
        fprintf('# %s: started ...\n', method_name);
    end

    % intialize for als
    if strcmp(options.alg, 'acc_hals')
        eit1 = cputime;
        VHt = V*H';
        HHt = H*H';

        scaling = sum(sum(VHt.*W))/sum(sum( HHt.*(W'*W) ));
        W = W * scaling;

        options_halsupdt = [];
    end

    if options.scaling
        [W, H] = normalize_WH(V, W, H, rank, 'type1');
    end

    [options, beta, betamax] = check_momemtum_setting(options);

    if options.warm_restart
        nV = norm(V, 'fro');
        rel_error = zeros(1, options.max_epoch);
        rel_error(1) = sqrt(nV^2 - 2*sum(sum(V * H' .* W)) + sum(sum( H * H' .* (W'*W)))) / nV;
    end
    W_prev = W;
    H_prev = H;

    % store initial info
    clear infos;
    [infos, f_val, optgap] = store_nmf_info(V, W, H, [], options, [], epoch, grad_calc_count, 0);

    if options.verbose > 1
        fprintf('%s: Epoch = 0000, cost = %.16e, optgap = %.4e\n', method_name, f_val, optgap);
    end

    % set start time
    start_time = tic();

    % main loop
    while true

        % check stop condition
        [stop_flag, reason, max_reached_flag] = check_stop_condition(epoch, infos, options);
        if stop_flag
            display_stop_reason(epoch, infos, options, method_name, reason, max_reached_flag);
            break;
        end

        %% update H
        VtW = V'*W;
        WtW = W'*W;
        WtV = W' * V;

        if strcmp(options.alg, 'als')

            %H = (W*pinv(W'*W))' * V;
            H = WtW \ WtV;        % H = inv(W'*W) * W' * V;
            H = H .* (H>0);

        elseif strcmp(options.alg, 'hals')

            for k=1:rank
                tmp = (VtW(:,k)' - (WtW(:,k)' * H) + (WtW(k,k) * H(k,:))) / WtW(k,k);
                tmp(tmp<=eps) = eps;
                H(k,:) = tmp;
            end

        elseif strcmp(options.alg, 'acc_hals')

            eit1 = cputime;
            options_halsupdt.max_epoch = change_inner_max_epoch(V, W, options);
            H = HALSupdt(H, WtW, WtV, eit1, options.alpha, options.delta, options_halsupdt);

        end

        % perform momentum for H
        if strcmp(options.sub_mode, 'momentum')
            [H, H_tmp1, H_tmp2] = do_momentum_h(H, H_prev, beta, epoch, options);
        end

        %% update W
        VHt = V * H';
        HHt = H * H';

        if strcmp(options.alg, 'als')

            %W = ((inv(H*H')*H)*V')';
            W = VHt / HHt;        % W = V * H' * inv(H*H');
            W = (W>0) .* W;

            % normalize columns to unit
            W = W ./ (repmat(sum(W), m, 1)+eps);

        elseif strcmp(options.alg, 'hals')

            for k=1:rank
                tmp = (VHt(:,k) - (W * HHt(:,k)) + (W(:,k) * HHt(k,k))) / HHt(k,k);
                tmp(tmp<=eps) = eps;
                W(:,k) = tmp;
            end

        elseif strcmp(options.alg, 'acc_hals')

%            if epoch > 0 % Do not recompute A and B at first pass
                % Use actual computational time instead of estimates rhoU
                eit1 = cputime;
                eit1 = cputime-eit1;
%           end
            options_halsupdt.max_epoch = change_inner_max_epoch(V', H', options);
            W = HALSupdt(W', HHt',VHt', eit1, options.alpha, options.delta, options_halsupdt);
            W = W';

        end

        % perform momentum for W
        if strcmp(options.sub_mode, 'momentum')
            [W, H, W_tmp1] = do_momentum_w(W, W_prev, H, H_prev, H_tmp1, beta, epoch, options);
        end

        % perform warm_restart
        if options.warm_restart
            [W, H, W_prev, H_prev, rel_error, beta, betamax, options] = ...
                warm_restart(V, W, H, rank, W_prev, H_prev, W_tmp1, H_tmp1, H_tmp2, rel_error, beta, betamax, epoch, options);
        end

        % measure elapsed time
        elapsed_time = toc(start_time);

        % measure gradient calc count
        grad_calc_count = grad_calc_count + m*n;

        % update epoch
        epoch = epoch + 1;

        % store info
        infos = store_nmf_info(V, W, H, [], options, infos, epoch, grad_calc_count, elapsed_time);

        % display info
        display_info(method_name, epoch, infos, options);

    end

    x.W = W;
    x.H = H;

end


function [ mColumnVectorImage ] = ImageToColumnsSliding( mInputImage, vBlockSize )
% ----------------------------------------------------------------------------------------------- %
% [ mColumnImage ] = ImageToColumns( mInputImage, blockRadius )
%   Creates an column image from the sliding neighborhood in mInpuImage
% Input:
%   - mInputImage           -   Input image.
%                               Structure: Image Matrix (Single Channel)
%                               Type: 'Single' / 'Double'.
%                               Range: [0, 1].
%   - vBlockSize            -   Block Size.
%                               Structure: 2D Vector.
%                               Type: 'Single' / 'Double'.
%                               Range: [0, 1].
% Output:
%   - mColumnVectorImage    -   Column Vector Image.
%                               Structure: Image Matrix (Single Channel)
%                               Type: 'Single' / 'Double'.
%                               Range: [0, 1].
% Remarks:
%   1.  Prefixes:
%       -   'm' - Matrix.
%       -   'v' - Vector.
%   2.  Converts each sliding `vBlockSize(1)` by `vBlockSize(2) block of
%       `mInputImage` into a column of `mColumnVectorImage` with no zero
%       padding.
%   3.  Shouldn't be used for images larger than 400x400 and blocks of
%       51x51.
% TODO:
%   1.  I
%   Release Notes:
%   -   1.0.000     20/03/2015  Royi Avital
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

[numRows, numCol] = size(mInputImage);
blockNumRows = vBlockSize(1);
blockNumCols = vBlockSize(2);

% Create Hankel-like indexing sub matrix.
nc = numRows - blockNumRows + 1;
nn = numCol - blockNumCols + 1;

vColumnIdx = [(0:(blockNumRows - 1))]';
vRowIdx = [1:nc];

t = vColumnIdx(:, ones(nc, 1)) + vRowIdx(ones(blockNumRows, 1), :);    % Hankel Subscripts
tt = zeros(blockNumRows * blockNumCols, nc);
rows = 1:blockNumRows;
for ii = 0:(blockNumCols - 1)
    tt(((ii * blockNumRows) + rows), :) = t + (numRows * ii);
end
mColumnVectorImageIdx = zeros((blockNumRows * blockNumCols), (nc * nn));
cols = 1:nc;
for jj = 0:(nn - 1)
    mColumnVectorImageIdx(:, ((jj * nc) + cols)) = tt + (numRows * jj);
end

mColumnVectorImage = mInputImage(mColumnVectorImageIdx);

end


function X = VBA_spm_inv(A,TOL)
% inverse for ill-conditioned matrices
% FORMAT X = spm_inv(A,TOL)
%
% A   - matrix
% X   - inverse
%
% TOL - tolerance: default = max(eps(norm(A,'inf'))*max(m,n),exp(-32))
%
% This routine simply adds a small diagonal matrix to A and calls inv.m
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_inv.m 4360 2011-06-14 16:46:37Z ged $

% check A
%--------------------------------------------------------------------------
[m,n] = size(A);
if isempty(A), X = sparse(n,m); return, end

% tolerance
%--------------------------------------------------------------------------
if nargin == 1
    TOL  = max(eps(norm(A,'inf'))*max(m,n),exp(-32));
end

% inverse
%--------------------------------------------------------------------------
X     = inv(A + speye(m,n)*TOL);


%%*************************************************************************
%% HSDbicgstab
%%
%% [xx,resnrm,flag] = HSDbicgstab(A,b,M1,tol,maxit)
%%
%% iterate on  bb - (M1)*AA*x
%%
%% r = b-A*xtrue;
%%
%%*************************************************************************

function [xx,resnrm,flag] = HSDbicgstab(A,b,M1,tol,maxit,printlevel)

N = length(b);
if (nargin < 6); printlevel = 1; end
if (nargin < 5) || isempty(maxit); maxit = max(20,length(A.mat22)); end;
if (nargin < 4) || isempty(tol); tol = 1e-8; end;
tolb = min(1e-4,tol*norm(b));
flag = 1;

x = zeros(N,1);
if isstruct(A); r = b-matvec(A,x); else r = b-mexMatvec(A,x); end;
err = norm(r); resnrm(1) = err;  minresnrm = err; xx = x;
%%if (err < tolb); return; end

omega = 1.0;
r_tld = r;
%%
%%
%%
smtol = 1e-40;
for iter = 1:maxit,

    rho   = (r_tld'*r);
    if (abs(rho) < smtol)
        flag = 2;
        if (printlevel); fprintf('*'); end;
        break;
    end
    if (iter > 1)
        beta  = (rho/rho_1)* (alp/omega);
        p = r + beta*(p - omega*v);
    else
        p = r;
    end
    p_hat = precond(A,M1,p);
    if isstruct(A); v = matvec(A,p_hat); else v = mexMatvec(A,p_hat); end;
    alp = rho / (r_tld'*v);
    s = r - alp*v;
    %%
    s_hat = precond(A,M1,s);
    if isstruct(A); t = matvec(A,s_hat); else t = mexMatvec(A,s_hat); end;
    omega = (t'*s) / (t'*t);
    x = x + alp*p_hat + omega*s_hat;
    r = s - omega*t;
    rho_1 = rho;
    %%
    %% check convergence
    %%
    err = norm(r); resnrm(iter+1) = err; %#ok
    if (err < minresnrm);
        xx = x; minresnrm = err;
    end
    if (err < tolb)
        break;
    end
    if (err > 10*minresnrm)
        if (printlevel); fprintf('^'); end
        break;
    end
    if (abs(omega) < smtol)
        flag = 2;
        if (printlevel); fprintf('*'); end;
        break;
    end
end
%%
%%*************************************************************************
%%*************************************************************************
%% matvec: matrix-vector multiply.
%% matrix = [A.mat11, A.mat12; A.mat12', A.mat22]
%%*************************************************************************

function Ax = matvec(A,x)

m = length(A.mat11); m2 = length(x)-m;
if (m2 > 0)
    x1 = full(x(1:m));
else
    x1 = full(x);
end
Ax = mexMatvec(A.mat11,x1);
if (m2 > 0)
    x2 = full(x(m+1:m+m2));
    Ax = Ax + mexMatvec(A.mat12,x2);
    Ax2 = mexMatvec(A.mat12,x1,1) + mexMatvec(A.mat22,x2);
    Ax = [Ax; Ax2];
end
%%*************************************************************************
%% precond:
%%*************************************************************************

function Mx = precond(A,L,x)

m = L.matdim; m2 = length(x)-m;
if (m2 > 0)
    x1 = full(x(1:m));
else
    x1 = full(x);
end
if (m2 > 0)
    x2 = x(m+1:m+m2);
    w = linsysolvefun(L,x1);
    z = mexMatvec(A.mat12,w,1) -x2;
    z = L.Mu \ (L.Ml \ (L.Mp*z));
    x1 = x1 - mexMatvec(A.mat12,z);
end
%%
Mx = linsysolvefun(L,x1);
%%
if (m2 > 0)
    Mx = [Mx; z];
end
%%*************************************************************************


function test225 ( )

%*****************************************************************************80
%
%% TEST225 tests L4MAT_PRINT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 November 2011
%
%  Author:
%
%    John Burkardt
%
  m = 20;
  n = 50;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST225\n' );
  fprintf ( 1, '  L4MAT_PRINT prints a logical matrix.\n' );

  a = zeros ( m, n );

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

  l4mat_print ( m, n, a, '  A(I,J) = I is divisible by J' );

  return
end


%++++++++++++++++++++++++++++++++++++++++++++
%PURPOSE: TO DETERMINE THE OPTIMUM VALUE OF
%OVER-RELAXATION (OMEGA) FOR APPLICATION
%IN SOR GS, PICKED OVER MINIMUM VALUE OF
%SOR/GS ITERATION.
%++++++++++++++++++++++++++++++++++++++++++++
function [T]=optimum(om1,om2)
N=200;
eps=1e-6;%error
L=2;
dx=L/N;
n2=25;  %n^2=hP/kA
tb=400+273.15;
ta=34+273.15;
kira=0;

global omega eps N tb ta
warning off

[gs,T]=GS;

omega=om1;
while omega<om2
[sor,T]=SOR;
kira=kira+1;
wye(kira)=sor/gs;
exx(kira)=omega;
omega=omega+0.01;
end

plot(exx,wye);
ylabel('#SOR/#GS RATIO');
xlabel('OMEGA');
grid


function [GHz] = Hz2GHz(Hz)
% Convert frequency from hertz to gigahertz.
% Chad A. Greene 2012
GHz = Hz*1e-9;


function [adfstat,pval,critval,resid,lags,ICs]=augdfautolag(y,p,maxlags,IC)
% Dickey-Fuller and Augmented Dickey Fuller with automatic lag selection
%
% USAGE:
%  [ADFSTAT,PVAL,CRITVAL] = augdfautolag(Y,P,LAGS,IC)
%  [ADFSTAT,PVAL,CRITVAL,RESID,LAGS] = augdfautolag(Y,P,LAGS,IC)
%
% INPUTS:
%  Y         - A T by 1 vector of data
%  P         - Order of the polynomial of include in the ADF regression:
%                0 : No deterministic terms
%                1 : Constant
%                2 : Time Trend
%                3 : Constant, DGP assumed to have a time trend
%  MAXLAGS   - The maximum number of lags to include in the ADF test
%  IC        - [OPTIONAL] String, either 'AIC' (default) or 'BIC' to choose the criteria to select
%                the model
%
% OUTPUTS:
%  ADFSTAT   - Dickey-Fuller statistic
%  PVAL      - Probability the series is a unit root
%  CRITVALS  - A 6 by 1 vector with the [.01 .05 .1 .9 .95 .99] values from the DF distribution
%  LAGS      - The selected number of lags
%  IC        - The value at all lags of the selected IC
%
% COMMENTS:
%
% See also AUGDF

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if nargin<3 || nargin>4
    error('3 or 4 inputs required.')
end
T=length(y);
if T<=(maxlags+1)
    error('Length of data must be larger than LAGS')
end
if size(y,1)~=T,
    y=y';
end
if size(y,2)~=1
    error('Y must be a column vector')
end
if ~isscalar(maxlags) && maxlags>0 && floor(maxlags)==maxlags
    error('LAGS must be a positive integer')
end
if ~isscalar(p)
    error('P must be a scalar integer in {0, 1, 2, 3}')
end
if ~ismember(p,[0 1 2 3])
    error('P must be a scalar integer in {0, 1, 2, 3}')
end
if nargin==3
    IC='AIC';
else
    if ~ischar(IC)
        error('IC must be a string, either ''AIC'' or ''BIC''');
    elseif ~ismember(IC,{'AIC','BIC'})
        error('IC must be a string, either ''AIC'' or ''BIC''');
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Setup common to all problems
%y=y-y(1);
ydiff=diff(y);
[ydiffcurr, ydifflags]=newlagmatrix(ydiff,maxlags); %#ok<ASGLU>
T=length(y);
Y=y(maxlags+2:T);
tau=length(Y);
%
switch p
    case 0
        % Case 1
        i=0;
        X=y(maxlags+1:T-1);
        rho = X\Y;
        % Compute the errors
        e= Y-X*rho;
        s2(i+1)=e'*e/tau;
        K(i+1)=size(X,2);

        % Loop
        for i=1:maxlags
            X=[y(maxlags+1:T-1) ydifflags(:,1:i)];
            rho = X\Y;
            % Compute the errors
            e= Y-X*rho;
            s2(i+1)=e'*e/tau;
            K(i+1)=size(X,2);
        end

    case {1,3}
        %Case 2
        i=0;
        X=[ones(size(Y)) y(maxlags+1:T-1)];
        rho = X\Y;
        % Compute the errors
        e= Y-X*rho;
        s2(i+1)=e'*e/tau;
        K(i+1)=size(X,2);

        % Loop
        for i=1:maxlags
            X=[ones(size(Y)) y(maxlags+1:T-1) ydifflags(:,1:i)];
            rho = X\Y;
            % Compute the errors
            e= Y-X*rho;
            s2(i+1)=e'*e/tau;
            K(i+1)=size(X,2);
        end

    case 2
        %Case 4
        i=0;
        X=[ones(size(Y)) y(maxlags+1:T-1) (1:tau)'];
        rho = X\Y;
        % Compute the errors
        e= Y-X*rho;
        s2(i+1)=e'*e/tau;
        K(i+1)=size(X,2);

        % Loop
        for i=1:maxlags
            X=[ones(size(Y)) y(maxlags+1:T-1) (1:tau)' ydifflags(:,1:i)];
            rho = X\Y;
            % Compute the errors
            e= Y-X*rho;
            s2(i+1)=e'*e/tau;
            K(i+1)=size(X,2);
        end
end

if strcmp(IC,'AIC')
    ICs=log(s2) + 2*K/tau;
else
    ICs=log(s2) + K*log(tau)/tau;
end
[~,lags]=min(ICs);
lags=lags-1;
[adfstat,pval,critval,resid]=augdf(y,p,lags);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ldbparam.m
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ldbparam.m
%
%% This function evaluate several room acoustic parameters, with the signal
%% processing done by Lundeby method.
%%
%% [s]=ldbparam(IR,fs,flag)
%%
%% The input is a room impulse response and its sampling frequency. The
%% flag variable indicates if the function should generate (1) or not (0)
%% the Schroeder decay plots.
%% The output it a text file with the value of several parameter for each
%% frequency band. If desired, returns a matrix with this values, where the
%% first line contains the frequency band central frequencies, instead of
%% text.

function [saida]=ldbparam(IR,fs,flag)

banda = filtros(IR,fs);
t = size(banda,2);
for n = 1:t
    s(1,n) = ceil(1000*2^(n-5));
    comeco = inicio(banda(:,n));
    fim = lundeby(banda(comeco:end,n),fs,flag);
    title(['Ponto de Cruzamento - Banda ',num2str(s(1,n))])
    if n == t-2
        title('Ponto de Cruzamento - Compensacao A ')
    elseif n == t-1
        title('Ponto de Cruzamento - Compensacao C ')
    elseif n == t
        title('Ponto de Cruzamento - Linear ')
    end

    aux = banda(comeco:fim,n).^2;
    [s(2,n),s(3,n),s(4,n),s(5,n),s(6,n)] = energeticos(aux,fs);
    [s(7,n),s(8,n),s(9,n),s(10,n)] = reverberacao(aux,fs,flag);
    title(['Curva de Decaimento - Banda ',num2str(s(1,n))])
    if n == t-2
        title('Curva de Decaimento - Compensacao A ')
    elseif n == t-1
        title('Curva de Decaimento - Compensacao C ')
    elseif n == t
        title('Curva de Decaimento - Linear ')
    end

end

if nargout == 1
    saida = s;
    saida(1,t-2) = (['A']);
    saida(1,t-1) = (['C']);
    saida(1,t) = (['L']);
else
    tabela(s,size(banda,2))
end


function demo
% Demo of the M2DP descriptor described in the following paper:
%
% Li He, Xiaolong Wang and Hong Zhang, M2DP: A Novel 3D Point Cloud
% Descriptor and Its Application in Loop Closure Detection, IROS 2016.
%
% Li He, Dept. of Computing Science, University of Alberta
% lhe2@ualberta.ca

%% 0. Please run mex CountVote2D.c in Matlab
% mex CountVote2D.c

%% 1. Read a point cloud
% 000000.bin is from KITTI dataset of sequence 00.
% http://www.cvlibs.net/datasets/kitti/eval_odometry.php
[data, numData] = ReadBinData('000000.bin');

% display the input data
figure(10);grid on;hold on
pcshow(data);title('Input point cloud');

%% 2. M2DP
% desM2DP: descriptor of data; A: 2D signatures of data.
tstart = tic;
[desM2DP, A] = M2DP(data);
telapsed = toc(tstart);
disp(['Processing time of M2DP on ' num2str(numData) ' points: ' num2str(telapsed) ' seconds']);

function [data, numData] = ReadBinData(nameDataFile)

fid = fopen(nameDataFile,'r');
if fid==-1
    disp('Cannot find input file\n');
    data=[];
    numData=[];
    return;
end

% get the size of file
fseek(fid,0,'eof');
fsize = ftell(fid);
% number points = total bytes / 4 per float32 / 4 float32 per row
numData = floor(fsize/16);

% now, read data
fseek(fid,0,'bof');
data = fread(fid,fsize,'float32');
fclose(fid);

% reshape data
data = reshape(data,[4, numData]);
data = data';
% ignoring the last feature, laser intensity
data = data(:,1:3);


function qr=rotqc2qr(qc)
%ROTQC2QR converts a matrix of complex quaternion row vectors into real form
%
% Inputs:
%
%     QC(2m,n)   mxn matrix of complex-valued quaternions
%
% Outputs:
%
%     QR(4m,n)   mxn matrix of real-valued quaternions
%
% The complex-valued quaternion [r+j*b  a+j*c] becomes [r a b c]

%
%      Copyright (C) Mike Brookes 2000-2006
%      Version: $Id: rotqc2qr.m 713 2011-10-16 14:45:43Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[m,n]=size(qc);
i=(1:2:2*m)-mod(0:m-1,2);
qr=zeros(2*m,n);
qr(i,:)=real(qc);
qr(i+2,:)=imag(qc);


function labelImg = segToLabel(u, conn)
%SEGTOLABEL Converts the output image of a segmentation function (e.g. minL2Potts2DADMM) to a
%label image with integer labels (each connected component gets a unique integer)
% u: image to label (typically the output of minL2Potts2DADMM))
% conn: connectivity as in bwconncomp (default: 8)

if ~exist('conn', 'var')
    conn = 8;
end

[m,n,c] = size(u);
vals = unique(reshape(u, m*n, c), 'rows');
labelImg = zeros(m, n);
labelCounter = 0;
for i=1:size(vals,1)
    bw_mult = (vals(i,:) == reshape(u, m*n, c));
    bw = bw_mult(:,1);
    CC = bwconncomp(reshape(double(bw), m, n), conn);
    for j = 1:CC.NumObjects
        labelImg(CC.PixelIdxList{j}) = labelCounter;
        labelCounter = labelCounter + 1;
    end
end

end


%% simplexImIntersect
% Below is a demonstration of the features of the |simplexImIntersect| function

%% Syntax
% |[regionlabel]=simplexImIntersect(F1,V1,V2,voxelSize);|

%% Description
% This function uses the input simplex (or simplices) defined by F1
% (faces), V1 (vertices), and C1 (boundary labels) to label the vertices V2
% according to what simplex regions they are contained in. The labels are
% stored in the output regionLabel. A vertex inside a simplex region is
% labelled with the boundary label of the simplex region. Vertices outside
% of all simplices are labelled with NaN. Points are labelled based on the
% patch2Im function, i.e. the following steps are used:
% 1) The simplex is converted to an image where voxels intensities denote
% simplex region labels. (see patch2Im).
% 2) The vertices in V2 are converted to image coordinates in this image
% 3) The image coordinates are converted to image voxel indices
% 4) Voxel indices are used to retrieve the labels from the simplex image.
%
% The image constructed uses the optional input voxelSize (the default if
% not provided is half of the mean edge size of the input simplex). The
% completeness/accuracy of the labelling depends on the voxel size. Some
% points are labelled as 0 which means they are found inside boundary
% voxels and cannot, based on the current voxel size, be assigned as
% outside or inside a particular region. All vertices labelled as NaN or a
% value >0 are labelled correctly. However some of the vertices labelled as
% 0 may actually be inside a simplex region or outside all simplex regions,
% i.e. 0 denotes that their status is unknown given the voxel size used.
%
% See also |patch2im|

%% Examples
clear; close all; clc;

%%
% Plot settings
faceAlpha1=1;
faceAlpha2=0.3;
fontSize=15;
markerSize=35;

%%
% Create example geometries
[F1,V1,~]=geoSphere(3,1); %First simplex
[F2,V2,~]=geoSphere(3,0.75); %Second simplex
V2(:,2)=V2(:,2)+1; %Shift second sphere

%%
% Determine optional voxel size input from mean edge size
[D1]=patchEdgeLengths(F1,V1);
[D2]=patchEdgeLengths(F2,V2);
d=mean([D1(:);D2(:)]);
voxelSize=d/2;

%% Find points outside of a simplex

[regionLabel]=simplexImIntersect(F1,V1,[],V2,voxelSize);
logicOut=isnan(regionLabel);
logicFacesOut=all(logicOut(F2),2);

%%
% Visualize results

cFigure;
subplot(1,3,1); hold on;
title('Intersecting sets')
gpatch(F1,V1,'g','none',faceAlpha2);
gpatch(F2,V2,'kw','k',faceAlpha1);
axisGeom;
view(-90,0);
camlight headlight;

subplot(1,3,2); hold on;
title('Point labels')
gpatch(F1,V1,'g','none',faceAlpha2);
gpatch(F2,V2,'kw','none',faceAlpha1);
scatterV(V2,markerSize,regionLabel,'filled');
colormap gjet; icolorbar;
axisGeom;
view(-90,0);
camlight headlight;

subplot(1,3,3); hold on;
title('Cropped geometry')
gpatch(F1,V1,'g','none',faceAlpha2);
gpatch(F2(logicFacesOut,:),V2,'kw','k',faceAlpha1);
axisGeom;
view(-90,0);
camlight headlight;

drawnow;

%%
% Defining a multi boundary set

%Example surface set 1
r=2; %Sphere radius
rc=3; %Central radius
nr=15;
nc=25;
ptype='quad';
[Fs1,Vs1]=patchTorus(r,nr,rc,nc,ptype);
[Fs2,Vs2]=quadSphere(2,r,2);
Vs2(:,2)=Vs2(:,2)-5;
[Fs3,Vs3]=quadSphere(2,r/2,2);
Vs3(:,2)=Vs3(:,2)-5;
[Fs4,Vs4]=quadSphere(3,r/2,2);
Vs4(:,1)=Vs4(:,1)+2;
Vs4(:,2)=Vs4(:,2)+2;

[F1,V1,C1]=joinElementSets({Fs1,Fs2,Fs3,Fs4},{Vs1,Vs2,Vs3,Vs4});

%Example surface 2
[F2,V2]=stanford_bunny; %Bunny surface
[F2,V2]=subtri(F2,V2,1); %Refine surface
r=sqrt(sum(V2.^2,2)); %Radii from centre of bunny to "normalize" shape
V2=V2./max(r); %"Normalize"
V2=V2*11; %Scale up

%%

[regionLabel]=simplexImIntersect(F1,V1,C1,V2,voxelSize);
logicOut=isnan(regionLabel);
logicFacesOut=all(logicOut(F2),2);

%%
% Visualize results

cFigure;
subplot(1,3,1); hold on;
title('Intersecting sets')
gpatch(F1,V1,C1,'none',faceAlpha2);
gpatch(F2,V2,'kw','none',faceAlpha1);
axisGeom;
camlight headlight;

subplot(1,3,2); hold on;
title('Point labels')
gpatch(F1,V1,C1,'none',faceAlpha2);
gpatch(F2,V2,'kw','none',faceAlpha1);
scatterV(V2,markerSize,regionLabel,'filled');
colormap gjet; icolorbar;
axisGeom;
camlight headlight;

subplot(1,3,3); hold on;
title('Cropped geometry')
gpatch(F1,V1,C1,'none',faceAlpha2);
gpatch(F2(logicFacesOut,:),V2,'kw','none',faceAlpha1);
axisGeom;
camlight headlight;

drawnow;

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function [logCr,logr]=gencorint(x,dim,tau,logr,p,w,svd,q)
%Syntax: [logCr,logr]=gencorint(x,dim,tau,logr,p,w,svd,q)
%________________________________________________________
%
% Calculates the generalized Correlation Integral (Cr) of a time
% series x.
%
% logCr is the the value of log(Cr).
% logr is log(range).
% x is the time series.
% dim is the embedding dimension.
% tau is the time delay.
% p is defines the norm.
% w is the Theiler's correction.
% svd is the number of singular values taken into account.
% q is the generalization index.
%
%
% References:
%
% Grassberger P, Procaccia I (1983): Characterization of strange
% attractors. Physical Review Letters 50(5):346-349
%
% Theiler J (1986): Spurious dimension from correlation algorithms
% applied tolimited time-series data. Physical Review A 34(3):2427-
% 2432
%
% Albano A M, Muench J, Schwartz C, Mees A I, Rapp P E, (1988):
% Singular-value decomposition and the Grassberger-Procaccia algorithm.
% Physical Review A38:3017-3026
%
%
% Alexandros Leontitsis
% Department of Education
% University of Ioannina
% 45110 - Dourouti
% Ioannina
% Greece
%
% University e-mail: me00743@cc.uoi.gr
% Lifetime e-mail: leoaleq@yahoo.com
% Homepage: http://www.geocities.com/CapeCanaveral/Lab/1421
%
% June 15, 2001.

if nargin<1 | isempty(x)==1
   error('You should provide a time series.');
else
   % x must be a vector
   if min(size(x))>1
      error('Invalid time series.');
   end
   x=x(:);
   % n is the time series length
   n=length(x);
end

if nargin<2 | isempty(dim)==1
   dim=2;
else
   % dim must be either a scalar or a vector
   if min(size(dim))>1
      error('dim must be a scalar or a vector.');
   end
   % dim must be an integer
   dim=round(dim);
   % dim values must be above 0
   dim=dim(find(dim>0));
end

if nargin<3 | isempty(tau)==1
   tau=1;
else
   % tau must be either a scalar or a vector
   if min(size(tau))>1
      error('tau must be a scalar or a vector.');
   end
   % tau must be an integer
   tau=round(tau);
   % tau values must be above 0
   tau=tau(find(tau>0));
end

if nargin<4  | isempty(logr)==1
   r=(max(x)-min(x))/10*(1:20)'/20;
   logr=log10(r);
else
    % logr must be either a scalar or a vector
    if min(size(dim))>1
        error('logr must be a scalar or a vector.');
    end
    % if it is a scalar, it determines the maximum range
    if length(logr)==1
        div=logr;
        % div must be positive
        if div<=0
            error('logr must be a positive scalar or vector');
        end
        r=(max(x)-min(x))/div*(1:20)'/20;
        logr=log10(r);
    else
        logr=logr(:);
        r=10.^logr;
    end
end

if nargin<5 | isempty(p)==1
   p=inf;
else
   % p must be either a scalar or a vector
   if min(size(dim))>1
      error('p must be a scalar.');
   end
   % p values must be positive
   p=p(find(p>0));
end

if nargin<6  | isempty(w)==1
   w=1;
else
   % w must be either a scalar or a vector
   if min(size(w))>1
      error('w must be either a scalar or a vector.');
   end
   % w must be an integer
   w=round(w);
   % w must be positive
   w=w(find(w>0));
end

if nargin<7 | isempty(svd)==1
    svd=[];
else
    % svd must be a scalar or a vector
    if min(size(svd))>1
        error('svd must be either a scalar or a vector.');
    end
    % svd must be an integer
    svd=round(svd);
    % svd must be positive
    svd=svd(find(svd>0));
end

if nargin<8 | isempty(q)==1
    q=2;
else
    % q must be either a scalar or a vector
    if min(size(q))>2
        error('q must be either a scalar or a vector.');
    end
end

% Only one of dim, tau, p, w, or q should be vector
l=[length(dim),length(tau),length(p),length(w),length(svd),length(q)];
if length(find(l>1))>1
   error('Only one of dim, tau, p, w, svd, or q should be vector.');
end

m=max(l);
dim=ones(1,m).*dim;
tau=ones(1,m).*tau;
p=ones(1,m).*p;
w=ones(1,m).*w;
if isempty(svd)==0
   svd=ones(1,m).*svd;
end
q=ones(1,m).*q;

for i=1:m

   % Reconstruct the time-delay phase-space
   [Y,T]=phasespace(x,dim(i),tau(i));
   if isempty(svd)==0
      svd(i)=min(svd(i),dim(i));
      % SVD on X
      [u,s,v]=svd(Y,0);

      % Calculate the Principal Components
      pc=Y*v;

      % Reconstruct the first svd Principal Components
      Y=pc(:,1:svd(i))*v(:,1:svd(i))';
   end

   % Initialize the logCr
   Cr=zeros(size(r));

   if q(i)==2 % ...fast

       for i1=1:T-w(i)
           for i2=i1+w(i):T
               dist=norm(Y(i1,:)-Y(i2,:),p(i));
               s=find(r>dist);
               Cr(s)=Cr(s)+1;
           end
       end
       Cr=2*Cr/T/(T-1);
       logCr(:,i)=log10(Cr);

   else % slow...

       for i1=1:T
           c1=zeros(size(r));
           for i2=1:T
               if i1<i2-(w(i)-1) | i1>i2+(w(i)-1)
                   dist=norm(Y(i1,:)-Y(i2,:),p(i));
                   s=find(r>dist);
                   c1(s)=c1(s)+1;
               end
           end
           c1=c1/(T-1);
           if q(i)~=1
               Cr=Cr+c1.^(q(i)-1);
           else
               Cr=Cr+c1;
           end
       end
       if q(i)~=1
           Cr=Cr/T;
           logCr(:,i)=log10(Cr.^(1/(q(i)-1)));
       else
           Cr=log10(Cr)/T;
           logCr(:,i)=Cr;
       end

   end
end


% ST | GRASTA | Grassmannian Robust Adaptive Subspace Tracking Algorithm (He et al. 2012)
% process_video('ST', 'GRASTA', 'dataset/demo.avi', 'output/demo_ST-GRASTA.avi');

% clear, clc;
% load('dataset/trafficdb/traffic_patches.mat');
% V = im2double(imgdb{100});
% [M,m,n,p] = convert_video3d_to_2d(V);
% I = reshape(M(:,1),m,n); % imshow(I);

%%% predefine your video parameters
%VIDEO_ROWS  =  320; % Row of each frame
%VIDEO_COLS  =  240; % Column of each fram
%DIM         = VIDEO_ROWS * VIDEO_COLS;
DIM = size(M,1);

%%% GRASTA parameters
subsampling                 = 1;   % how much patial information will be used in your application
OPTIONS.RANK                = 1;     % the estimated rank
OPTIONS.rho                 = 1.8;
OPTIONS.MAX_MU              = 10000; % set max_mu large enough for initial subspace training
OPTIONS.MIN_MU              = 1;
OPTIONS.ITER_MAX            = 20;
OPTIONS.DIM_M               = DIM;   % your data's dimension
OPTIONS.USE_MEX             = 0;     % If you do not have the mex-version of Alg 2
% please set Use_mex = 0.
OPTS                        = struct(); % initiate a empty struct for OPTS
status.init                 = 0;        % status of grasta at each iteration
U_hat                       = zeros(1); % initiate a zero U_hat at beginning

%%% Initial subspace training [optional]
% You may use some frames to train the initial subspace.
OPTIONS.CONSTANT_STEP       = 0; % use adaptive step-size for initial subspace training
max_cycles                  = 30;
training_frames             = 10;
for outiter = 1:max_cycles
  frame_order = randperm(training_frames);
  for i = 1:training_frames
    % prepare the training frame
    %I = imread(fname);
    %I = double(rgb2gray(I));
    %I = I/max(max(I));
    I = M(:,frame_order(i));

    % random subsampling the frame I
    Z = round(subsampling * DIM);
    rp = randperm(DIM);
    idx = rp(1:Z)';
    I_Omega = I(idx);

    % training the background
    [U_hat,status,OPTS] = grasta_stream(I_Omega,idx,U_hat,status,OPTIONS,OPTS);
    %fprintf('Training %d/%d ...\n',outiter,max_cycles);
  end
end

%%% Real-time background/foreground separation
OPTIONS.CONSTANT_STEP = 1e-2; % use small constant step-size
nframes = size(M,2);
for i = 1:nframes
  % prepare the image I whether it is saved as file or caputured by
  % camera
  % I = imread(fname);
  %I = double(rgb2gray(I));
  %I = I/max(max(I));
  I = M(:,i);

  % random subsampling the frame I
  Z = round(subsampling * DIM);
  rp = randperm(DIM);
  idx = rp(1:Z)';
  I_Omega = I(idx);

  % tracking the background
  [U_hat,status,OPTS] = grasta_stream(I_Omega,idx,U_hat,status,OPTIONS,OPTS);

  % bg_img is the background
  L_hat = U_hat * status.w * status.SCALE;
  %bg_img = reshape(U_hat * status.w * status.SCALE, VIDEO_ROWS,VIDEO_COLS);

  % s_img is the separated foreground
  %s_hat = I(:) - U_hat * status.w * status.SCALE;
  %s_img = reshape(s_hat,VIDEO_ROWS,VIDEO_COLS);
  S_hat = I - L_hat;

  L(:,i) = L_hat;
  S(:,i) = S_hat;

  %fprintf('Processing %d/%d ...\n',i,nframes);
end

%%
% show_results(M,L,S,hard_threshold(S),p,m,n);


function determ = fourier_sine_determinant ( n )

%*****************************************************************************80
%
%% FOURIER_SINE_DETERMINANT returns the determinant of the FOURIER_SINE matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real DETERM, the determinant.
%
  if ( mod ( n, 2 ) == 1 )
    determ = + 1.0;
  else
    determ = - 1.0;
  end

  return
end


function bessel_i0_int_values_test ( )

%*****************************************************************************80
%
%% BESSEL_I0_INT_VALUES_TEST demonstrates the use of BESSEL_IO_INT_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 February 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BESSEL_I0_INT_VALUES_TEST:\n' );
  fprintf ( 1, '  BESSEL_I0_INT_VALUES stores values of \n' );
  fprintf ( 1, '  the integral of the Bessel function I0.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      X           FX\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, fx ] = bessel_i0_int_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %12f  %24.16f\n', x, fx );

  end

  return
end


% Fig. 6.21   Feedback Control of Dynamic Systems, 5e
%             Franklin, Powell, Emami
%

clear all;
close all;

num=1;
den=[1 2 1];
rlocus(num,den);
grid;
title('Fig. 6.21: Root locus of G(s)=1/(s+2)^2 vs K');


function F  = ...
    obj_actuator_param_pulse_FFT_method(x,frequency_20,frequency_100)
% Computes objective function for determination of basic parameters of
% the proportional valve actuator that match the requirted frequency
% response. The required and actual frequency responses are compared by
% the frequency at which phase shift in -90 deg takes place. The frequency
% response of a nonlinear system is obtained by processing the pulse
% transient characteristic with the FFT algorythm.
% Copyright 2010 MathWorks, Inc.

% frequency_20   - frequency (Hz) at phase shift in -pi/2 at 20% input signal
% frequency_100  - frequency (Hz) at phase shift in -pi/2 at 100% input signal

model = 'actuator_freq_testrig_pulse_FFT_method';
load_system(model);

assignin('base','act_gain', x(1));
assignin('base','time_const', x(2));
assignin('base','act_saturation', x(3));

sim(model);

y_20 = yout(:,2);               % Pulse transient characteristic at 20% input
y_100 = yout(:,1);              % Pulse transient characteristic at 100% input
fs = 1000;                      % Sampling frequency
n = length(y_20);               % Window length = Transform length
y_20_fft = fft(y_20,n);         % Discrete Fourier Transform
y_100_fft = fft(y_100,n);       % Discrete Fourier Transform
f0 = (0:n/2-1)*(fs/n);          % Shifted frequency range, positive range
y_20_0 = fftshift(y_20_fft);    % Shifted DFT at 20% input
y_100_0 = fftshift(y_100_fft);  % Shifted DFT at 100% input
% Phase characteristic at 20% input for positive frequencies after unwrap
phase_20 = unwrap(angle(y_20_0(257:end)));
% Phase characteristic at 100% input for positive frequencies after unwrap
phase_100 = unwrap(angle(y_100_0(257:end)));

% Computing frequency at 90 deg phase shift by interpolation of phase
% characteristics
frq_20 = interp1(phase_20,f0,-pi/2);
frq_100 = interp1(phase_100,f0,-pi/2);

% Objective function as a sum of squared differences between the specified
% and computed frequencies at phase shift angle in -pi/2
F = (frequency_20 - frq_20)^2 + (frequency_100 - frq_100)^2;

end

% EOF


classdef poisson_naive_bayes_CL

% poisson_naive_bayes_CL is a classifier (CL) object that implements a
%   Poisson Naive Bayes classifier.  For each class, the expected number of occurances
%   (denoted lambda) is calculated separately for each feature/neuron, by taking the mean
%   values from the training data for each class.  To evaluate whether a given test point
%   belongs to class i, the log of the likelihood function is calculated using the lambda values as
%   parameters of Poisson distributions for each feature/neuron (i.e., a separate Poisson distribution
%   is calculated for each neuron), and the probability of observing a given number of spikes for
%   a particular neuron is calculated using the lambda value for that
%   neuron.  The overall likelihood value is calculated by multiplying the
%   probabilities for each neuron together (i.e., Naive Bayes classifiers assume that each
%   neuron is independent), or equivalently, adding the log of the probabilities for
%   each neuron together.  The class with the highest likelihood value is chosen as the
%   predicted label, and the decision values are the log likelihood values.
%
%
% Like all CL objects, there are two main methods, which are:
%
%  1.  cl = train(cl, XTr, YTr)
%       This method takes the training data (XTr, YTr) and learns a mean vector
%       (i.e., the lambda values) for each class.
%
%  2. [predicted_labels decision_values] = test(cl, XTe)
%       This method takes the test data an[d calculates the log of the likelihood
%       function for each class (which are the decision values), and returns the class
%       with the highest likelihood as the predicted_label.
%
%  Notes:
%
%  1. This classifier assumes that all feature values are integers.
%
%  2. If the estimate rate parameter (lambda) for any feature is 0 (i.e., if the mean of a feature in the training data is 0),
%       then this 0 lambda estimate will be replaced by the value 1/(num_training_points_in_class_k +1); i.e., we will assume
%       that there is one more training point in which an event occurred.  This prevents errors if an event occurred on a test point
%       but the estimated lambda was 0.
%
%
%   XTr and XTe are in the form [num_features x num_examples]
%   YTr is in the form [num_examples x 1]
%

%==========================================================================

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

%==========================================================================

  properties
        lambdas = [];   % the expected number of occurances for each neuron for each class
        labels = [];  % the unique labels for the different classes
    end

    methods

        % constructor
        function cl = poisson_naive_bayes_CL
        end

        function cl = train(cl, XTr, YTr)

            % added sanity check
            if size(YTr, 2) ~= size(XTr, 2)  &&  size(YTr, 1) ~= size(XTr, 2)
               error('Number of columns in YTr, and XTr must be the same (i.e., there must be one and exactly one label for each data point)')
            end

            % sanity check to make sure the training data only contains integers
%            if sum(sum(abs(round(XTr) - XTr))) > 10.^-2  % (not having it be exactly zero to account for round off error)
%                error(['The training data must only contain integers to use this classifier.  Make sure the data was loaded as integers (e.g., using the appropriate flag in ' ...
%                    'basic_DS/generalization_DS datasources, and that only appropriate feature preprocessors were used (e.g., do not use the zscore_normalize_FP)'])
%            end

            cl.labels = unique(YTr);

            for iLabel = 1:length(cl.labels )

                lambdas(:, iLabel) = mean(XTr(:, (YTr == cl.labels(iLabel))), 2);

                % If the rate parameter (lambda) is zero, there can be problems if the test data has a value greater than zero
                % (i.e., will get zero probability, which creates problems when taking the log).
                % We will deal with this problem by assuming that there is one more data point with a 1 (when all training data has zero occurances)
                zero_lambda_replacement_value = 1/(length(find(YTr == cl.labels(iLabel))) + 1);   % if found zeros for all trials, assume that there is one more trial where a 1 was found...
                lambdas((lambdas(:, iLabel) == 0), iLabel) = zero_lambda_replacement_value;

            end

            cl.lambdas = lambdas;

        end

        function [predicted_labels decision_values] = test(cl, XTe)

            % sanity check to make sure the test data only contains integers
%            if sum(sum(abs(round(XTe) - XTe))) > 10.^-2   % (not having it be exactly zero to account for round off error)
%                error(['The test data must only contain integers to use this classifier.  Make sure the data was loaded as integers (e.g., using the appropriate flag in ' ...
%                    'basic_DS/generalization_DS datasources, and that only appropriate feature preprocessors were used (e.g., do not use the zscore_normalize_FP)'])
%            end

            % compute simultaneously all p-values for all features and test points (easy b/c everything is independent)
            curr_lambdas = repmat(cl.lambdas, [1, 1, size(XTe, 2)]);
            XTe_repmat_for_all_classes = permute(repmat(XTe, [1, 1, size(cl.lambdas, 2)]), [1 3 2]);
            %p_vals = exp(-curr_lambdas + XTe_repmat_for_all_classes  .* log(curr_lambdas) - gammaln(XTe_repmat_for_all_classes  + 1));
            %log_likelihoods = squeeze(sum(log(p_vals)));

            log_likelihoods = squeeze(sum(-curr_lambdas + XTe_repmat_for_all_classes  .* log(curr_lambdas) - gammaln(XTe_repmat_for_all_classes  + 1), 1)); % possibly even faster...
            % % %log_likelihoods = squeeze(sum(-curr_lambdas + XTe_repmat_for_all_classes  .* log(curr_lambdas))); % since the factorial term does not depend on the class, code might be faster
                                                                                % if this term is removed.  The decision values returned
                                                                                % will no longer be exactly the log likelihood values but rather the log likelihood
                                                                                % values minus a constant.  Using this will make AUROC values worse, so don't do it!!!!

           % prior to version 1.0.4 the following line was used.  I added a the sum over the first dimention (i.e., sum(XXX, 1)) so that the code will run even when only a single site is used
           %log_likelihoods = squeeze(sum(-curr_lambdas + XTe_repmat_for_all_classes  .* log(curr_lambdas) - gammaln(XTe_repmat_for_all_classes  + 1))); % possibly even faster...

            [vals inds] = randmax(log_likelihoods);
            predicted_labels  = cl.labels(inds);
            decision_values = log_likelihoods';

        end

    end  % end public methods

end   % end classdef


function [Z, Z_L, Z_U, T, P, rho, c, g, mu, nu, k, n, n_sum] = atmo(alt,division,units)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Program:    1976 Standard Atmosphere Calculator[0-1000 km]
%   Author:     Brent Lewis(RocketLion@gmail.com)
%               University of Colorado-Boulder
%   History:    Original-1/10/2007
%               Revision-1/12/2007-Corrected for changes in Matlab versions
%               for backward compatability-Many thanks to Rich
%               Rieber(rrieber@gmail.com)
%   Input:      alt:        Final Geometric Altitude[km]
%               division:   Reporting points for output arrays[km]
%                           (.01 km & Divisible by .01 km)
%               units:      1-[Metric]
%                           2-{English}
%   Default:    Values used if no input
%               alt:        1000 km
%               division:   1 km
%               units:      Metric
%   Output:     Each value has a specific region that it is valid in with this model
%               and is only printed out in that region
%               Z:          Total Reporting Altitudes[0<=alt<=1000 km][km]{ft}
%               Z_L:        Lower Atmosphere Reporting Altitudes[0<=alt<=86 km][km]{ft}
%               Z_U:        Upper Atmosphere Reporting Altitudes[86<=alt<=1000 km][km]{ft}
%               T:          Temperature array[0<=alt<=1000 km][K]{R}
%               P:          Pressure array[0<=alt<=1000 km][Pa]{in_Hg}
%               rho:        Density array[0<=alt<=1000 km][kg/m^3]{lb/ft^3}
%               c:          Speed of sound array[0<=alt<=86 km][m/s]{ft/s}
%               g:          Gravity array[0<=alt<=1000 km][m/s^2]{ft/s^2}
%               mu:         Dynamic Viscosity array[0<=alt<=86 km][N*s/m^2]{lb/(ft*s)}
%               nu:         Kinematic Viscosity array[0<=alt<=86 km][m^2/s]{ft^2/s}
%               k:          Coefficient of Thermal Conductivity
%                           array[0<=alt<=86 km][W/(m*K)]{BTU/(ft*s*R)}
%               n:          Number Density of individual gases
%                           (N2 O O2 Ar He H)[86km<=alt<=1000km][1/m^3]{1/ft^3}
%               n_sum:      Number Density of total gases
%                           [86km<=alt<=1000km][1/m^3]{1/ft^3}
%   Acknowledgements:       1976 U.S. Standard Atmosphere
%                           Prof. Adam Norris-Numerical Analysis Class
%                           Steven S. Pietrobon USSA1976 Program
%   Notes:                  Program uses a 5-point Simpson's Rule in 10
%                           meter increments.  Results DO vary by less 1%
%                           compared to tabulated values and is probably
%                           caused by different integration techniques
%   Examples:               atmo() will compute the full atmosphere in 1 km
%                           increments and output in Metric Units
%                           atmo(10) will compute the atmosphere between 0
%                           and 10 km in 1 km increments and output in
%                           Metric Units
%                           atmo(20,.1,2) will compute the atmosphere
%                           between 0 and 20 km in 100 m increments and
%                           output in English Units
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if nargin == 0
    alt = 1000;
    division = 1;
    units = 1;
elseif nargin == 1
    division = 1;
    units = 1;
elseif nargin == 2
    units = 1;
end

%   Error Reporting
if nargin > 3
    error('Too many inputs')
elseif mod(division,.01) ~= 0
    error('Divisions must be multiples of .01 km')
elseif units ~= 1 && units ~= 2
    error('Units Choice Invalid[1-Metric,2-English]')
elseif alt<0 || alt>1000
    error('Program only valid for 0<altitudes<1000 km')
end

%   Matrix Pre-allocation
if alt <= 86
    Z_L = (0:division:alt)';
    Z_U = [];
    n = [];
else
    Z_L = (0:division:86)';
    Z_U = (86:division:alt)';
    if mod(86,division) ~= 0
        Z_L = [Z_L; 86];
    end
    if mod(alt-86,division) ~= 0
        Z_U = [Z_U; alt];
    end
end
T_L = zeros(size(Z_L));
T_M_L = T_L;
T_U = zeros(size(Z_U));

%   Conversion Factor Used in 80<alt<86 km
Z_M = 80:.5:86;
M_M_0 = [1 .999996 .999989 .999971 .999941 .999909 ...
    .999870 .999829 .999786 .999741 .999694 .999641 .999579];

%   Constants
M_0 = 28.9644;
M_i = [28.0134; 15.9994; 31.9988; 39.948; 4.0026; 1.00797];
beta = 1.458e-6;
gamma = 1.4;
g_0 = 9.80665;
R = 8.31432e3;
r_E = 6.356766e3;
S = 110.4;
N_A = 6.022169e26;

%   Temperature
for i = 1 : length(Z_L)
    T_L(i,1) = atmo_temp(Z_L(i));
    T_M_L(i,1) = T_L(i,1);
    if Z_L(i) > 80 && Z_L(i) < 86
        T_L(i,1) = T_L(i)*interp1(Z_M,M_M_0,Z_L(i));
    end
end
for i = 1 : length(Z_U)
    T_U(i,1) = atmo_temp(Z_U(i));
end

%   Number Density
if alt > 86
    n = atmo_compo(alt,division);
    n_sum = sum(n,2);
else
    n = [];
    n_sum = [];
end

%   Pressure
P_L = atmo_p(Z_L);
P_U = atmo_p(Z_U,T_U,n_sum);

%   Density
rho_L = M_0*P_L./(R*T_M_L);
if ~isempty(P_U)
    rho_U = n*M_i/N_A;
else
    rho_U = [];
end

%   Speed of Sound
c = sqrt(gamma*R*T_M_L/M_0);
%   Dynamic Viscosity
mu = beta*T_L.^1.5./(T_L+S);
%   Kinematic Viscosity
nu = mu./rho_L;
%   Thermal Conductivity Coefficient
k = 2.64638e-3*T_L.^1.5./(T_L+245*10.^(-12./T_L));

%   Combine Models
T = [T_L(1:end-1*double(~isempty(T_U)));T_U];
P = [P_L(1:end-1*double(~isempty(T_U)));P_U];
rho = [rho_L(1:end-1*double(~isempty(T_U)));rho_U];
Z = [Z_L(1:end-1*double(~isempty(T_U)));Z_U];

%   Gravity
g = g_0*(r_E./(r_E+Z)).^2;

if units == 2
    unit_c = [3.048e-1 3.048e-1 3.048e-1 5/9 0.0001450377 1.6018463e1...
        3.048e-1 3.048e-1 1.488163944 9.290304e-2 6.226477504e-3...
        3.531466672e2 3.531466672e2];
    Z = Z/unit_c(1);
    Z_L = Z_L/unit_c(2);
    Z_U = Z_U/unit_c(3);
    T = T/unit_c(4);
    P = P/unit_c(5);
    rho = rho/unit_c(6);
    c = c/unit_c(7);
    g = g/unit_c(8);
    mu = mu/unit_c(9);
    nu = nu/unit_c(10);
    k = n/unit_c(11);
    n_sum = n_sum/unit_c(12);
end


function [mask, fvc]=VObjEllipsoid(p,x,y,z,flag)
%create 3D ellipsoid virtual object

% Initialize parameters
RadiusX=p.RadiusX;
RadiusY=p.RadiusY;
RadiusZ=p.RadiusZ;
CenterX=p.CenterX;
CenterY=p.CenterY;
CenterZ=p.CenterZ;
FaceNum=p.FaceNum;

% Generate ellipsoid coordinate
[X,Y,Z] = ellipsoid(CenterX,CenterY,CenterZ,RadiusX,RadiusY,RadiusZ,FaceNum);

fvc = surf2patch(X,Y,Z);

if flag~=1 % Render object only
    mask=0;
    return;
end

mask = vert2mask(fvc.vertices,x,y,z);

end


function r = overlap_ratio(rect1, rect2)
% OVERLAP_RATIO
% Compute the overlap ratio between two rectangles
%
% Hyeonseob Nam, 2015
%

inter_area = rectint(rect1,rect2);
union_area = rect1(:,3).*rect1(:,4) + rect2(:,3).*rect2(:,4) - inter_area;

r = inter_area./union_area;
end


function [ss_estimates,ss_estimates_constant,ss_estimates_exogenous,ss_estimates_contribution_exo]=ssestimates(ss_record,n,T,cband)

% function [ss_estimates]=ssestimates(ss_record,n,T,cband)
% calculates the point estimate (median), lower bound and upper bound of the steady-state from the posterior distribution
% inputs:  - cell 'ss_record': record of the gibbs sampler draws for the steady-state
%          - integer 'n': number of endogenous variables in the BVAR model (defined p 7 of technical guide)
%          - integer 'T': number of sample time periods (defined p 7 of technical guide)
%          - scalar 'cband': confidence level for VAR coefficients
% outputs: - cell 'ss_estimates': lower bound, point estimates, and upper bound for the steady-state

% create first the cell that will contain the estimates
ss_estimates=cell(n,1);
ss_estimates_constant=cell(n,1);
ss_estimates_exogenous=cell(n,1);
ss_estimates_contribution_exo=cell(1,1);
% for each variable and each sample period, compute the median, lower and upper bound from the Gibbs sampler records
% consider variables in turn
for ii=1:n
   % consider sample periods in turn
   for jj=1:T
   % compute first the lower bound
   ss_estimates{ii,1}(1,jj)=quantile(ss_record{ii,1}(:,jj),(1-cband)/2);
   % then compute the median
   ss_estimates{ii,1}(2,jj)=quantile(ss_record{ii,1}(:,jj),0.5);
   % finally compute the upper bound
   ss_estimates{ii,1}(3,jj)=quantile(ss_record{ii,1}(:,jj),(1-(1-cband)/2));
% % %    if m>1
% % %    % same for only the constant
% % %    ss_estimates_constant{ii,1}(1,jj)=quantile(ss_record_constant{ii,1}(:,jj),(1-cband)/2);
% % %    % then compute the median
% % %    ss_estimates_constant{ii,1}(2,jj)=quantile(ss_record_constant{ii,1}(:,jj),0.5);
% % %    % finally compute the upper bound
% % %    ss_estimates_constant{ii,1}(3,jj)=quantile(ss_record_constant{ii,1}(:,jj),(1-(1-cband)/2));
% % %    % same for only the exogenous
% % %    ss_estimates_exogenous{ii,1}(1,jj)=quantile(ss_record_exogenous{ii,1}(:,jj),(1-cband)/2);
% % %    % then compute the median
% % %    ss_estimates_exogenous{ii,1}(2,jj)=quantile(ss_record_exogenous{ii,1}(:,jj),0.5);
% % %    % finally compute the upper bound
% % %    ss_estimates_exogenous{ii,1}(3,jj)=quantile(ss_record_exogenous{ii,1}(:,jj),(1-(1-cband)/2));
% % %    % compute contribution from only the exogenous part
% % %    ss_estimates_contribution_exo{ii,1}(1,jj)=(quantile(ss_record_constant{ii,1}(:,jj),0.5)-quantile(ss_record{ii,1}(:,jj),0.5));
% % %    else
% % %    ss_estimates_constant=[];
% % %    ss_estimates_exogenous=[];
% % %    end
   end
end


function [gen, ave1, ave2] = fusion_strategy(features_a, features_b, source_a, source_b, unit)

[m,n] = size(features_a);
[m1,n1] = size(source_a);
ave_temp1 = zeros(m1,n1);
ave_temp2 = zeros(m1,n1);
weight_ave_temp1 = zeros(m1,n1);
weight_ave_temp2 = zeros(m1,n1);

for i=2:m-1
    for j=2:n-1
        A1 =sum(sum(features_a(i-1:i+1,j-1:j+1)))/9;
        A2 =sum(sum(features_b(i-1:i+1,j-1:j+1)))/9;

        % weight average
        weight_ave_temp1(((i-2)*unit+1):((i-1)*unit),((j-2)*unit+1):((j-1)*unit)) = A1/(A1+A2);
        weight_ave_temp2(((i-2)*unit+1):((i-1)*unit),((j-2)*unit+1):((j-1)*unit)) = A2/(A1+A2);
        ave_temp1(((i-2)*unit+1):((i-1)*unit),((j-2)*unit+1):((j-1)*unit)) = A1;
        ave_temp2(((i-2)*unit+1):((i-1)*unit),((j-2)*unit+1):((j-1)*unit)) = A2;
%         % choose max
%         if A1>A2
%             gen_temp(((i-2)*unit+1):((i-1)*unit),((j-2)*unit+1):((j-1)*unit)) = ones(unit,unit);
%             temp_mask(i,j) = 1;
%         end
    end
end
% figure;imshow(temp_mask);
weight_ave_temp1 = weight_ave_temp1(1:m1,1:n1);
weight_ave_temp2 = weight_ave_temp2(1:m1,1:n1);
% figure;imshow(weight_ave_temp1);
% figure;imshow(weight_ave_temp2);

gen = source_a.*weight_ave_temp1 + source_b.*weight_ave_temp2;
% figure;imshow(gen);

ave1 = ave_temp1;
ave2 = ave_temp2;
end


% Assume a target positioned at x = 1, travelling with speed v = 0.1
state = [1;0.1;2;0];    % Assume state with four dimensions [x_pos, x_vel, y_pos, y_vel]
measurement = [1.2;1.8;3];% Measurement state [x_pos,y_pos]

% Create an instance of a 1D Constant Velocity model
obs = LinearGaussianX('NumMeasDims', 3,'NumStateDims',4,...
                      'MeasurementErrVariance',50,'Mapping',[1 3 2]);

% View the transition matrix and process covariance matrices
H = obs.feval();
R = obs.covar();

% Predict the target's position and velocity after the interval has passed
projectedState = obs.feval(state);

% Do the same as above, but this time add process noise to the prediction
projectedState2 = obs.feval(state,true);

% Generate 50 random noise samples from the dynamic model
noise = obs.random(50);

% Check how likely the predictions we made are
lik = obs.pdf(projectedState,state);
lik2 = obs.pdf(projectedState2,state); % HINT: newState2 should be less likely


function [xi] = logSE3(chi)
C = chi(1:3,1:3);
r = chi(1:3,4);
[vecs,eigs] = eig(C);
[~,idx] = min(abs(diag(eigs)-1));
a = vecs(:,idx);

phi = acos(1/2*(trace(C)-1));
if phi == 0
    a = zeros(3,1);
    iJ = eye(3);
else
    C1 = cos(phi)*eye(3) + (1-cos(phi))*a*transpose(a) + sin(phi)*[[0,-a(3),a(2)];[a(3),0,-a(1)];[-a(2),a(1),0]];
    C2 = cos(-phi)*eye(3) + (1-cos(-phi))*a*transpose(a) + sin(-phi)*[[0,-a(3),a(2)];[a(3),0,-a(1)];[-a(2),a(1),0]];
    if norm(C2-C)<norm(C1-C)
        phi = -phi;
    end
    iJ = phi/2*cot(phi/2)*eye(3) + (1-phi/2*cot(phi/2))*a*a' - phi/2*[[0,-a(3),a(2)];[a(3),0,-a(1)];[-a(2),a(1),0]];
end
rho = iJ*r;
xi = real([phi*a;rho]);
end


function b = GetB_Values(protocol)
% Computes the b value for each measurement in the protocol
%
% b = GetB_Values(protocol)
% returns an array of b values, one for each measurement defined
% in the list of measurements in the protocol.
%
% author: Daniel C Alexander (d.alexander@ucl.ac.uk)
%

GAMMA = 2.675987E8;
if(strcmp(protocol.pulseseq, 'PGSE') || strcmp(protocol.pulseseq, 'STEAM'))
    modQ = GAMMA*protocol.smalldel.*protocol.G;
    diffTime = protocol.delta - protocol.smalldel/3;
    b = diffTime.*modQ.^2;

elseif(strcmp(protocol.pulseseq, 'DSE'))
    b = getB_ValuesDSE(protocol.G, protocol.delta1, protocol.delta2, protocol.delta3, protocol.t1, protocol.t2, protocol.t3);

elseif(strcmp(protocol.pulseseq, 'OGSE'))
    b = GetB_ValuesOGSE(protocol.G, protocol.delta, protocol.smalldel, protocol.omega);
end