classdef DEAGNG < ALGORITHM
% <multi/many> <real/integer/label/binary/permutation>
% Decomposition based evolutionary algorithm guided by growing neural gas
% aph ---   0.1 --- Parameter alpha
% eps --- 0.314 --- Parameter epsilon

%------------------------------- Reference --------------------------------
% Y. Liu, H. Ishibuchi, N. Masuyama, and Y. Nojima, Adapting reference
% vectors and scalarizing functions by growing neural gas to handle
% irregular Pareto fronts. IEEE Transactions on Evolutionary Computation,
% 2020, 24(3): 439-453.
%--------------------------------------------------------------------------
% Copyright Yiping Liu
% Please contact {yiping0liu@gmail.com} if you have any problem.
%--------------------------------------------------------------------------

    methods
        function main(Algorithm,Problem)
            %% Parameter setting
            [aph,eps] = Algorithm.ParameterSet(0.1,0.314);

            %% DEA Initialization
            [Ru,Problem.N] = UniformPoint(Problem.N,Problem.M);	% Uniform reference vectors
            Population     = Problem.Initialization();          % Random population
            Zmin           = min(Population.objs,[],1);         % Ideal Point
            AS             = [];                                % Input Signal Archive
            Ruq            = Ru;                                % Refernce vectors in Ru for selection
            [FrontNo,~]    = NDSort(Population.objs,Problem.N); % Fitness for the first mating selection
            crd            = zeros(1,Problem.N);                % Fitness for the first mating selection
            MaxGen         = ceil(Problem.maxFE/Problem.N);    	% Maximum Generation

            %% GNG Initialization
            ArchiveSize = Problem.M*Problem.N;	% Size of Input Signal Archive
            NoG = aph*MaxGen;                   % Number of generations of Not Training GNG
            GNGnet.maxIter = 1;                 % Number of iterations to train GNG per Generation
            GNGnet.maxAge = Problem.N;          % Maximum cluster age
            GNGnet.maxNode = Problem.N;         % Max number of nodes
            GNGnet.lambda = 0.2*Problem.N;      % Cycle for topology reconstruction
            GNGnet.hp = [];                     % Hit point of node
            GNGnet.maxHP = 2*ArchiveSize;       % Max HP of node
            GNGnet.Node = [];                   % Node
            GNGnet.NodeS = [];                  % Expanded node
            GNGnet.NodeP = [];                  % Node mapped to hyperplane
            GNGnet.Err = [];                    % Error
            GNGnet.edge = zeros(2,2);           % Edge between nodes
            GNGnet.age = zeros(2,2);            % Age of edge
            GNGnet.epsilon_a = 0.2;             % Learning coefficient
            GNGnet.epsilon_nb = 0.01;           % Learning coefficient of neighbor
            GNGnet.alpha = 0.5;                 % Nodes r1max and r2max error reduction constant
            GNGnet.delta = 0.9;                 % Error reduction coefficient

            %% Optimization
            while Algorithm.NotTerminated(Population)
                MatingPool = TournamentSelection(2,Problem.N,FrontNo,crd);
                Offspring  = OperatorGA(Problem,Population(MatingPool));
                Zmin       = min([Zmin;Offspring.objs],[],1);

                %% GNG-based adaptation
                if ceil(Problem.FE/Problem.N) <= MaxGen - NoG
                    % Input Signal Archive Update
                    AS = ArchiveUpdate([AS;Offspring.objs],ArchiveSize,Ruq,GNGnet.NodeS,Zmin);
                    nAS = length(AS);
                    % GNG Update (and Algorithm 3)
                    GNGnet.maxNode = min(Problem.N,floor(nAS/2)); % paramter reset
                    GNGnet.maxHP = 2*nAS; % paramter reset
                    GNGnet = GNGUpdate(AS,GNGnet);
                    % Reference Vector Adaptation (Algorithm 4)
                    if size(GNGnet.NodeS,1)>2
                        [Ruq,GNGnet] = ReferenceCombination(Ru,GNGnet);
                    end
                    % Scalarizing Function Adaptation
                    theta = TunePBI(GNGnet,eps); % Tune theta in PBI function
                end

               %% Environmental Selection
               [Population,FrontNo,crd] = ESelection([Population,Offspring],Problem.N,Ruq,GNGnet.NodeS,theta,Zmin);
            end
        end
    end
end


% FitDataNDFast : Fits several ND Gaussians to N-dimensional image data
% Synopsis: [fitted,params,myfunct] = FitDataNDFast(InitParm,Data,numdims,maxiter,method)
% AFunctString should use the variable 'x' as a vector and x{1}, x{2} to access its components,
% and the values to fit: c(1),c(2),...
%
% InitParm : Vector of initial parameters first row is global parameters,
% meaning is as follows: [global background, global width, intensity, posy, posx, intensity2, posy2, posx2...]
% other rows are local parameters
% Data : Experimental multidimensional (e.g. image) data to be fitted
% maxiter : maximum number of iterations in fit (default = 300)
% method : figure of merit to use for fitting 'mes', 'idiv' or 'fidiv'
%
% Example:
% a=noise(51.2*exp(-((xx(20,20)-2).^2+(yy(20,20)-1.2).^2)/20)+33,'poisson')
% [params,res,fitted]=FitDataNDFast([30 15 40 2 2],a,2,300,'idiv')
% fitted is the residual image (depending on method)
% params contains the result of the fit

function [params,res,fitted,residual] = FitDataNDFast(InitParm,Data,numdims,maxiter,mymethod)

if nargin < 3
    numdims = 2;
end
if nargin < 4
    maxiter = 3000;
end

if nargin < 5
    mymethod = 'mse';
end

%if nargin < 5
%    placement = 'right';
%end

%fixedparams=[0 20];
MultiGaussMSE(double(Data),mymethod,numdims)

