% StackExchange Signal Processing Q60916
% https://dsp.stackexchange.com/questions/60916
% What Is the Bilateral Filter Category: LPF, HPF, BPF or BSF?
% References:
%   1.  A
% Remarks:
%   1.  B
% TODO:
% 	1.  C
% Release Notes
% - 1.0.001     02/03/2022
%   *   Using 'sgtitle()' instead of 'suptitle()' (Requires R2018b and above).
% - 1.0.000     19/08/2019
%   *   First release.

%% General Parameters

subStreamNumberDefault = 79;

run('InitScript.m');

figureIdx           = 0;
figureCounterSpec   = '%04d';

generateFigures = ON;

%% Simulation Parameters

inputImageFileName  = 'Lenna.png';

kernelRadius    = 7;
spatialStd      = 2;
vRangeStd       = [0.001, 0.01, 0.05, 0.5, 1e10];

%% Generate Data

mI = im2double(imread(inputImageFileName));
mI = mean(mI, 3);
mO = repmat(mI, [1, 1, 3]);

mRefPixels  = [84, 432; 82, 442; 267, 159; 420, 320];
mColors     = [1, 0, 0; 0, 1, 0; 0, 0, 1; 1, 0, 1];

%% Analysis

numPatches      = size(mRefPixels, 1);
kernelLength    = (2 * kernelRadius) + 1;

for ii = 1:numPatches
    mO(mRefPixels(ii, 1), mRefPixels(ii, 2), :) = reshape(mColors(ii, :), [1, 1, 3]);
    mO(mRefPixels(ii, 1) - kernelRadius - 1, mRefPixels(ii, 2) - kernelRadius - 1:mRefPixels(ii, 2) + kernelRadius + 1, :) = repmat(reshape(mColors(ii, :), [1, 1, 3]), [1, kernelLength + 2, 1]);
    mO(mRefPixels(ii, 1) + kernelRadius + 1, mRefPixels(ii, 2) - kernelRadius - 1:mRefPixels(ii, 2) + kernelRadius + 1, :) = repmat(reshape(mColors(ii, :), [1, 1, 3]), [1, kernelLength + 2, 1]);
    mO(mRefPixels(ii, 1) - kernelRadius - 1:mRefPixels(ii, 1) + kernelRadius + 1, mRefPixels(ii, 2) - kernelRadius - 1, :) = repmat(reshape(mColors(ii, :), [1, 1, 3]), [kernelLength + 2, 1, 1]);
    mO(mRefPixels(ii, 1) - kernelRadius - 1:mRefPixels(ii, 1) + kernelRadius + 1, mRefPixels(ii, 2) + kernelRadius + 1, :) = repmat(reshape(mColors(ii, :), [1, 1, 3]), [kernelLength + 2, 1, 1]);
end

figureIdx = figureIdx + 1;

hFigure     = figure('Position', figPosLarge);
hAxes       = axes;
hImageObj   = imshow(mO);
set(get(hAxes, 'Title'), 'String', {['Lenna Image with Patches']}, ...
    'FontSize', fontSizeTitle);

if(generateFigures == ON)
    saveas(hFigure,['Figure', num2str(figureIdx, figureCounterSpec), '.png']);
end

for ii = 1:length(vRangeStd)
    rangeStd = vRangeStd(ii);

    figureIdx = figureIdx + 1;
    hFigure = figure('Position', figPosLarge);

    plotIdx = 0;
    patchIdx = 0;

    for jj = 1:4
        patchIdx = patchIdx + 1;
        plotIdx = plotIdx + 1;

        hAxes       = subplot(4, 5, plotIdx);
        hImageObj   = imshow(mI(mRefPixels(patchIdx, 1) - kernelRadius:mRefPixels(patchIdx, 1) + kernelRadius, mRefPixels(patchIdx, 2) - kernelRadius:mRefPixels(patchIdx, 2) + kernelRadius));
        set(get(hAxes, 'Title'), 'String', {['Input Patch']}, ...
            'FontSize', fontSizeTitle);

        plotIdx = plotIdx + 1;

        % Spatial Weight
        mWs = CalcSpatialWeights(kernelRadius, spatialStd);

        hAxes       = subplot(4, 5, plotIdx);
        hImageObj   = imshow(mWs, []);
        set(get(hAxes, 'Title'), 'String', {['Spatial Weight']}, ...
            'FontSize', fontSizeTitle);

        plotIdx = plotIdx + 1;

        % Spatial Weight
        mWr = CalcRangeWeights(mI, mRefPixels(patchIdx, :), kernelRadius, rangeStd);

        hAxes       = subplot(4, 5, plotIdx);
        hImageObj   = imshow(mWr, []);
        set(get(hAxes, 'Title'), 'String', {['Range Weight']}, ...
            'FontSize', fontSizeTitle);

        plotIdx = plotIdx + 1;

        % Kernel Weight
        mW = mWr .* mWs;
        mW = mW / sum(mW(:));

        hAxes       = subplot(4, 5, plotIdx);
        hImageObj   = imshow(mW, []);
        set(get(hAxes, 'Title'), 'String', {['Kernel Weight']}, ...
            'FontSize', fontSizeTitle);

        plotIdx = plotIdx + 1;

        hAxes       = subplot(4, 5, plotIdx);
        hImageObj   = imshow(log(1 + abs(fftshift(fft2(mW)))), []);
        set(get(hAxes, 'Title'), 'String', {['Kernel - Frequency']}, ...
            'FontSize', fontSizeTitle);

    end

    sgtitle(hFigure, ['Kernel Analysis for rangeStd - ', num2str(rangeStd)]);

    if(generateFigures == ON)
        saveas(hFigure,['Figure', num2str(figureIdx, figureCounterSpec), '.png']);
    end

end

%% Restore Defaults

% set(0, 'DefaultFigureWindowStyle', 'normal');
% set(0, 'DefaultAxesLooseInset', defaultLoosInset);


function TV = compute_total_variation(y, options)

% compute_total_variation - compute the total variation of an image
%
%   TV = compute_total_variation(y, options);
%
%   See also: perform_tv_projection, grad, div.
%
%   Copyright (c) 2007 Gabriel Peyre

options.null = 0;

nbdims = 2;
if size(y,1)==1 || size(y,2)==1
    nbdims = 1;
end
if size(y,1)>1 && size(y,2)>1 && size(y,3)>1
    nbdims = 3;
end

if nbdims==1
    TV = sum( abs(diff(y)) );
    return;
end

weight_tv = getoptions(options, 'weight_tv', ones(size(y)) );
tv_norm = getoptions(options,'tv_norm', 'l2');

% options.bound = 'per';
g = grad(y, options);

switch tv_norm
    case 'l2'
        TV = sqrt( sum(g.^2,nbdims+1) );
    case 'l1'
        TV = sum( abs(g),nbdims+1);
    case 'linf'
        TV = max( abs(g),[],nbdims+1);
    otherwise
        error('Unknown norm.');
end
TV = TV .* weight_tv;
TV = sum(TV(:));


function A5=inflowclassstat1(file)
%INFLOWCLASSSTAT1 berfungsi untuk mendistribusikan matriks inflow dalam
%berbagai kelas
%INFLOWCLASSSTAT1 dipergunakan ketika memilih mendistribusikan kelas dalam
%format statistikal
%dalam hal ini kelas dipastikan hanya 3 buah
A1=load(file); %untuk membuka file txt disimpan di A1
[m,n]=size(A1);
%penentuan rerata
A2=mean(A1);
A3=std(A1);
%penentuan nilai maksimum per kelas
A4(1,:)=A2-0.5*A3;
A4(2,:)=A2+0.5*A3;
A4(3,:)=max(A1);
for a=1:m
    for b=1:n
        c=1;
        for d=1:3+1
        if A1(a,b)<=A4(c,b)
           A5(a,b)=c;
        else
           c=d;
        end
        end
    end
end
A4;
A5


function K = covLINone(hyp, x, z, i)

% Linear covariance function with a single hyperparameter. The covariance
% function is parameterized as:
%
% k(x^p,x^q) = (x^p'*x^q + 1)/t^2;
%
% where the P matrix is t2 times the unit matrix. The second term plays the
% role of the bias. The hyperparameter is:
%
% hyp = [ log(t) ]
%
% Copyright (c) by Carl Edward Rasmussen and Hannes Nickisch, 2010-09-10.
%
% See also COVFUNCTIONS.M.

if nargin<2, K = '1'; return; end                  % report number of parameters
if nargin<3, z = []; end                                   % make sure, z exists
xeqz = isempty(z); dg = strcmp(z,'diag');                       % determine mode

it2 = exp(-2*hyp);                                                  % t2 inverse

% precompute inner products
if dg                                                               % vector kxx
  K = sum(x.*x,2);
else
  if xeqz                                                 % symmetric matrix Kxx
    K = x*x';
  else                                                   % cross covariances Kxz
    K = x*z';
  end
end

if nargin<4                                                        % covariances
  K = it2*(1+K);
else                                                               % derivatives
  if i==1
    K = -2*it2*(1+K);
  else
    error('Unknown hyperparameter')
  end
end


function theta=Theta_ADMM(Y,W,H,X,L,alfa,beta,theta,rho,maxiter)

%%
%  Solve the JPLAY's subproblem: theta, using ADMM

%% Initializing Setting
epsilon = 1e-6;
iter=0;

G=zeros(size(theta));
Q=zeros(size(theta*X));
P=zeros(size(theta*X));
% M=zeros(size(theta*X));
lamda1=zeros(size(theta*X));
lamda2=zeros(size(theta));
lamda3=zeros(size(theta*X));
lamda4=zeros(size(theta*X));
% lamda5=zeros(size(theta*X));

stop = false;
mu=1e-3;
% rho=2;%1.4:Houston2018 2:Houston2013
mu_bar=1e+6;

GL=(X*L*X');%Graph Laplacian

  while ~stop && iter < maxiter+1

      iter=iter+1;
      %solve theta
      theta=(mu*(H*X')+lamda1*X'+mu*G+lamda2+mu*(Q*X')+lamda3*X'+mu*(P*X')+lamda4*X')/(3*mu*(X*X')+beta*GL+mu*eye(size(X*X')));
      %solve H
      H=(alfa*(W'*W)+(G*G')+mu*eye(size(W'*W)))\(alfa*(W'*Y)+(G*X)+mu*(theta*X)-lamda1);
      %solve G
      G=((H*H')+mu*eye(size(H*H')))\(mu*theta-lamda2+(H*X'));
      %solve Q
      Q=max(theta*X-(lamda3/mu),0);
      %solve P
      MidV=theta*X-lamda4/mu;
      for i=1:size(MidV,2)
          if norm(MidV(:,i))<=1
             P(:,i)=MidV(:,i);
          else
             P(:,i)=MidV(:,i)/norm(MidV(:,i));
          end
      end
%       M=max(abs(theta*(X-XW)-lamda5/mu)-(beta*0.01/mu),0).*sign(theta*(X-XW)-lamda5/mu);
     %update Lagrange multipliers
     lamda1=lamda1+mu*(H-theta*X);
     lamda2=lamda2+mu*(G-theta);
     lamda3=lamda3+mu*(Q-theta*X);
     lamda4=lamda4+mu*(P-theta*X);
%      lamda5=lamda5+mu*(M-theta*(X-XW));
     %update penalty parameter
     mu=min(mu*rho,mu_bar);
     %computer errors
     r_H=norm(H-theta*X,'fro');
     r_G=norm(G-theta,'fro');
     r_Q=norm(Q-theta*X,'fro');
     r_P=norm(P-theta*X,'fro');
%      r_M=norm(M-theta*(X-XW),'fro');
     %check the convergence conditions
     if r_H<epsilon&&r_G<epsilon&&r_Q<epsilon&&r_P<epsilon%&&r_M<epsilon
         stop = true;
         break;
     end
  end
end


%  Core QP Solver. Use qpng.m instead.
%
% This routine solves the following optimization problem:
%
% min_x .5x' H x + q' x
% s.t.  Aeq x =  beq
%       Ain x <= bin
%
% note that x0 is a feasible starting point.
%
% (C) Nicolo Giorgetti, 2006.
function [x, lam, k, status]=qpsolng(H, q, Aeq, beq, Ain, bin, x0)

nmax=200; % max number of iterations
tol=1e-6; % tolerance

neq=length(beq); % number of eqs
nin=length(bin); % number of ineqs

x=x0;
n=size(x);
Wact=[];
nact=0; % number of rows in Wact (only active inequalities)

status=1; % problem feasible
lam=zeros(neq+nin,1);
k=[];

for k=1:nmax
   % Construct KKT
   K=H;
   r=-q-H*x;
   if neq > 0
      % Add equality constraints
      A=Aeq;
   end
   if nin > 0
      % Add active inequality constraints
      for j=1:nact
         i=Wact(j);
         if j+neq==1
            A=Ain(i,:);
         else
            A=[A; Ain(i,:)];
         end
      end
   end
   nneq=neq+nact;
   if nneq>0
      K=[K, A'; A, zeros(nneq,nneq)];
      r=[r; zeros(nneq,1)];
   end

   y=K\r; %%%%%%%% Check this: we should use pinv instead. Possible numerical problems
   p=y(1:n);

   if norm(p)<tol
      % check optimality or add to work set
      if nact==0
         x=x+p;
         status=1;
         return % successfully
      else
         lam=y(n+neq+1:n+neq+nact);
         [lmin,arg]=min(lam);
         if lmin >= 0
            x=x+p;
            status=1;
            return; % successfully
         else
            % remove constraints from W
            nact=nact-1;
            for j=arg:nact
               Wact(j)=Wact(j+1);
            end
         end
      end
   else % x is not the minimizer in W
      count=nin+1;
      val(1:count)=1.1;
      for j=1:nin
         if Ain(j,1:n)*p > max(tol^2,1e-15)
            val(j)=(bin(j)-Ain(j,1:n)*x)/(Ain(j,1:n)*p);
         end
      end
      val(count)=1;

      [alpha,ind]=min(val); % this is the allowed step size

      x=x+alpha*p;

      if ind < count % there are blocking constraints, add one to W
         nact=nact+1;
         Wact(nact)=ind;
      end
   end
end

eq_infeas=0;
in_infeas=0;

if neq>0
   eq_infeas = (norm(Aeq*x-beq) > rtol*(1+norm(beq)));
end
if nin>0
   in_infeas = (any(Ain*x-bin > -rtol*(1+norm(bin))));
end

if eq_infeas | in_infeas
   status=2; % Max iterations reached, no solution found.
else
   status=3; % Max iterations reached but a feasible solution found.
end

return;


function theta = TLDA_initialize_SAE(numK, numM, numC, TrainData, TestData)
   %% Initialize parameters randomly based on layer sizes.

    train_x = [TrainData TestData]';
    %%  ex1 train a 10 hidden unit SDAE and use it to initialize a FFNN
    %  Setup and train a stacked denoising autoencoder (SDAE)
    rand('state',0)
    sae = saesetup([numM numK]);
    sae.ae{1}.activation_function       = 'sigm';
    sae.ae{1}.learningRate              = 100;
    sae.ae{1}.inputZeroMaskedFraction   = 0;
    opts.numepochs =   1;
    opts.batchsize = size(train_x,1);
    sae = saetrain(sae, train_x, opts);
    % visualize(sae.ae{1}.W{1}(:,2:end)')

    W1 = sae.ae{1}.W{1}(:,2:end);
    b1 = sae.ae{1}.W{1}(:,1:1);
    W11 = sae.ae{1}.W{2}(:,2:end);
    b11 = sae.ae{1}.W{2}(:,1:1);

    data=[TrainData TestData];
    hiddeninputs = W1 * data + b1 * ones(1, size(data,2)); % hiddensize * numpatches
    hiddenvalues = sigmoid( hiddeninputs ); % hiddensize * numpatches

    train_x = hiddenvalues';
    %%  ex1 train a 10 hidden unit SDAE and use it to initialize a FFNN
    %  Setup and train a stacked denoising autoencoder (SDAE)
    rand('state',0)
    sae = saesetup([numK numC]);
    sae.ae{1}.activation_function       = 'sigm';
    sae.ae{1}.learningRate              = 100;
    sae.ae{1}.inputZeroMaskedFraction   = 0;
    opts.numepochs =   1;
    opts.batchsize = size(train_x,1);
    sae = saetrain(sae, train_x, opts);
    % visualize(sae.ae{1}.W{1}(:,2:end)')

    W2 = sae.ae{1}.W{1}(:,2:end);
    b2 = sae.ae{1}.W{1}(:,1:1);
    W22 = sae.ae{1}.W{2}(:,2:end);
    b22 = sae.ae{1}.W{2}(:,1:1);

	% Convert weights and bias gradients to the vector form.
	theta = [W1(:) ; W2(:) ; W22(:) ; W11(:) ; b1(:) ; b2(:) ; b22(:) ; b11(:)];
end

function sigm = sigmoid(x)

    sigm = 1 ./ (1 + exp(-x));
end


classdef StudentT2C
%%STUDENTT2C A collection of functions for the bivariate Student-t copulae.
%           Implemented functions include: PDF, tau.
%
%REFERENCES:
%[1] H. Joe and D. Kurowicka, Dependence Modeling: Vine Copula Handbook.
%    World Scientific, 2011.
%
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    methods(Static)
        function jprob = PDF(u,R,nu)
            %PDF Generates a joint probability for column vector u or
            %    a row vector of joint probabilities for matrix u
            %    with each column as the input vector.
            %
            %INPUTS:
            % u: A 2-by-n matrix where each column represents a
            %    2-dimensional random vector and n is the number of
            %    random vectors. Entries must be in [0,1].
            % R: A 2-by-2 correlation matrix, or a scalar which is the
            %    correlation.
            % nu: Scalar degrees of freedom.
            %
            %OUTPUTS: jprob: A 1-by-n vector of joint probabilities.
            %
            %EXAMPLE 1: Make a contour plot of the density.
            % x = linspace(0,1,1e2);
            % [X,Y] = meshgrid(x);
            % Z = zeros(length(X),length(Y));
            % for idx = 1:length(X)
            %     for iidx = 1:length(Y)
            %         Z(idx,iidx) = StudentT2C.PDF([X(idx,iidx);Y(idx,iidx)],0.5,3);
            %     end
            % end
            % contourf(X,Y,Z)
            %
            %EXAMPLE 2: Generate an exact joint density plot.
            % x = linspace(0,1,1e2);
            % [X,Y] = meshgrid(x);
            % Z = zeros(length(X),length(Y));
            % for idx = 1:length(X)
            % for iidx = 1:length(Y)
            % Z(idx,iidx) = StudentT2C.PDF([X(idx,iidx);Y(idx,iidx)],0.95,3);
            % X(idx,iidx) = GumbelD.invCDF(X(idx,iidx),1,2);
            % Y(idx,iidx) = GaussianD.invCDF(Y(idx,iidx));
            % Z(idx,iidx) = Z(idx,iidx).*GumbelD.PDF(X(idx,iidx),1,2).*GaussianD.PDF(Y(idx,iidx));
            % end
            % end
            % surf(X,Y,Z)
            %
            %October 2020 Codie T. Lewis, Naval Research Laboratory, Washington D.C.
            %
            if ~exist('u','var') || isempty(u)
                jprob = [];
                return
            end
            if ~exist('R','var') || isempty(R)
                rho = 0;
            elseif ~isscalar(R)
                rho = R(2,1);
            else
                rho = R;
            end

            a = StudentTD.invCDF(u(1,:),nu);
            b = StudentTD.invCDF(u(2,:),nu);
            c = (a.^2+b.^2-2*rho*a.*b)/(nu*(1-rho^2));

            jprob = (gamma(0.5*nu+1)/(gamma(nu/2)*nu*pi*sqrt(1-rho^2)))...
                    ./(StudentTD.PDF(a,0,1,nu).*StudentTD.PDF(b,0,1,nu).*(1+c).^(0.5*nu+1));

            %NaN results from 0*inf in the calculation.
            %These should be 0s.
            nanIdxs = isnan(jprob);
            jprob(nanIdxs) = 0;

        end

        function t = tau(R)
            %%TAU Compute Kendall's tau for the copula.
            %
            %INPUTS:
            % R: A 2-by-2 correlation matrix, or a scalar which is the
            %    correlation.
            %
            %OUTPUTS:
            % t: The scalar value of Kendall's tau.
            %
            %Note: This does not depend on the degrees of freedom nu.
            %
            %October 2020 Codie T. Lewis, Naval Research Laboratory, Washington D.C.
            %
            if ~exist('R','var') || isempty(R)
                rho = 0;
            elseif ~isscalar(R)
                rho = R(2,1);
            else
                rho = R;
            end

            t = 2*arcsin(rho)/pi;
        end
    end
end

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


function [H]=freqresp(b,a,w)
% Frequency response function from difference equation
% [H] = freqresp(b,a,w)
% H = frequency response array evaluated a w frequencies
% b = numerator coefficient array
% a = denominator coefficient array (a(1)=1)
% w = frequency location array
m = 0:length(b)-1;
l = 0:length(a)-1;
num = b*exp(-j*m'*w);
den = a*exp(-j*l'*w);
H = num./den;


function [func, grad, Hess] = FuncGradHessSub(x, y, F, R, kin, rho)
% `SubGradHess` provides the function value, the gradient, and the Hessian
% of the subproblem of DCA and BDCA to be used in `fminunc`.
%
% USAGE:
%
%    [func, grad, Hess] = FuncGradHessSub(x, y, F, R, kin, rho)
%
% INPUTS:
%    x,y:     points
%    F:       Forward stoichiometric matrix
%    kin:     kinetics parameter in :math:`R^{2n}`
%    rho:     strongly comvex modulus
%
%
% OUTPUTS:
%    f:       function value
%    grad:    gradient
%    H:       Hessian

m            = size(F,1);
FR           = [F,R];
RF           = [R,F];
FR_plus_RF   = FR+RF;
exp_x        = exp(kin+FR'*x);
exp_y        = exp(kin+FR'*y);
ux           = FR*exp_x;
wx           = RF*exp_x;
uy_plus_wy   = FR_plus_RF*exp_y;
FRdiag_x     = FR*diag(exp_x);
dux          = FRdiag_x*FR';
dwx          = FRdiag_x*RF';
FRdiag_y     = FR*diag(exp_y);
duy_plus_dwy = FRdiag_y*FR_plus_RF';
gx           = 2*(norm(ux)^2+norm(wx)^2)+rho/2*norm(x)^2;
dgx          = 4*(dux*ux+dwx*wx)+rho*x;
dhy          = 2*duy_plus_dwy*uy_plus_wy+rho*y;

func = gx-dhy'*x;
grad = dgx-dhy;
H    = dux*dux.'+dwx*dwx.';
for j = 1:m
    H = H+ux(j)*FR*diag(exp_x.*FR(j,:)')*FR'+ ...
                                     wx(j)*FR*diag(exp_x.*RF(j,:)')*FR';
end
Hess = 4*H+rho*eye(m);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%% End of FuncGradHessSub.m %%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% Tutorial for FAIR: elastic regularizer, matrix based and matrix free
%
% illustrates the tools for L2-norm based regularization
%
%  S(y) = alpha/2 * norm(B*(y-yRef)^2,
%
% where
%  alpha regularization parameter, weights regularization versus
%        distance in the joint objective function, alpha = 1 here
%  yRef  is a reference configuration, e.g. yRef = x or a
%        pre-registration result
%  B     a discretized partial differential operator either in explicit
%        matrix form or as a structure containing the necessary
%        parameters to compute B*y, here B = elastic
%==============================================================================

clear, close all, help(mfilename);

%% 2D example
omega  = [0,1,0,1]; % physical domin
m      = [16,12];   % number of discretization points
hd     = prod((omega(2:2:end)-omega(1:2:end))./m);

alpha  = 1;         % regularization parameter, irrelevant in this tutorial
mu     = 1;         % Lame constants, control elasticity properties
lambda = 0;         % Youngs modulus and Poisson ratio
para   = {'alpha',alpha,'mu',mu,'lambda',lambda};

yRef   = getStaggeredGrid(omega,m); % reference for regularization
yc     = rand(size(yRef));          % random transformation

%% 1. build elasticity operator on a staggered grid
%  - explicitly
%  - via regularizer.m
%  - via A(:,j) = B(e_j), B is the matrix free version

B = getElasticMatrixStg(omega,m,mu,lambda);
Amb = hd*alpha*(B'*B);

regularizer('reset','regularizer','mbElastic',para{:});
% regularizer('disp');
[Smb,dSmb,d2Smb] = regularizer(yc-yRef,omega,m);

regularizer('reset','regularizer','mfElastic',para{:});
% regularizer('disp');
[Scmf, dSmf, d2Smf] = regularizer(yc,omega,m);

% note: B*y can also be computed without storing B. The following loop
% computes A(:,j) = mfBy(e(j)), where mfBy is the matrix free operation B*y
% and e(j) is the j-th unit vector; hence A == B,

e = @(j) ((1:size(B,2))' == j); % j-th unit vector
A = sparse(size(B,1),size(B,2));
for j=1:size(B,2), A(:,j) = d2Smf.By(e(j),omega,m); end;

% now the transpose
e = @(j) ((1:size(B,1))' == j); % j-th unit vector
C = sparse(size(B,2),size(B,1));
for j=1:size(B,1), C(:,j) = d2Smf.BTy(e(j),omega,m); end;

% visualize matrices and transpose
FAIRfigure(1); clf;
subplot(2,3,1); spy(B);    title('B =elastic operator on staggered grid')
subplot(2,3,2); spy(A);    title('B from mfBy');
subplot(2,3,3); spy(B-A);  title('difference |B-mfBy()|');
subplot(2,3,4); spy(B');   title('B''')
subplot(2,3,5); spy(C);    title('B'' from mfBy');
subplot(2,3,6); spy(B'-C); title('difference |B''-mfBy(...,''BTy'')|');

%% 3D example
omega  = [0,4,0,2,0,1]; % physical domin
m      = [16,12,8];     % number of discretization points
hd     = prod((omega(2:2:end)-omega(1:2:end))./m);

yRef   = getStaggeredGrid(omega,m); % reference for regularization
yc     = rand(size(yRef));          % random transformation

% build elasticity operator on a staggered grid
B = getElasticMatrixStg(omega,m,mu,lambda);
Amb = hd*alpha*(B'*B);

regularizer('reset','regularizer','mbElastic',para{:});
% regularizer('disp');
[Smb,dSmb,d2Smb] = regularizer(yc-yRef,omega,m);

regularizer('reset','regularizer','mfElastic',para{:});
% regularizer('disp');
[Smf, dSmf, d2Smf] = regularizer(yc-yRef,omega,m);

zc     = randn(size(B,1),1);        % random for the adjoint

FAIRfigure(2); clf;
subplot(2,2,1); spy(B);          title('B elastic on staggered grid')
subplot(2,2,3); spy(Amb);        title('mb: hd\alpha B''*B')
subplot(2,2,4); spy(d2Smb);      title('mb: d2S')
subplot(2,2,2); spy(Amb-d2Smb);  title('difference')

Splain = 0.5*(yc-yRef)'*Amb*(yc-yRef);

fprintf('Splain = %-10s\n',num2str(Splain))
fprintf('Smb    = %-10s, Splain-Smb =%-10s\n',num2str(Smb),num2str(Splain-Smb))
fprintf('Smf    = %-10s, Splain-Smf =%-10s\n',num2str(Smf),num2str(Splain-Smf))

compare = @(s,l,r) fprintf('%-25s = %s\n',s,num2str(norm(l-r)));
compare('||B*yc-mfBy(y)||',B*yc,d2Smf.By(yc,omega,m));
compare('||B''*z-mfBy(z,''BTy'')||',B'*zc,d2Smf.BTy(zc,omega,m));

%% check elasticity

%% 3D example
omega  = [0,4,0,2,0,1]; % physical domin
m      = [32,16,8];     % number of discretization points
hd     = prod((omega(2:2:end)-omega(1:2:end))./m);

yRef   = getStaggeredGrid(omega,m); % reference for regularization
yc     = rand(size(yRef));          % random transformation

% build elasticity operator on a staggered grid
B = getElasticMatrixStg(omega,m,mu,lambda);

regularizer('reset','regularizer','mfElastic',para{:});
% regularizer('disp');
[Smf, dSmf, d2Smf] = regularizer(yc-yRef,omega,m);

H.d2S = d2Smf;
H.omega  = omega;
H.m      = m;
H.alpha  = regularizer('get','alpha');
H.mu     = regularizer('get','mu');
H.lambda = regularizer('get','lambda');
H.regularizer = regularizer;

By = B*yc;
testMF  = norm(By-d2Smf.By(yc,omega,m));
testMFT = norm(B'*By-d2Smf.BTy(By,omega,m));
fprintf('|By-mfBy|     = %s\n',num2str(testMF));
fprintf('|B''*By-mfBy''| = %s\n',num2str(testMFT));
% ---------------------------------------------------------------------
% using multigrid to solve A*uc = fc, where A = I + alpha*hd*B'*B

xc  = getStaggeredGrid(omega,m);
M   = speye(length(xc),length(xc)); % idenity matrix
A   = M + hd*alpha*B'*B;
fc  = A*xc;
u0  = zeros(size(xc));

% prepare for multigrid
H.MGlevel      = log2(m(1))+1;
H.MGcycle      = 4;
H.MGomega      = 2/3;   %% !!! 0.5 should be better
H.MGsmoother   = 'mfJacobi';
H.MGpresmooth  = 10;
H.MGpostsmooth = 10;
H.d2D.M        = full(diag(M));

uMG = mfvcycle(H,u0,fc,1e-12,H.MGlevel,5);

testMG = norm( A\fc - uMG);
fprintf('|A\\fc-uMG|=%s\n',num2str(testMG));

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

%==============================================================================
FAIRmessage(sprintf('<%s> done',mfilename));
%==============================================================================


function [reg_spectrum, chi2_regNNLS] = do_regNNLS(decay_matrix, measurements, sigma, mu)

%-------------------------------------------------------------------
% function [s_NNLS, chi2_NNLS] = do_regNNLS(num_t2_vals, decay_matrix, measurements, sigma, mu)
%
% * Runs through regularized NNLS (energy)
%
% ~~~ Charmaine Chia (May 3, 2005) ~~~
%-------------------------------------------------------------------

% generate minimum energy regularization
num_t2_vals = size(decay_matrix,2);
H = eye(num_t2_vals);
H_temp = mu*H;
this_decay_matrix = [decay_matrix; H_temp];
this_data = [measurements'; zeros(num_t2_vals,1)];

%-------------------------------------------------------------------
% --- EAO: addition to fit 2D spectrum (T2* times, with freq. offsets)
%-------------------------------------------------------------------
fit_delf = 1;

if fit_delf == 1

    % compute default tolerance used by lsqnonneg (as per help lsqnonneg )
    default_tolx = 10*max(size(this_decay_matrix))*norm(this_decay_matrix,1)*eps;
    % initialize fitting options structure
    opts = optimset('TolX',default_tolx);

    [reg_spectrum, bla, blo, exitflag, output] = lsqnonneg(this_decay_matrix,  this_data);

    % if num of iterations exceeded
    while ~exitflag
        % increase the tolerance
        opts = optimset(opts,'TolX',opts.TolX*10);
        % re-fit
        [reg_spectrum, bla, blo, exitflag] = lsqnonneg(this_decay_matrix, this_data, opts);
    end

else
    reg_spectrum = lsqnonneg(this_decay_matrix, this_data);
end
%-------------------------------------------------------------------

chi2_regNNLS = sum((this_decay_matrix*reg_spectrum - this_data).^2)/sigma^2;


function Y = prtRvUtilLaplaceCdf(X,mu,theta)
% Y = laplacecdf(X,mu,theta)

if numel(X) > length(X)
    error('X must have 1 singleton dimension')
end
Y = zeros(size(X));

Y(X>=mu) = 1-1/2*exp(-(X(X>=mu)-mu)./theta);
Y(X<mu) = 1/2*exp((X(X<mu)-mu)./theta);


function facedemo ( n )

%*****************************************************************************80
%
% FACEDEMO: Example polygonal geometries for MESHFACES.
%
%  Author:
%
%    Darren Engwirda
%
  if ( nargin < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'The usual call is to FACEDEMO ( N ), with N = 1 or 2.\n' );
    fprintf ( 1, '  Since N was not specified, the value 1 will be chosen.\n' );
    n = 1;
  end

  switch n

    case 1

      node = [0.0, 0.0; 1.0,0.0; 1.0,1.0; 0.0,1.0; 1.01,0.0; 1.01,1.0; 3.0,0.0; 3.0,1.0];
      edge = [1,2; 2,3; 3,4; 4,1; 2,5; 5,6; 6,3; 5,7; 7,8; 8,6];
      face{1} = [1,2,3,4];
      face{2} = [5,6,7,2];
      face{3} = [8,9,10,6];

      meshfaces ( node, edge, face );

    case 2

      % Geometry
      dtheta = pi/36;
      theta = (-pi:dtheta:(pi-dtheta))';
      node1 = [cos(theta), sin(theta)];
      node2 = [-2.0,-2.0; 2.0,-2.0; 2.0,2.0; -2.0, 2.0];
      edge1 = [(1:size(node1,1))',[(2:size(node1,1))'; 1]];
      edge2 = [1,2; 2,3; 3,4; 4,1];

      edge = [edge1; edge2+size(node1,1)];
      node = [node1; node2];

      face{1} = 1:size(edge1,1);
      face{2} = 1:size(edge,1);

      meshfaces(node,edge,face);

    otherwise
      error('Invalid demo. N must be between 1-2');

  end

  return
end


function tan_test ( )

%*****************************************************************************80
%
%% TAN_TEST tests R4_TAN and R8_TAN.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2011
%
%  Author:
%
%    John Burkardt
%
  addpath ( '../test_values' );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TAN_TEST:\n' );
  fprintf ( 1, '  Test TAN_VALUES, R4_TAN, R8_TAN.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '             X         TAN(X)\n' );
  fprintf ( 1, '                    R4_TAN(X)         Diff\n' );
  fprintf ( 1, '                    R8_TAN(X)         Diff\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, fx1 ] = tan_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fx2 = r4_tan ( single ( x ) );
    fx3 = r8_tan ( x );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  %14.4f  %14.6g\n', x, fx1 );
    fprintf ( 1, '                  %14.6g  %14.6g\n', fx2, abs ( fx1 - fx2 ) );
    fprintf ( 1, '                  %14.6g  %14.6g\n', fx3, abs ( fx1 - fx3 ) );

  end

  rmpath ( '../test_values' );

  return
end


function vgc_plot(k,kstar,v,Polc,g, METHOD)
% plot the value function against k
% plot capital policy for next period against captial this period
% save variable
% input is the k vector, optimal-- kstar, value --v, capital policy - g
% METHOD=1 if deterministic noninterpolation
% Method=2----Deterministic, continuous state space model, With interpolation
gstar=ppval(spline(k,g),kstar);  % g value at optimal kstar
vstar=ppval(spline(k,v),kstar);  % v value at optimal kstar

T=char( ' No Interpolation, Deterministic',...
        ' With Interpolation, Deterministic');

figure(1);
plot(k,v); hold on;
plot(kstar,vstar,'ro');
text(kstar+.1,vstar-.1,['k*=' num2str(kstar)]);
title(['Value Function--' T(METHOD,:)]);
xlabel('capital k');  ylabel('value');
axis square; hold off;
print('-dpsc',['ValFun' num2str(METHOD)]);

figure(2);
subplot(2,1,1); plot(k,Polc);
title('Policy Function-Consumption');
xlabel('Capital Kt');   ylabel('Consumption');

subplot(2,1,2); plot(k,g,k,k,'-'); hold on;
plot(kstar,gstar,'ro');
text(kstar+.1,gstar-.1,['g*=' num2str(gstar) '| k*=' num2str(kstar)]);
title('Policy Function-Capital');
xlabel('Capital Kt');   ylabel('Capital Next Period');
hold off;

print('-dpsc', ['PolFun' num2str(METHOD)]);
save(['growth' num2str(METHOD)]);


function A = randomize_graph_partial_und(A,B,maxswap)
% RANDOMIZE_GRAPH_PARTIAL_UND    Swap edges with preserved degree sequence
%
%   A = RANDOMIZE_GRAPH_PARTIAL_UND(A,B,MAXSWAP) takes adjacency matrices A
%   and B and attempts to randomize matrix A by performing MAXSWAP
%   rewirings. The rewirings will avoid any spots where matrix B is
%   nonzero.
%
%   Inputs:       A,      undirected adjacency matrix
%                 B,      edges to avoid
%           MAXSWAP,      number of rewirings
%
%   Outputs:      A,      randomized matrix
%
%   Richard Betzel, Indiana University, 2013
%
%   Notes:
%   1. Based on the script randmio_und.m.
%   2. Graph may become disconnected as a result of rewiring. Always
%      important to check.
%   3. A can be weighted, though the weighted degree sequence will not be
%      preserved.
%

[i,j] = find(triu(A,1));
m = length(i);
nswap = 0;
while nswap < maxswap
    while 1
        e1 = randi(m); e2 = randi(m);
        while e2 == e1
            e2 = randi(m);
        end
        a = i(e1); b = j(e1);
        c = i(e2); d = j(e2);
        if all(a~=[c,d]) && all(b~=[c,d])
            break
        end
    end
    if rand > 0.5
        i(e2) = d; j(e2) = c;
        c = i(e2); d = j(e2);
    end
    if ~(A(a,d) || A(c,b) || B(a,d) || B(c,b))
        A(a,d) = A(a,b); A(a,b) = 0;
        A(d,a) = A(b,a); A(b,a) = 0;
        A(c,b) = A(c,d); A(c,d) = 0;
        A(b,c) = A(d,c); A(d,c) = 0;
        j(e1) = d;
        j(e2) = b;
        nswap = nswap + 1;
    end
end


% StackExchange Signal Processing Q59325
% https://dsp.stackexchange.com/questions/59325
% Learning the Coefficients of Auto Regressive Model Using Least Mean
% Squares Filter for Signal Prediction
% References:
%   1.
% Remarks:
%   1.  B
% TODO:
% 	1.  C
% Release Notes Royi Avital RoyiAvital@yahoo.com
% - 1.0.000     14/04/2022
%   *   First release.

%% General Parameters

subStreamNumberDefault = 79;

run('InitScript.m');

figureIdx           = 0;
figureCounterSpec   = '%04d';

generateFigures = ON;

%% Simulation Constants

%% Simulation Parameters

% Signal Generation
numSamples  = 2000;
numSignals = 30;

% Estimaiton
arModelOrder = 101;
numSamplesEst = round(0.75 * numSamples);

%% Generate / Load Data

vA = rand(numSignals, 1) + 0.35;
vF = 5 * rand(numSignals, 1) + 1;
vP = pi * rand(numSignals, 1);

vT = linspace(0, 5, numSamples).';
vX = zeros(numSamples, 1);

for ii = 1:length(vA)
    vX(:) = vX(:) + vA(ii) * sin(2 * pi  * vF(ii) * vT + vP(ii));
end

vW = zeros(arModelOrder, 1); %<! LMS initialization

%% LMS

vD = vX((arModelOrder + 1):(arModelOrder + 1 + numSamplesEst));
vY = vX(1:(numSamplesEst + 1));

vWW = LmsFilter(vW, vY, vD, arModelOrder, numSamplesEst, 5e-4, OFF);

vYY = filter(1, [1; vWW], vX); %<! Prediction by the AR Model as estimated by the LMS

%% Analysis

figureIdx = figureIdx + 1;

hFigure = figure('Position', figPosLarge);
hAxes   = axes(hFigure);
set(hAxes, 'NextPlot', 'add');
hLineObj = plot(vT(1:numSamplesEst), vX(1:numSamplesEst));
set(hLineObj, 'LineWidth', lineWidthNormal);
hLineObj = plot(vT((numSamplesEst + 1):end), vX((numSamplesEst + 1):end));
set(hLineObj, 'LineWidth', lineWidthNormal);
set(get(hAxes, 'Title'), 'String', {['Linear Combination of ', num2str(numSignals), ' Sine Signals']}, ...
    'FontSize', fontSizeTitle);
set(get(hAxes, 'XLabel'), 'String', {['Time Index [Sec]']}, ...
    'FontSize', fontSizeAxis);
set(get(hAxes, 'YLabel'), 'String', {['Value']}, ...
    'FontSize', fontSizeAxis);
hLegend = ClickableLegend({['Input Signal: Samples for LMS'], ['Input Signal: Samples to Predict']});

if(generateFigures == ON)
    % saveas(hFigure, ['Figure', num2str(figureIdx, figureCounterSpec), '.png']);
    print(hFigure, ['Figure', num2str(figureIdx, figureCounterSpec), '.png'], '-dpng', '-r0'); %<! Saves as Screen Resolution
end

figureIdx = figureIdx + 1;

hFigure = figure('Position', figPosLarge);
hAxes   = axes(hFigure);
set(hAxes, 'NextPlot', 'add');
hLineObj = plot(vT, vX);
set(hLineObj, 'LineWidth', lineWidthNormal);
hLineObj = plot(vT, vYY);
set(hLineObj, 'LineWidth', lineWidthNormal, 'LineStyle', ':');
set(get(hAxes, 'Title'), 'String', {['LMS Filter Prediction'], ['The RMSE of the Estimation: ', num2str(sqrt(mean((vYY - vX) .^ 2)))]}, ...
    'FontSize', fontSizeTitle);
set(get(hAxes, 'XLabel'), 'String', {['Time Index [Sec]']}, ...
    'FontSize', fontSizeAxis);
set(get(hAxes, 'YLabel'), 'String', {['Value']}, ...
    'FontSize', fontSizeAxis);
hLegend = ClickableLegend({['Input Signal'], ['Predicted Signal (Model Order: ', num2str(arModelOrder), ')']});

if(generateFigures == ON)
    % saveas(hFigure, ['Figure', num2str(figureIdx, figureCounterSpec), '.png']);
    print(hFigure, ['Figure', num2str(figureIdx, figureCounterSpec), '.png'], '-dpng', '-r0'); %<! Saves as Screen Resolution
end

%% Restore Defaults

% set(0, 'DefaultFigureWindowStyle', 'normal');
% set(0, 'DefaultAxesLooseInset', defaultLoosInset);


function res=rot2eulZYX(R)
% calculates ZYX euler rotation angles from
% the rotation matrix R

% Matlab implementation of the method descirped in:
% "Computing Euler angles from a rotation matrix"
% Author: Gregory G. Slabaugh

% Copyright, Mohammd SAFEEA, 9th August 2018

% test:
% R=rotz(45)*roty(22)*rotx(30);
% res=rot2eulZYX(R)*180/pi

% res=rot2eulZYX(eye(3))

    [res]=eulerAngles(R) ;
end

function y=closeEnough(a,b)
    epsilon=0.000001;
    if (epsilon>abs(a-b))
        y=1;
    else
        y=0;
    end
end

function [res]=eulerAngles(R)
    %% check for gimbal lock
    R=R';
    if (closeEnough(R(1,3), -1.0))
        b = 0; %% gimbal lock, value of x doesn't matter
        c = pi / 2;
        a = b + atan2(R(2,1), R(3,1));
    elseif (closeEnough(R(1,3), 1.0))
        b = 0;
        c = -pi / 2;
        a = -b + atan2(-R(2,1), -R(3,1));
    else  %% two solutions exist
        b1 = -asin(R(1,3));
        b2 = pi - b1;

        c1 = atan2(R(2,3) / cos(b1), R(3,3) / cos(b1));
        c2 = atan2(R(2,3) / cos(b2), R(3,3) / cos(b2));

        a1 = atan2(R(1,2) / cos(b1), R(1,1)/ cos(b1));
        a2 = atan2(R(1,2) / cos(b2), R(1,1) / cos(b2));

        %% choose one solution to return
        %% for example the "shortest" rotation
        condition=((abs(b1) + abs(c1) + abs(a1)) <= (abs(b2) + abs(c2) + abs(a2)));
        if condition
            a=a1;
            b=b1;
            c=c1;
        else
            a=a2;
            b=b2;
            c=c2;
        end
    end
    res=[a,b,c];
end


%% Bingham Distribution
%
%% Theory
%
% The Bingham distribution has the density function
%
% $$ f(g;K,U) = _1\!F_1 \left(\frac{1}{2},2,K \right)^{-1} \exp
% \left\{ g^T UKU  g \right\},\qquad g\in S^3, $$
%
% where $U$ is an $4 \times 4$ orthogonal matrix with unit quaternions
% $u_{1,..,4}\in S^3$ in the columns and $K$ is a $4 \times 4$ diagonal matrix
% with the entries $k_1,..,k_4$ describing the shape of the distribution.
% $_1F_1(\cdot,\cdot,\cdot)$ is the hypergeometric function with matrix
% argument normalizing the density.
%
% The shape parameters $k_1 \ge k_2 \ge k_3 \ge k_4$ give
%
% * a _bipolar_   distribution, if $k_1 + k_4 > k_2 + k_3$,
% * a _circular_  distribution, if $k_1 + k_4 = k_2 + k_3$,
% * a _spherical_ distribution, if $k_1 + k_4 < k_2 + k_3$,
% * a _uniform_  distribution, if $k_1 = k_2 = k_3 = k_4$,
%
%%
% The general setup of the Bingham distribution in MTEX is done as follows

cs = crystalSymmetry('1');

kappa = [100 90 80 0];   % shape parameters
U     = eye(4);          % orthogonal matrix

odf = BinghamODF(kappa,U,cs)

%%
%

h = [Miller(0,0,1,cs) Miller(1,0,0,cs) Miller(1,1,1,cs)];
plotPDF(odf,h,'antipodal','silent');

%%
%
plot(odf,'sections',6)

%% The bipolar case and unimodal distribution
% First, we define some unimodal odf

odf_spherical = unimodalODF(orientation.rand(cs),'halfwidth',20*degree)

%%
%

plotPDF(odf_spherical,h,'antipodal','silent')

%%
% Next, we simulate individual orientations from this odf, in a scattered
% axis/angle plot in which the simulated data looks like a sphere

ori_spherical = discreteSample(odf_spherical,1000);
close all
scatter(ori_spherical)

%%
% From this simulated EBSD data, we can estimate the parameters of the
% Bingham distribution,

odf_est = calcBinghamODF(ori_spherical)

plotPDF(odf_est,h,'antipodal','silent')

%% TODO
%
% where |U| is the orthogonal matrix of eigenvectors of the orientation
% tensor and |kappa| the shape parameters associated with the |U|.
%
% next, we test the different cases of the distribution on rejection

%T_spherical = bingham_test(ori_spherical,'spherical','approximated');
%T_oblate    = bingham_test(ori_spherical,'prolate',  'approximated');
%T_prolate   = bingham_test(ori_spherical,'oblate',   'approximated');

%t = [T_spherical T_oblate T_prolate]

%%
% The spherical test case failed to reject for some level of
% significance, hence we would dismiss the hypothesis prolate and oblate.

%df_spherical = BinghamODF(kappa,U,crystalSymmetry,specimenSymmetry)

%%
%

%plotPDF(odf_spherical,h,'antipodal','silent')

%% Prolate case and fibre distribution
% The prolate case correspondes to a fibre.

odf_prolate = fibreODF(Miller(0,0,1,crystalSymmetry('1')),zvector,...
  'halfwidth',20*degree)

%%
%

plotPDF(odf_prolate,h,'upper','silent')

%%
% As before, we generate some random orientations from a model odf. The
% shape in an axis/angle scatter plot reminds of a cigar

ori_prolate = discreteSample(odf_prolate,1000);
close all
scatter(ori_prolate)

%%
% We estimate the parameters of the Bingham distribution

calcBinghamODF(ori_prolate)

%%
% and test on the three cases

%T_spherical = bingham_test(ori_prolate,'spherical','approximated');
%T_oblate    = bingham_test(ori_prolate,'prolate',  'approximated');
%T_prolate   = bingham_test(ori_prolate,'oblate',   'approximated');

%t = [T_spherical T_oblate T_prolate]

%%
% The test clearly rejects the spherical and prolate case, but not the
% prolate. We construct the Bingham distribution from the parameters, it
% might show some skewness

odf_prolate = BinghamODF(kappa,U,crystalSymmetry,specimenSymmetry)

%%
%

plotPDF(odf_prolate,h,'antipodal','silent')

%% Oblate case
% The oblate case of the Bingham distribution has no direct counterpart in
% terms of texture components, thus we can construct it straightforward

odf_oblate = BinghamODF([50 50 50 0],eye(4),crystalSymmetry,specimenSymmetry)

%%
%

plotPDF(odf_oblate,h,'antipodal','silent')

  %%
% The oblate cases in axis/angle space remind on a disk

ori_oblate = discreteSample(odf_oblate,1000);
close all
scatter(ori_oblate)

%%
% We estimate the parameters again

calcBinghamODF(ori_oblate)

%%
% and do the tests

%T_spherical = bingham_test(ori_oblate,'spherical','approximated');
%T_oblate    = bingham_test(ori_oblate,'prolate',  'approximated');
%T_prolate   = bingham_test(ori_oblate,'oblate',   'approximated');

%t = [T_spherical T_oblate T_prolate]

%%
% the spherical and oblate case are clearly rejected, the prolate case
% failed to reject for some level of significance

odf_oblate = BinghamODF(kappa, U,crystalSymmetry,specimenSymmetry)

%%
%

plotPDF(odf_oblate,h,'antipodal','silent')

%%
% *Bingham unimodal ODF*

% a modal orientation
cs = crystalSymmetry('-3m');
mod = orientation.byEuler(45*degree,0*degree,0*degree,cs);

% the corresponding Bingham ODF
odf = BinghamODF(20,mod)

plot(odf,'sections',6,'silent','contourf','sigma')

%%
% *Bingham fibre ODF*

odf = BinghamODF([-10,-10,10,10],quaternion(eye(4)),cs)

plot(odf,'sections',6,'silent','sigma')

%%
% *Bingham spherical ODF*

odf = BinghamODF([-10,10,10,10],quaternion(eye(4)),cs)

plot(odf,'sections',6,'silent','sigma');


function W = threshold_proportional(W, p)
%THRESHOLD_PROPORTIONAL     Proportional thresholding
%
%   W_thr = threshold_proportional(W, p);
%
%   This function "thresholds" the connectivity matrix by preserving a
%   proportion p (0<p<1) of the strongest weights. All other weights, and
%   all weights on the main diagonal (self-self connections) are set to 0.
%
%   Inputs: W,      weighted or binary connectivity matrix
%           p,      proportion of weights to preserve
%                       range:  p=1 (all weights preserved) to
%                               p=0 (no weights preserved)
%
%   Output: W_thr,  thresholded connectivity matrix
%
%
%   Mika Rubinov, U Cambridge,
%   Roan LaPlante, Martinos Center, MGH
%   Zitong Zhang, Penn Engineering

%   Modification history:
%   2010: Original (MR)
%   2012: Bug fix for symmetric matrices (RLP)
%   2015: Improved symmetricity test (ZZ)

n=size(W,1);                                %number of nodes
W(1:n+1:end)=0;                             %clear diagonal

if max(max(abs(W-W.'))) < 1e-10;            %if symmetric matrix
    W=triu(W);                              %ensure symmetry is preserved
    ud=2;                                   %halve number of removed links
else
    ud=1;
end

ind=find(W);                                %find all links
E=sortrows([ind W(ind)], -2);               %sort by magnitude
en=round((n^2-n)*p/ud);                     %number of links to be preserved

W(E(en+1:end,1))=0;                         %apply threshold

if ud==2                                    %if symmetric matrix
    W=W+W.';                                %reconstruct symmetry
end


function [Dx,Dy] = PhysDmatrices2D(x1, y1, interp)

% function [Dr,Ds] = PhysDmatrices2D(x1, y1, interp)
% Purpose : Initialize the (x,y) differentiation matrices on the simplex
%	        evaluated at (x1,y1) at order N

Globals2D;
IDr = interp*Dr; IDs = interp*Ds;

[rx1,sx1,ry1,sy1,J1] = GeometricFactors2D(x1, y1, IDr, IDs);

n = size(interp, 1);
Dx = spdiags(rx1, 0, n, n)*IDr + spdiags(sx1, 0, n, n)*IDs;
Dy = spdiags(ry1, 0, n, n)*IDr + spdiags(sy1, 0, n, n)*IDs;
return;


classdef SLERP < handle

    properties (Access = public)
        tau
    end

    properties (Access = private)
        phi
        theta
        scalar_product
    end

    methods (Access = public)

        function obj = SLERP(cParams)
            obj.init(cParams);
        end

        function x = update(obj,g,~)
            obj.computeTheta(g);
            x = obj.computeNewLevelSet(g);
        end

        function computeFirstStepLength(obj,~,~,~)
            obj.tau = 1;
        end

        function is = isTooSmall(obj)
            is = obj.tau < 1e-10;
        end

        function increaseStepLength(obj,f)
            obj.tau = min(f*obj.tau,1);
        end

        function decreaseStepLength(obj)
            obj.tau = obj.tau/1.5;
        end

    end

    methods (Access = private)

        function init(obj,cParams)
            obj.phi            = cParams.designVar;
            obj.scalar_product = cParams.uncOptimizerSettings.scalarProductSettings;
        end

        function computeTheta(obj,g)
            pN   = obj.normalizeFunction(obj.phi.value);
            gN   = obj.normalizeFunction(g);
            phiG = obj.scalar_product.computeSP(pN,gN);
            obj.theta = max(acos(phiG),1e-14);
        end

        function p = computeNewLevelSet(obj,g)
            k  = obj.tau;
            t  = obj.theta;
            pN = obj.normalizeFunction(obj.phi.value);
            gN = obj.normalizeFunction(g);
            a  = sin((1-k)*t)*pN;
            b  = sin(k*t)*gN;
            p  = (a + b)/sin(t);
            p  = obj.normalizeFunction(p);
        end

        function x = normalizeFunction(obj,x)
            norm2 = obj.scalar_product.computeSP(x,x);
            xNorm = sqrt(norm2);
            x = x/xNorm;
        end

    end

end


classdef quaternion
%
% The class quaternion realizes the internal representation of rotations
% and orientations in MTEX. An important difference is that an unit
% quaternion *q* and its antipodal *-q* are considered as different while
% the corresponding rotations are equal.
%
% Syntax
%   q = quaternion(a,b,c,d)
%   q = quaternion.rand
%   q = quaternion.id
%   q = quaternion.nan
%
% Input
%  a, b, c, d - double
%
% Class Properties
%  a, b, c, d - quaternion coefficients
%
% See also
% rotation.rotation orientation.orientation

  properties
    a % real part
    b % * i
    c % * j
    d % * k
  end

  methods
    function q = quaternion(varargin)

      if nargin == 0, return;end

      if isa(varargin{1},'quaternion')   % copy constructor

        if nargin == 1
          q.a = varargin{1}.a;
          q.b = varargin{1}.b;
          q.c = varargin{1}.c;
          q.d = varargin{1}.d;
        else
          q.a = varargin{1}.a(varargin{2:end});
          q.b = varargin{1}.b(varargin{2:end});
          q.c = varargin{1}.c(varargin{2:end});
          q.d = varargin{1}.d(varargin{2:end});
        end
      elseif isa(varargin{1},'vector3d')

        q.a = zeros(size(varargin{1}));
        [q.b,q.c,q.d] = double(varargin{1});

      elseif isnumeric(varargin{1})

        switch nargin

          case 1

            D = varargin{1};

            q.a = D(1,:); q.b = D(2,:); q.c = D(3,:); q.d = D(4,:);

            s = size(D);
            s = [1 s(2:ndims(D))];

            q = reshape(q,s);

          case 2

            if length(varargin{1}) ~= 1
              q.a = varargin{1};
            else
              q.a = repmat(varargin{1},size(varargin{2}));
            end

            [q.b,q.c,q.d] = double(varargin{2});

          case 4

            q.a = varargin{1};
            q.b = varargin{2};
            q.c = varargin{3};
            q.d = varargin{4};

        end
      end

    end

    function d = quat_dot(g1,g2)
      d = g1.a .* g2.a + g1.b .* g2.b + g1.c .* g2.c + g1.d .* g2.d;
    end

    function d = quat_dot_outer(g1,g2,varargin)

      if ~isempty(g1) && ~isempty(g2)

        q1 = [g1.a(:) g1.b(:) g1.c(:) g1.d(:)];
        q2 = [g2.a(:) g2.b(:) g2.c(:) g2.d(:)];

        d = q1 * q2';

      else
        d = [];
      end
    end

    function n = numArgumentsFromSubscript(varargin)
      n = 0;
    end

  end

  methods (Static = true)

    function q = nan(varargin)
      a = nan(varargin{:});
      q = quaternion(a,a,a,a);
    end

    function q = id(varargin)
      a = ones(varargin{:});
      b = zeros(varargin{:});
      q = quaternion(a,b,b,b);
    end

    function q = rand(varargin)

      isNum = cellfun(@isnumeric,varargin);
      [~,last] = find(~isNum,1,'first');

      s = varargin;
      if ~isempty(last), s = s(1:last-1); end

      if check_option(varargin,'maxAngle')

        omega = linspace(0,get_option(varargin,'maxAngle'),1e4);

        v = vector3d.rand(s{:});

        id = discretesample(sin(omega).^2,prod([s{:}]));
        omega = reshape(omega(id),size(v));

        q = axis2quat(v,omega);

      else
        if length(s) < 2, s = [s 1]; end

        alpha = 2*pi*rand(s{:});
        beta  = acos(2*(rand(s{:})-0.5));
        gamma = 2*pi*rand(s{:});

        q = euler2quat(alpha,beta,gamma);
      end
    end

  end

end


function [out] = percolation_5(p1,p2,S,Smax,dt)
%percolation_5

% Copyright (C) 2019, 2021 Wouter J.M. Knoben, Luca Trotter
% This file is part of the Modular Assessment of Rainfall-Runoff Models
% Toolbox (MARRMoT).
% MARRMoT is a free software (GNU GPL v3) and distributed WITHOUT ANY
% WARRANTY. See <https://www.gnu.org/licenses/> for details.

% Flux function
% ------------------
% Description:  Non-linear percolation
% Constraints:  f <= S/dt
%               S >= 0      prevents complex numbers
% @(Inputs):    p1   - base percolation rate [mm/d]
%               p2   - exponential scaling parameter [-]
%               S    - current storage [mm]
%               Smax - maximum contributing storage [mm]
%               dt   - time step size [d]

out = min(S/dt,p1.*((max(S,0)./Smax).^p2));

end


function [d,i0,i1,j0,j1,t0,t1,anglefrom1to2] = dnose2center_pair(trx,fly1,fly2)

% initialize
d = nan(1,trx(fly1).nframes);
anglefrom1to2 = [];

% get start and end frames of overlap
t0 = max(trx(fly1).firstframe,trx(fly2).firstframe);
t1 = min(trx(fly1).endframe,trx(fly2).endframe);

% indices for these frames
i0 = t0 + trx(fly1).off;
i1 = t1 + trx(fly1).off;
j0 = t0 + trx(fly2).off;
j1 = t1 + trx(fly2).off;

% no overlap
if t1 < t0,
  return;
end

% position of nose1
xnose1 = trx(fly1).x_mm(i0:i1) + 2*trx(fly1).a_mm(i0:i1).*cos(trx(fly1).theta_mm(i0:i1));
ynose1 = trx(fly1).y_mm(i0:i1) + 2*trx(fly1).a_mm(i0:i1).*sin(trx(fly1).theta_mm(i0:i1));

% position of center2
x_mm2 = trx(fly2).x_mm(j0:j1);
y_mm2 = trx(fly2).y_mm(j0:j1);

dx = x_mm2-xnose1;
dy = y_mm2-ynose1;

z = sqrt(dx.^2 + dy.^2);
d(i0:i1) = z;

% anglefrom1to2
if nargout >= 8,
  theta1 = trx(fly1).theta_mm(i0:i1);
  theta2 = atan2(dy,dx);
  anglefrom1to2 = modrange(theta2-theta1,-pi,pi);
end


%%
%  Copyright: Copyright (c) MOSEK ApS, Denmark. All rights reserved.
%
%  File:      lownerjohn_inner.m
%
%  Purpose:
%  Computes the Lowner-John inner ellipsoidal
%  approximation of a polytope.
%
%
%  The inner ellipsoidal approximation to a polytope
%
%     S = { x \in R^n | Ax < b }.
%
%  maximizes the volume of the inscribed ellipsoid,
%
%     { x | x = C*u + d, || u ||_2 <= 1 }.
%
%  The volume is proportional to det(C)^(1/n), so the
%  problem can be solved as
%
%    maximize         t
%    subject to       t       <= det(C)^(1/n)
%                || C*ai ||_2 <= bi - ai^T * d,  i=1,...,m
%                  C is PSD
%
%  which is equivalent to a mixed conic quadratic and semidefinite
%  programming problem.
%
%
%  References:
%  [1] "Lectures on Modern Optimization", Ben-Tal and Nemirovski, 2000.
%
function [C, d] = lownerjohn_inner(A, b)

import mosek.fusion.*;
import mosek_lownerjohn.det_rootn;

M = Model('lownerjohn_inner');

[m, n] = size(A);

% Setup variables
t = M.variable('t', 1, Domain.greaterThan(0.0));
C = M.variable('C', NDSet(n,n), Domain.unbounded());
d = M.variable('d', n, Domain.unbounded());

% (bi - ai^T*d, C*ai) \in Q
for i=1:m,
    M.constraint( sprintf('qc%d', i),  ...
                  Expr.vstack(Expr.sub(b(i) ,Expr.dot(A(i,:),d) ) , Expr.mul(C,A(i,:) )), ...
                  Domain.inQCone() );
end

% t <= det(C)^{1/n}
det_rootn(M, C, t)

% Objective: Maximize t
M.objective(ObjectiveSense.Maximize, t)

M.solve()

C = reshape(C.level(), n, n);
d = reshape(d.level(), n, 1);

M.dispose();


function fk=dirft2d3(nj,xj,yj,cj,iflag,nk,sk,tk)
%DIRFT2D3: Direct (slow) computation of nonuniform FFT in R^2 - Type 3.
%
%  FK = DIRFT2D3(NJ,XJ,YJ,CJ,IFLAG,NK,SK,TK);
%
%                 1  nj
%     fk(k)    = -- SUM cj(j) exp(+/-i s(k) xj(j)) exp(+/-i t(k) yj(j))
%                nj j=1
%
%     If (iflag .ge.0) the + sign is used in the exponential.
%     If (iflag .lt.0) the - sign is used in the exponential.
%
%  Input parameters:
%
%     nj     number of sources   (integer)
%     xj,yj  location of sources (real *8)
%
%            on interval [-pi,pi].
%
%     cj     strengths of sources (complex *16)
%     iflag  determines sign of FFT (see above)
%     nk     number of (noninteger) Fourier modes computed
%     sk,tk  k-values (locations) of desired Fourier modes
%
%  Output parameters:
%
%     fk     Fourier transform values (complex *16)
%

fk=zeros(nk,1)+1i*zeros(nk,1);

mex_id_ = 'dirft2d3(i int[x], i double[], i double[], i dcomplex[], i int[x], i int[x], i double[], i double[], io dcomplex[])';
[fk] = nufft2d(mex_id_, nj, xj, yj, cj, iflag, nk, sk, tk, fk, 1, 1, 1);


%% Wiener Deconvolution for Image Deblurring
%
% Sample shows how DFT can be used to perform
% <https://en.wikipedia.org/wiki/Wiener_deconvolution Weiner deconvolution>
% of an image with user-defined point spread function (PSF).
%
% Use controls to adjust PSF parameters, and swtich between linear/cirular PSF.
%
% See also: |deconvwnr|,
% <https://www.mathworks.com/help/images/image-restoration-deblurring.html>
%
% Sources:
%
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/python/deconvolution.py>
%

function varargout = weiner_deconvolution_demo_gui(im)
    % load grayscale image
    if nargin < 1
        im = fullfile(mexopencv.root(), 'test', 'licenseplate_motion.jpg');
        if exist(im, 'file') ~= 2
            disp('Downloading image...')
            url = 'https://cdn.rawgit.com/opencv/opencv/3.2.0/samples/data/licenseplate_motion.jpg';
            urlwrite(url, im);
        end
        img = cv.imread(im, 'Grayscale',true);
    elseif ischar(im)
        img = cv.imread(im, 'Grayscale',true);
    else
        img = im;
    end
    img = single(img) / 255;  % convert 8-bit to floating-point precision

    % create the UI
    app = initApp(img);
    h = buildGUI(app);
    if nargout > 0, varargout{1} = h; end
end

function app = initApp(img)
    %INIT_APP  Initialize options and state structure

    app = struct();

    % options
    app.snr = 25;       % signal/noise ratio in dB
    app.angle = 135;    % blur angle in degrees [0,180]
    app.d = 22;         % blur diameter [1,ksz]
    app.ksz = 65;       % kernel size

    % process image and compute its DFT
    app.img0 = img;
    app.img = blur_edge(img);
    app.IMG = cv.dft(app.img, 'ComplexOutput',true);
end

function img = blur_edge(img, d)
    %BLUR_EDGE  Blur image edges to reduce ringing effect in deblurred image
    %
    %     img = blur_edge(img)
    %     img = blur_edge(img, d)
    %
    % ## Input
    % * __img__ input image
    % * __d__ gaussian size, default 31
    %
    % ## Output
    % * __img__ output image
    %
    % See also: edgetaper
    %

    if nargin < 2, d = 31; end
    img_blur = cv.copyMakeBorder(img, [d d d d], 'BorderType','Wrap');
    img_blur = cv.GaussianBlur(img_blur, 'KSize',[d d]*2+1, 'SigmaX',-1);
    img_blur = img_blur(d+1:end-d, d+1:end-d);

    [h,w] = size(img);
    [y,x] = ndgrid(1:h, 1:w);
    D = min(cat(3, x, w-x+1, y, h-y+1), [], 3);
    a = min(single(D)/d, 1);

    img = img.*a + img_blur.*(1-a);
end

function kern = motion_kernel(ang, d, sz)
    %MOTION_KERNEL  Create linear motion filter
    %
    %     kern = motion_kernel(ang, d)
    %     kern = motion_kernel(ang, d, sz)
    %
    % ## Input
    % * __ang__ linear motion angle
    % * __d__ linear motion length
    % * __sz__ kernel size, default 65
    %
    % ## Output
    % * __kern__ kernel
    %
    % See also: fspecial (motion)
    %

    if nargin < 3, sz = 65; end
    sz2 = floor(sz / 2);
    A = [cos(ang) -sin(ang); sin(ang) cos(ang)];
    A(:,3) = [sz2; sz2] - A*[(d-1)*0.5; 0];
    kern = ones(1,d,'single');
    kern = cv.warpAffine(kern, A, 'DSize',[sz sz], 'Interpolation','Cubic');
end

function kern = defocus_kernel(d, sz)
    %DEFOCUS_KERNEL  Create circular defocus kernel
    %
    %     kern = defocus_kernel(d)
    %     kern = defocus_kernel(d, sz)
    %
    % ## Input
    % * __d__ circular motion diameter
    % * __sz__ kernel size, default 65
    %
    % ## Output
    % * __kern__ kernel
    %
    % See also: fspecial (gaussian)
    %

    if nargin < 2, sz = 65; end
    kern = zeros(sz,sz,'uint8');
    kern = cv.circle(kern, [sz sz], d, ...
        'Color',255, 'Thickness','Filled', 'LineType','AA', 'Shift',1);
    kern = single(kern) / 255;
end

function onChange(~,~,h,app)
    %ONCHANGE  Event handler for UI controls

    % retrieve current values from UI controls
    psf_type = get(h.pop, 'Value');
    psf_viz = get(h.cbox, 'Value') == get(h.cbox, 'Max');
    app.snr = round(get(h.slid(1), 'Value'));
    app.d = round(get(h.slid(2), 'Value'));
    app.angle = round(get(h.slid(3), 'Value'));
    set(h.txt(1), 'String',sprintf('SNR (dB): %d', app.snr));
    set(h.txt(2), 'String',sprintf('Diameter: %d', app.d));
    set(h.txt(3), 'String',sprintf('Angle: %d', app.angle));

    if psf_type == 3
        % show original blurred image
        res = app.img0;
    else
        % linear/cirular PSF
        if psf_type == 1
            psf = motion_kernel(deg2rad(app.angle), app.d, app.ksz);
        else
            psf = defocus_kernel(app.d, app.ksz);
        end

        % deconvolution
        noise = 10^(-0.1 * app.snr);
        [kh,kw] = size(psf);
        PSF = zeros(size(app.img), class(app.img));
        PSF(1:kh, 1:kw) = psf / sum(psf(:));
        PSF = cv.dft(PSF, 'ComplexOutput',true, 'NonzeroRows',kh);
        PSF = bsxfun(@rdivide, PSF, sum(PSF.^2, 3) + noise);
        res = cv.mulSpectrums(app.IMG, PSF);
        res = cv.dft(res, 'Inverse',true, 'Scale',true, 'RealOutput',true);
        res = circshift(res, floor(-[kh kw]/2));

        % visualize PSF kernel (overlaid on top of output image)
        if psf_viz
            res(1:kh,1:kw) = psf;
        end
    end

    % show result
    set(h.img, 'CData',res);
    drawnow;
end

function h = buildGUI(app)
    %BUILDGUI  Creates the UI

    % parameters
    sz = size(app.img0);
    sz(2) = max(sz(2), 300);  % minimum figure width

    % build the user interface (no resizing to keep it simple)
    h = struct();
    h.fig = figure('Name','Deconvolution', ...
        'NumberTitle','off', 'Menubar','none', 'Resize','off', ...
        'Position',[200 200 sz(2) sz(1)+105-1]);
    if ~mexopencv.isOctave()
        %HACK: not implemented in Octave
        movegui(h.fig, 'center');
    end
    h.ax = axes('Parent',h.fig, 'Units','pixels', 'Position',[1 105 sz(2) sz(1)]);
    if ~mexopencv.isOctave()
        h.img = imshow(app.img0, 'Parent',h.ax);
    else
        %HACK: https://savannah.gnu.org/bugs/index.php?45473
        axes(h.ax);
        h.img = imshow(app.img0);
    end
    h.txt(1) = uicontrol('Parent',h.fig, 'Style','text', ...
        'FontSize',11, 'HorizontalAlignment','left', ...
        'Position',[5 5 100 20], 'String','SNR (dB):');
    h.txt(2) = uicontrol('Parent',h.fig, 'Style','text', ...
        'FontSize',11, 'HorizontalAlignment','left', ...
        'Position',[5 30 100 20], 'String','Diameter:');
    h.txt(3) = uicontrol('Parent',h.fig, 'Style','text', ...
        'FontSize',11, 'HorizontalAlignment','left', ...
        'Position',[5 55 100 20], 'String','Angle:');
    h.txt(4) = uicontrol('Parent',h.fig, 'Style','text', ...
        'FontSize',11, 'HorizontalAlignment','left', ...
        'Position',[5 80 100 20], 'String','PSF:');
    h.slid(1) = uicontrol('Parent',h.fig, 'Style','slider', ...
        'Value',app.snr, 'Min',0, 'Max',50, 'SliderStep',[1 5]./(50-0), ...
        'Position',[105 5 sz(2)-105-5 20]);
    h.slid(2) = uicontrol('Parent',h.fig, 'Style','slider', ...
        'Value',app.d, 'Min',1, 'Max',50, 'SliderStep',[1 5]./(50-1), ...
        'Position',[105 30 sz(2)-105-5 20]);
    h.slid(3) = uicontrol('Parent',h.fig, 'Style','slider', ...
        'Value',app.angle, 'Min',0, 'Max',180, 'SliderStep',[1 10]./(180-0), ...
        'Position',[105 55 sz(2)-105-5 20]);
    h.pop = uicontrol('Parent',h.fig, 'Style','popupmenu', ...
        'Position',[105 80 100 20], 'String',{'Linear','Circular','-None-'});
    h.cbox = uicontrol('Parent',h.fig, 'Style','checkbox', ...
        'Position',[210 80 100 20], 'Value',1, 'String','Show PSF');

    % hook event handlers, and trigger default start
    set([h.slid, h.pop, h.cbox], 'Callback',{@onChange,h,app}, ...
        'Interruptible','off', 'BusyAction','cancel');
    onChange([],[],h,app);
end


function gsp_plot_jft(G,Xhat,param)
%GSP_PLOT_JFT  Plot the magnitude squared of the joint Fourier transform matrix Xhat
%   Usage:  gsp_plot_jft(G,X);
%           gsp_plot_jft(G,X,param);
%
%   Input parameters:
%       G       : Time-Vertex graph structure
%       Xhat    : Joint Time-Vertex Fourier Coefficients Matrix
%       param   : Structure of optional parameters
%   Output parameters:
%       none
%
%   Additional parameters
%   ---------------------
%
%   * *param.dim*       : '2d' for imagesc and '3d' for surf (default 3d)
%   * *param.logscale*  : Use log-scale to visualize the JFT (default 0)
%   * *param.dB*        : Range of plotting in dB for the logscale
%   * *param.fftshift*  : Put zero frequency at center (default 1)
%

% Author: Francesco Grassi, Nathanael

if nargin<3
    param=struct;
end

if ~isfield(param,'dim');      param.dim='3d';end
if ~isfield(param,'logscale'); param.logscale=0; end
if ~isfield(param,'dB');       param.dB = Inf; end
if ~isfield(param,'fftshift'); param.fftshift = 1; end

if isfield(G,'e')
    lambda = G.e;
else
    if isfield(G,'lmax')
        lambda = linspace(0,G.lmax,G.N);
    else
        lambda = linspace(0,G.N,G.N);
    end
end

if param.logscale
    Xhat = 20*log(abs(Xhat)+1);
    maxXhat = max(Xhat(:));
    Xhat(Xhat < (maxXhat-param.dB)) = maxXhat-param.dB;
else
    Xhat=abs(Xhat.^2);
end

if param.fftshift
    omega = gsp_jtv_fa( G,1 );
    Xhat  = fftshift(Xhat,2);
else
    omega = gsp_jtv_fa( G,0 );
end

switch param.dim
    case {2,'2','2d'}
        imagesc(omega,lambda,Xhat);
        set(gca,'YTickLabel','')
        axis xy
        colorbar

    case {3,'3','3d'}
        surf(omega,lambda,Xhat,'linestyle','none');
        view([0 90])
        xlim([min(omega) max(omega)])
        ylim([min(lambda) max(lambda)])

    otherwise
        error('unkown plot method');
end


function CM = confMatrix( IDXtrue, IDXpred, ntypes )
% Generates a confusion matrix according to true and predicted data labels.
%
% CM(i,j) denotes the number of elements of class i that were given label
% j.  In other words, each row i contains the predictions for elements whos
% actual class was i.  If IDXpred is perfect, then CM is a diagonal matrix
% with CM(i,i) equal to the number of instances of class i.
%
% To normalize CM to [0,1], divide each row by sum of that row:
%  CMnorm = CM ./ repmat( sum(CM,2), [1 size(CM,2)] );
%
% USAGE
%  CM = confMatrix( IDXtrue, IDXpred, ntypes )
%
% INPUTS
%  IDXtrue     - [nx1] array of true labels [int values in 1-ntypes]
%  IDXpred     - [nx1] array of predicted labels [int values in 1-ntypes]
%  ntypes      - maximum number of types (should be > max(IDX))
%
% OUTPUTS
%  CM          - ntypes x ntypes confusion array with integer values
%
% EXAMPLE
%  IDXtrue = [ones(1,25) ones(1,25)*2];
%  IDXpred = [ones(1,10) randint2(1,30,[1 2]) ones(1,10)*2];
%  CM = confMatrix( IDXtrue, IDXpred, 2 )
%  confMatrixShow( CM, {'class-A','class-B'}, {'FontSize',20} )
%
% See also CONFMATRIXSHOW
%
% Piotr's Computer Vision Matlab Toolbox      Version 2.12
% Copyright 2014 Piotr Dollar.  [pdollar-at-gmail.com]
% Licensed under the Simplified BSD License [see external/bsd.txt]

IDXtrue=IDXtrue(:); IDXpred=IDXpred(:);

%%% convert common binary labels [-1/+1] or [0/1] to [1/2]
if( ntypes==2 )
  IDX = [IDXtrue;IDXpred];
  if( min(IDX)>=-1 && max(IDX)<=1 && all(IDX~=0))
    IDXtrue=IDXtrue+2;  IDXpred=IDXpred+2;
    IDXtrue(IDXtrue==3) = 2;  IDXpred(IDXpred==3) = 2;
  elseif( min(IDX)>=0 && max(IDX)<=1 )
    IDXtrue=IDXtrue+1;  IDXpred=IDXpred+1;
  end
end

%%% error check
[IDXtrue,er] = checkNumArgs( IDXtrue, [], 0, 2 ); error(er);
[IDXpred,er] = checkNumArgs( IDXpred, [], 0, 2 ); error(er);
if( length(IDXtrue)~=length(IDXpred) )
  error('Lengths of IDXs must match up.'); end
if( max([IDXtrue;IDXpred])>ntypes )
  error(['ntypes = ' int2str(ntypes) ' not large enough']); end

%%% generate CM
CM = zeros(ntypes);
for i=1:ntypes
  vals = IDXpred( IDXtrue==i );
  for j=1:ntypes; CM(i,j) = sum(vals==j); end
end


function ival = c8mat_is_hermitian ( m, n, a )

%*****************************************************************************80
%
%% C8MAT_IS_HERMITIAN checks if a complex matrix is hermitian.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns of A.
%
%    Input, complex A(M,N), the matrix.
%
%    Output, integer IVAL:
%    -1, the matrix is not symmetric because M /= N.
%    -2, the matrix is not symmetric because A(I,J) = conjg ( A(J,I) ) fails.
%    1, the matrix is symmetric.
%
  if ( m ~= n )
    ival = -1;
    return
  end

  for i = 1 : n
    ffor j = 1 : i - 1
      if ( a(i,j) ~= conj ( a(j,i) ) )
        ival = -2;
      end
    end
  end

  ival = 1;

  return
end


function monomial_test07 ( )

%*****************************************************************************80
%
%% MONOMIAL_TEST07 tests MONO_RANK_GRLEX.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 November 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'MONOMIAL_TEST07\n' );
  fprintf ( 1, '  MONO_RANK_GRLEX returns the rank of a monomial\n' );
  fprintf ( 1, '  under the grlex ordering, in the sequence\n' );
  fprintf ( 1, '  of all monomials in M dimensions of degree N or less.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Print a monomial sequence with ranks assigned.\n' );

  n = 4;
  m = 3;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Let M = %d\n', m );
  fprintf ( 1, '      N = %d\n', n );
  fprintf ( 1, '\n' );

  x = [ 0, 0, 0 ];
  i = 1;

  while ( 1 )

    fprintf ( 1, '  %3d:', i );
    for j = 1 : m
      fprintf ( 1, '  %1d', x(j) );
    end
    fprintf ( 1, '\n' );

    if ( x(1) == n )
      break
    end

    x = mono_upto_next_grlex ( m, n, x );
    i = i + 1;

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Now, given a monomial, retrieve its rank in the sequence:\n' );
  fprintf ( 1, '  (Should get 1, 2, 4, 8, 16, 32, 64 and 128.)\n' );
  fprintf ( 1, '\n' );

  test_num = 8;
  x_test = [ ...
    0, 0, 0; ...
    1, 0, 0; ...
    0, 0, 1; ...
    0, 2, 0; ...
    1, 0, 2; ...
    0, 3, 1; ...
    3, 2, 1; ...
    5, 2, 1 ]';

  for test = 1 : test_num
    x(1:m) = x_test(1:m,test);
    rank = mono_rank_grlex ( m, x );
    fprintf ( 1, '  %3d:', rank );
    for j = 1 : m
      fprintf ( 1, '  %1d', x(j) );
    end
    fprintf ( 1, '\n' );
  end

  return
end


function K = hole_correlation ( s, t )

%*****************************************************************************80
%
%% HOLE_CORRELATION evaluates the hole correlation function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 October 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real S(*), T(*), pairs of argument values.
%
%    Output, real K(*), the correlation function values
%
  d = abs ( s - t );

  K = ( 1 - d ) .* exp ( - d );

  return
end


function pass = test_iszero(pref)

if ( nargin == 0 )
    pref = chebfunpref();
end

% Test scalars:
f = chebfun(0, pref);
pass(1) = iszero(f);

f = chebfun([], pref);
pass(2) = iszero(f);

f = chebfun(2, pref);
pass(3) = ~iszero(f);

% Test piecewise domains:
f = chebfun(0, [-1, 0, 1], pref);
pass(4) = iszero(f);

f = chebfun([], [-1, 0, 1], pref);
pass(5) = iszero(f);

f = chebfun({0, 1}, [-1, 0, 1], pref);
pass(6) = ~iszero(f);

f = chebfun({1, 0}, [-1, 0, 1], pref);
pass(7) = ~iszero(f);

% Test arrays:
f = chebfun([0 0], pref);
pass(8) = all(iszero(f));

f = chebfun([0 1], pref);
pass(9) = all(iszero(f) == [1 0]);

%% Test on singular function:
dom = [-2 7];
pow = -1.64;
f = chebfun(@(x) sin(100*x).*(x-dom(1)).^pow, dom, 'exps', [pow 0], ...
    'splitting', 'on');
pass(10) = ~iszero(f);

%% Test for functions defined on unbounded domain:

% Set the domain:
dom = [-Inf Inf];

% Blow-up function:
op = @(x) x.^2.*(1-exp(-x.^2));
f = chebfun(op, dom, 'exps', [2 2]);
pass(11) = ~iszero(f);

% Function defined on [0 Inf]:

% Specify the domain:
dom = [0 Inf];

op = @(x) 0.75+sin(10*x)./exp(x);
f = chebfun(op, dom, 'splitting', 'on');
pass(12) = ~iszero(f);

% Function defined on [0 Inf]:

% Set the domain:
dom = [-Inf -3*pi];

% Blow-up function:
op = @(x) x.*(5+exp(x.^3))./(dom(2)-x);
f = chebfun(op, dom, 'exps', [0 -1]);
pass(13) = ~iszero(f);

% Zero function:
f = chebfun(@(x) 0*x, dom);
pass(14) = iszero(f);

% [TODO]: Add these once SUBSREF is implemented.
% f = chebfun(0, pref);
% f(0) = 1;
% pass(10) = iszero(f);
%
% f = chebfun([0 0], pref);
% f(0) = [0, 1];
% pass(11) = all(iszero(f) == [1, 0]);

end


function laguerre_polynomial_test02 ( )

%*****************************************************************************80
%
%% LAGUERRE_POLYNOMIAL_TEST02 tests L_POLYNOMIAL_COEFFICIENTS.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 March 2012
%
%  Author:
%
%    John Burkardt
%
  n = 10;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'LAGUERRE_POLYNOMIAL_TEST02\n' );
  fprintf ( 1, '  L_POLYNOMIAL_COEFFICIENTS determines polynomial coefficients of L(n,x).\n' );

  c = l_polynomial_coefficients ( n );

  for i = 0 : n
    fprintf ( 1, '\n' );
    fprintf ( 1, '  L(%d) = \n', i );
    fprintf ( 1, '\n' );
    for j = i : -1 : 0
      if ( c(i+1,j+1) == 0.0 )

      elseif ( j == 0 )
        fprintf ( 1, '  %g\n', c(i+1,j+1) );
      elseif ( j == 1 )
        fprintf ( 1, '  %g * x\n', c(i+1,j+1) );
      else
        fprintf ( 1, '  %g * x^%d\n', c(i+1,j+1), j );
      end
    end
  end

  return
end


function y=triangle_wave(x)

p=2.*pi;
y=(4.*(abs((x./p)-floor((x./p)+0.5))))-1;

%%
% _*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 fx = p00_fun ( problem, option, nvar, x )

%*****************************************************************************80
%
%% P00_FUN evaluates the function for any problem.
%
%  Discussion:
%
%    These problems were collected by Professor Werner Rheinboldt, of the
%    University of Pittsburgh, and were used in the development of the
%    PITCON program.
%
%  Index:
%
%     1  The Freudenstein-Roth function
%     2  The Boggs function
%     3  The Powell function
%     4  The Broyden function
%     5  The Wacker function
%     6  The Aircraft stability function
%     7  The Cell kinetic function
%     8  The Riks mechanical problem
%     9  The Oden mechanical problem
%    10  Torsion of a square rod, finite difference solution
%    11  Torsion of a square rod, finite element solution
%    12  The materially nonlinear problem
%    13  Simpson's mildly nonlinear boundary value problem
%    14  Keller's boundary value problem
%    15  The Trigger Circuit
%    16  The Moore-Spence Chemical Reaction Integral Equation
%    17  The Bremermann Propane Combustion System
%    18  The semiconductor problem
%    19  The nitric acid absorption flash
%    20  The Buckling Spring
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 October 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Rami Melhem, Werner Rheinboldt,
%    A Comparison of Methods for Determining Turning Points of Nonlinear Equations,
%    Computing,
%    Volume 29, Number 3, September 1982, pages 201-226.
%
%    Werner Rheinboldt,
%    Numerical Analysis of Parameterized Nonlinear Equations,
%    Wiley, 1986,
%    ISBN: 0-471-88814-1,
%    LC: QA372.R54.
%
%    Werner Rheinboldt,
%    Sample Problems for Continuation Processes,
%    Technical Report ICMA-80-?,
%    Institute for Computational Mathematics and Applications,
%    Department of Mathematics,
%    University of Pittsburgh, November 1980.
%
%    Werner Rheinboldt, John Burkardt,
%    A Locally Parameterized Continuation Process,
%    ACM Transactions on Mathematical Software,
%    Volume 9, Number 2, June 1983, pages 215-235.
%
%    Werner Rheinboldt, John Burkardt,
%    Algorithm 596:
%    A Program for a Locally Parameterized
%    Continuation Process,
%    ACM Transactions on Mathematical Software,
%    Volume 9, Number 2, June 1983, pages 236-241.
%
%    Werner Rheinboldt,
%    Computation of Critical Boundaries on Equilibrium Manifolds,
%    SIAM Journal on Numerical Analysis,
%    Volume 19, Number 3, June 1982, pages 653-669.
%
%  Parameters:
%
%    Input, integer PROBLEM, the problem index.
%
%    Input, integer OPTION, the option index.
%
%    Input, integer NVAR, the number of variables.
%
%    Input, real X(NVAR), the argument of the function.
%
%    Output, real FX(NVAR-1), the value of the function at X.
%
  if ( problem == 1 )
    fx = p01_fun ( option, nvar, x );
  elseif ( problem == 2 )
    fx = p02_fun ( option, nvar, x );
  elseif ( problem == 3 )
    fx = p03_fun ( option, nvar, x );
  elseif ( problem == 4 )
    fx = p04_fun ( option, nvar, x );
  elseif ( problem == 5 )
    fx = p05_fun ( option, nvar, x );
  elseif ( problem == 6 )
    fx = p06_fun ( option, nvar, x );
  elseif ( problem == 7 )
    fx = p07_fun ( option, nvar, x );
  elseif ( problem == 8 )
    fx = p08_fun ( option, nvar, x );
  elseif ( problem == 9 )
    fx = p09_fun ( option, nvar, x );
  elseif ( problem == 10 )
    fx = p10_fun ( option, nvar, x );
  elseif ( problem == 11 )
    fx = p11_fun ( option, nvar, x );
  elseif ( problem == 12 )
    fx = p12_fun ( option, nvar, x );
  elseif ( problem == 13 )
    fx = p13_fun ( option, nvar, x );
  elseif ( problem == 14 )
    fx = p14_fun ( option, nvar, x );
  elseif ( problem == 15 )
    fx = p15_fun ( option, nvar, x );
  elseif ( problem == 16 )
    fx = p16_fun ( option, nvar, x );
  elseif ( problem == 17 )
    fx = p17_fun ( option, nvar, x );
  elseif ( problem == 18 )
    fx = p18_fun ( option, nvar, x );
  elseif ( problem == 19 )
    fx = p19_fun ( option, nvar, x );
  elseif ( problem == 20 )
    fx = p20_fun ( option, nvar, x );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, 'P00_FUN - Fatal error!\n' );
    fprintf ( 1, '  Unrecognized problem number = %d\n', problem );
    error ( 'P00_FUN - Fatal error!' );
  end
%
%  Ensure the result is a COLUMN vector.
%
  fx = fx(:);

  return
end


function newnums=roundtowardvec(X,roundvec,type)
%function newnums=roundtowardvec(X,[roundvec],[type])
%
% This function rounds number(s) toward given values. If more than one
% number is given to round, it will return the matrix with each rounded
% value, otherwise it will return the single rounded value. It will ignore
% NaNs and return them back with NaNs.
%
% Inputs: X: the number(s) that you want rounded
%
%         roundvec:(opt) the values to round X to. If none given, it will
%           default to -inf:1:inf (and use the built in functions).
%
%         type:(opt) specifies which kind of rounding you want
%           the function to use.
%
%           Choices are: 'round' - round to nearest value
%                        'floor' - round toward -Inf
%                        'ceil'  - round toward Inf
%                        'fix'   - round toward 0
%                        'away'  - round away from 0 (ceil if positive and floor if negative)
%                     (see help files for more clarity)
%
%           If no type is given, the function will default to rounding to
%           the nearest value.
%
% Outputs: newnums: rounded values, in same shape as X input matrix

% For some reason, loops seem to be faster than vectorizing this code

if nargin==0
    help roundtowardvec; %if nothing given, tell what to give
    return
elseif isempty(X)
    newnums=[]; %if given empty, return empty without going through whole script
    return
end
if ~exist('type','var') || isempty(type)
    type='round';  %%round to nearest value if not specified
end
if ~exist('roundvec','var') || isempty(roundvec)
    if strcmpi(type,'round')
        newnums=round(X);
        %to nearest integer
    elseif strcmpi(type,'away')
        newnums=ceil(abs(X)).*sign(X);
        %nearest integer away from 0
    elseif strcmpi(type,'fix')
        newnums=fix(X);
        %nearest integer toward 0
    elseif strcmpi(type,'floor')
        newnums=floor(X);
        %nearest integer toward -inf
    elseif strcmpi(type,'ceil')
        newnums=ceil(X);
        %nearest integer toward inf
    else
        error(sprintf('Round type not recognized. Options are:\n''round'' - round to nearest value\n''floor'' - round toward -Inf\n''ceil''  - round toward Inf\n''fix''   - round toward 0\n''away''  - round away from 0')) %#ok<SPERR>
    end
else
    %%make newnums size of X
    newnums=X;
    if strcmpi(type,'round') %to nearest value
        roundvec=reshape(unique(roundvec),1,[]);
        for i=numel(X):-1:1
            if ~any(X(i)==roundvec)
                DIFFs=abs(roundvec-X(i));
                if X(i)>=0
                    [~,ind]=min(DIFFs(:,end:-1:1));
                    newnums(i)=roundvec(length(DIFFs)-ind+1);
                elseif X(i)<0
                    [~,ind]=min(DIFFs);
                    newnums(i)=roundvec(ind);
                end
            end
        end
    elseif strcmpi(type,'fix') %to nearest value toward 0
        roundvec=reshape(unique([roundvec 0]),1,[]);
        for i=numel(X):-1:1
            if ~any(X(i)==roundvec)
                if X(i)>0
                    if X(i)>min(roundvec)
                        newnums(i)=roundvec(find(X(i)>roundvec,1,'last'));
                    else
                        newnums(i)=0;
                    end
                elseif X(i)<0
                    if X(i)<max(roundvec)
                        newnums(i)=roundvec(find(X(i)<roundvec,1,'first'));
                    else
                        newnums(i)=0;
                    end
                end
            end
        end
    elseif strcmpi(type,'ceil') %nearest value toward inf
        roundvec=reshape(unique(roundvec),1,[]);
        for i=numel(X):-1:1
            if ~isnan(X(i)) && ~any(X(i)==roundvec)
                if X(i)<max(roundvec)
                    newnums(i)=roundvec(find(X(i)<roundvec,1,'first'));
                else
                    newnums(i)=inf;
                end
            end
        end
    elseif strcmpi(type,'floor') %nearest value toward -inf
        roundvec=reshape(unique(roundvec),1,[]);
        for i=numel(X):-1:1
            if ~isnan(X(i)) && ~any(X(i)==roundvec)
                if X(i)>min(roundvec)
                    newnums(i)=roundvec(find(X(i)>roundvec,1,'last'));
                else
                    newnums(i)=-inf;
                end
            end
        end
    elseif strcmpi(type,'away') %nearest value away from 0
        roundvec=reshape(unique(roundvec),1,[]);
        for i=numel(X):-1:1
            if ~any(X(i)==roundvec)
                if X(i)>0
                    if X(i)<max(roundvec)
                        newnums(i)=roundvec(find(X(i)<roundvec,1,'first'));
                    else
                        newnums(i)=inf;
                    end
                elseif X(i)<0
                    if X(i)>min(roundvec)
                        newnums(i)=roundvec(find(X(i)>roundvec,1,'last'));
                    else
                        newnums(i)=-inf;
                    end
                elseif X(i)==0
                    DIFFs=abs(roundvec-X(i));
                    [~,ind]=min(DIFFs(:,end:-1:1));
                    newnums(i)=roundvec(length(DIFFs)-ind+1);
                end
            end
        end
    else
        error(sprintf('Round type not recognized. Options are:\n''round'' - round to nearest value\n''floor'' - round toward -Inf\n''ceil''  - round toward Inf\n''fix''   - round toward 0\n''away''  - round away from 0')) %#ok<SPERR>
    end
end
end


% VL_DEMO_DSIFT Demo: DSIFT

randn('state',0) ;
rand('state',0) ;

% read a test image
I = imread(fullfile(vl_root,'data','roofs1.jpg')) ;
I = single(vl_imdown(rgb2gray(I))) ;

% --------------------------------------------------------------------
%                                                      Basic benchmark
% --------------------------------------------------------------------

binSize = 4 ; % bin size in pixels
magnif = 3 ; % bin size / keypoint scale

elaps_dsift = [] ;
elaps_dsift_fast = [] ;
err_dsift = [] ;
err_dsift_fast = [] ;

binSizeRange = [3 4 5 6] ;
for wi = 1:length(binSizeRange)
  binSize = binSizeRange(wi) ;
  scale = binSize / magnif ;

  tic ;
  [f, d] = vl_dsift(vl_imsmooth(I, sqrt(scale.^2 - .25)), ...
                    'size', binSize, ...
                    'step', 2, ...
                    'bounds', [20,20,210,140], ...
                    'floatdescriptors', ...
                    'verbose') ;
  elaps_dsift(wi) = toc ;

  tic ;
  [f, dfast] = vl_dsift(vl_imsmooth(I, sqrt(scale.^2 - .25)), ...
                        'size', binSize, ...
                        'step', 2, ...
                        'bounds', [20,20,210,140], ...
                        'floatdescriptors', ...
                        'fast', ...
                        'verbose') ;
  elaps_dsift_fast(wi) = toc ;

  numKeys = size(f, 2) ;
  f_ = [f ; ones(1, numKeys) * scale ; zeros(1, numKeys)] ;

  tic ;
  [f_, d_] = vl_sift(I, ...
                     'magnif', magnif, ...
                     'frames', f_, ...
                     'firstoctave', -1, ...
                     'levels', 5, ...
                     'floatdescriptors') ;
  elaps_sift(wi) = toc ;

  err_dsift(wi)      = mean(mean(abs(d     - d_)) ./ mean(d_)) * 100 ;
  err_dsift_fast(wi) = mean(mean(abs(dfast - d_)) ./ mean(d_)) * 100 ;
end

figure(1) ; clf ; title('Descriptor  SIFT') ;
plot(binSizeRange, [err_dsift ; err_dsift_fast]', 'linewidth', 3) ;
legend('DSIFT', 'DSIFT fast') ;
ylabel('Approx error (%)') ;
xlabel('binSize parameter') ;
grid on ;
axis square ;

figure(2) ; title('Speedup on regular SIFT') ;
plot(binSizeRange, ...
     [elaps_sift ./ elaps_dsift ; ...
      elaps_sift ./ elaps_dsift_fast ; ...
      elaps_sift ./ elaps_sift], 'linewidth', 3) ;
legend('DSIFT', 'DSIFT fast', 'SIFT') ;
ylabel('Speedup') ;
xlabel('binSize parameter') ;
grid on ;
axis square ;

figure(1) ; vl_demo_print('dsift_accuracy') ;
figure(2) ; vl_demo_print('dsift_speedup') ;


function [ nxe, cnt ] = linop_normest( op, cmode, tol, maxiter )

%LINOP_NORMEST Estimates the operator norm.
%    EST = LINOP_NORMEST( OP ) estimates the induced norm of the operator:
%           || OP || = max_{||x||<=1} ||OP(X)||
%    using a simple power method similar to the MATLAB NORMEST functon.
%
%    When called with a single argument, LINOP_TEST begins with a real
%    initial vector. To test complex operators, use the two-argument
%    version LINOP_NORMEST( OP, cmode ), where:
%        cmode = 'R2R': real input, real output
%        cmode = 'R2C': real input, complex output
%        cmode = 'C2R': imag input, imag output
%        cmode = 'C2C': complex input, complex output
%
%    LINOP_NORMEST( OP, CMODE, TOL, MAXITER ) stops the iteration when the
%    relative change in the estimate is less than TOL, or after MAXITER
%    iterations, whichever comes first.
%
%    [ EST, CNT ] = LINOP_NORMEST( ... returns the estimate and the number
%    of iterations taken, respectively.

if isnumeric( op ),
    op = linop_matrix( op, 'C2C' );
elseif ~isa( op, 'function_handle' ),
    error( 'Argument must be a function handle or matrix.' );
end
x_real = true;
if nargin >= 2 && ~isempty( cmode ),
    switch upper( cmode ),
        case { 'R2R', 'R2C' }, x_real = true;
        case { 'C2R', 'C2C' }, x_real = false;
        otherwise, error( 'Invalid cmode: %s', cmode );
    end
end
if nargin < 3 || isempty( tol ),
    tol = 1e-8;
end
if nargin < 4 || isempty( maxiter ),
    maxiter = 50;
end
sz = op([],0);
if iscell( sz ),
    sz = sz{1};
elseif ~isempty(sz)
    sz = [sz(2),1];
else
    % if the input is the identity, it may not have a size associated with it,
    % so current behavior is to try an arbitrary size:
    sz = [50,1];
end
cnt = 0;
nxe = 0;
while true,
    if nxe == 0,
        if x_real,
            xx = randn(sz);
        else
            xx = randn(sz) + 1j*randn(sz);
        end
        nxe = sqrt( tfocs_normsq( xx ) );
    end
    yy = op( xx / max( nxe, realmin ), 1 );
    nye = sqrt( tfocs_normsq( yy ) );
    xx = op( yy / max( nye, realmin ), 2 );
    nxe0 = nxe;
    nxe = sqrt( tfocs_normsq( xx ) );
    if abs( nxe - nxe0 ) < tol * max( nxe0, nxe ),
        break;
    end
    cnt = cnt + 1;
    if cnt >= maxiter,
        break;
    end
end

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


function HT = SoftThreshold(input,H);
% HT = input;
% for i = 1:numel(input)
%     if abs(input(i)) < H
%         HT(i) = 0;
%     end
%     if abs(input(i)) > H
%         HT(i) = sign(input(i))*(abs(input(i))-H);
%     end
% end
HT = sign(input).*max(abs(input)-H,0);
end


function Archive = UpdateArchive(N,combinePopulation)

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

% This function is written by Huangke Chen

    % Remove the dominated solutions
    Archive = combinePopulation(NDSort(combinePopulation.objs,1)==1);

    % Update the Archive outPopulation, if the number of solutions is larger than the population size
    if length(Archive) > N
        PopObj = Archive.objs;

        Choose = false(1,size(PopObj,1));
        % Select the extreme solutions
        [~,extreme]     = min(pdist2(PopObj,eye(size(PopObj,2)),'cosine'),[],1);
        Choose(extreme) = true;

        %% Lp-norm-distances between each two solutions
        LpNormD = pdist2(PopObj,PopObj,'minkowski',0.5);
        while sum(Choose) < N
            Remain   = find(~Choose);
            [~, rho] = max(min(LpNormD(Remain,Choose),[],2));
            Choose(Remain(rho)) = true;
        end
        Archive = Archive(Choose);
    end
end


function k = invcmpndKernCompute(kern, x, x2)

% INVCMPNDKERNCOMPUTE Compute the INVERSE-PRECISION-CMPND kernel given the parameters and X.
% FORMAT
% DESC computes the kernel parameters for the kernel that is the inverse of
% the sum of the precisions of 1 or more kernels, given
% inputs associated with rows and columns.
% That is, K(x,x2) = inv(inv(K1(x,x2)) + inv(K2(x,x2)) + ... )
% Notice that the individual kernels Ki are each allowed to take a different
% subset of the inputs x, x2 (defined in the field kern.comp{i}.index).
% ARG kern : the kernel structure for which the matrix is computed.
% ARG x : the input matrix associated with the rows of the kernel.
% ARG x2 : the input matrix associated with the columns of the kernel.
% RETURN k : the kernel matrix computed at the given points.
%
% FORMAT
% DESC computes the kernel parameters for the kernel that is the inverse of
% the sum of the precisions of 1 or more kernels
% given a design matrix of inputs.
% That is, K(x,x2) = inv(inv(K1(x1,x2)) + inv(K2(x1,x2)) + ... )
% Notice that the individual kernels Ki are each allowed to take a different
% subset of the inputs x, x2 (defined in the field kern.comp{i}.index).
% ARG kern : the kernel structure for which the matrix is computed.
% ARG x : input data matrix in the form of a design matrix.
% RETURN k : the kernel matrix computed at the given points.
%
% SEEALSO : invcmpndKernParamInit, kernCompute, kernCreate, invcmpndKernDiagCompute
%
% COPYRIGHT : Andreas C. Damianou, 2012

% KERN

% !!! TODO: There are several precomputations and tricks to speed this up...!

if nargin > 2
  i = 1;
  if ~isempty(kern.comp{i}.index)
    % only part of the data is involved in the kernel.
    k = kernCompute(kern.comp{i}, ...
                         x(:, kern.comp{i}.index), ...
                         x2(:, kern.comp{i}.index));
  else
    % all the data is involved with the kernel.
    k = kernCompute(kern.comp{i}, x, x2);
  end
  k = pdinv(k); %%%
  for i = 2:length(kern.comp)
    if ~isempty(kern.comp{i}.index)
      % only part of the data is involved in the kernel.
      k_cur = kernCompute(kern.comp{i}, ...
                           x(:, kern.comp{i}.index), ...
                           x2(:, kern.comp{i}.index));
    else
      % all the data is involved with the kernel.
      k_cur = kernCompute(kern.comp{i}, x, x2);
    end
    k = k + pdinv(k_cur);
  end
else
  i = 1;
  if ~isempty(kern.comp{i}.index)
    % only part of the data is involved with the kernel.
    k  = kernCompute(kern.comp{i}, x(:, kern.comp{i}.index));
  else
    % all the data is involved with the kernel.
    k  = kernCompute(kern.comp{i}, x);
  end
  k = pdinv(k); %%%
  for i = 2:length(kern.comp)
    if ~isempty(kern.comp{i}.index)
      % only part of the data is involved with the kernel.
      k_cur = kernCompute(kern.comp{i}, x(:, kern.comp{i}.index));
    else
      % all the data is involved with the kernel.
      k_cur = kernCompute(kern.comp{i}, x);
    end
    k = k + pdinv(k_cur);
  end
end

k = pdinv(k);


U=randn(10,1000);

param.lambda=0.1; % regularization parameter
param.num_threads=-1; % all cores (-1 by default)
param.verbose=true;   % verbosity, false by default
param.pos=false;       % can be used with all the other regularizations
param.intercept=false; % can be used with all the other regularizations

fprintf('First tree example\n');
% Example 1 of tree structure
% tree structured groups:
% g1= {0 1 2 3 4 5 6 7 8 9}
% g2= {2 3 4}
% g3= {5 6 7 8 9}
tree.own_variables=int32([0 2 5]);   % pointer to the first variable of each group
tree.N_own_variables=int32([2 3 5]); % number of "root" variables in each group
                              % (variables that are in a group, but not in its descendants).
                              % for instance root(g1)={0,1}, root(g2)={2 3 4}, root(g3)={5 6 7 8 9}
tree.eta_g=[1 1 1];           % weights for each group, they should be non-zero to use fenchel duality
tree.groups=sparse([0 0 0; ...
                    1 0 0; ...
                    1 0 0]);    % first group should always be the root of the tree
                                % non-zero entriees mean inclusion relation ship, here g2 is a children of g1,
                                % g3 is a children of g1

fprintf('\ntest prox tree-l0\n');
param.regul='tree-l0';
alpha=mexProximalTree(U,tree,param);

fprintf('\ntest prox tree-l2\n');
param.regul='tree-l2';
alpha=mexProximalTree(U,tree,param);

fprintf('\ntest prox tree-linf\n');
param.regul='tree-linf';
alpha=mexProximalTree(U,tree,param);

fprintf('Second tree example\n');
% Example 2 of tree structure
% tree structured groups:
% g1= {0 1 2 3 4 5 6 7 8 9}    root(g1) = { };
% g2= {0 1 2 3 4 5}            root(g2) = {0 1 2};
% g3= {3 4}                    root(g3) = {3 4};
% g4= {5}                      root(g4) = {5};
% g5= {6 7 8 9}                root(g5) = { };
% g6= {6 7}                    root(g6) = {6 7};
% g7= {8 9}                    root(g7) = {8};
% g8 = {9}                     root(g8) = {9};
tree.own_variables=  int32([0 0 3 5 6 6 8 9]);   % pointer to the first variable of each group
tree.N_own_variables=int32([0 3 2 1 0 2 1 1]); % number of "root" variables in each group
tree.eta_g=[1 1 1 2 2 2 2.5 2.5];
tree.groups=sparse([0 0 0 0 0 0 0 0; ...
                    1 0 0 0 0 0 0 0; ...
                    0 1 0 0 0 0 0 0; ...
                    0 1 0 0 0 0 0 0; ...
                    1 0 0 0 0 0 0 0; ...
                    0 0 0 0 1 0 0 0; ...
                    0 0 0 0 1 0 0 0; ...
                    0 0 0 0 0 0 1 0]);  % first group should always be the root of the tree

fprintf('\ntest prox tree-l0\n');
param.regul='tree-l0';
alpha=mexProximalTree(U,tree,param);

fprintf('\ntest prox tree-l2\n');
param.regul='tree-l2';
alpha=mexProximalTree(U,tree,param);

fprintf('\ntest prox tree-linf\n');
param.regul='tree-linf';
alpha=mexProximalTree(U,tree,param);

% mexProximalTree also works with non-tree-structured regularization functions
fprintf('\nprox l1, intercept, positivity constraint\n');
param.regul='l1';
param.pos=true;       % can be used with all the other regularizations
param.intercept=true; % can be used with all the other regularizations
alpha=mexProximalTree([U; ones(1,size(U,2))],tree,param);

% Example of multi-task tree
fprintf('\nprox multi-task tree\n');
param.pos=false;
param.intercept=false;
param.lambda2=param.lambda;
param.regul='multi-task-tree';  % with linf
alpha=mexProximalTree(U,tree,param);

tree.own_variables=int32([0 1 2 3 4 5 6]);   % pointer to the first variable of each group
tree.N_own_variables=int32([1 1 1 1 1 1]); % number of "root" variables in each group
                              % (variables that are in a group, but not in its descendants).
                              % for instance root(g1)={0,1}, root(g2)={2 3 4}, root(g3)={5 6 7 8 9}
tree.eta_g=[1 1 1 1 1 1];           % weights for each group, they should be non-zero to use fenchel duality
tree.groups=sparse([0 0 0; ...
                    1 0 0; ...
                    1 0 0]);    % first group should always be the root of the tree
                                % non-zero entriees mean inclusion relation ship, here g2 is a children of g1,
                                % g3 is a children of g1


function [y, C] = noisy_joint_density(s, w, mu, sigma, noise_dev)

% Evaluate the joint density of hidden and observed variables
% Note that the joint density is *not* normalized, i.e., it is only
% evaluated up to proportionality. This is because it can assume Inf values
% if mu << 0 and sigma >> 0 and the proportionalty factor is not needed in
% the EM algorithm.
%
% Syntax:
%   [y, C] = joint_density(s, w, mu, sd, sn)
%
% Input:
%   s  : Value of hidden variable.
%
%   w  : Value of wavelet coefficient. Must have the same size as s
%
%   mu : Mean of hidden variable.
%
%   sd : Standard deviation of hidden variable.
%
%   sn : Standard deviation of noise.
%
%
% Output:
%   y  : Value of density
%
%   C  : Normalizing factor for density.
%
% See also: JOINT_DENSITY

if isscalar(w)
    w2 = w^2 * ones(size(s));
else
    w2 = w.^2;
end

% The 'upper' exponential
logy = -0.5*((s - mu)/sigma).^2;

non_zero_idx = w2 ~= 0;
observed_var = exp(s) + noise_dev^2;
logy(non_zero_idx) = logy(non_zero_idx) ...
    - 0.5*w2(non_zero_idx)./observed_var(non_zero_idx);

y = exp( logy ) ./ sqrt(observed_var);

% The normalization factor
if nargout == 2
	C = 1/(2*pi*sigma);
end

end