[res, fitted] = MultiGaussMSE(InitParm');
%params=fixedparams;

if (0)  % old method
options=optimset('Display','notify','TolX',10^-4,'TolFun',10^-8,'MaxIter',maxiter);
[params,msevalue]=fminsearch(@MultiGaussMSE,InitParm',options);
else    % new method with smarter amd faster optimisation routine from http://www.cs.ubc.ca/~schmidtm
options=struct('Display','off','notify',1,'numDiff',1,'TolX',10^-4,'TolFun',10^-8,'MaxIter',maxiter);
[params,msevalue,moreinfo]=minFunc(@MultiGaussMSE,InitParm',options);
end

[res, fitted,residual] = MultiGaussMSE(params);
params = params';
fitted=dip_image(fitted);
residual=dip_image(residual);


function qout=qnorm(qin)
% QNORM(Q) normalizes quaternions.
%     Works on vectors of quaternions too.  If input is a vector of four
%     quaternions, QNORM will determine whether the quaternions are row or
%     column vectors according to ISQ.
%
% See also ISQ.

% Release: $Name: quaternions-1_2_2 $
% $Revision: 1.9 $
% $Date: 2001/05/01 20:20:31 $

% Copyright (C) 2001, Jay A. St. Pierre.  All rights reserved.

if nargin~=1
  error('qnorm() requires one input argument');
else
  qtype = isq(qin);
  if ( qtype == 0 )
    error(['Invalid input: must be a quaternion or a vector of' ...
          ' quaternions'])
  elseif ( qtype==3 )
    warning(['Component quaternion shape indeterminate... assuming row' ...
             ' vectors'])
  end
end

% Make sure qin is a column of quaternions
if( qtype == 1 )
  qin=qin.';
end

% Find the magnitude of each quaternion
qmag=sqrt(sum(qin.^2,2));

% Make qmag the same size a q
qmag=[qmag qmag qmag qmag];

% Divide each element of q by appropriate qmag
qout=qin./qmag;

% Make sure output is same shape as input
if( qtype == 1 )
  qout=qout.';
end


% make the structure of an embedded HMM with 2 rows and 3 columns

% 1------------>2
% |\   \        | \  \
% 3->4->5       6->7->8

n = 8;
dag = zeros(n);
dag(1,[2 3 4 5])=1;
dag(2,[6 7 8])=1;
for i=3:4
  dag(i,i+1)=1;
end
for i=6:7
  dag(i,i+1)=1;
end
ns = 2*ones(1,n);
bnet = mk_bnet(dag,ns);
for i=1:n
  bnet.CPD{i}=tabular_CPD(bnet,i);
end
[jtree, root, cliques] =  graph_to_jtree(moralize(bnet.dag), ones(1,n), {}, {});
%[jtree, root, cliques, B, w, elim_order, moral_edges, fill_in_edges] = dag_to_jtree(bnet);


function val = source(x_g, param)

% Evaluate source terms (if any) at points x_g

% characteristic functions for source terms
    chi1_g = (x_g(:,1) < 0.2*param.L).*(x_g(:,2) < 0.5*param.W);
    chi3_g = (x_g(:,1) > 0.8*param.L).*(x_g(:,2) > 0.5*param.W);
%   chi2_g = ~chi1_g & ~chi3_g;

    val =   0*chi1_g +   0*chi3_g;
%

  return
end


function r = mpower(a,b)
%MPOWER       Hessian power  a ^ n
%

% written  04/04/04     S.M. Rump
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
%

  % for scalars use .^ with improved diameter for even exponent
  if ( prod(size(a))==1 ) & ( prod(size(b))==1 )
    r = a .^ b ;
    return
  end

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  if isa(b,'double') & isreal(b) & prod(size(b))==1 & b==round(b)
    if b<0
      error('negative exponent in mpower')
    end
    [m n] = size(a);
    if m~=n
      error('hessian mpower of non-square matrix')
    end
    if b==0
      if issparse(a)
        r = typeadj( hessian(speye(size(a))) , typeof(a) );
      else
        r = typeadj( hessian(eye(size(a))) , typeof(a) );
      end
    else                        % b is integer, at least 1
      b = b - 1;
      r = a;
      while b>0
        if mod(b,2)==1
          r = r*a;
        end
        b = floor(b/2);
        if b~=0
          a = a*a;
        end
      end
    end
  else
    error('invalid call of hessian mpower ^')
  end

  if rndold
    setround(rndold)
  end


function ls=v_lpcar2ls(ar)
%V_LPCAR2LS convert ar polynomial to line spectrum pair frequencies LS=(AR)
% output vector elements will be in range 0 to 0.5
% the returned vector will be of length p

% This routine is nowhere near as efficient as it might be

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: v_lpcar2ls.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[nf,p1]=size(ar);
p = p1-1;
p2 = fix(p/2);
d=0.5/pi;

if rem(p,2)		% odd order
  for k=1:nf
    aa=[ar(k,:) 0];
    r = aa + fliplr(aa);
    q = aa - fliplr(aa);
    fr = sort(angle(roots(r)));
    fq = [sort(angle(roots(deconv(q,[1 0 -1])))); 0];
    f = [fr(p2+2:p+1).' ; fq(p2+1:p).'];
    f(p+1) = [];
    ls(k,:) = d*f(:).';
  end
else
  for k=1:nf
    aa=[ar(k,:) 0];
    r = aa + fliplr(aa);
    q = aa - fliplr(aa);
    fr = sort(angle(roots(deconv(r,[1 1]))));
    fq = sort(angle(roots(deconv(q,[1 -1]))));
    f = [fr(p2+1:p).' ; fq(p2+1:p).'];
    ls(k,:) = d*f(:).';
  end
end


function [x, infos] = fro_mu_partial_nmf(V, rank, in_options)
%
% This file is part of NMFLibrary
%
% Created by H.Kasai on Feb. 16, 2017
%
% Change log:
%
%       June 16, 2022 (Hiroyuki Kasai): Initial version.
%
%       Jul. 12, 2022 (Hiroyuki Kasai): Modified code structures.
%

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

    % set local options
    local_options = [];
    local_options.alg       = 'mu';
    local_options.norm_h    = 0;
    local_options.norm_w    = 1;
    local_options.alpha     = 2;
    local_options.delta     = 0.1;
    local_options.myeps     = 1e-16;

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

    if ~strcmp(options.alg, 'mu')
        fprintf('Invalid algorithm: %s. Therfore, we use mu (i.e., multiplicative update).\n', options.alg);
        options.alg = 'mu';
    end

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

    % initialize
    method_name = sprintf('MU-Partial (%s:%s)', options.alg, options.metric_type);
    epoch = 0;
    R_zero = zeros(m, n);
    grad_calc_count = 0;

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

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

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

    % set start time
    start_time = tic();

    % main loop
    while true

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

        if strcmp(options.alg, 'mu')

            % update H
            H = H .* (W' * V) ./ (W' * W * H);
            H = H + (H<options.myeps) .* options.myeps;

            % update W
            W(:, options.updateW) = W(:, options.updateW) .* (V * H(options.updateW, :)') ...
                ./ (W * (H * H(options.updateW, :)'));
            W = W + (W<options.myeps) .* options.myeps;

        end

        % measure elapsed time
        elapsed_time = toc(start_time);

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

        % update epoch
        epoch = epoch + 1;

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

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

    end

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

end


function [C, S] = QLiftDec2MaxMax(X, N)
%-----------------------------------------------------------------------------
% QLiftDec2MaxMax
% Multilevel 2-D decomposition by the lifting scheme and using quincunx grids
%
% The MaxMax scheme has been proposed by Heijmans and Goutsias, see e.g.
%    H.J.A.M. Heijmans, J. Goutsias,
%    Multiresolution signal decomposition schemes.
%    Part 2: morphological wavelets.
%    CWI Report PNA-R9905, Amsterdam, 1999.
%    http://repository.cwi.nl:8888/cwi_repository/docs/IV/04/04625D.pdf
%
% Calls for: QLmaxlev,
%            storeQ1001, storeR,
%            getcolor01, getcolor10, getcolor00, getcolor11,
%            putcolor01, putcolor10, putcolor00, putcolor11.
% See also: QLiftRec2MaxMin
%
% Design and implementation by:
% Dr. Paul M. de Zeeuw <Paul.de.Zeeuw@cwi.nl>  http://homepages.cwi.nl/~pauldz/
% Last Revision: February 6, 2003.
% (c) 1999-2002 Stichting CWI, Amsterdam
%-----------------------------------------------------------------------------
%Firstly, check input data
%
if  isempty(X)
  error(' QLiftDec2MaxMax - empty matrix ');
else
  if mod(N, 2) == 1
    error(' QLiftDec2MaxMax - only an even number of levels is accepted ');
  end
  if QLmaxlev(size(X), 'maxmax') < N
    error(' QLiftDec2MaxMax - too many levels requested ');
  end
  if N < 2
    disp([' QLiftDec2MaxMax - WARNING too few levels requested ' ...
          '-> empty decomposition ']);
  end
end
%
%Secondly, start decomposition
%
O = X; % For the sake of efficient use of memory this could be improved upon.
% We descend to coarser grids, integer lev indicates number of scale.
C = []; S = [];

for lev=1:2:N
%
   [nO, mO] = size(O);
   if ( nO < 3 ) || ( mO < 3)
     error(' QLiftDec2MaxMax - too many levels ');
   end
   minO = min(min(O));
   maxO = max(max(O));
   cmin = minO-(maxO-minO);
%  cmax = maxO+(maxO-minO);
%
%  The Lifting Scheme proceeds from a rectangular grid
%  towards a quincunx grid.
%
%  Stage: predict
   A00 =getcolor00(O);
   A11 =getcolor11(O);
%  Quincunx grid Q0011 is the union of the values at .00 and .11: "even slots"
%  Quincunx grid Q1001 is the union of the values at .10 and .01: "odd slots"
   Q1001D01 = getcolor01(O) - synA01max(A11, A00, cmin);                % Y1
   Q1001D10 = getcolor10(O) - synA10max(A11, A00, cmin);                % Y1
%  At this point the union (quincunx) of Q1001D01 & Q1001D10
%  contains the DETAILS of O.
%
%  For the inverse transform Q1001D01 and Q1001D10 have to be stored:
   [C, S] = storeQ1001( Q1001D10, Q1001D01, lev, 'd', C, S);
%
%  Stage: update
   Q0011A00 = A00 + ...
        max(zeros(size(A00)), synA00max(Q1001D10, Q1001D01, cmin));     % X1
   clear A00;
   Q0011A11 = A11 + ...
        max(zeros(size(A11)), synA11max(Q1001D10, Q1001D01, cmin));     % X1
   clear A11 Q1001D10 Q1001D01;
%  At this point the union (quincunx) of Q0011A00 & Q0011A11
%  contains the updated APPROXIMATION of O, the DETAILS of O
%  were in the union (quincunx) of Q1001D01 & Q1001D10 (see above).
%
%  The Lifting Scheme proceeds by a subsequent step from quincunx
%  to rectangular grid.
%
%  Q0011 is split into the 11 colour with the "odd slots" and
%  the 00 colour with the "even slots".
%
%  Stage: predict
   DETAIL11 = Q0011A11 - synA11Qmax(Q0011A00, size(Q0011A11), cmin);        % Y2
   clear Q0011A11;
%  Stage: update
   APPROX00 = Q0011A00 + ...
     max(zeros(size(Q0011A00)), synA00Qmax(DETAIL11, size(Q0011A00), cmin));% X2
%
%  DETAIL11 presents the detail gridfunction w.r.t. Q0011
%  APPROX00 now represents the updated version of the approximation of Q0011
%
%  For the inverse transform DETAIL11 has to be stored:
   [C, S] = storeR( DETAIL11, lev+1, 'd', C, S);
   clear Q0011A00 DETAIL11;
%
%  At this point gridfunction DETAIL11 containing the DETAILS has been stored,
%  gridfunction APPROX00 contains the updated APPROXIMATION, on the (down-
%  sampled) rectangular grid and has to be stored as well if at the highest
%  scale.
%  Note that APPROX00 is downsampled onto a rectangular grid with dimensions of
%  half size of the original O.
   if lev+1 >= N
     [C, S] = storeR(APPROX00, lev+1, 'a', C, S);
%    It is obligatory that at least at one scale the Approximation has to be
%    stored or else the scheme cannot be inverted.
     clear APPROX00;
%    In the Lifting Scheme all scales have now been processed!
   else
%    We proceed to the next scale.
     O = APPROX00; clear APPROX00;
   end
end
%-----------------------------------------------------------------------------


function result = Main_Process(Img, n)
if ndims(Img) == 3
    I = rgb2gray(Img);
else
    I = Img;
end
g1 = [0 1 0
    0 1 0
    0 1 0];
g2 = [0 0 0
    1 1 1
    0 0 0];
g3 = [0 0 1
    0 1 0
    1 0 0];
g4 = [1 0 0
    0 1 0
    0 0 1];
g5 = [0 1 0
    1 1 1
    0 1 0];
Gi1 = Multi_Process(I, g1, n);
Gi2 = Multi_Process(I, g2, n);
Gi3 = Multi_Process(I, g3, n);
Gi4 = Multi_Process(I, g4, n);
Gi5 = Multi_Process(I, g5, n);
G{1} = Gi1;
G{2} = Gi2;
G{3} = Gi3;
G{4} = Gi4;
G{5} = Gi5;
ua1 = Coef(Gi1, G);
ua2 = Coef(Gi2, G);
ua3 = Coef(Gi3, G);
ua4 = Coef(Gi4, G);
ua5 = Coef(Gi5, G);
u = [ua1, ua2, ua3, ua4, ua5];
u = u/sum(u);
Gf1 = Edge_One(G, u);
result = Gf1;


function sF = regularisation(nodes,y,lambda,varargin)
% computes a regularisation
% Syntax
%   sF = S2FunHarmonic.regularisation(S2Grid,f,lambda)
%   sF = S2FunHarmonic.regularisation(S2Grid,f,lambda,'bandwidth',
%        bandwidth,'node_weights',W,'fourier_weights',What)
%
% Input
%  S2Grid - grid on the sphere
%  f      - function values on the grid (may be multidimensional)
%  lambda - parameter for regularisation
%
% Options
%  bandwidth  - maximum harmonic degree
%  W          - weight w_n for the node nodes (default: Voronoi weights)
%  What       - weight what_{m,l} for the Fourier space (default Sobolev weights for s = 2)
%

[nodes,idx] = unique(nodes(:));
y = reshape(y(idx),length(nodes),[]);
N = length(nodes);
W = get_option(varargin,'node_weights');
if isempty(W)
  W = nodes.calcVoronoiArea;
end
bw = get_option(varargin,'bandwidth',floor(sqrt(2*pi/mean(W))));

What = get_option(varargin,'fourier_weights');
if isempty(What)
  s = 2;
  What = (2*(0:bw)+1).^(2*s);
  What = repelem(What,1:2:(2*bw+1))';
end

% initialize nfsft
nfsftmex('precompute',bw,1000,1,0);
plan = nfsftmex('init_advanced',bw,N,1);
nfsftmex('set_x',plan,[nodes.rho';nodes.theta']);
nfsftmex('precompute_x',plan);

y = W.*y;

% adjoint nfsft
nfsftmex('set_f',plan, y);
nfsftmex('adjoint',plan);
fhat = nfsftmex('get_f_hat_linear',plan);

[fhat,~] = pcg(@(x) afun(x,plan,lambda,W,What),fhat);

sF = S2FunHarmonic(fhat);

% finalize nfsft
nfsftmex('finalize',plan);

end

function y = afun(x,plan,lambda,W,What)
  nfsftmex('set_f_hat_linear',plan,x);
  nfsftmex('trafo',plan);
  y = nfsftmex('get_f',plan);

  y = W.*y;

  nfsftmex('set_f',plan,y);
  nfsftmex('adjoint',plan);
  y = nfsftmex('get_f_hat_linear',plan);

  y = y+lambda*What.*x;
end


function ans = calc_meanDose(doseBinsMidPtsV, volsHistV, volumeType)
%Calculate the mean dose for a given DVH
%  The last parameter 'volumeType' is a wash in this function again
%
%  MODIFICATION ALERT:  THIS FUNCTION IS UTILIZED BY THE DREXLER CODEBASE
%
%  LM: 6 Oct 06, JOD, corrected slight error in not taking middle of dose
%  bin.  Added warning if relative volume not close to one (0.5%
%  tolerance).
%      4 Apr 17, APA, passed the dose as min-points of bins. Hence, no need
%  to add half the binwidth to the passed dose. Call this function after
%  obtaining DVH from loadDVHMatrix.m, since it returns mid-points of dose bins.
%
% Usage: calc_meanDose(doseBinsMidPtsV, volsHistV)
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR 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.
%
% CERR 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 CERR.  If not, see <http://www.gnu.org/licenses/>.

% doseBinsMidPtsV = (doseBinsLowerPtsV(1:end-1)+doseBinsLowerPtsV(2:end))/2;
% ans = (sum(doseBinsMidPtsV.*volsHistV(1:end-1))+doseBinsLowerPtsV(end)*volsHistV(end))/sum(volsHistV);

ans = sum(doseBinsMidPtsV.*volsHistV)/sum(volsHistV);

return;


% SP_EXTERIOR_DERIVAITVE: computes the exterior derivative as a matrix with size
%  given by the dimension of two consecutive spaces in the De Rham sequence.
%
%   diff_op = sp_exterior_derivative (space1, space2);
%
% INPUT:
%
%   space1:  domain space of the exterior derivative (number of columns)
%   space2:  image space of the exterior derivative (number of rows)
%
% OUTPUT:
%
%   diff_op: sparse matrix representation of the differential operator.
%
% Copyright (C) 2020-2023 Bernard Kapidani, Rafael Vazquez
%
%    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 diff_op = sp_exterior_derivative (space1, space2)

  assert (~strcmpi(space1.transform, 'integral-preserving'), ...
    'The first space cannot be the one for integral-preserving splines (or n-forms)')

  ndim = numel (space1.scalar_spaces{1}.knots);
  if (ndim == 1)
    error ('Not implemented. For dimension 1, use the scalar spaces')
  elseif (ndim == 2)
    if (strcmpi (space1.transform, 'curl-preserving'))
      grad_curl = 'grad';
      output_der = 2;
      deg_shift = {[0 1], [1 0]};
      knt_shift = {[0 2], [2 0]};
      degree = space1.scalar_spaces{1}.degree + [1 0];
      knots = {space1.scalar_spaces{2}.knots{1}, space1.scalar_spaces{1}.knots{2}};
    elseif (strcmpi (space1.transform, 'div-preserving'))
      grad_curl = 'curl';
      output_der = 2;
      deg_shift = {[1 0], [0 1]};
      knt_shift = {[2 0], [0 2]};
      degree = space1.scalar_spaces{1}.degree + [0 1];
      knots = {space1.scalar_spaces{1}.knots{1}, space1.scalar_spaces{2}.knots{2}};
    else
      error ('The second space should be either curl-preserving or div-preserving')
    end
    assert (isa(space2, 'sp_scalar'), 'The two spaces are not compatible')
    for idim = 1:ndim
      assert (all(space1.scalar_spaces{idim}.degree == space2.degree+deg_shift{idim}), 'The degrees are not compatible')
      assert (all(cellfun(@numel,space1.scalar_spaces{idim}.knots) == (cellfun(@numel, space2.knots)+knt_shift{idim})), ...
        'The knot vectors are not compatible')
    end
  elseif (ndim == 3)
    grad_curl = 'grad';
    if (strcmpi (space1.transform, 'curl-preserving'))
      output_der = 2;
      deg_shift = {[-1 1 1], [1 -1 1], [1 1 -1]};
      knt_shift = {[-2 2 2], [2 -2 2], [2 2 -2]};
      degree = space1.scalar_spaces{1}.degree + [1 0 0];
      knots = {space1.scalar_spaces{2}.knots{1}, space1.scalar_spaces{1}.knots{2}, space1.scalar_spaces{1}.knots{3}};
      assert (isa(space2, 'sp_vector'), 'The two spaces are not compatible')
      for idim = 1:ndim
        assert (all(space1.scalar_spaces{idim}.degree == space2.scalar_spaces{idim}.degree+deg_shift{idim}), 'The degrees are not compatible')
        assert (all(cellfun(@numel,space1.scalar_spaces{idim}.knots) == (cellfun(@numel, space2.scalar_spaces{idim}.knots)+knt_shift{idim})), ...
          'The knot vectors are not compatible')
      end
    elseif (strcmpi (space1.transform, 'div-preserving'))
      output_der = 3;
      deg_shift = {[1 0 0], [0 1 0], [0 0 1]};
      knt_shift = {[2 0 0], [0 2 0], [0 0 2]};
      degree = space1.scalar_spaces{1}.degree + [0 1 1];
      knots = {space1.scalar_spaces{1}.knots{1}, space1.scalar_spaces{2}.knots{2}, space1.scalar_spaces{3}.knots{3}};
      assert (isa(space2, 'sp_scalar'), 'The two spaces are not compatible')
      for idim = 1:ndim
        assert (all(space1.scalar_spaces{idim}.degree == space2.degree+deg_shift{idim}), 'The degrees are not compatible')
        assert (all(cellfun(@numel,space1.scalar_spaces{idim}.knots) == (cellfun(@numel, space2.knots)+knt_shift{idim})), ...
          'The knot vectors are not compatible')
      end
    else
      error ('Only implemented for curl-preserving and div-preserving transforms')
    end
  end

  diff_ops = op_geom_exterior (knots, degree, grad_curl);
  diff_op = diff_ops{output_der};
end


function em = errormatrix(fet,clu)

cluIx = unique(clu);
nClu = length(cluIx);
nDim = size(fet,2);

mu = zeros(nClu,nDim);
sigma = zeros(nDim,nDim,nClu);
p = zeros(1,nClu);
for ii=1:nClu
    f = fet(clu==cluIx(ii),:);
    % Mean fet vector
    mu(ii,:) = mean(f);
    % Covariance. We add 1e-5 to the diagonal to ensure that it will be
    % positive definite
    sigma(:,:,ii) = cov(f)+1e-5*eye(nDim,nDim);
    p(ii) = sum(clu==cluIx(ii));
end
obj = gmdistribution(mu,sigma,p);
%keyboard

em = zeros(nClu,nClu);

for ii=1:nClu
    p = posterior(obj,fet(clu==cluIx(ii),:));
    %keyboard
    em(ii,:) = mean(p);
end


function ll = dnetLogLikelihood(model)

% DNETLOGLIKELIHOOD Density network log likelihood.
% FORMAT
% DESC computes the log likelihood of a density network
% model.
% ARG model : the model structure for computing the log likelihood.
% RETURN ll : the model log likelihood.
%
% SEEALSO : modelLogLikeihood
%
% COPYRIGHT : Neil D. Lawrence, 2008

% MLTOOLS

ll = 0.5*model.d*model.N*log(model.beta/(2*pi)) ...
     - model.N*log(model.M);

ll = ll - 0.5*model.alpha*sum(sum(model.A.*model.A)) ...
     - 0.5*model.alpha*sum(model.b.*model.b);

llPointComp = zeros(model.N, model.M);
% Get projections of latent samples.
Ypred = dnetOut(model);

if model.N > model.M
  for i = 1:model.M
    diffY = model.y - repmat(Ypred(i, :), model.N, 1);
    diffY = diffY.*diffY;
    llPointComp(:, i) = - 0.5*model.beta*sum(diffY, 2);
  end
else
  for i = 1:model.N
    diffY = repmat(model.y(i, :), model.M, 1) - Ypred;
    diffY = diffY.*diffY;
    llPointComp(i, :) = - 0.5*model.beta*sum(diffY, 2)';
  end
end

maxllPointComp = max(llPointComp, [], 2);
llPointComp = exp(llPointComp - repmat(maxllPointComp, 1, model.M));
ll = ll + sum(log(sum(llPointComp, 2)) + maxllPointComp);


function [imgOut, FC_MAX_L] = FalseColor(img, FC_compress, FC_Vis, FC_LMax, FC_figure, FC_title, FC_lin, FC_title_color_map)
%
%
%       [imgOut, FC_MAX_L] = FalseColor(img, compress, FC_Vis, LMax)
%
%       This function creates a false color image for re-mapping luminance
%       into an LDR RGB image.
%
%       Input:
%           -img: the input img
%           -FC_compress: compression option for HDR images:
%               -'lin': no compression to the dynamic range (it typically creates
%               good results for LDR images only!)
%               -'log': the HDR domain is compressed using natural
%               logarithm (default parameter)
%               -'log2': the HDR domain is compressed using base 2
%               logarithm
%               -'log10': the HDR domain is compressed using base 10
%               logarithm
%               -'sigmoid': the HDR domain is compressed using a basic
%               sigmoid curve
%           -FC_Vis: a boolean parameter. If it is set to 1, it will show.
%           Default values is 1, so the image will visualized
%           the image as a complete figure including the visualization bar
%           -FC_LMax: the maximum luminance for the color re-mapping functions.
%               This needs to be used when creating false color images with the same scale.
%           -FC_figure: index for the figure
%           -FC_title: title for the false color window
%           -FC_lin: linear scale or exponential one.
%           -FC_title_color_map: color map unit
%
%       Output:
%           -imgOut: the false color LDR and RGB image (no gamma is
%           required for visualization purposes)
%           -FC_MAX_L: the maxium luminance value in the selected
%           FC_compress domain
%
%     Copyright (C) 2011-17 Francesco Banterle
%
%     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/>.
%

L = lum(img);

if(~exist('FC_Vis', 'var'))
    FC_Vis = 1;
end

if(~exist('FC_compress', 'var'))
    FC_compress = 'log';
end

if(~exist('FC_figure', 'var'))
    FC_figure = 1;
end

if(~exist('FC_title', 'var'))
    FC_title = 'False color visualization';
end

if(~exist('FC_lin', 'var'))
    FC_lin = 0;
end

if(~exist('FC_title_color_map', 'var'))
    FC_title_color_map = 'Lux';
end

%minimum luminance
LMin = min(L(:));

%maximum luminance
if(~exist('FC_LMax', 'var'))
    LMax = max(L(:));
else
    if(FC_LMax < 0)
        LMax = max(L(:));
    else
        tLMax = max(L(:));
        if(FC_LMax < tLMax)
            LMax = tLMax;
        else
            LMax = FC_LMax - LMin;
        end
    end
end

FC_MAX_L = LMax;

%luminance compression
epsilon = 1e-6; %for avoiding singularities
switch FC_compress
    case 'log2'
        L = log2(L + epsilon);
        LMax = log2(LMax + epsilon);
        LMin = log2(LMin + epsilon);

    case 'log'
        L = log(L + epsilon);
        LMax = log(LMax + epsilon);
        LMin = log(LMin + epsilon);

    case 'log10'
        L = log10(L + epsilon);
        LMax = log10(LMax + epsilon);
        LMin = log10(LMin + epsilon);

    case 'sigmoid'
        L = (L ./ (L + 1.0)).^(1.0 / 2.2);
        LMax = (LMax ./ (LMax + 1.0)).^(1.0 / 2.2);
        LMin = (LMin ./ (LMax + 1.0)).^(1.0 / 2.2);

    otherwise
end

%creating ticks for the visualization
if(FC_Vis)
    delta = LMax - LMin;
    yticks = LMin:(delta / 4):LMax;

    switch FC_compress
        case 'log2'
            yticks = 2.^yticks - epsilon;
        case 'log'
            yticks = exp(yticks) - epsilon;
        case 'log10'
            yticks = 10.^yticks - epsilon;
        case 'sigmoid'
            yticks = yticks.^2.2;
            yticks = yticks ./ (1.0 - yticks);
        otherwise
    end
end

L = L - LMin;
LMax = LMax - LMin;
L = L / LMax;

contrast = 1.0;
L = L.^(1.0 / contrast);

%Create a color map
n_bit = 8;
res = 2^n_bit;
color_map = colormap(jet(res));
%color_map = ldrimread('fc_colormap.png');

%Coloring using the colormap
L = ClampImg(round(L * res), 1, res);
imgOut = ind2rgb(L, color_map);

if(FC_Vis)%Visualization
    close(FC_figure);
    h = figure(FC_figure);
    axes1 = axes('Parent', h);
    axis off
    hold(axes1,'on');

    set(h, 'Name', FC_title);

    image(imgOut, 'Parent', axes1);%'InitialMagnification', 'fit');
    colormap(color_map);

    box(axes1,'on');
    axis(axes1,'ij');
    set(axes1,'DataAspectRatio',[1 1 1],'Layer','top','TickDir','out');

    if(FC_lin)
        str = {sprintf('%2.1f', yticks(1)), sprintf('%2.1f', yticks(2)), sprintf('%2.1f', yticks(3)), sprintf('%2.1f', yticks(4)), sprintf('%2.1f', yticks(5))};
    else
        str = {sprintf('%2.1e', yticks(1)), sprintf('%2.1e', yticks(2)), sprintf('%2.1e', yticks(3)), sprintf('%2.1e', yticks(4)), sprintf('%2.1e', yticks(5))};
    end

    hcb = colorbar('peer',axes1, 'Position',...
    [0.743515850144089 0.299424184261036 0.0374639769452479 0.566218809980805],...
    'Ticks',[0 0.25 0.5 0.75 1],...
    'Ticks', 0:(1/4):1 ,'YtickLabel', str);%, 'Position',...
%    [0.329470198675497 0.345738295318127 0.302152317880795 0.0261944917126994]);

    set(hcb, 'FontSize', 24);
    set(hcb, 'FontName', 'Times New Roman');

    pos = hcb.Position;
    set(get(hcb, 'XLabel'), 'Rotation', 0, 'String', FC_title_color_map, 'FontSize', 24, 'FontName', 'Times New Roman', 'Position', [pos(1), 0.0 , 0.0]);
    hold(axes1,'off');

end

end


% MAIN.m
%
% Solve the cart-pole swing-up problem  --  minimum time
%
% Note:  This problem is much more difficult to solve than the
% minimum-force version. This is because most of the control trajectory is
% sitting on a constraint: the maximum or minimum control force. This is
% generally true of minimum-time trajectories: they have bang-bang
% solutions. To get the exact solution, you would need to do many steps of
% mesh refinement. Here I only do two iterations, to keep total time
% reasonable. Another problem with minimum-time objective functions is that
% they sometimes have singular arcs: solutions where there is no single
% best control trajectory. This will manifest itself as "chattering" in the
% control trajectory and slow convergence. One solution is to include a
% regularization term, such as force squared with a very small coefficient,
% which forces a unique solution along the singular arc.
%

clc; clear;
addpath ../../

p.m1 = 2.0;  % (kg) Cart mass
p.m2 = 0.5;  % (kg) pole mass
p.g = 9.81;  % (m/s^2) gravity
p.l = 0.5;   % (m) pendulum (pole) length

dist = 1.0;  %How far must the cart translate during its swing-up
maxForce = 50;  %Maximum actuator forces

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                     Set up function handles                             %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

problem.func.dynamics = @(t,x,u)( cartPoleDynamics(x,u,p) );
problem.func.pathObj = @(t,x,u)( ones(size(t)) );

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                     Set up problem bounds                               %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

problem.bounds.initialTime.low = 0;
problem.bounds.initialTime.upp = 0;
problem.bounds.finalTime.low = 0.01;
problem.bounds.finalTime.upp = inf;

problem.bounds.initialState.low = zeros(4,1);
problem.bounds.initialState.upp = zeros(4,1);
problem.bounds.finalState.low = [dist;pi;0;0];
problem.bounds.finalState.upp = [dist;pi;0;0];

problem.bounds.state.low = [-2*dist;-2*pi;-inf;-inf];
problem.bounds.state.upp = [2*dist;2*pi;inf;inf];

problem.bounds.control.low = -maxForce;
problem.bounds.control.upp = maxForce;

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                    Initial guess at trajectory                          %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

problem.guess.time = [0,2];
problem.guess.state = [problem.bounds.initialState.low, problem.bounds.finalState.low];
problem.guess.control = [0,0];

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                         Solver options                                  %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

problem.options(1).nlpOpt = optimset(...
    'Display','iter',...
    'TolFun',1e-3,...
    'MaxFunEvals',1e5);
problem.options(1).method = 'trapezoid';
problem.options(1).trapezoid.nGrid = 10;

problem.options(2).nlpOpt = optimset(...
    'Display','iter',...
    'TolFun',1e-6,...
    'MaxFunEvals',1e5);
problem.options(2).method = 'trapezoid';
problem.options(2).trapezoid.nGrid = 30;

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                            Solve!                                       %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

soln = optimTraj(problem);

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                        Display Solution                                 %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

%%%% Unpack the simulation
t = linspace(soln(end).grid.time(1), soln(end).grid.time(end), 150);
z = soln(end).interp.state(t);
u = soln(end).interp.control(t);

%%%% Plots:

%%%% Draw Trajectory:
[p1,p2] = cartPoleKinematics(z,p);

figure(2); clf;
nFrame = 9;  %Number of frames to draw
drawCartPoleTraj(t,p1,p2,nFrame);

%%%% Show the error in the collocation constraint between grid points:
%
if strcmp(soln(end).problem.options.method,'trapezoid') || strcmp(soln(end).problem.options.method,'hermiteSimpson')
    % Then we can plot an estimate of the error along the trajectory
    figure(5); clf;

    % NOTE: the following commands have only been implemented for the direct
    % collocation(trapezoid, hermiteSimpson) methods, and will not work for
    % chebyshev or rungeKutta methods.
    cc = soln(end).interp.collCst(t);

    subplot(2,2,1);
    plot(t,cc(1,:))
    title('Collocation Error:   dx/dt - f(t,x,u)')
    ylabel('d/dt cart position')

    subplot(2,2,3);
    plot(t,cc(2,:))
    xlabel('time')
    ylabel('d/dt pole angle')

    idx = 1:length(soln(end).info.error);
    subplot(2,2,2); hold on;
    plot(idx,soln(end).info.error(1,:),'ko');
    title('State Error')
    ylabel('cart position')

    subplot(2,2,4); hold on;
    plot(idx,soln(end).info.error(2,:),'ko');
    xlabel('segment index')
    ylabel('pole angle');
end

%%%% Plot the state and control against time
figure(1); clf;
plotPendulumCart(t,z,u,p);


% Demonstration of generative model functions.
%
% See GENERATIVE_MODEL and EVALUATE_GENERATIVE_MODEL for further details
% and interpretation.

clear
close all
clc

data = load('demo_generative_models_data');
A     = data.A;
Aseed = data.Aseed;
D     = data.D;

% get cardinality of network
n = length(A);

% set model type
modeltype = 'sptl';

% set whether the model is based on powerlaw or exponentials
modelvar = [{'powerlaw'},{'powerlaw'}];

% choose some model parameters
nparams = 100;
params = unifrnd(-10,0,nparams,1);

% generate synthetic networks and energy for the neighbors model;
[B,E,K] = evaluate_generative_model(Aseed,A,D,modeltype,modelvar,params);
X = [E,K];

% show scatterplot of parameter values versus energy and KS statistics
names = [...
    {'energy'},...
    {'degree'},...
    {'clustering'},...
    {'betweenness'},...
    {'edge length'}];

f = figure(...
    'units','inches',...
    'position',[2,2,4,4]);
for i = 1:size(X,2)
    subplot(3,2,i);
    scatter(params,X(:,i),100,X(:,i),'filled');
    set(gca,...
        'ylim',[0,1],...
        'clim',[0,1]);
    colormap(jet);
    xlabel('geometric parameter, \eta');
    ylabel(names{i});
end


function [ theta,f1,m,W_new,beta,grt ] = armijo_theta_fullcycle_v3( Ltheta,dir,f0,phi0, grad,grt,W,K,M,Pt,omega,Hd,Hr,G)
    m=0;
    rhom=0.95;
    rho0=1/Ltheta*100;
    len=-real(grad'*dir);
    sig=0.4;
    while(1)
        rho=rho0*rhom^m;
        phi=phi0-rho*grad;
        x=exp(1j.*phi);
        [f1,W_new,beta,grt] = par_fun_B(x,W,K,M,Pt,omega,Hd,Hr,G);
        if (f1-f0)>=sig*rho*len
            break
        end
        if (rho)<1/Ltheta/10
            break
        end
        m=m+1;
    end
    theta=x;
end


classdef MW13 < PROBLEM
% <multi> <real> <large/none> <constrained>
% Constrained benchmark MOP proposed by Ma and Wang

%------------------------------- Reference --------------------------------
% Z. Ma and Y. Wang, Evolutionary constrained multiobjective optimization:
% Test suite construction and performance comparisons. IEEE Transactions on
% Evolutionary Computation, 2019, 23(6): 972-986.
%------------------------------- 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".
%--------------------------------------------------------------------------

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 2;
            if isempty(obj.D); obj.D = 15; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values and constraint violations
        function Population = Evaluation(obj,varargin)
            X = varargin{1};
            X = max(min(X,repmat(obj.upper,size(X,1),1)),repmat(obj.lower,size(X,1),1));
            z = 1 - exp(-10*(X(:,obj.M:end) - (repmat(obj.M:obj.D,size(X,1),1) - 1)/obj.D).^2);
            g = 1 + sum((1.5 + (0.1/obj.D)*z.^2 - 1.5*cos(2*pi*z)),2);
            PopObj(:,1) = g.*X(:,1)*1.5;
            PopObj(:,2) = g.*(5 - exp(PopObj(:,1)./g) - abs(0.5*sin(3*pi*PopObj(:,1)./g)));
            PopCon(:,1) = (5 - exp(PopObj(:,1)) - 0.5*sin(3*pi*PopObj(:,1)) - PopObj(:,2)).*(5 - (1 + 0.4*PopObj(:,1)) - 0.5*sin(3*pi*PopObj(:,1)) - PopObj(:,2));
            PopCon(:,2) = -(5 - (1 + PopObj(:,1) + 0.5*PopObj(:,1).^2) - 0.5*sin(3*pi*PopObj(:,1)) - PopObj(:,2)).*(5 - (1 + 0.7*PopObj(:,1)) - 0.5*sin(3*pi*PopObj(:,1)) - PopObj(:,2));
            Population  = SOLUTION(X,PopObj,PopCon,varargin{2:end});
            obj.FE      = obj.FE + length(Population);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R(:,1)  = (0:1.5/(N-1):1.5)';
            R(:,2)  = 5 - exp(R(:,1)) - 0.5*abs(sin(3*pi*R(:,1)));
            c1      = (5-exp(R(:,1))-0.5*sin(3*pi*R(:,1))-R(:,2)).*(5-(1+0.4*R(:,1))-0.5*sin(3*pi*R(:,1))-R(:,2));
            invalid = c1>0;
            while any(invalid)
                R(invalid,:) = R(invalid,:).*1.001;
                c1      = (5-exp(R(:,1))-0.5*sin(3*pi*R(:,1))-R(:,2)).*(5-(1+0.4*R(:,1))-0.5*sin(3*pi*R(:,1))-R(:,2));
                invalid = c1>0;
            end
            R = R(NDSort(R,1)==1,:);
        end
        %% Generate the feasible region
        function R = GetPF(obj)
            [x,y] = meshgrid(linspace(0,2,400),linspace(0,4.5,400));
            z     = nan(size(x));
            fes1  = (5-exp(x)-0.5*sin(3*pi*x)-y).*(5-(1+0.4*x)-0.5*sin(3*pi*x)-y) <= 0;
            fes2  = -(5-(1+x+0.5*x.^2)-0.5*sin(3*pi*x)-y).*(5-(1+0.7*x)-0.5*sin(3*pi*x)-y) <= 0;
            z(fes1 & fes2 & exp(x)+abs(0.5*sin(3*pi*x))+y>=5) = 0;
            R = {x,y,z};
        end
    end
end


function f = sum( f, dim )
%SUM   Definite Integration of a DISKFUN.
%   G = sum(F,DIM) where DIM is 1 or 2 integrates only over theta
%   (angular direction) or r (radial direction) respectively,
%   and returns as its output a chebfun in the remaining variable.
%
%   G = sum(F) is the same as sum(F,1)
%
% See also SUM2.

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

% Empty check:
if ( isempty( f ) )
    f = [];
    return;
end

% Default to radial direction:
if ( nargin == 1 )
    dim = 1;
end

% Get the low rank representation for f.
[cols, D, rows] = cdr(f);
dom = f.domain;
%restrict cols to match domain

if ( dim == 1 )
    % Integrate over r. Need to include the measure on the disk.
    r = chebfun('x');
    cols = r.*cols;
    cols = restrict(cols, [0 1]);
    f = rows * ( sum(cols) * D ).';
    if ( isa(f, 'chebfun') )
        f = simplify( f, [], 'globaltol' );
    else
        % f = double
        f = chebfun(f, dom(1:2)).';
    end
elseif ( dim == 2 )
    f = cols * ( D * sum( rows ).' );
    if  ( isa(f, 'chebfun') )
        f = simplify( f.', [], 'globaltol' );
    else
        f = chebfun( f, dom(3:4) );
    end
else
    error('CHEBFUN:DISKFUN:sum:unknown', ...
          'Undefined function ''sum'' for that dimension');
end

end


%% alternating minimization scheme for low-rank+sparse decomposition

Z_norm=norm(Z(:));
stat.ener=[]; stat.ener(1)=vm.f(A,B);
if input_sig==1
A_norm=norm(A_0(:));
B_norm=norm(B_0(:));
stat.err_a=[]; stat.err_a(1)=norm(A(:)-A_0(:))/A_norm;
stat.err_b=[]; stat.err_b(1)=norm(B(:)-B_0(:))/B_norm;
end
stat.f_a=[]; stat.f_a(1)=norm(proj_fr(A+B-Z,U,V),'fro');
% stat.f_b=[]; %stat.f_b(1)=norm(proj_l0(A+B-Z,B),'fro');
% stat.ts1=[]; stat.ts2=[];
% stat.vi1=[]; stat.vi2=[];
stat.ang1=[]; stat.ang2=[];
stat.ls_count=[];
stat.cg_count=[];
stat.n3=[]; stat.n3(1)=n3;
stat.n4=[]; stat.n4(1)=n4;
stat.sfg_a=[]; stat.sfg_b=[];
stat.cpu=[]; %stat.cpu=time_main;
if ~isempty(A_ref), stat.diff_a=[]; stat.diff_a=norm(A(:)-A_ref(:))/A_norm; end
if ~isempty(B_ref), stat.diff_b=[]; stat.diff_b=norm(B(:)-B_ref(:))/B_norm; end
A_old=A; B_old=B;

% I1=eye(n1); I2=eye(n2);
waiting=waitbar(0,'iterating...');
%time_main=tic;
for k=1:slv.kmax
    waiting=waitbar(k/slv.kmax);

    % A-subproblem
    if sg.sig~=1
    % first shot via partial SVD
    [U,S,V]=lansvd(Z-B,n3);
    A=U*S*V';

    % safeguard by projected dogleg method
    v1=A_old(:)-A(:);
    stat.ang1(end+1)=(vm.f(A_old,B)-vm.f(A,B))/norm(v1(:))^2;

    if sg.sig==2 && stat.ang1(end)<sg.thr
        A=A_old;
        stat.sfg_a(end+1)=1;
        disp(['A safeguarded by Riemannian optim at iter ',num2str(k)])
        riem_optim
    else
        stat.sfg_a(end+1)=0;
    end

    else    % directly via projected dogleg method
        riem_optim
    end

    % B-subproblem by sorting
    B=rtr_l0(Z-A,n4);

    % safeguard
    v1=B_old(:)-B(:);
    stat.ang2(end+1)=(vm.f(A,B_old)-vm.f(A,B))/norm(v1(:))^2;

    if sg.sig~=0 && stat.ang2(end)<sg.thr
        B=Z-A; B(B_old==0)=0;
        stat.ang2(end)=.5;
        stat.sfg_b(end+1)=1;
        disp(['B safeguarded at iter ',num2str(k)])
    else
        stat.sfg_b(end+1)=0;
    end

%     fprintf('|A^k-A^{k-1}|=%1.4e\n',norm(A(:)-A_old(:)))

    % trimming
    trimming

    % stats
    %stat.cpu(end+1)=stat.cpu(1)+toc(time_main);
    stat.ener(end+1)=vm.f(A,B);
    if input_sig==1
    stat.err_a(end+1)=norm(A(:)-A_0(:))/A_norm;
    stat.err_b(end+1)=norm(B(:)-B_0(:))/B_norm;
    end
    stat.f_a(end+1)=norm(proj_fr(vm.g(A,B_old),U,V),'fro');
%     stat.f_b(end+1)=norm(proj_l0(A+B-Z,B),'fro');
    if ~isempty(A_ref), stat.diff_a(end+1)=norm(A(:)-A_ref(:))/A_norm; end
    if ~isempty(B_ref), stat.diff_b(end+1)=norm(B(:)-B_ref(:))/B_norm; end
    stat.n3(end+1)=n3; stat.n4(end+1)=n4;

%     if sg.sig==1, cg.tol=1e-2*(stat.f_a(end)/stat.f_a(1))^.2; end

    % stopping rule
    if stat.f_a(end)<slv.ktol*stat.f_a(1)
%     if stat.err_a(end)<slv.ktol
% 	if stat.ener(end-1)-stat.ener(end)<slv.ktol*stat.ener(end-1)
        break
    end

    A_old=A;
    B_old=B;
end
%toc(time_main)
close(waiting)

% [sum(stat.ls_count),sum(stat.cg_count)]


function [TAR,FAR,threshold] = rocplot(match, nonmatch)
%TAR=Ver

%match=-match;
%nonmatch=-nonmatch;

 R1=min([match; nonmatch]);
 R2=max([nonmatch;match]);

a=100/(R2-R1);
b=-a*R1;

matchN=a*match+b;
matchN(matchN<0)=0;
nonmatch_score=a*nonmatch+b;
TAR=[1:length(matchN)]'/length(matchN);
FRR=1-TAR;

nonmatchL=zeros(size(matchN));
smatch=sort(matchN);
for i=1:length(nonmatch_score)
    low=1;
    high=length(smatch);
	while low <=high
		mid=ceil((low+high)*.5);
%[nonmatch_score(i),smatch(mid)]
		if nonmatch_score(i)>smatch(mid) low=mid+1;
		else high=mid-1;
		end
	end

 %   [val,idx]=min((nonmatch_score(i)>smatch));
 %    nonmatchL(idx)=nonmatchL(idx)+1;
	if low > length(smatch)
	low= length(smatch);
	end
    nonmatchL(low)=nonmatchL(low)+1;
end
cnonmatchL=cumsum(nonmatchL);
FAR=cnonmatchL/cnonmatchL(end);
iind=find(FAR>=0.1);
VER1= TAR(iind(1));
iind=find(FAR>=0.01);
VER01= TAR(iind(1));
iind=find(FAR>=0.001);
VER001= TAR(iind(1));

threshold=(smatch-b)/a;


%+========================================================================+
%|                                                                        |
%|            This script uses the GYPSILAB toolbox for Matlab            |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%|             francois.alouges@polytechnique.edu                         |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : nrtDomND.m                                    |
%|    #    |   VERSION    : 0.40                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 14.03.2018                                    |
%| ( === ) |   SYNOPSIS   : Multi-dimensional quadrature                  |
%|  `---'  |                                                              |
%+========================================================================+

% Cleaning
clear all
close all
clc

% Gypsilab path
run('../../addpathGypsilab.m')

% Tetra mesh
Nvtx = 1e2;
L    = [5 4 3];
mesh = mshCube(Nvtx,L);

% Boundary mesh
bound = mesh.bnd;
ctr   = bound.ctr;
bound.col(ctr(:,3)==0.5*L(3)) = 1;

% Upperface
upperface = bound.sub(bound.col==1);
rectangle = upperface.bnd;

% Domain
omega = dom(mesh,5);
figure
plot(mesh)
hold on
plot(omega)
alpha(0.2)
axis equal
view(30,30)

% Boundary domain
sigma = dom(bound,3);
figure
plot(bound)
hold on
plot(sigma,'.y')
plotNrm(sigma,'y')
alpha(0.5)
axis equal
view(30,30)

% Rectangle
gamma = dom(rectangle,3);
figure
plot(rectangle)
hold on
plot(gamma)
alpha(0.5)
axis equal
view(30,30)

% Numerical integration on the cube
fct = @(X) ones(size(X,1),1);
V   = integral(omega,fct);
norm(V-prod(L),'inf')

% Numerical integration on the bundary of the cube
fct = @(X) ones(size(X,1),1);
S   = integral(sigma,fct);
norm(S-2*(L(1)*L(2)+L(2)*L(3)+L(1)*L(3)),'inf')

% Numerical integration on the rectangle
fct = @(X) ones(size(X,1),1);
P   = integral(gamma,fct);
norm(P-2*(L(1)+L(2)),'inf')

disp('~~> Michto gypsilab !')


function M = oned_bilinear ( kernel, phi, test, w_g )

%*****************************************************************************80
%
%% ONED_BILINEAR integrates a kernel times a basis times a test function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 December 2012
%
%  Author:
%
%    Jeff Borggaard
%
%  Parameters:
%
%    Input, real KERNEL(N_GAUSS), the kernel function evaluated
%    at the Gauss points.
%
%    Input, real PHI(N_GAUSS,N_DOF), the element test functions evaluated
%    at the Gauss points.
%
%    Input, real TEST(N_GAUSS,N_TEST), the test functions evaluated
%    at the Gauss points.
%
%    Input, real W_G(N_GAUSS), the Gauss weights.
%
%    Output, real M(N_TEST,N_DOF), the integral values.
%
  M = test' * diag ( kernel .* w_g ) * phi;

  return
end


function [T1, M0] = Compute_M0_T1_OnSPGR(data, flipAngles, TR, b1Map, roi, verbose)
%Perform a simple linear-least squares data fit on variable flip angle SPGR data
%
% function [M0, T1] = Compute_M0_T1_OnSPGR(data, flipAngles, TR [, b1Map, roi, verbose])
% -----------------------------------------------------------
% INPUTS:
%   data: width x length x slices x flipAngles matrix
%   flipAngles: vector of flip angles (in degrees) corresponding to last dimension of 'data'
%   TR: Repetition time in seconds
%   b1Map: width x length x slices matrix containing relative flip angle
%         (i.e. if nominal alpha is 60 and measured alpha is 61, then b1Map = 61/60
%   roi: width x length x slices binary mask
%   verbose: logical - for debugging

if ndims(data)<3, data = permute(data(:),[2 3 4 1]); end
[nX, nY, nZ, nFlip] = size(data);
nVox = nX*nY*nZ;

if ~exist('b1Map', 'var') || isempty(b1Map)
    b1Map = ones(nX, nY, nZ);
end

if ~exist('roi', 'var') || isempty(roi)
    roi = true(nX, nY, nZ);
end

if ~exist('verbose', 'var')
    verbose = 0;
end

if length(b1Map) ~= length(data(:,:,:,1)), error('B1 size is different from data size'); end
if ~islogical(roi), roi = logical(roi); end

% Reshape data into 2D array so that future steps are simpler
data = reshape(data,[nVox nFlip])'; % Transpose because MATLAB is column-major
data = data(:, roi(:));
nVox = sum(roi(:));

% Large matrix with redundant values will make computations faster,
% but will also consume more memory
alpha = deg2rad(flipAngles);
if isrow(alpha), alpha = alpha'; end
alpha = repmat(alpha,[1 nVox]) .* repmat(b1Map(roi(:))', [nFlip 1]);

% Do the linear least squares fit and estimate M0 & T1
y = data ./ sin(alpha);
x = data ./ tan(alpha);
[fittedSlope, fittedIntercept] = LinLeastSquares(x,y);
estM0 = fittedIntercept ./ (1-fittedSlope);
estT1 = -TR./log(fittedSlope);

% Assign arbitrary M0 & T1 value if fitted value is unphysical.
% Might be better to set this to NaN so users can identify voxels where fit fails
failedFit = isnan(fittedSlope)      | fittedSlope<0     | ...
            isnan(fittedIntercept)  | fittedIntercept<0 ;
failedFitValue = NaN;
estM0(failedFit) = failedFitValue;
estT1(failedFit) = failedFitValue;

% Assign estimated M0 and T1 values to correct voxel
[T1, M0] = deal(zeros(nX,nY,nZ));
M0(roi(:)) = estM0;
T1(roi(:)) = estT1;
end % END OF Compute_M0_T1_OnSPGR

function [fittedSlope, fittedIntercept] = LinLeastSquares(x,y)
% Simple linear least squares fit
% Inputs:
%   - x and y, arrays of equal sizes
% The first dimension should contain the measurements
% The second dimension could be different samples (e.g. voxels)

if size(x)~=size(y), error('X and Y must have same size for linear fitting'); end
% Use the fact that slope = cov(x,y) / cov(x,x)
lengthX = size(x,1);
numerator = sum(x.*y) - sum(x).*sum(y) / lengthX;
denominator = sum(x.^2) - sum(x).^2 / lengthX;
fittedSlope = numerator ./ denominator;
% Line of best fit has to pass through point (meanX, meanY)
% Use this fact to get intecept
fittedIntercept = mean(y) - fittedSlope .* mean(x);
end % END OF LinLeastSquares


function sphere_llq_grid_line_count_test ( )

%*****************************************************************************80
%
%% SPHERE_LLQ_GRID_LINE_COUNT_TEST tests SPHERE_LLQ_GRID_LINE_COUNT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 April 2015
%
%  Author:
%
%    John Burkardt
%
  lat_num = 3;
  long_num = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPHERE_LLQ_GRID_LINE_COUNT_TEST\n' );
  fprintf ( 1, '  SPHERE_LLQ_GRID_LINE_COUNT counts the lines used for a\n' );
  fprintf ( 1, '  grid based on quadrilaterals defined by latitude and longitude\n' );
  fprintf ( 1, '  lines on a sphere in 3D.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     LAT_NUM    LONG_NUM   LINE_NUM\n' );
  for lat_num = 1 : 2 : 17
    fprintf ( 1, '\n' );
    long_num = 1;
    for long_log = 1 : 4
      long_num = long_num * 2;
      line_num = sphere_llq_grid_line_count ( lat_num, long_num );
      fprintf ( 1, '  %8d  %8d  %8d\n', lat_num, long_num, line_num );
    end
  end

  return
end


function [ a, ipvt, info ] = zgefa ( a, lda, n )

%*****************************************************************************80
%
%% ZGEFA factors a complex matrix by Gaussian elimination.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 April 2007
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%
%  Parameters:
%
%    Input, complex A(LDA,N), the matrix to be factored.
%
%    Input, integer LDA, the leading dimension of A.
%
%    Input, integer N, the order of the matrix.
%
%    Output, complex A(LDA,N); an upper triangular matrix and the multipliers
%    which were used to obtain it.  The factorization can be written A = L*U
%    where L is a product of permutation and unit lower triangular matrices
%    and U is upper triangular.
%
%    Output, integer IPVT(N), the pivot indices.
%
%    Output, integer INFO,
%    0, normal value.
%    K, if U(K,K) == 0.0.  This is not an error condition for this
%    subroutine, but it does indicate that ZGESL or ZGEDI will divide by zero
%    if called.  Use RCOND in ZGECO for a reliable indication of singularity.
%

%
%  Gaussian elimination with partial pivoting.
%
  info = 0;

  for k = 1 : n - 1
%
%  Find L = pivot index.
%
    l = izamax ( n-k+1, a(k:n,k), 1 ) + k - 1;
    ipvt(k) = l;
%
%  Zero pivot implies this column already triangularized.
%
    if ( zabs1 ( a(l,k) ) == 0.0 )
      info = k;
      continue;
    end
%
%  Interchange if necessary.
%
    if ( l ~= k )
      temp = a(l,k);
      a(l,k) = a(k,k);
      a(k,k) = temp;
    end
%
%  Compute multipliers
%
    a(k+1:n,k) = - a(k+1:n,k) / a(k,k);
%
%  Row elimination with column indexing
%
    for j = k+1 : n
      t = a(l,j);
      if ( l ~= k )
        a(l,j) = a(k,j);
        a(k,j) = t;
      end
      a(k+1:n,j) = a(k+1:n,j) + t * a(k+1:n,k);
    end

  end

  ipvt(n) = n;

  if ( zabs1 ( a(n,n) ) == 0.0 )
    info = n;
  end

  return
end


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

function p = pnig(xnew, xold, t, r, alpha, beta, delta)
% probability distribution of the NIG model
d = delta * t;                          % time dependent parameter

gamma = sqrt(alpha^2 - beta^2);

% martingale correction removes the drift!
omega = delta * gamma - delta*sqrt(alpha^2-(1+beta)^2); % martingale correction

x = xnew -xold - (r - omega) * t;           %
arg = sqrt(1+(x./delta).^2);

p = real(alpha / pi * besselk(1,alpha .* d .* arg)./arg.* exp(d.*(gamma+beta.*(x./d))));
end


function aa=lpcrf2aa(rf)
%LPCRF2AA Convert reflection coefficients to area function AA=(RF)
%The areas are normalised so that aa(p+2)=1: the effective area of the free air beyond the lips.
% aa(1) is the area of the glottis. This will be zero if rf(:,1)=1.

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: lpcrf2aa.m,v 1.4 2007/05/04 07:01:39 dmb Exp $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

aa = fliplr(cumprod([ones(1,size(rf,1)); fliplr((1-rf)./(1+rf)).']).');


%  demo_RoPS_FeatureMatching_Mesh.m
%  Author: Yulan Guo {yulan.guo@nudt.edu.cn}
%  NUDT, China & CSSE, UWA, Australia
% This function performs feature matching on two input meshes to obtain feature
% correspondences

close all;
clc;
clear all;

keypntNum = 1000;

load ../../StanfordData/Bunny
SelIdx = [ 1 3];
pcData0 = bunny{SelIdx(1)}';
tmp = pcdownsample( pointCloud(pcData0'), 'gridAverage', 0.0015 );
pcData0 = tmp.Location';
pcData1 = bunny{SelIdx(2)}';
tmp = pcdownsample( pointCloud(pcData1'), 'gridAverage', 0.0015 );
pcData1 = tmp.Location';
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
load data\pointcloud_view1;
pointcloud = pcData0';
% %============================transform a pointcloud into a triangular mesh============================%
mesh = pointCloud2mesh(pointcloud,[0 0 1],0.4);                                         %other methods can also be used to perform triangulation
%============================preprocessing============================%
out = preprocessingFunc(mesh);
mesh.faceCenter = out.centroid;
mesh.faceArea = out.area;
mesh.res = out.res ;
%============================detect keypoints============================%
%keypoints are randomly seleted in this demo, any other 3D keypoint detection methods can be used
temp = randperm(length(mesh.vertices));
mesh.keypntIdx = temp(1:keypntNum);
%============================extract RoPS features at the keypoints on a mesh============================%
para.RoPS_nbSize = 15*mesh.res;
para.RoPS_binSize = 5;
para.RoPS_rotaSize = 3;
mesh.LRF =  LRFforMeshFunc(mesh, mesh.keypntIdx, para.RoPS_nbSize);
disp('LRFs calculated');
RoPS = RoPSFunc(mesh, para.RoPS_nbSize, para.RoPS_binSize, para.RoPS_rotaSize,mesh.LRF);
mesh.RoPS = RoPS;
disp(['RoPS features generated']);
mesh1 = mesh;
mesh1Features = [];
for keypntIdx = 1:keypntNum
    temp = trans2Dto1DFunc(mesh.RoPS{keypntIdx});
    mesh1Features = [mesh1Features; temp];
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
load data\pointcloud_view2;
pointcloud = pcData1';
% %============================transform a pointcloud into a triangular mesh============================%
mesh = pointCloud2mesh(pointcloud,[0 0 1],0.4);                                         %other methods can also be used to perform triangulation
%============================preprocessing============================%
out = preprocessingFunc(mesh);
mesh.faceCenter = out.centroid;
mesh.faceArea = out.area;
mesh.res = out.res ;
%============================detect keypoints============================%
%keypoints are randomly seleted in this demo, any other 3D keypoint detection methods can be used
temp = randperm(length(mesh.vertices));
mesh.keypntIdx = temp(1:keypntNum);
%============================extract RoPS features at the keypoints on a mesh============================%
para.RoPS_nbSize = 15*mesh.res;
para.RoPS_binSize = 5;
para.RoPS_rotaSize = 3;
mesh.LRF =  LRFforMeshFunc(mesh, mesh.keypntIdx, para.RoPS_nbSize);
disp('LRFs calculated');
RoPS = RoPSFunc(mesh, para.RoPS_nbSize, para.RoPS_binSize, para.RoPS_rotaSize,mesh.LRF);
mesh.RoPS = RoPS;
disp(['RoPS features generated']);
mesh2 = mesh;
mesh2Features = [];
for keypntIdx = 1:keypntNum
    temp = trans2Dto1DFunc(mesh.RoPS{keypntIdx});
    mesh2Features = [mesh2Features; temp];
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %============================feature matching============================%
NNDRthreshold = 0.9;
corNum = 0;
kdtreeMesh1Features = KDTreeSearcher(mesh1Features,'Distance','euclidean');
for keypntIdx1 = 1:size(mesh2Features,1)
    [idxSort,distSort] = knnsearch(kdtreeMesh1Features, mesh2Features(keypntIdx1,:),'k',2,'Distance','euclidean');
    IDX = idxSort(1);
    if distSort(1)/distSort(2)<=NNDRthreshold
        corNum = corNum+1;
        corPntIdx(corNum,:) = [IDX, keypntIdx1];
        featureDis(corNum) = distSort(1);
    end
end
showCorresFunc(mesh1, mesh2, mesh1.keypntIdx(corPntIdx(:,1)), mesh2.keypntIdx(corPntIdx(:,2)), [0,200,0]);

%============================links============================%
%we may find more test datasets via the following links
% url = 'https://sites.google.com/site/yulanguo66/research-resources/3d-object-recognition-datasets';
% web(url,'-browser')


function CPad = directFilter(A,D,tensorOrder,lambda)
% bsarray\directFilter: compute B-spline coefficients from data array
% usage: C = directFilter(A,D,tensorOrder,lambda);
%
% arguments:
%   A - N-dimensional array (vector, image, volume, etc.)
%   D (1xN vector) - degree of each dimension of tensor product BSpline.
%   tensorOrder (scalar) - number of nonsingleton dimensions of A
%   lambda (1xN vector) - smoothing factor for each dimension
%
%   C - array of BSpline coefficients
%

% author: Nathan D. Cahill
% email: ndcahill@gmail.com
% date: 18 April 2008

% if D = 0 or 1, and lambda = 0, return A, with reflection padding
if all(D<2) && all(lambda==0)
    padNum = floor(D/2);
    idx   = cell(1,tensorOrder);
    for k = 1:tensorOrder
        M = size(A,k);
        dimNums = [1:M (M-1):-1:2];
        p = padNum(k);
        idx{k}   = dimNums(mod(-p:M+p, 2*M-2) + 1);
    end
    CPad = A(idx{:});
    return;
end

% get coefficients for BSpline filters
F = cell(tensorOrder,1);
F0 = cell(tensorOrder,1);
for i=1:tensorOrder
    % NEED TO UPDATE THIS TO SCALE LAMBDA FOR ELEMENT SPACING
    [F{i},F0{i}] = getBSplineFiltCoeffs(D(i),true,'direct',lambda(i));
end

% initialize output
C = double(A);

% choose tolerance for K0
K0Tol = eps;

switch tensorOrder
    case 1 % X is a vector - easy case

        CLen = numel(C);

        % loop through poles of direct BSpline filter
        for i = 1:length(F{1})

            % compute K0 for current pole
            K0 = ceil(log(K0Tol)./log(abs(F{1}(i))));

            % now compute vector of indices of length K0 that runs back and
            % forth through the length of the data vector (to mimic
            % reflection of the data at each end)
            indReflect = [1:CLen (CLen-1):-1:2];
            numReps = ceil(K0/(2*CLen-2));
            KVec = repmat(indReflect,[1 numReps]);
            KVec = KVec(1:K0);

            % compute scaling factor for current pole
            C0 = -F{1}(i)./(1-F{1}(i)^2);

            % apply symmetric exponential filter
            C(:) = symExpFilt(C(:),CLen,C0,F{1}(i),K0,KVec);

        end

        % multiply by numerator of direct BSpline filter
        C = real(C)*F0{1};

    case 2 % X is a matrix - a little more difficult

        % first perform direct filtering over each column
        [CRows,CCols] = size(C);

        % loop through poles of direct BSpline filter
        for i = 1:length(F{1})

            % compute K0 for current pole
            K0 = ceil(log(K0Tol)./log(abs(F{1}(i))));

            % now compute vector of indices of length K0 that runs back and
            % forth through the length of the data vector (to mimic
            % reflection of the data at each end)
            indReflect = [1:CRows (CRows-1):-1:2];
            numReps = ceil(K0/(2*CRows-2));
            KVec = repmat(indReflect,[1 numReps]);
            KVec = KVec(1:K0);

            % compute scaling factor for current pole
            C0 = -F{1}(i)./(1-F{1}(i)^2);

            % apply symmetric exponential filter for each column
            for k = 1:CCols
                C(:,k) = symExpFilt(C(:,k),CRows,C0,F{1}(i),K0,KVec);
            end
        end

        % multiply by numerator of direct BSpline filter
        C = real(C)*F0{1};

        % now perform direct filtering across each row
        % loop through poles of direct BSpline filter
        for i = 1:length(F{2})

            % compute K0 for current pole
            K0 = ceil(log(K0Tol)./log(abs(F{2}(i))));

            % now compute vector of indices of length K0 that runs back and
            % forth through the length of the data vector (to mimic
            % reflection of the data at each end)
            indReflect = [1:CCols (CCols-1):-1:2];
            numReps = ceil(K0/(2*CCols-2));
            KVec = repmat(indReflect,[1 numReps]);
            KVec = KVec(1:K0);

            % compute scaling factor for current pole
            C0 = -F{2}(i)./(1-F{2}(i)^2);

            % apply symmetric exponential filter for each column
            for k = 1:CRows
                C(k,:) = symExpFilt(C(k,:),CCols,C0,F{2}(i),K0,KVec);
            end
        end

        % multiply by numerator of direct BSpline filter
        C = real(C)*F0{2};

    case 3 % X is a volume - a bit more difficult

        % first perform direct filtering over each column
        [CRows,CCols,CSlices] = size(C);

        % loop through poles of direct BSpline filter
        for i = 1:length(F{1})

            % compute K0 for current pole
            K0 = ceil(log(K0Tol)./log(abs(F{1}(i))));

            % now compute vector of indices of length K0 that runs back and
            % forth through the length of the data vector (to mimic
            % reflection of the data at each end)
            indReflect = [1:CRows (CRows-1):-1:2];
            numReps = ceil(K0/(2*CRows-2));
            KVec = repmat(indReflect,[1 numReps]);
            KVec = KVec(1:K0);

            % compute scaling factor for current pole
            C0 = -F{1}(i)./(1-F{1}(i)^2);

            % apply symmetric exponential filter for each column
            for k = 1:CSlices
                for j = 1:CCols
                    C(:,j,k) = symExpFilt(C(:,j,k),CRows,C0,F{1}(i),K0,KVec);
                end
            end
        end

        % multiply by numerator of direct BSpline filter
        C = real(C)*F0{1};

        % now perform direct filtering across each row
        % loop through poles of direct BSpline filter
        for i = 1:length(F{2})

            % compute K0 for current pole
            K0 = ceil(log(K0Tol)./log(abs(F{2}(i))));

            % now compute vector of indices of length K0 that runs back and
            % forth through the length of the data vector (to mimic
            % reflection of the data at each end)
            indReflect = [1:CCols (CCols-1):-1:2];
            numReps = ceil(K0/(2*CCols-2));
            KVec = repmat(indReflect,[1 numReps]);
            KVec = KVec(1:K0);

            % compute scaling factor for current pole
            C0 = -F{2}(i)./(1-F{2}(i)^2);

            % apply symmetric exponential filter for each column
            for k = 1:CSlices
                for j = 1:CRows
                    C(j,:,k) = symExpFilt(C(j,:,k),CCols,C0,F{2}(i),K0,KVec);
                end
            end
        end

        % multiply by numerator of direct BSpline filter
        C = real(C)*F0{2};

        % now perform direct filtering across each slice
        % loop through poles of direct BSpline filter
        for i = 1:length(F{3})

            % compute K0 for current pole
            K0 = ceil(log(K0Tol)./log(abs(F{3}(i))));

            % now compute vector of indices of length K0 that runs back and
            % forth through the length of the data vector (to mimic
            % reflection of the data at each end)
            indReflect = [1:CSlices (CSlices-1):-1:2];
            numReps = ceil(K0/(2*CSlices-2));
            KVec = repmat(indReflect,[1 numReps]);
            KVec = KVec(1:K0);

            % compute scaling factor for current pole
            C0 = -F{3}(i)./(1-F{3}(i)^2);

            % apply symmetric exponential filter for each column
            for k = 1:CCols
                for j = 1:CRows
                    C(j,k,:) = symExpFilt(C(j,k,:),CSlices,C0,F{3}(i),K0,KVec);
                end
            end
        end

        % multiply by numerator of direct BSpline filter
        C = real(C)*F0{3};

    otherwise % X is multidimensional - the most difficult

        % get size of coefficients array
        Csz = size(C);

        % set up vector of indices into each dimension
        idx = cell(1,tensorOrder);
        for k = 1:tensorOrder
            idx{k} = 1:size(C,k);
        end

        % loop through each dimension
        for d = 1:tensorOrder

            % copy original version of idx
            idxCurrent = idx;

            % construct vector of indices which will be used in computing
            % the initial element of the filtered data
            indReflect = [1:Csz(d) (Csz(d)-1):-1:2];

            % construct list of dimensions without d
            dimList = setdiff(1:tensorOrder,d);

            % construct index into entries of remaining dimensions
            [idxCurrent{dimList}] = ndgrid(idx{dimList});
            dimListInd = sub2ind(Csz(dimList),idxCurrent{dimList});
            dimListInd = dimListInd(:);

            % loop through poles of BSpline filter
            for i = 1:length(F{d})

                % compute K0 for current pole
                K0 = ceil(log(K0Tol)./log(abs(F{d}(i))));

                % now compute vector of indices of length K0 that runs back and
                % forth through the length of the data vector (to mimic
                % reflection of the data at each end)
                numReps = ceil(K0/(2*Csz(d)-2));
                KVec = repmat(indReflect,[1 numReps]);
                KVec = KVec(1:K0);

                % compute scaling factor for current pole
                C0 = -F{d}(i)./(1-F{d}(i)^2);

                % now loop through remaining dimensions, applying symmetric
                % exponential filter
                for k=1:numel(dimListInd)
                    [idxCurrent{dimList}] = ind2sub(Csz(dimList),dimListInd(k));
                    C(idxCurrent{:}) = symExpFilt(C(idxCurrent{:}),Csz(d),C0,F{d}(i),K0,KVec);
                end
            end

            % multiply by numerator of direct BSpline filter
            C = real(C)*F0{d};

        end

end

% now pad dimensions by reflection
padNum = floor(D/2);

% Form index vectors to subsasgn input array into output array.
% Also compute the size of the output array.
idx   = cell(1,tensorOrder);
for k = 1:tensorOrder
  M = size(C,k);
  dimNums = [1:M (M-1):-1:2];
  p = padNum(k);
  idx{k}   = dimNums(mod(-p:M+p, 2*M-2) + 1);
end
CPad = C(idx{:});


function f = fuison_base_parts(b1, b2)
% ZCA + l1 norm
% featrue1 = whitening_norm(b1, 1, 0);
% featrue2 = whitening_norm(b2, 1, 0);
% figure;imshow(featrue1);
% figure;imshow(featrue2);

% f_sum = featrue1+featrue2;
% f_sum(f_sum==0) = 1;

% w1 = featrue1./f_sum;
% w2 = featrue2./f_sum;
% figure;imshow(w1);
% figure;imshow(w2);

w1 = 0.5;
w2 = 0.5;
f = w1.*b1+w2.*b2;
% figure;imshow(f);
end


function [sz] = size(tt,p)
%Mode sizes of the TT-matrix
%   [SZ]=SIZE(TT) Returns the mode sizes of the TT-matrix as a dx2 array of
%   integers
%
%   [SZ]=SIZE(TT,P) Returns the mode-P sizes of the TT-matrix as an array
%   of d integers
%
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------

if (nargin==1) || isempty(p)
    n=tt.n; m=tt.m;
    sz=[n,m];
else
    switch p
        case 1
            sz=tt.n;
        case 2
            sz=tt.m;
        otherwise
            error('tt_matrix/size : illegal value of parameter p.\n');
    end
end

return
end


function status = test_exp_imp(modus)
%TEST_EXP_IMP tests the different expansions of sound fields in time-domain
%
%   Usage: status = test_exp_imp(modus)
%
%   Input parameters:
%       modus   - 0: numerical
%                 1: visual
%
%   Output parameters:
%       status  - true or false
%
%   TEST_EXP_IMP(modus) checks, if the circular basis expansions for plane waves
%   and point sources are working. The circular basis expansions are converted
%   to plane wave decompositions. Additionally, modal weighting functions are
%   tested. Optionally, sound field plots of the plane wave decompositions are
%   used for verification.

%*****************************************************************************
% The MIT License (MIT)                                                      *
%                                                                            *
% Copyright (c) 2010-2019 SFS Toolbox Developers                             *
%                                                                            *
% Permission is hereby granted,  free of charge,  to any person  obtaining a *
% copy of this software and associated documentation files (the "Software"), *
% to deal in the Software without  restriction, including without limitation *
% the rights  to use, copy, modify, merge,  publish, distribute, sublicense, *
% and/or  sell copies of  the Software,  and to permit  persons to whom  the *
% Software is furnished to do so, subject to the following conditions:       *
%                                                                            *
% The above copyright notice and this permission notice shall be included in *
% all copies or substantial portions of the Software.                        *
%                                                                            *
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
% IMPLIED, INCLUDING BUT  NOT LIMITED TO THE  WARRANTIES OF MERCHANTABILITY, *
% FITNESS  FOR A PARTICULAR  PURPOSE AND  NONINFRINGEMENT. IN NO EVENT SHALL *
% THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
% LIABILITY, WHETHER  IN AN  ACTION OF CONTRACT, TORT  OR OTHERWISE, ARISING *
% FROM,  OUT OF  OR IN  CONNECTION  WITH THE  SOFTWARE OR  THE USE  OR OTHER *
% DEALINGS IN THE SOFTWARE.                                                  *
%                                                                            *
% The SFS Toolbox  allows to simulate and  investigate sound field synthesis *
% methods like wave field synthesis or higher order ambisonics.              *
%                                                                            *
% https://sfs.readthedocs.io                            sfstoolbox@gmail.com *
%*****************************************************************************

status = false;

%% ===== Checking of input  parameters ===================================
nargmin = 1;
nargmax = 1;
narginchk(nargmin,nargmax);

%% ===== Configuration ===================================================
% Parameters
conf = SFS_config;
conf.plot.loudspeakers = 0;  % do not plot loudspeakers
conf.plot.usedb = 1;  % use decibel for sound field plot
conf.modal_window_parameter = 2.0;  % parameter for kaiser window

c = conf.c;  % speed of sound

% for sound field plots
X = [-2, 2];
Y = [-2, 2];
Z = 0;

fhp = 1000;  % high-pass filter corner-frequency for pwd of point source

% plane waves with equi-angular distribution
Npw = 1024;
phi0 = (0:Npw-1).'*2*pi/Npw;
x0 = [cos(phi0) sin(phi0)];
x0(:,3) = 0;
x0(:,4:6) = x0(:,1:3);
x0(:,7) = 1;

% test scenarios
scenarios = { ...
  'pw', [ 0.0 -1.0 0.0],  5, 'rect'   , [0.0 0.0 0.0]
  'pw', [ 0.0 -1.0 0.0], 15, 'rect'   , [0.0 0.0 0.0]
  'pw', [ 0.0 -1.0 0.0], 15, 'kaiser' , [0.0 0.0 0.0]
  'pw', [ 0.0 -1.0 0.0],  5, 'rect'   , [0.5 1.0 0.0]
  'pw', [ 0.0 -1.0 0.0], 15, 'rect'   , [0.5 1.0 0.0]
  'pw', [ 0.0 -1.0 0.0], 15, 'kaiser' , [0.5 1.0 0.0]
  'ps', [ 0.0  2.5 0.0],  5, 'rect'   , [0.0 0.0 0.0]
  'ps', [ 0.0  2.5 0.0], 15, 'rect'   , [0.0 0.0 0.0]
  'ps', [ 0.0  2.5 0.0], 15, 'kaiser' , [0.0 0.0 0.0]
  'ps', [ 0.0  2.5 0.0],  5, 'rect'   , [0.5 1.0 0.0]
  'ps', [ 0.0  2.5 0.0], 15, 'rect'   , [0.5 1.0 0.0]
  'ps', [ 0.0  2.5 0.0], 15, 'kaiser' , [0.5 1.0 0.0]
  };

%% ===== Main ============================================================

for ii=1:size(scenarios)

    src = scenarios{ii,1};  % source type
    xs = scenarios{ii,2};  % source position / direction of plane wave
    Nce = scenarios{ii,3};  % modal order
    conf.modal_window = scenarios{ii,4};  % type of modal weighting function
    xq = scenarios{ii,5};  % expansion centre

    % circular expansion coefficients
    switch src
    case 'pw'
        [pm, delay_offset] = circexp_imp_pw(xs,Nce,xq,conf);
        t = xq*xs.'./c;
    case 'ps'
        [pm, delay_offset] = circexp_imp_ps(xs,Nce,xq,fhp,conf);
        t = norm(xq-xs)./c;
    end

    % modal weighting of coefficients
    wm = modal_weighting(Nce, conf);
    pm = bsxfun(@times, pm, wm);

    % conversion to plane wave decomposition
    ppwd = pwd_imp_circexp(pm, Npw);

    if modus
      % delay plane waves according to expansion centre
      delays = -xq*x0(:,1:3).'./c;
      delay_offset = delay_offset - min(delays);
      delays = delays - min(delays);
      ppwd = delayline(ppwd,delays,1,conf);

      % sound field plot
      sound_field_imp(X,Y,Z,x0,'pw',ppwd,t+delay_offset,conf);

      % title string
      str = 'Plane wave decompostion of modally bandlimited';
      switch src
      case 'pw'
          str = sprintf('%s plane wave', str);
      case 'ps'
          str = sprintf('%s point source', str);
      end
      str = sprintf(['%s ([%1.1f %1.1f %1.1f]):\n%s-window (M=%d), ' ...
        'center of modal expansion at [%1.1f %1.1f %1.1f]'], ...
        str, xs, conf.modal_window, Nce, xq);
      title(str);

    end
end

status = true;


function [anchorIDs, featIDs, PrQfeatIDs] = sampleAnchorAndFeatIDsToSplitMerge( Psi, data, algParams );
% Sample anchor ids and feature ids to perform split/merge move.
% Every split-merge move is defined by:
%   -- two *distinct* sequences (ii,jj), which we call "anchors"
%   -- two features, ki and kj, where F(ii,ki)=1 and F(jj,kj) = 1
% If ki == kj, we propose splitting these into two features,
%   otherwise, we merge ki and kj into a single feature.

F = Psi.F==1;
stateSeq = Psi.stateSeq;
ThetaM = Psi.ThetaM;

% ---------------------------------------------  select anchor sequences
if ~isfield( Psi, 'anchorIDs' )
    anchorIDs = randsample( data.N, 2 );
    else
    anchorIDs = Psi.anchorIDs;
end
ii = anchorIDs(1);
jj = anchorIDs(2);

if isfield( Psi, 'activeFeatIDs' )
    ki = Psi.activeFeatIDs(1);
    if length( Psi.activeFeatIDs ) > 1
        kj = Psi.activeFeatIDs(2);
    else
        kj = ki;
    end
end

% ---------------------------------------------  select feature IDs
switch algParams.SM.featSelectDistr
    case 'random'
        qs_ki = F(ii,:);
        if ~exist('ki','var')
            ki = multinomial_single_draw( qs_ki );
        end
        qs_kj = F(jj,:);
        if ~exist('kj','var')
            kj = multinomial_single_draw( qs_kj );
        end
    case 'splitBias'
        qs_ki = F(ii,:);
        if ~exist('ki','var')
            ki = multinomial_single_draw( qs_ki );
        end

        delta_ki = false( size(F,2) );
        delta_ki( ki ) = 1;

        qs_kj = F(jj,:) .* ~delta_ki;
        qs_kj( ki ) = F(jj,ki)*2*sum( qs_kj );
        if ~exist('kj','var')
            kj = multinomial_single_draw( qs_kj );
        end
    case 'splitBias+margLik'
        % Build cond distr. kj | ki, jj
        %  based on margLik ratio between ki and kj

        qs_ki = F(ii,:);
        if ~exist('ki','var')
            ki = multinomial_single_draw( qs_ki );
        end
        log_qs_kj = -inf( 1, size(F,2)  );
        for kk = find( F(jj,:) )
            if kk == ki
               continue;
            end
            log_qs_kj(kk) = ThetaM.calcMargLikRatio_MergeFeats( data, stateSeq, ki, kk );
        end
        M = max( log_qs_kj );
        if all( isinf(log_qs_kj) )
            qs_kj = zeros(1, size(F,2) );
            qs_kj(ki) = F(jj,ki);
        else
        qs_kj = exp( log_qs_kj - M );
        qs_kj( ki ) = F(jj,ki)*2*sum( qs_kj );
        qs_kj = qs_kj ./ sum( qs_kj );
        end
        % Final smoothing: take convex combo of 99% our qs and 1% us
        %   this avoids terrible reverse probabilities
        us = false( 1, size(F,2) );
        us( F(jj,:) ) = 1;
        us = us./sum(us);
        qs_kj = .99*qs_kj + 0.01*us;

        if ~exist('kj','var')
            kj = multinomial_single_draw( qs_kj );
        end
end
qs_ki = qs_ki ./ sum( qs_ki );
qs_kj = qs_kj ./ sum( qs_kj );
featIDs = [ki kj];

assert( ~any( isnan(qs_kj) ), 'ERROR: bad numerical calc of feat select distr.');

PrQfeatIDs = qs_ki( ki ) * qs_kj( kj );


function net = cnn_mnist_init(varargin)
% Initialize a simple TensorNet for MNIST.

net.layers = {} ;
inputModeSize = [4, 8, 8, 4] ;
secondModeSize = [5, 5, 5, 5] ;
ranks = [1, 2, 2, 2, 1] ;
W = tt_rand(secondModeSize.*inputModeSize, length(secondModeSize), ranks, []) ;
W = tt_matrix(W, secondModeSize, inputModeSize) ;
W.core = single(W.core) ;
net.layers{end+1} = struct('type', 'custom', ...
                           'forward', @vl_nntt_forward, ...
                           'backward', @vl_nntt_backward, ...
                           'W', W, ...
                           'weights', {{W.core, zeros(1,1,prod(secondModeSize),'single')}}, ...
                           'learningRate', [1, 2], ...
                           'weightDecay', [1, 0], ...
                           'outHeight', 1, ...
                           'outWidth', 1, ...
                           'outChannels', prod(secondModeSize)) ;
net.layers{end+1} = struct('type', 'relu') ;
net.layers{end+1} = struct('type', 'conv', ...
                           'weights', {{0.1*randn(1,1,prod(secondModeSize),10, 'single'), zeros(1, 10, 'single')}}, ...
                           'learningRate', [1, 2], ...
                           'weightDecay', [1, 0], ...
                           'stride', 1, ...
                           'pad', 0) ;
net.layers{end+1} = struct('type', 'softmaxloss') ;


function S = globMatrixNed3DStiff(fun,mesh,fem1,fem2)

%% USAGE: generate stiffness global matrix on a 3D mesh
%
% INPUTS:
% fun --- coefficient function
% mesh --- a struct data contains very rich mesh information.
% fem1 --- global DoF for test function space
% fem2 --- global DoF for trial function space
%
% OUTPUTS:
% [IN JN XN] --- triplets of the sparse matrix from regular elements.
% [II JI XI] --- triplets of the sparse matrix from interface elements.

% Last Modified: 08/07/2020 by Xu Zhang

%% 0. Initializaiton
feEvalBas1 = @EvalNed1Bas3D;
feEvalBas2 = @EvalNed1Bas3D;

dof1 = fem1.ldof; dof2 = fem2.ldof; nloc = dof1*dof2;
nt = length(mesh.t);
A = fem1.area; gx = fem1.gx; gy = fem1.gy; gz = fem1.gz; gw = fem1.gw;
X = zeros(nloc*nt, 1);

coef = feval(fun,gx,gy,gz);
Ibasx = cell(dof1,1); Ibasy = cell(dof1,1); Ibasz = cell(dof1,1);
Jbasx = cell(dof2,1); Jbasy = cell(dof2,1); Jbasz = cell(dof2,1);
for i = 1:dof1
    Ibasx{i} = feEvalBas1(fem1.bas, ':', gx, gy, gz, i, 1, 1).*fem1.t_e_orit(:,i);
    Ibasy{i} = feEvalBas1(fem1.bas, ':', gx, gy, gz, i, 1, 2).*fem1.t_e_orit(:,i);
    Ibasz{i} = feEvalBas1(fem1.bas, ':', gx, gy, gz, i, 1, 3).*fem1.t_e_orit(:,i);
end
for j = 1:dof2
    Jbasx{j} = feEvalBas2(fem2.bas, ':', gx, gy, gz, j, 1, 1).*fem2.t_e_orit(:,j);
    Jbasy{j} = feEvalBas2(fem2.bas, ':', gx, gy, gz, j, 1, 2).*fem2.t_e_orit(:,j);
    Jbasz{j} = feEvalBas2(fem2.bas, ':', gx, gy, gz, j, 1, 3).*fem2.t_e_orit(:,j);
end

I = reshape(repmat(fem1.g2ldof,6,1),nloc*nt,1);
J = repmat(reshape(fem2.g2ldof,dof2*nt,1),6,1);
ind = 0;
for i = 1:dof1
    for j = 1:dof2
        X(ind+1:ind+nt) = A.*(sum(((Ibasx{i}.*(coef.*Jbasx{j})).*gw'),2) + ...
            sum(((Ibasy{i}.*(coef.*Jbasy{j})).*gw'),2) + ...
            sum(((Ibasz{i}.*(coef.*Jbasz{j})).*gw'),2));
        ind = ind + nt;
    end
end
ID = find(X~=0);
S = sparse(I(ID),J(ID),X(ID),size(fem1.gdof,1),size(fem2.gdof,1));


function [model, time] = rbf_build(Xtr, Ytr, bf_type, bf_c, usePolyPart, verbose)
% RBFBUILD
% Builds a Radial Basis Function (RBF) interpolant using training data
%
% Call
%   [model, time] = rbf_build(Xtr, Ytr, bf_type, bf_c, usePolyPart, verbose)
%   [model, time] = rbf_build(Xtr, Ytr, bf_type, bf_c, usePolyPart)
%   [model, time] = rbf_build(Xtr, Ytr, bf_type, bf_c)
%   [model, time] = rbf_build(Xtr, Ytr, bf_type)
%   [model, time] = rbf_build(Xtr, Ytr)
%
% Input
% Xtr, Ytr    : Training data points (Xtr(i,:), Ytr(i)), i = 1,...,n
%               Note that the input variables must be scaled to e.g. [0,1]
%               or [-1,1] for better predictive performance.
% bf_type     : Type of the basis functions (default = 'MQ'):
%               'BH' = Biharmonic
%               'MQ' = Multiquadric
%               'IMQ' = Inverse Multiquadric
%               'TPS' = Thin plate spline
%               'G' = Gaussian
% bf_c        : Parameter c value (default = 1)
% usePolyPart : Use also the polynomial term P of the model Y = P + RBF
%               (default = 0, do not use)
% verbose     : Set to 0 for no verbose (default = 1)
%
% Output
% model     : RBF model - a struct with the following elements:
%    n      : Number of data points in the training data set
%    meanY  : Mean of Ytr
%    bf_type: Type of the basis functions
%    bf_c   : Parameter c value
%    poly   : Use also the polynomial term
%    coefs  : Coefficients of the model
% time      : Execution time
%
% Please give a reference to the software web page in any publication
% describing research performed using the software, e.g. like this:
% Jekabsons G. Radial Basis Function interpolation for Matlab, 2009,
% available at http://www.cs.rtu.lv/jekabsons/

% This source code is tested with Matlab version 7.1 (R14SP3).

% =========================================================================
% RBF interpolation
% Version: 1.1
% Date: August 12, 2009
% Author: Gints Jekabsons (gints.jekabsons@rtu.lv)
% URL: http://www.cs.rtu.lv/jekabsons/
%
% Copyright (C) 2009  Gints Jekabsons
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program. If not, see <http://www.gnu.org/licenses/>.
% =========================================================================

if nargin < 2
    error('Too few input arguments.');
else

    [n, d] = size(Xtr);
    [ny, dy] = size(Ytr);
    if (n < 2) || (d < 1) || (ny ~= n) || (dy ~= 1)
        error('Wrong training data sizes.');
    end

    if nargin < 3
        bf_type = 'MQ';
    end
    if nargin < 4
        bf_c = 1;
    end
    if nargin < 5
        usePolyPart = 1;
    end
    if nargin < 6
        verbose = 0;
    end

    tic;

    model.n = n;
    model.meanY = mean(Ytr);
    model.bf_type = bf_type;
    model.bf_c = bf_c;
    model.poly = usePolyPart;

    %calculate and transform distances between all the points in the training data
    dist = zeros(n, n);
    switch upper(model.bf_type)
        case 'BH'
            if verbose
                fprintf('Building RBF (biharmonic) model...\n');
            end
            for i = 1 : n
                %for j = i : n
                %    dist(i, j) = norm(Xtr(i,:) - Xtr(j,:));
                %end
                dist(i, i:n) = sqrt(sum((repmat(Xtr(i,:),n-i+1,1) - Xtr(i:n,:)).^2,2));
                dist(i+1:n, i) = dist(i, i+1:n);
            end
        case 'IMQ'
            if verbose
                fprintf('Building RBF (inverse multiquadric) model...\n');
            end
            for i = 1 : n
                %for j = i : n
                %    dist(i, j) = 1 / sqrt(sum((Xtr(i,:) - Xtr(j,:)).^2) + bf_c^2);
                %end
                dist(i, i:n) = 1 ./ sqrt(sum((repmat(Xtr(i,:),n-i+1,1) - Xtr(i:n,:)).^2,2) + bf_c^2);
                dist(i+1:n, i) = dist(i, i+1:n);
            end
        case 'TPS'
            if verbose
                fprintf('Building RBF (thin plate spline) model...\n');
            end
            for i = 1 : n
                %for j = i : n
                %    dist(i, j) = sum((Xtr(i,:) - Xtr(j,:)).^2);
                %    dist(i, j) = (dist(i, j) + bf_c^2) * log(sqrt(dist(i, j) + bf_c^2));
                %end
                dist(i, i:n) = sum((repmat(Xtr(i,:),n-i+1,1) - Xtr(i:n,:)).^2,2);
                dist(i, i:n) = (dist(i, i:n) + bf_c^2) .* log(sqrt(dist(i, i:n) + bf_c^2));
                dist(i+1:n, i) = dist(i, i+1:n);
            end
        case 'G'
            if verbose
                fprintf('Building RBF (Gaussian) model...\n');
            end
            for i = 1 : n
                %for j = i : n
                %    dist(i, j) = exp(-sum((Xtr(i,:) - Xtr(j,:)).^2) / (2*bf_c^2));
                %end
                dist(i, i:n) = exp(-sum((repmat(Xtr(i,:),n-i+1,1) - Xtr(i:n,:)).^2,2) / (2*bf_c^2));
                dist(i+1:n, i) = dist(i, i+1:n);
            end
        otherwise %MQ
            if verbose
                fprintf('Building RBF (multiquadric) model...\n');
            end
            for i = 1 : n
                %for j = i : n
                %    dist(i, j) = sqrt(sum((Xtr(i,:) - Xtr(j,:)).^2) + bf_c^2);
                %end
                dist(i, i:n) = sqrt(sum((repmat(Xtr(i,:),n-i+1,1) - Xtr(i:n,:)).^2,2) + bf_c^2);
                dist(i+1:n, i) = dist(i, i+1:n);
            end
    end

    %calculate coefs
    if model.poly == 0
        model.coefs = dist \ (Ytr - model.meanY);
    else
        A = [dist, ones(n,1), Xtr; [ones(n,1), Xtr]', zeros(d+1,d+1)];
        model.coefs  = A \ [Ytr; zeros(d+1,1)];
    end

    time = toc;

    if verbose
        fprintf('Execution time: %0.2f seconds\n', time);
    end

end
return