% GP_LEARN_PSEUDO - Learns the hyperparameters of a Gaussian process
%                   using pseudo inputs.
%
% Usage:
%
%   [THETA_F, THETA_NOISE, LOGLIKELIHOOD] = ...
%     GP_LEARN_PSEUDO(Y, COVFUNC_PSEUDO, THETA0_F, COVFUNC_NOISE, THETA0_NOISE)
%
% Y              : Nx1 vector of observations
% COVFUNC_PSEUDO : A special covariance function, see GP_COV_PSEUDO
% THETA0_F       : Initial parameter values for COVFUNC_PSEUDO
% COVFUNC_NOISE  : A diagonal noise covariance function
% THETA0_NOISE   : Initial parameter values for COVFUNC_NOISE
%
% THETA_F       : Optimized parameter values for COVFUNC_PSEUDO
% THETA_NOISE   : Optimized parameter values for COVFUNC_NOISE
% LOGLIKELIHOOD : The loglikelihood lower bound at the optimum
%
% Optional parameters can be given as
%
%   [...] = GP_LEARN_PSEUDO(..., 'PARAMETER', VALUE)
%
% where the possible parameters are
%
% 'MAXITER' : Maximum number of iterations (default: 100)
% 'CHECKGRAD' : Numerical check of the gradients (default: false)
%
% See also GP_PREDICT_PSEUDO, GP_LEARN, GP_COV_PSEUDO,
% GP_LOGLIKELIHOOD_PSEUDO.

% Last modified 2010-01-27
% Copyright (c) Jaakko Luttinen (jaakko.luttinen@tkk.fi)

function [theta_f, theta_noise, loglikelihood] = gp_learn_pseudo(y, ...
                                                  covfunc_pseudo, ...
                                                  theta_f, ...
                                                  covfunc_noise, ...
                                                  theta_noise, ...
                                                  varargin)

options = struct('maxiter', 100, ...
                 'checkgrad', false);
[options, errmsg] = argparse(options, varargin{:});
error(errmsg);

n_theta_f = numel(theta_f);
theta = [theta_f(:); theta_noise(:)];

if options.checkgrad
  mycheckgrad(@cost, log(theta), 1e-6);
end

[logtheta, logpdfs] = minimize(log(theta), @cost, options.maxiter);
loglikelihood = -min(logpdfs);
theta = exp(logtheta);

  function [c, dc] = cost(logtheta)
  theta = exp(logtheta);
  theta_f = theta(1:n_theta_f);
  theta_noise = theta((n_theta_f+1):end);
  [c, dc_f, dc_noise] = gp_loglikelihood_pseudo(y, covfunc_pseudo, theta_f, ...
                                                covfunc_noise, theta_noise);
  c = -c;
  dc = -[dc_f; dc_noise] .* theta;
  end

end


function [dtdx,dtrp,vart]=trop_model_prec(time,pos,azel,x)

zhd=tropmodel(pi/2,pos,0); %zenith hydrostatic delay

[mh,mw]=tropmapf(time,pos,azel); %hydrostatic and wet projection coefficient

if azel(2)>0
    %equation: m_w=m_0+m_0*cot(el)*(Gn*cos(az)+Ge*sin(az))
    cotz=1.0/tan(azel(2));
    grad_n=mw*cotz*cos(azel(1)); %north wet projection coefficient
    grad_e=mw*cotz*sin(azel(1)); %east wet projection coefficient
    mw=mw+grad_n*x(2)+grad_e*x(3); %total wet projection coefficient
    dtdx(2)=grad_n*(x(1)-zhd); %north wet delay
    dtdx(3)=grad_e*(x(1)-zhd); %east wet delay
end

dtdx(1)=mw;
dtrp=mh*zhd+mw*(x(1)-zhd);
vart=0.01^2;

return;


%imu model parameter should be defined in cont. time
function [STM Qd]=mdl_ned_dcm(pos_n, vel_n, Cbn, acc, Aimu, Qimu, Cimu, Rimu, dt)

%continious model
[Anav N]=sys_ned_dcm_v000(pos_n, vel_n, Cbn, acc, 0, []);
nst_imu=size(Aimu,1);
nst=9+nst_imu;
Ac=[Anav N*Cimu;zeros(nst_imu,9) Aimu];
Qc=[N*Rimu*N' zeros(9,nst_imu);zeros(nst_imu,9) Qimu];

%Discretize the model
mx_a = dt*[-Ac,Qc;zeros(nst),Ac'];
mx_b = expm(mx_a);
STM = mx_b(nst+1:2*nst,nst+1:2*nst)';
Qd = STM*mx_b(1:nst,nst+1:2*nst);


 function mac = xray_atten_interp(kev, mac, kev_in, varargin)
%function mac = xray_atten_interp(kev, mac, kev_in, [options])
%|
%| Interpolate mass attenuation coefficients (mac) onto desired energies.
%|
%| in
%|	kev	[M,1]
%|	mac	[M,1]
%|	kev_in	[N,1]		desired energies [in keV]
%|
%| option
%|	'interp' {}		default {'pchip', 'extrap'}
%| out
%|	mac	[N,1]		mass attenuation coefficients [cm^2/g],
%|
%| Copyright 2004-05-1, Jeff Fessler, University of Michigan

% default is to show example
if nargin < 1, ir_usage, end
if nargin == 1 && streq(kev, 'test'), xray_atten_interp_test, return, end

% = {'linear', 'extrap'};
% = {'spline', 'extrap'};
arg.interp = {'pchip', 'extrap'};
arg = vararg_pair(arg, varargin);

% trick: allow for the k-edge jumps!
mac = log(mac); % interpolate on a log scale
mac = interp1_jump(kev, mac, kev_in, arg.interp{:});
mac = exp(mac);

% xray_atten_interp_test()
% example usage, cf Fig. 3.4 of Macovski 1983
function xray_atten_interp_test
mtype = 'water'; ax = 10.^[1 3 -2 1.];
mtype = 'lead'; ax = 10.^[1 3 -1 2.5];
[mac kev] = xray_read_atten(mtype);
kev1 = logspace(1,3,1+2^9);
mac1 = xray_atten_interp(kev, mac, kev1);
if im
	clf, loglog(kev, mac, '.', kev1, mac1, '-')
	xlabel 'KeV', ylabel 'mass attenuation coefficient [cm^2/g]'
	axis(ax)
	texts(0.7, 0.7, mtype)
	if streq(mtype, 'lead')
		text(14, 40, 'L edge', 'horizontalalignment', 'center')
		text(88, 1., 'K edge', 'horizontalalignment', 'center')
	end
end
% ir_savefig(['fig_atten_' mtype])


function [w, infos] = sd(problem, in_options)
% Full steepest descent gradient algorithm.
%
% Inputs:
%       problem     function (cost/grad/hess)
%       in_options  options
% Output:
%       w           solution of w
%       infos       information
%
% This file is part of GDLibrary and SGDLibrary.
%
% Created by H.Kasai on Feb. 15, 2016
% Modified by H.Kasai on Mar. 23, 2018
% Modified by H.Kasai on Oct. 20, 2020

    % set dimensions and samples
    d = problem.dim;
    n = problem.samples;

    % set local options
    local_options = [];
    local_options.algorithm = 'SD';
    local_options.sub_mode = 'STANDARD';

    % merge options
    options = mergeOptions(get_default_options(d), local_options);
    options = mergeOptions(options, in_options);

    % initialise
    iter = 0;
    grad_calc_count = 0;
    w = options.w_init;
    w_old = w;
    prev_step = options.step_init;

    if ~isfield(options, 'S')
        if strcmp(options.step_alg, 'exact')
            options.S = eye(d);
        end
    else
        %
    end

    % initialize by BB step-size
    if strcmp(options.step_init_alg, 'bb_init')
        options.step_init = bb_init(problem, w);
    end

    % store first infos
    clear infos;
    [infos, f_val, optgap, grad, gnorm] = store_infos(problem, w, options, [], iter, grad_calc_count, 0);
    grad_old = grad;

    % display info
    if options.verbose
        if ~problem.prox_flag
            fprintf('SD: Iter = %03d, cost = %.24e, gnorm = %.4e, optgap = %.4e\n', iter, f_val, gnorm, optgap);
        else
            fprintf('PG: Iter = %03d, cost = %.24e, gnorm = %.4e, optgap = %.4e\n', iter, f_val, gnorm, optgap);
        end
    end

    % set start time
    start_time = tic();

    % main loop
    while (optgap > options.tol_optgap) && (gnorm > options.tol_gnorm) && (iter < options.max_epoch)

        options.iter = iter;
        [step, ~] = options.linesearchfun(options.step_alg, problem, w, w_old, grad, grad_old, prev_step, options);

        prev_step = step;
        w_old = w;
        if strcmp(options.sub_mode, 'SCALING')
            % diagonal scaling
            if isempty(options.S)
                h = problem.full_hess(w);
                options.S = diag(1./diag(h));
            end

            % update w
            w = w - step * options.S * grad;
        else
            % update w
            w = w - step * grad;
        end

        % proximal operator
        if problem.prox_flag
            w = problem.prox(w, step);
        end

        % store gradient
        grad_old = grad;

        % measure elapsed time
        elapsed_time = toc(start_time);

        % count gradient evaluations
        grad_calc_count = grad_calc_count + n;

        % update iter
        iter = iter + 1;

        % store infos
        [infos, f_val, optgap, grad, gnorm] = store_infos(problem, w, options, infos, iter, grad_calc_count, elapsed_time);

        % display infos
        if options.verbose
            if ~problem.prox_flag
                fprintf('SD: Iter = %03d, cost = %.24e, gnorm = %.4e, optgap = %.4e\n', iter, f_val, gnorm, optgap);
            else
                fprintf('PG: Iter = %03d, cost = %.24e, gnorm = %.4e, optgap = %.4e\n', iter, f_val, gnorm, optgap);
            end
        end
    end

    if gnorm < options.tol_gnorm
        fprintf('Gradient norm tolerance reached: tol_gnorm = %g\n', options.tol_gnorm);
    elseif optgap < options.tol_optgap
        fprintf('Optimality gap tolerance reached: tol_optgap = %g\n', options.tol_optgap);
    elseif iter == options.max_epoch
        fprintf('Max iter reached: max_epoch = %g\n', options.max_epoch);
    end

end


function fh = GNSSspectrumgen
%==========================================================================
%GNSSspectrumgen Functions to generate GNSS analytical spectra.
%   fh = GNSSspectrumgen  returns the handlers of the local functions:
%
%   spectrumgen_call
%   [SI SQ] = spectrumgen_call(signal,F) returns the analytical complex
%   spectra of a GNSS signal with normalized complex power.
%
%       Inputs
%           signal --> GPS L1: L1CA, L1P, L1M, L1C, L1Cd, L1Cp, L1Cp1,L1Cp2
%                              L1, L1_new.
%                      GPS L2: L2C, L2P, L2M, L2, L2_new.
%                      GPS L5: L5I, L5Q, L5, L5_new
%                      Galileo E1: E1PRS/EA, E1OS.
%                      Galileo E6: E6PRS/E6OS.
%                      Galileo E5: E5, E5A, E5B.
%                      BeiDou-2 Current:
%                      	B1: B11, B12, B1
%                       B2: B2I, B2Q, B2
%                       B3: B3
%                      BeiDou-2 Future:
%                       B1: B1Cd, B1Cp, B1C, B1_new
%                       B2: B2_new
%                       B3: B3_new, B3A, B3composite
%           F --> Baseband frequency points.
%
%       Ouputs
%           SI/SQ --> Normalized complex spectrum.
%
%   spectrum_BPSK
%   S = spectrum_BPSK(fc,F) returns spectrum of a BPSK modulation with a
%   chipping rate fc.
%
%   spectrum_BOCs
%   S = spectrum_BOCs(n,m,F) returns spectrum of a sine-phased even BOC
%   modulation with a chipping rate fc=m*1.023e6 and sub-carrier rate
%   fs = n*1.023e6.
%
%   spectrum_BOCc
%   S = spectrum_BOCc(n,m,F) returns spectrum of a cosine-phased even BOC
%   modulation with a chipping  rate fc=m*1.023e6 and sub-carrier rate
%   fs = n*1.023e6.
%
%   spectrum_AltBOC returns spectrum of a sine-phased
%   modified even AltBOC modulation with a chipping rate fc=m*1.023e6 and
%   sub-carrier rate fs = n*1.023e6.
%
%   Observations
%       # I think that L2 can also transmit a C/A component, but that they
%       actually never do it.
%       # I am not sure if the future BeiDou signals will replace or
%       complement the former signals. Now I assume they will replace them.
%       # I cound't find the official transmitted powers of the BeiDou
%       signals anywhere. I have assumed they are -163dBm per component.
%
%   References
%       # L1CA/L2: GPS Interface Control Document IS-GPS-200
%       # L1C:  GPS Interface Control Document IS-GPS-800
%       # L5: GPS Interface Control Document IS-GPS-705
%       # Galileo: Galileo Open Service Signal In Space Interface Control
%       Document (OS SIS ICD)
%       # BeiDou: BeiDou Navigation Satellite System Signal In Space
%       Interface Control Document 2.0
%--------------------------------------------------------------------------
% Version log (main changes)
%   02/03/2017 --> Log started
%--------------------------------------------------------------------------
% Author: Daniel Pascual (daniel.pascual [at] protonmail.com)
% Copyright 2017 Daniel Pascual
% License: GNU GPLv3
%==========================================================================

% Copyright 2017 Daniel Pascual
%
% 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/>.

    fh.spectrumgen_call = @spectrumgen_call;
    fh.spectrum_BPSK = @spectrum_BPSK;
    fh.spectrum_BOCs = @spectrum_BOCs;
    fh.spectrum_BOCc = @spectrum_BOCc;
    fh.spectrum_AltBOC = @spectrum_AltBOC;
end

function [SI, SQ] = spectrumgen_call(signal,F)
%==========================================================================
%
%--------------------------------------------------------------------------
% Author: Daniel Pascual (daniel.pascual [at] protonmail.com)
% Copyright 2017 Daniel Pascual
% License: GNU GPLv3
%==========================================================================

    switch(signal)
        %-------------------- All GNSS individual -------------------------
        % BPSK
        case {'L1CA', 'L2C'}
            SI = spectrum_BPSK(1.023e6,F);
            SQ = zeros(size(SI));

        case {'E6CS'}
            SI = spectrum_BPSK(5*1.023e6,F);
            SQ = zeros(size(SI));

        case {'L1P', 'L2P', 'L5I', 'L5Q'}
            SI = spectrum_BPSK(10.23e6,F);
            SQ = zeros(size(SI));

        % BOCs
        case{'L1Cd','L1Cp1'}
            SI = spectrum_BOCs(1,1,F);
            SQ = zeros(size(SI));

        case{'L1Cp2'}
            SI = spectrum_BOCs(6,1,F);
            SQ = zeros(size(SI));

        case {'L1M','L2M'}
            SI = spectrum_BOCs(10,5,F);
            SQ = zeros(size(SI));

        case{'L1Cp'}
            P_Cp1 = GNSS_POWERS.GPS_L1_Cp1/GNSS_POWERS.GPS_L1_Cp;
            P_Cp2 = GNSS_POWERS.GPS_L1_Cp2/GNSS_POWERS.GPS_L1_Cp;

            S_Cp1 = P_Cp1*spectrumgen_call('L1Cp1',F);
            S_Cp2 = P_Cp2*spectrumgen_call('L1Cp2',F);

            SI = S_Cp1 + S_Cp2;
            SQ = zeros(size(SI));

        case{'L1C'}
            P_Cd = GNSS_POWERS.GPS_L1_Cd/GNSS_POWERS.GPS_L1_C;
            P_Cp = GNSS_POWERS.GPS_L1_Cp/GNSS_POWERS.GPS_L1_C;

            S_Cd = P_Cd*spectrumgen_call('L1Cd',F);
            S_Cp = P_Cp*spectrumgen_call('L1Cp',F);

            SI = S_Cd;
            SQ = S_Cp;

        case 'E1OS'
            SI = (10/11)*spectrum_BOCs(1,1,F)+(1/11)*spectrum_BOCs(6,1,F);
            SQ = zeros(size(SI));

        % BOCc
        case {'E6A','E6PRS'}
            SI = spectrum_BOCc(10,5,F);
            SQ = zeros(size(SI));

        case {'E1A','E1PRS'}
            SI = spectrum_BOCc(15,2.5,F);
            SQ = zeros(size(SI));

        %-------------------- GPS L1  -------------------------------------
        % Prior to Block III
        case 'L1'
            P_M = GNSS_POWERS.GPS_L1_M/GNSS_POWERS.GPS_L1;
            P_P = GNSS_POWERS.GPS_L1_P/GNSS_POWERS.GPS_L1;
            P_CA =  GNSS_POWERS.GPS_L1_CA/GNSS_POWERS.GPS_L1;

            S_CA = P_CA*spectrumgen_call('L1CA',F);
            S_P = P_P*spectrumgen_call('L1P',F);
            S_M = P_M*spectrumgen_call('L1M',F);

            SI = S_P+S_M;
            SQ = S_CA;

        % Block III (includes L1C)
        case 'L1_new'

            P_M = GNSS_POWERS.GPS_L1_M/GNSS_POWERS.GPS_L1_new;
            P_P = GNSS_POWERS.GPS_L1_P/GNSS_POWERS.GPS_L1_new;
            P_CA = GNSS_POWERS.GPS_L1_CA/GNSS_POWERS.GPS_L1_new;
            P_C = GNSS_POWERS.GPS_L1_C/GNSS_POWERS.GPS_L1_new;

            S_CA = P_CA*spectrumgen_call('L1CA',F);
            S_P = P_P*spectrumgen_call('L1P',F);
            S_M = P_M*spectrumgen_call('L1M',F);
            [S_Cd, S_Cp] = spectrumgen_call('L1C',F);
            S_Cd = S_Cd*P_C;
            S_Cp = S_Cp*P_C;

            SI = S_P+S_M+S_Cd;
            SQ = S_CA+S_Cp;

        %-------------------- GPS L2  -------------------------------------
        case 'L2_new'

            P_P = GNSS_POWERS.GPS_L2_P_new/GNSS_POWERS.GPS_L2_new;
            P_C = GNSS_POWERS.GPS_L2_C/GNSS_POWERS.GPS_L2_new;
            P_M = GNSS_POWERS.GPS_L2_M/GNSS_POWERS.GPS_L2_new;

            S_P = P_P*spectrumgen_call('L2P',F);
            S_C = P_C*spectrumgen_call('L2C',F);
            S_M = P_M*spectrumgen_call('L2M',F);

            SI = S_P+S_M;
            SQ = S_C;

        case 'L2_new_2'

            P_P = GNSS_POWERS.GPS_L2_P_new/GNSS_POWERS.GPS_L2_new_2;
            P_C =  GNSS_POWERS.GPS_L2_C_new/GNSS_POWERS.GPS_L2_new_2;
            P_M =  GNSS_POWERS.GPS_L2_M/GNSS_POWERS.GPS_L2_new_2;

            S_P =   P_P*spectrumgen_call('L2P',F);
            S_C =   P_C*spectrumgen_call('L2C',F);
            S_M =   P_M*spectrumgen_call('L2M',F);

            SI = S_P+S_M;
            SQ = S_C;

        %-------------------- GPS L5  -------------------------------------
        case {'L5', 'L5_new'}
            SI = 0.5*spectrumgen_call('L5I',F);
            SQ = SI;

        %-------------------- Galileo E1 ----------------------------------
        case {'E1'}
            P_E1PRS = 0.5;
            P_E1OS = 0.5;

            S_E1PRS = P_E1PRS*spectrumgen_call('E1PRS',F);
            S_E1OS = P_E1OS*spectrumgen_call('E1OS',F);

            SI = S_E1OS;
            SQ = S_E1PRS;

        %-------------------- Galileo E6 ----------------------------------
        case 'E6'
            P_E6A = 0.5;
            P_E6B = 0.5;

            S_E6PRS = P_E6A*spectrumgen_call('E6PRS',F);
            S_E6CS = P_E6B*spectrumgen_call('E6CS',F);

            SI = S_E6CS;
            SQ = S_E6PRS;

        %-------------------- Galileo E5 ----------------------------------
       case 'E5'
            SI = 0.5*spectrum_AltBOC(15,10,F);
            SQ = SI;

        case {'E5A','E5B'}
            aux = spectrum_AltBOC(15,10,F);
            len = floor(length(aux)/2);
            aux2 = [aux(1:len+1) zeros(1,len)];
            len2 = floor(length(aux2)/2);
            [~, aux3] =  max(aux2);
            SI =  0.5*2*circshift(aux2',len2-aux3)';  % actually is not exactly multply by 2..
            SQ = SI;

        %-------------------- BeiDou-2 current ----------------------------
        % B1
        case {'B1','B11','B12'}
            SI = 0.5*spectrum_BPSK(2*1.023e6,F);
            SQ = 0.5*spectrum_BPSK(2*1.023e6,F);

        % B2
        case {'B2Q'}
            SI = spectrum_BPSK(10*1.023e6,F);
            SQ = zeros(size(SI));
        case {'B2I'}
            SI = spectrum_BPSK(2*1.023e6,F);
            SQ = zeros(size(SI));
        case{'B2'}
            P_B2Q = 0.5;
            P_B2I = 0.5;

            SQ = P_B2Q*spectrumgen_call('B2Q',F);
            SI = P_B2I*spectrumgen_call('B2I',F);

        % B3
        case{'B3'}
             SI = 0.5*spectrum_BPSK(10*1.023e6,F);
             SQ = SI;

        %-------------------- BeiDou-2 future -----------------------------
        % B1
        case {'B1Cd'}
            SI = spectrum_BOCs(1,1,F);
            SQ = zeros(size(SI));

        case {'B1Cp'}
            SI = spectrum_BOCs(6,1,F);
            SQ = zeros(size(SI));

        case {'B1C'}
            P_B1Cd = (10/11);
            P_B1Cp = (1/11);

            S_B1Cd = P_B1Cd*spectrumgen_call('B1Cd',F);
            S_B1Cp = P_B1Cp*spectrumgen_call('B1Cp',F);

            SI = S_B1Cd + S_B1Cp;
            SQ = zeros(size(SI));

        case {'B1_new'}
            SI = spectrum_BOCs(14,2,F);
            SQ = zeros(size(SI));

        case {'B1composite'}
            P_B1C = GNSS_POWERS.BEIDOU_B1C/GNSS_POWERS.BEIDOU_B1_composite;
            P_B1_new = GNSS_POWERS.BEIDOU_B1_new/GNSS_POWERS.BEIDOU_B1_composite;

            S_B1C = P_B1C*spectrumgen_call('B1C',F);
            S_B1_new = P_B1_new*spectrumgen_call('B1_new',F);

            SI = S_B1C+S_B1_new;
            SQ = zeros(size(SI));

        % B2
        case{'B2_new'}
            SI = 0.5*spectrum_AltBOC(15,10,F);
            SQ = 0.5*spectrum_AltBOC(15,10,F);

        % B3
        case{'B3_new'}
            SI = 0.5*spectrum_BPSK(10*1.023e6,F);
            SQ = 0.5*SI;
        case{'B3A'}
            S = spectrum_BOCs(15,2.5,F);
            SQ = zeros(size(SI));

        case {'B3composite'}
            P_B3_new = GNSS_POWERS.BEIDOU_B3_new/GNSS_POWERS.BEIDOU_B3_composite;
            P_B3A = GNSS_POWERS.BEIDOU_B3A/GNSS_POWERS.BEIDOU_B3_composite;

            S_B3_new = P_B3_new*spectrumgen_call('B3_new',F);
            S_B3A = P_B3A*spectrumgen_call('B3A',F);

            SI = S_B3_new;
            SQ = S_B3A;
    end
    end

function S = spectrum_BPSK(fc,F)
%==========================================================================
% BPSK of chipping rate fc.
%--------------------------------------------------------------------------
% Author: Daniel Pascual (daniel.pascual [at] protonmail.com)
% Copyright 2017 Daniel Pascual
% License: GNU GPLv3
%==========================================================================

    S = sinc((1/fc)*F);
    aux = sum(abs(S).^2)/length(S);
    S = S/sqrt(aux);
    S = abs(S).^2;
end

function S = spectrum_BOCs(n,m,F)
%==========================================================================
% sine-phased BOC even of a chipping rate fc=m*1.023e6 and sub-carrier rate
% of fs = n*1.023e6.
%--------------------------------------------------------------------------
% Author: Daniel Pascual (daniel.pascual [at] protonmail.com)
% Copyright 2017 Daniel Pascual
% License: GNU GPLv3
%==========================================================================

    fs = n*1.023e6;
    fc = m*1.023e6;

    S = sinc((1/fc)*F).*tan(pi*F/(2*fs));
    aux = sum(abs(S).^2)/length(S);
    S = S/sqrt(aux);
    S = abs(S).^2;
end

function S = spectrum_BOCc(n,m,F)
%==========================================================================
% cosine-phased BOC even of a chipping rate fc=m*1.023e6 and sub-carrier
% rate of fs = n*1.023e6.
%--------------------------------------------------------------------------
% Author: Daniel Pascual (daniel.pascual [at] protonmail.com)
% Copyright 2017 Daniel Pascual
% License: GNU GPLv3
%==========================================================================

    fs = n*1.023e6;
    fc = m*1.023e6;

    S = 2*sinc((1/fc)*F).*((sin(pi*F/(4*fs)).^2)./cos(pi*F/(2*fs)));
    aux = sum(abs(S).^2)/length(S);
    S = S/sqrt(aux);
    S = abs(S).^2;
end

function S = spectrum_AltBOC(n,m,F)
%==========================================================================
% sine-phased AltBOC even of a chipping rate fc=m*1.023e6 and sub-carrier
% rate of fs = n*1.023e6.
%--------------------------------------------------------------------------
% Author: Daniel Pascual (daniel.pascual [at] protonmail.com)
% Copyright 2017 Daniel Pascual
% License: GNU GPLv3
%==========================================================================

    fs = n*1.023e6;
    fc = m*1.023e6;

    S = (4*fc./((pi*F).^2)).* ((cos(pi*F/(fc))).^2).* (((cos(pi*F/(2*fs))).^2) - cos(pi*F/(2*fs)) - 2*cos(pi*F/(2*fs)).*cos(pi*F/(4*fs)) + 2 ) ./ ((cos(pi*F/(2*fs))).^2);
    S_ = S;
    S_(find(isnan(S))) = 0;
    aux = sum(abs(S_))/length(S);
    S = S/aux;
    S = abs(S);
end


function [p] = dot(qt1,qt2, do_qr)
%Dot product of two QTT-Tuckers
%   [PR]=DOT(TT1,TT2) -- dot product of two TT-tensors
%
%   [PR]=DOT(TT1,TT2, DO_QR) if DO_QR==true is specified, perform the
%   left-to-right QRs of TT1,TT2
%   before the scalar product. It increases the  accuracy in some cases.
%
% In general, returns a 4D tensor of sizes
% r0(tt1), r0(tt2), rd(tt1), rd(tt2)
% If r0(tt1) = r0(tt2) = 1 it returns a matrix of size rd(tt1) x rd(tt2)
%
%
% 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<3)||(isempty(do_qr))
    do_qr = false;
end;

d=qt1.dphys;
for i=1:d
    % dot product of factors gives rt1 times rt2 matrix, to be convolved
    % between cores
    % Make it more precise by QRs? <-- already inside @tt_tensor/dot
%     [qt1.tuck{i}, rv1]=qr(qt1.tuck{i}, 'lr');
%     [qt2.tuck{i}, rv2]=qr(qt2.tuck{i}, 'lr');
%     rv1 = eye(qt1.tuck{i}.r(end));
%     rv2 = eye(qt2.tuck{i}.r(end));
    Pfac = squeeze(dot(qt1.tuck{i}, qt2.tuck{i}, do_qr)); % size rt1,rt2
%     rt1 = size(rv1, 2); rt2 = size(rv2, 2);
%     rt1new = size(rv1,1); rt2new = size(rv2,1);
    % Now, merge Pfac to the core of qt1
    curcr = qt2.core{i};
    rc1 = size(curcr, 1); rt2 = size(curcr, 2); rc2 = size(curcr, 3);
    curcr = permute(curcr, [1, 3, 2]);
    curcr = reshape(curcr, rc1*rc2, rt2);
%     curcr = curcr*(rv1.');
    curcr = curcr*(Pfac.'); % Now, core2 has the tucker ranks of qt1
    curcr = reshape(curcr, rc1, rc2, qt1.core.n(i));
    qt2.core{i} = permute(curcr, [1, 3, 2]);

%     curcr = qt2.core{i};
%     rc1 = size(curcr, 1); rc2 = size(curcr, 3);
%     curcr = permute(curcr, [1, 3, 2]);
%     curcr = reshape(curcr, rc1*rc2, rt2);
%     curcr = curcr*(rv2.');
%     curcr = reshape(curcr, rc1, rc2, rt2new);
%     qt2.core{i} = permute(curcr, [1, 3, 2]);
end;
% Finaly, dot product of cores. It is consistent, since we merged Pfacs
p = dot(qt1.core, qt2.core, do_qr);
end


function Population = LCSA_NSGAIIIEnvironmentalSelection(Population,N,Z,Zmin)
% -----------------------------------------------------------------------
%  Copyright (C) 2020 Heiner Zille
%
%  This work is licensed under the Creative Commons Attribution-NonCommercial-ShareAlike 4.0
%  International License. (CC BY-NC-SA 4.0). To view a copy of this license,
%  visit http://creativecommons.org/licenses/by-nc-sa/4.0/ or see the
%  pdf-file "License-CC-BY-NC-SA-4.0.pdf" that came with this code.
%
%  You are free to:
%  * Share ? copy and redistribute the material in any medium or format
%  * Adapt ? remix, transform, and build upon the material
%  Under the following terms:
%  * Attribution ? You must give appropriate credit, provide a link to the
%     license, and indicate if changes were made. You may do so in any reasonable
%     manner, but not in any way that suggests the licensor endorses you or your use.
%  * NonCommercial ? You may not use the material for commercial purposes.
%  * ShareAlike ? If you remix, transform, or build upon the material, you must
%    distribute your contributions under the same license as the original.
%  * No additional restrictions ? You may not apply legal terms or technological
%    measures that legally restrict others from doing anything the license permits.
%
%  Author of this Code:
%   Heiner Zille <heiner.zille@ovgu.de> or <heiner.zille@gmail.com>
%
%  This code is based on the following publications:
%
%  1) Heiner Zille
%     "Large-scale Multi-objective Optimisation: New Approaches and a Classification of the State-of-the-Art"
%     PhD Thesis, Otto von Guericke University Magdeburg, 2019
%     http://dx.doi.org/10.25673/32063
%
%  2) Heiner Zille and Sanaz Mostaghim
%     "Linear Search Mechanism for Multi- and Many-Objective Optimisation"
%     10th International Conference on Evolutionary Multi-Criterion Optimization (EMO 2019),
%        Lecture Notes in Computer Science, vol 11411.
%        Deb K. et al. (eds), Springer, Cham, East Lansing, Michigan, USA, March 2019
%     https://doi.org/10.1007/978-3-030-12598-1_32.
%
%  This file is intended to work with the PlatEMO framework version 2.5.
%  Date of publication of this code: 06.04.2020
%  Last Update of this code: 06.04.2020
%  A newer version of this algorithm may be available. Please contact the author
%  or see http://www.ci.ovgu.de/Research/Codes.html.
%
% The files may have been modified in Feb 2021 by the authors of the Platemo framework to work with the Platemo 3.0 release.
% -----------------------------------------------------------------------
% This file is derived from its original version containied in the PlatEMO
% framework.
% -----------------------------------------------------------------------

    if isempty(Zmin)
        Zmin = ones(1,size(Z,2));
    end

    %% Non-dominated sorting
    [FrontNo,MaxFNo] = NDSort(Population.objs,Population.cons,N);
    Next = FrontNo < MaxFNo;

    %% Select the solutions in the last front
    Last   = find(FrontNo==MaxFNo);
    Choose = LastSelection(Population(Next).objs,Population(Last).objs,N-sum(Next),Z,Zmin);
    Next(Last(Choose)) = true;
    % Population for next generation
    Population = Population(Next);
end

function Choose = LastSelection(PopObj1,PopObj2,K,Z,Zmin)
% Select part of the solutions in the last front

    PopObj = [PopObj1;PopObj2] - repmat(Zmin,size(PopObj1,1)+size(PopObj2,1),1);
    [N,M]  = size(PopObj);
    N1     = size(PopObj1,1);
    N2     = size(PopObj2,1);
    NZ     = size(Z,1);

    %% Normalization
    % Detect the extreme points
    Extreme = zeros(1,M);
    w       = zeros(M)+1e-6+eye(M);
    for i = 1 : M
        [~,Extreme(i)] = min(max(PopObj./repmat(w(i,:),N,1),[],2));
    end
    % Calculate the intercepts of the hyperplane constructed by the extreme
    % points and the axes
    Hyperplane = PopObj(Extreme,:)\ones(M,1);
    a = 1./Hyperplane;
    if any(isnan(a))
        a = max(PopObj,[],1)';
    end
    % Normalization
    PopObj = PopObj./repmat(a',N,1);

    %% Associate each solution with one reference point
    % Calculate the distance of each solution to each reference vector
    Cosine   = 1 - pdist2(PopObj,Z,'cosine');
    Distance = repmat(sqrt(sum(PopObj.^2,2)),1,NZ).*sqrt(1-Cosine.^2);
    % Associate each solution with its nearest reference point
    [d,pi] = min(Distance',[],1);

    %% Calculate the number of associated solutions except for the last front of each reference point
    rho = hist(pi(1:N1),1:NZ);

    %% Environmental selection
    Choose  = false(1,N2);
    Zchoose = true(1,NZ);
    % Select K solutions one by one
    while sum(Choose) < K
        % Select the least crowded reference point
        Temp = find(Zchoose);
        Jmin = find(rho(Temp)==min(rho(Temp)));
        j    = Temp(Jmin(randi(length(Jmin))));
        I    = find(Choose==0 & pi(N1+1:end)==j);
        % Then select one solution associated with this reference point
        if ~isempty(I)
            if rho(j) == 0
                [~,s] = min(d(N1+I));
            else
                s = randi(length(I));
            end
            Choose(I(s)) = true;
            rho(j) = rho(j) + 1;
        else
            Zchoose(j) = false;
        end
    end
end


function beta=calcStarBetaskBest(A,K)
%%CALCSTARBETASKBEST Calculate target-measurement association probabilities,
%                    as in the JPDAFStar given an association matrix using
%                    only the k-best hypotheses for the computation of the
%                    target-measurement association probabilities rather
%                    than goign through all joint association events.
%
%INPUTS:   A  A numTar X numMeas matrix of all-positive likelihood
%              ratios for assigning the target specified by the row to the
%              measurement specified by the column.
%          K  The number of hypotheses to generate.
%
%OUTPUTS:  beta A numTar X (numMeas+1) matrix of probabilities of assigning
%               the target given by the row to the measurement given by the
%               column. The final column is a set of missed detection
%               probabilities.
%
%The general idea behind this function is the same as that of the function
%calcStarBetasBF. However, rather than generating all possible
%joint association events, only the k-best events are generated.
%
%To reformulate the problem such that a 2D assignment algorithm can be
%used, the logarithm of the likelihood ratios must be taken, then the cost
%values are exponentiated to get traditional probabilities.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%In order to be able to use the k-Best 2D assignment algorithm, the
%association matrix must be augmented with extra columns that represent
%missed detection events. When the likelihood ratios in A have been
%appropriately computed, the likelihood ratio of a missed detection event
%is equal to one.

numTar=size(A,1);
numMeas=size(A,2);

%Adjust the cost matrix so that 2D assignment can be use.
A=log(A);

%Alocate space for the results.
beta=zeros(numTar,numMeas+1);

%Augment the A matrix to handle missed detection hypotheses. These are
%supposed to have a likelihood ratio of 1, so  the logarithm should be
%zero.
AClut=1-eye(numTar);
AClut(AClut==1)=-inf;
[col4rowBest,row4ColBest,gainBest]=kBest2DAssignment([A,AClut],K,true);

%Undo the logarithm from before.
gainBest=exp(gainBest);

%Adjust K to reflect the fact that fewer than K hypotheses might have been
%present.
K=size(col4rowBest,2);

%For each hypothesis, determine the set of measurements that are
%target-originated and save them order in tarMeas. Also find the set of
%targets that are observed and save them in obsTar.
tarMeas=zeros(numTar,K);
obsTar=zeros(numMeas,K);
for curHyp=1:K
    tarMeas(:,curHyp)=col4rowBest(:,curHyp);

    %Remove elements that are missed detections.
    misSel=col4rowBest(:,curHyp)>numMeas;
    tarMeas(misSel,curHyp)=0;

    %Sort the result.
    tarMeas(:,curHyp)=sort(tarMeas(:,curHyp));
    obsTar(:,curHyp)=sort(row4ColBest(1:numMeas,curHyp));
end

%For each set of observed targets and measurements that are
%target-originated, go through and only keep the most likely hypothesis.
%Since everything is already ordered in decreasing gain, the first
%occurrence of each set of observed targets and target-originated
%measurements is the most likely.
validHyps=true(K,1);
for curHyp=1:K
    if(validHyps(curHyp)==false)
        continue;
    end
    validHyps(curHyp)=false;

    for HypS=(curHyp+1):K
        if(isequal(tarMeas(:,curHyp),tarMeas(:,HypS))&&isequal(obsTar(:,curHyp),obsTar(:,HypS)))
            validHyps(HypS)=false;
        end
    end

    %Add the ML hypothesis with the given set of observed targets and
    %target-originated measurements to the beta terms.
    for curTar=1:numTar
        curMeas=col4rowBest(curTar,curHyp);
        if(curMeas>numMeas)
            %If it is a missed detection.
             beta(curTar,end)=beta(curTar,end)+gainBest(curHyp);
        else
            beta(curTar,curMeas)=beta(curTar,curMeas)+gainBest(curHyp);
        end
    end
end

%Normalize the betas so that sum of the probabilities of each target being
%assigned to a measurement or a missed detection sums to one.
beta=bsxfun(@rdivide,beta,sum(beta,2));

end

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


function value = dzasum ( n, x, incx )

%*****************************************************************************80
%
%% DZASUM takes the sum of the absolute values of a complex vector.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 May 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979.
%
%    Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
%    Basic Linear Algebra Subprograms for FORTRAN usage,
%    ACM Transactions on Mathematical Software,
%    Volume 5, Number 3, pages 308-323, 1979.
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Input, complex X(*), the vector.
%
%    Input, integer INCX, the increment between successive entries of X.
%
%    Output, real VALUE, the sum of the absolute values.
%
  value = sum ( abs ( real ( x(1:incx:1+(n-1)*incx) ) ) ...
              + abs ( imag ( x(1:incx:1+(n-1)*incx) ) ) );

  return
end


function F = erf(F, varargin)
%ERF   Error function of a CHEBFUN.
%   ERF(X) is the error function of the real-valued CHEBFUN X.
%
%   The error function is defined as:
%       erf(X)(s) = 2/sqrt(pi) * integral from 0 to X(s) of exp(-t^2) dt.
%
% See also ERFC, ERFCX, ERFINV, ERFCINV.

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

% Input must be real:
if ( ~isreal(F) )
    error('CHEBFUN:CHEBFUN:erf:notreal', 'Input must be real.');
end

% Call the compose method:
F = compose(F, @erf, varargin{:});

end


function surf = imJointSurfaceArea(img, L1, L2, varargin)
% Surface area of the interface between two labels.
%
%   S = imJointSurfaceArea(LBL, L1, L2)
%   Estimates the joint surface area between the two labels L1 and L2 in
%   the label image LBL.
%
%   S = imJointSurfaceArea(LBL, L1, L2, NDIRS)
%   Specifies the number of directions used for estimating surface area.
%   NDIRS can be either 3 or 13 (the default).
%
%   S = imJointSurfaceArea(..., RESOL)
%   Specifies image resolution. RESOL is a 1-by-3 row vector containing
%   resolution in the X, Y and Z direction (in that order).
%
%
%   Example
%     % generate a demo image
%     img = discreteBall(1:10, 1:100, 1:100, [50.12 50.23 50.34 40]);
%     % convert to image with two different labels
%     img2 = uint8(img + 1);
%     % compute joint surface area
%     imJointSurfaceArea(img2, 1, 2)
%     ans =
%         2.0102e+004
%
%   See also
%     imSurfaceArea

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2010-07-26,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

% check image dimension and type
if ndims(img) ~= 3 || islogical(img)
    error('first argument should be a 3D image');
end

%% Process input arguments

% default number of directions
ndir = 13;

% default image resolution
delta = [1 1 1];

% Process user input arguments
while ~isempty(varargin)
    var = varargin{1};
    if ~isnumeric(var)
        error('option should be numeric');
    end

    % option is either connectivity or resolution
    if isscalar(var)
        ndir = var;
    else
        delta = var;
    end
    varargin(1) = [];
end

%% Initialisations

% distances between a pixel and its neighbours.
d1  = delta(1);
d2  = delta(2);
d3  = delta(3);

% volume of a voxel (used for computing line densities)
vol = d1 * d2 * d3;

%% Main processing for 3 directions

% number of transitions along the 3 main directions
n1a = sum(sum(sum(img(:,1:end-1,:)==L1 & img(:,2:end,:)==L2)));
n1b = sum(sum(sum(img(:,1:end-1,:)==L2 & img(:,2:end,:)==L1)));
n2a = sum(sum(sum(img(1:end-1,:,:)==L1 & img(2:end,:,:)==L2)));
n2b = sum(sum(sum(img(1:end-1,:,:)==L2 & img(2:end,:,:)==L1)));
n3a = sum(sum(sum(img(:,:,1:end-1)==L1 & img(:,:,2:end)==L2)));
n3b = sum(sum(sum(img(:,:,1:end-1)==L2 & img(:,:,2:end)==L1)));

if ndir == 3
    % compute surface area by averaging over the 3 main directions
    surf = 4/3 * ((n1a+n1b)/d1 + (n2a+n2b)/d2 + (n3a+n3b)/d3) / 2 * vol;
    return;
end

%% Additional processing for 13 directions

% Number of connected components along diagonals contained in the three
% main planes
n4a = sum(sum(sum(img(2:end,1:end-1,:)==L1   & img(1:end-1,2:end,:)==L2)));
n4b = sum(sum(sum(img(2:end,1:end-1,:)==L2   & img(1:end-1,2:end,:)==L1)));
n5a = sum(sum(sum(img(1:end-1,1:end-1,:)==L1 & img(2:end,2:end,:)==L2)));
n5b = sum(sum(sum(img(1:end-1,1:end-1,:)==L2 & img(2:end,2:end,:)==L1)));
n6a = sum(sum(sum(img(:,2:end,1:end-1)==L1   & img(:,1:end-1,2:end)==L2)));
n6b = sum(sum(sum(img(:,2:end,1:end-1)==L2   & img(:,1:end-1,2:end)==L1)));
n7a = sum(sum(sum(img(:,1:end-1,1:end-1)==L1 & img(:,2:end,2:end)==L2)));
n7b = sum(sum(sum(img(:,1:end-1,1:end-1)==L2 & img(:,2:end,2:end)==L1)));
n8a = sum(sum(sum(img(2:end,:,1:end-1)==L1   & img(1:end-1,:,2:end)==L2)));
n8b = sum(sum(sum(img(2:end,:,1:end-1)==L2   & img(1:end-1,:,2:end)==L1)));
n9a = sum(sum(sum(img(1:end-1,:,1:end-1)==L1 & img(2:end,:,2:end)==L2)));
n9b = sum(sum(sum(img(1:end-1,:,1:end-1)==L2 & img(2:end,:,2:end)==L1)));

%TODO: add the case of 9 directions ?

% Number of connected components along lines corresponding to diagonals of
% the unit cube
n10a = sum(sum(sum(img(1:end-1,1:end-1,1:end-1)==L1 & img(2:end,2:end,2:end)==L2)));
n10b = sum(sum(sum(img(1:end-1,1:end-1,1:end-1)==L2 & img(2:end,2:end,2:end)==L1)));
n11a = sum(sum(sum(img(2:end,1:end-1,1:end-1)==L1 & img(1:end-1,2:end,2:end)==L2)));
n11b = sum(sum(sum(img(2:end,1:end-1,1:end-1)==L2 & img(1:end-1,2:end,2:end)==L1)));
n12a = sum(sum(sum(img(1:end-1,2:end,1:end-1)==L1 & img(2:end,1:end-1,2:end)==L2)));
n12b = sum(sum(sum(img(1:end-1,2:end,1:end-1)==L2 & img(2:end,1:end-1,2:end)==L1)));
n13a = sum(sum(sum(img(2:end,2:end,1:end-1)==L1 & img(1:end-1,1:end-1,2:end)==L2)));
n13b = sum(sum(sum(img(2:end,2:end,1:end-1)==L2 & img(1:end-1,1:end-1,2:end)==L1)));

% space between 2 voxels in each direction
d12  = hypot(d1, d2);
d13  = hypot(d1, d3);
d23  = hypot(d2, d3);
d123 = sqrt(d1^2 + d2^2 + d3^2);

% Compute weights corresponding to surface fraction of spherical caps
% For isotropic case, weights correspond to:
% c1 = 0.04577789120476 * 2;  % Ox
% c2 = 0.04577789120476 * 2;  % Oy
% c3 = 0.04577789120476 * 2;  % Oz
% c4 = 0.03698062787608 * 2;  % Oxy
% c6 = 0.03698062787608 * 2;  % Oxz
% c8 = 0.03698062787608 * 2;  % Oyz
% c10 = 0.03519563978232 * 2;  % Oxyz
c = computeDirectionWeights3d13(delta);

% compute the weighted sum of each direction
% intersection count * direction weight / line density
surf = 4 * vol * (...
    (n1a+n1b)*c(1)/d1 + (n2a+n2b)*c(2)/d2 + (n3a+n3b)*c(3)/d3 + ...
    (n4a+n4b+n5a+n5b)*c(4)/d12 + (n6a+n6b+n7a+n7b)*c(6)/d13 + ...
    (n8a+n8b+n9a+n9b)*c(8)/d23 + ...
    (n10a + n10b + n11a + n11b + n12a + n12b + n13a + n13b) * c(10) / d123) / 2;


function ker0_values_test ( )

%*****************************************************************************80
%
%% KER0_VALUES_TEST demonstrates the use of KER0_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 February 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'KER0_VALUES_TEST:\n' );
  fprintf ( 1, '  KER0_VALUES stores values of \n' );
  fprintf ( 1, '  the Kelvin function KER of order 0.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      X           FX\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

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

    if ( n_data == 0 )
      break
    end

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

  end

  return
end


function  bdiffma(newcat)
    %  This routine etsimates the b-value of a curve automatically
    %  The b-valkue curve is differenciated and the point
    %  of maximum curvature marked. The b-value will be calculated
    %  using this point and the point half way toward the high
    %  magnitude end of the b-value curve.

    %  Stefan Wiemer 1/95
    %
    think
    %zmap_message_center.set_info('  ','Calculating b-value...')
    global cluscat mess bfig backcat xt3 bvalsum3
    global  ttcat teb t0b cua b1 b2 n1 n2
    report_this_filefun(mfilename('fullpath'));

    [existFlag,figNumber]=figure_exists('frequency-magnitude distribution',1);
    if existFlag
        % figure_w_normalized_uicontrolunits(bfig);
        bfig = figNumber;
    else
        bfig=figure_w_normalized_uicontrolunits(...                  %build figure for plot
            'Units','normalized','NumberTitle','off',...
            'Name','frequency-magnitude distribution',...
            'MenuBar','none',...
            'visible','off',...
            'pos',[ 0.300  0.7 0.5 0.5]);

        uicontrol('Units','normal',...
            'Position',[.0 .85 .08 .06],'String','Info ',...
             'Callback','infoz(1)');
        uicontrol('Units','normal',...
            'Position',[.0 .55 .10 .06],'String','Manual ',...
             'Callback','bfitnew(newcat)');
        matdraw

        uicontrol('Units','normal',...
            'Position',[.0 .65 .08 .06],'String','Save ',...
             'Callback',{@calSave9,xt3, bvalsum3})

    end

    maxmag = max(newcat.Magnitude);
    mima = min(newcat.Magnitude);
    if mima > 0 ; mima = 0 ; end

    % number of mag units
    nmagu = (maxmag*10)+1;

    bval = zeros(1,nmagu);
    bvalsum = zeros(1,nmagu);
    bvalsum3 = zeros(1,nmagu);

    [bval,xt2] = hist(newcat.Magnitude,(mima:0.1:maxmag));
    bvalsum = cumsum(bval);                        % N for M <=
    bvalsum3 = cumsum(bval(length(bval):-1:1));    % N for M >= (counted backwards)
    xt3 = (maxmag:-0.1:mima);

    backg_be = log10(bvalsum);
    backg_ab = log10(bvalsum3);
    orient tall

    if hold_state
        axes(cua)
        hold on
    else
        figure_w_normalized_uicontrolunits(bfig);delete(gca);delete(gca);delete(gca);delete(gca)
        rect = [0.2,  0.3, 0.70, 0.6];           % plot Freq-Mag curves
        axes('position',rect);
    end

    pl =semilogy(xt3,bvalsum3,'b');
    set(pl,'LineWidth',2.0)
    hold on
    %semilogy(xt3,bvalsum3,'om')
    difb = [0 diff(bvalsum3) ];
    %pl =semilogy(xt3,difb,'g');
    %set(pl,'LineWidth',2.0)
    %semilogy(xt3,difb,'g')
    grid

    % Marks the point of maximum curvature
    %
    i = find(difb == max(difb));
    i = max(i);
    %te = semilogy(xt3(i),difb(i),'xk');
    %set(te,'LineWidth',2,'MarkerSize',ms10)
    %te = semilogy(xt3(i),bvalsum3(i),'xk');
    %set(te,'LineWidth',2,'MarkerSize',ms10)

    % Estimate the b-value
    %
    i2 = 1 ;
    %te = semilogy(xt3(i2),difb(i2),'xk');
    %set(te,'LineWidth',2,'MarkerSize',ms10)
    %te = semilogy(xt3(i2),bvalsum3(i2),'xk');
    %set(te,'LineWidth',2,'MarkerSize',ms10)

    xlabel('Magnitude','FontWeight','bold','FontSize',ZmapGlobal.Data.fontsz.m)
    ylabel('Cumulative Number','FontWeight','bold','FontSize',ZmapGlobal.Data.fontsz.m)
    set(gca,'Color',color_bg)
    set(gca,'visible','on','FontSize',ZmapGlobal.Data.fontsz.m,'FontWeight','bold',...
        'FontWeight','bold','LineWidth',1.5,...
        'Box','on')

    cua = gca;

    par2 = 0.1 * max(bvalsum3);
    par3 = 0.12 * max(bvalsum3);
    M1b = [];
    M1b = [xt3(i) bvalsum3(i)];
    tt3=num2str(fix(100*M1b(1))/100);
    text( M1b(1),M1b(2),['|: M1=',tt3],'Fontweight','bold' )

    M2b = [];
    M2b =  [xt3(i2) bvalsum3(i2)];
    tt4=num2str(fix(100*M2b(1))/100);
    %text( M2b(1),M2b(2),['|: M2=',tt4],'Fontweight','bold' )

    ll = xt3 >= M1b(1) & xt3 <= M2b(1);
    x = xt3(ll);

    [ av,bv,si] = bmemag(newcat);

    pause(0.1)

    y = backg_ab(ll);
    %[p,s] = polyfit(x,y,1)                    % fit a line to background
    [aw bw,  ew] = wls(x',y');
    p = [bw aw];
    f = polyval(p,x);
    (teb-t0b)/(10.^ polyval(p,5))
    (teb-t0b)/(10.^ polyval(p,6))
    (teb-t0b)/(10.^ polyval(p,7))
    (teb-t0b)/(10.^ polyval(p,8))
    f = 10.^f;
    hold on
    ttm= semilogy(x,f,'r');                         % plot linear fit to backg
    set(ttm,'LineWidth',1)
    set(gca,'XLim',[min(newcat.Magnitude)-0.5  max(newcat.Magnitude)+1.0])
    r = corrcoef(x,y);
    r = r(1,2);
    std_backg = ew;      % standard deviation of fit

    p=-p(1,1);
    p=fix(100*p)/100;
    std_backg=fix(100*std_backg)/100;
    tt2=num2str(std_backg);
    tt1=num2str(p);
    tt4=num2str(bv,2);
    tt5=num2str(si,2);

    rect=[0 0 1 1];
    h2=axes('position',rect);
    set(h2,'visible','off');

    if ~ho
        txt1=text(.16, .18,['b-value (w LS, M  > ', num2str(M1b(1)) '): ',tt1, ' +/- ', tt2]);
        set(txt1,'FontWeight','bold','FontSize',ZmapGlobal.Data.fontsz.m)
        txt1=text(.16, .12,['b-value (max lik, M > ', num2str(min(newcat.Magnitude)) '): ',tt4, ' +/- ', tt5]);
        set(txt1,'FontWeight','bold','FontSize',ZmapGlobal.Data.fontsz.m)

    else
        txt1=text(.16, .06,['b-value (weighted least square): ',tt1, ' +/- ', tt2]);
        set(txt1,'FontWeight','bold','FontSize',ZmapGlobal.Data.fontsz.m,'Color','r')
    end
    set(gcf,'visible','on');
    zmap_message_center.set_info('  ','Done')
    done

    if hold_state
        % calculate the probability that the two distributins are differnt
        b2 = str2double(tt1); n2 = newcat.Count;
        n = n1+n2;
        da = -2*n*log(n) + 2*n1*log(n1+n2*b1/b2) + 2*n2*log(n1*b2/b1+n2) -2;
        pr = exp(-da/2-2);
        disp(['Probability: ',  num2str(pr)]);
        txt1=text(.65, .85,['Utsu Test: ', num2str(pr)]);
        set(txt1,'FontWeight','bold','FontSize',ZmapGlobal.Data.fontsz.m)
    else
        b1 = str2double(tt1); n1 = newcat.Count;
    end


function [x, infos] = robust_online_mu_nmf(V, rank, in_options)
% Robust online non-negative matrix factorization (ONMF) with outliers (RONMF) algorithm.
%
% Inputs:
%       matrix      V
%       rank        rank
%       in_options  options
% Output:
%       w           solution of w
%       infos       information
%
% References:
%       R. Zhao and Y. F. Tan,
%       "Online nonnegative matrix factorization with outliers,"
%       ICASSP, 2016.
%
%
% This file is part of NMFLibrary.
%
% Created by H.Sakai and H.Kasai on Feb. 12, 2017
%
% Change log:
%
%       May. 20, 2019 (Hiroyuki Kasai): Added initialization module.
%
%       Jul. 12, 2022 (Hiroyuki Kasai): Modified code structures.
%

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

    % set local options
    local_options = [];
    local_options.lambda        = 1;
    local_options.x_init_robust = true;

    % 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);

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

    % initialize
    method_name = 'Robust-Online-MU';
    epoch = 0;
    grad_calc_count = 0;
    l = zeros(m, options.batch_size) + options.lambda;

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

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

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

    % set start time
    start_time = tic();

    % main outer 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

        % Reset sufficient statistic
        At = zeros(m, rank);
        Bt = zeros(rank, rank);
        Ct = zeros(m, rank);

        % main inner loop
        for t = 1 : options.batch_size : n - 1

            % Retrieve vt, ht and rt
            vt = V(:, t:t+options.batch_size-1);
            ht = H(:, t:t+options.batch_size-1);
            rt = R(:, t:t+options.batch_size-1);

            % update ht/rt
            ht = ht .* (Wt.' * vt) ./ (Wt.' * (Wt * ht + rt));
            ht = ht + (ht<eps) .* eps;
            rt = rt .* vt ./ (Wt * ht + rt + l);

            % update sufficient statistics
            At = At + vt *  ht';
            Bt = Bt + ht *  ht';
            Ct = Ct + rt *  ht';

            % Update Wt
            Wt = Wt .* At ./ (Wt * Bt + Ct);
            Wt = Wt + (Wt<eps) .* eps;

            % Update H
            H(:,t:t+options.batch_size-1) = ht;

            % Update R
            R(:,t:t+options.batch_size-1) = rt;

            grad_calc_count = grad_calc_count + m * options.batch_size;
        end

        % measure elapsed time
        elapsed_time = toc(start_time);

        % update epoch
        epoch = epoch + 1;

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

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

    end

    x.W = Wt;
    x.H = H;
    x.R = R;

end


function fmincon_test02 ( )

%*****************************************************************************80
%
%% FMINCON_TEST02 minimizes the Niederreiter-McCurley function 2.
%
%  Discussion:
%
%    The value determined by FMINCON seems highly sensitive to the
%    starting condition.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 September 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Harald Niederreiter, Kevin McCurley,
%    Optimization of functions by quasi-random search methods,
%    Computing,
%    Volume 22, Number 2, 1979, pages 119-123.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FMINCON_TEST02\n' );
  fprintf ( 1, '  Minimize the negative Niederreiter-McCurley #2 function\n' );

  n = 4;
  lb = [ 0, 0, 0, 0 ];
  ub = [ 1, 1, 1, 1 ];
  x0 = ( lb + ub ) / 2.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Initial data:\n' );
  fprintf ( 1, '\n' );
  for j = 1 : n
    fprintf ( 1, '  ---X(%d)---', j );
  end
  fprintf ( 1, '  ----F(X)----\n' );
  fprintf ( 1, '\n' );
  for j = 1 : n
    fprintf ( 1, '  %10f', x0(j) );
  end
  fprintf ( 1, '  %12e\n', - niederreiter_mccurley2 ( x0 ) );

  [ x, fval, exitflag, output ] = fmincon ( @niederreiter_mccurley2, x0, ...
    [], [], [], [], lb, ub );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Computed optimum after %d function evaluations:\n', ...
    output.funcCount );
  fprintf ( 1, '\n' );
  for j = 1 : n
    fprintf ( 1, '  ---X(%d)---', j );
  end
  fprintf ( 1, '  ----F(X)----\n' );
  fprintf ( 1, '\n' );
  for j = 1 : n
    fprintf ( 1, '  %10f', x(j) );
  end
  fprintf ( 1, '  %12e\n', - niederreiter_mccurley2 ( x ) );

  return
end


function[varargout] = vmedian(varargin)
%VMEDIAN  Median over finite elements along a specified dimension.
%
%   Y=VMEDIAN(X,DIM) takes the median of all finite elements of X along
%   dimension DIM.
%
%   [Y1,Y2,...YN]=VMEDIAN(X1,X2,...XN,DIM) also works.
%
%   VMEDIAN(X1,X2,...XN,DIM); with no output arguments overwrites the
%   original input variables.
%
%   VMEDIAN, like MATLAB's MEDIAN, defines the median over an even
%   number of values to be the average of the two middle elements.
%
%   VMEDIAN uses a fast algorithm which can be several times faster
%   than MEDIAN, but unlike MEDIAN, excludes both INFs and NANs.
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2008--2015 J.M. Lilly --- type 'help jlab_license' for details

if strcmpi(varargin{1}, '--t')
  vmedian_test,return
end

dim=varargin{end};

for i=1:length(varargin)-1
   if isreal(varargin{i})
       varargout{i}=vmedian1(varargin{i},dim);
   else
       varargout{i}=vmedian1(real(varargin{i}),dim)+sqrt(-1)*vmedian1(imag(varargin{i}),dim);
   end
end

eval(to_overwrite(nargin-1))

function[med]=vmedian1(data,dim)

numel=sum(isfinite(data),dim);
med=vzeros(size(numel),'nan');

temp=vswap(data,nan,inf);
temp=vswap(temp,-inf,inf);
sorted=sort(temp,dim,'ascend');
sorted=permute(sorted,[1:dim-1 dim+1:ndims(sorted) dim]);
sorted=reshape(sorted,[length(numel(:)),size(sorted,ndims(sorted))]);

ii=(1:length(numel(:)))';
ii=reshape(ii,size(numel));

boolodd=isodd(numel);
booleven=~boolodd&(numel~=0);

%ii(boolodd)

%(numel(boolodd)+1)./2

%looking to fix bug reported by Sandra
%indexodd=sub2ind(size(sorted),ii(boolodd),(numel(boolodd)+1)./2);
%indexeven1=sub2ind(size(sorted),ii(booleven),numel(booleven)./2);
%indexeven2=sub2ind(size(sorted),ii(booleven),numel(booleven)./2+1);

% vsize(sorted,ii,find(boolodd))
% anyany(~isfinite(boolodd))
% minmin(numel(boolodd))
% maxmax(numel(boolodd))
% median(vcolon(numel(boolodd)),1)
% minmin((numel(boolodd)+1)/2)
% maxmax((numel(boolodd)+1)/2)
% median((numel(boolodd)+1)/2,1)

if ~isempty(boolodd)
    indexodd=sub2ind(size(sorted),ii(boolodd),(numel(boolodd)+1)./2);
    med(boolodd)=sorted(indexodd);
end
if ~isempty(booleven)
    indexeven1=sub2ind(size(sorted),ii(booleven),numel(booleven)./2);
    indexeven2=sub2ind(size(sorted),ii(booleven),numel(booleven)./2+1);
    med(booleven)=sorted(indexeven1)./2+sorted(indexeven2)./2;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function[]=vmedian_test

N=6;
x1=randn(N,N,N,N,N);
bool=false(5,1);
tol=1e-4;

etime1=0;
etime2=0;
for i=1:5
    tic;y1=vmedian(x1,i);etime1=etime1+toc;
    tic;z1=median(x1,i);etime2=etime2+toc;
    bool(i)=aresame(y1,z1,tol);
end
reporttest('VMEDIAN 5-D with no NANs versus MEDIAN', allall(bool))
disp(['VMEDIAN was ' num2str(etime2./etime1) ' faster than MEDIAN.'])

N=30;
x1=randn(N,N,N);
x1(2:7:end)=nan;

tic
y1=vmedian(x1,1);
y2=vmedian(x1,2);
y3=vmedian(x1,3);
etime1=toc;

z1=zeros(size(y1));
z2=zeros(size(y2));
z3=zeros(size(y3));

tic
for i=1:N
    for j=1:N
        z1(1,i,j)=median(x1(isfinite(x1(:,i,j)),i,j),1);
        z2(i,1,j)=median(x1(i,isfinite(x1(i,:,j)),j),2);
        z3(i,j)=median(x1(i,j,isfinite(x1(i,j,:))),3);
    end
end
etime2=toc;
tol=1e-4;
reporttest('VMEDIAN 3-D including NANs', aresame(y1,z1,tol) && aresame(y2,z2,tol)&&aresame(y3,z3,tol))
disp(['VMEDIAN was ' num2str(etime2./etime1) ' faster than loop with MEDIAN.'])

N=30;
x1=round(randn(N,N,N));
x1(2:7:end)=nan;
x1(1:9:end)=x1(1);

tic
y1=vmedian(x1,1);
y2=vmedian(x1,2);
y3=vmedian(x1,3);
etime1=toc;

z1=zeros(size(y1));
z2=zeros(size(y2));
z3=zeros(size(y3));

tic
for i=1:N
    for j=1:N
        z1(1,i,j)=median(x1(isfinite(x1(:,i,j)),i,j),1);
        z2(i,1,j)=median(x1(i,isfinite(x1(i,:,j)),j),2);
        z3(i,j)=median(x1(i,j,isfinite(x1(i,j,:))),3);
    end
end
etime2=toc;
tol=1e-4;
reporttest('VMEDIAN 3-D including NANs and repeated data', aresame(y1,z1,tol) && aresame(y2,z2,tol)&&aresame(y3,z3,tol))
disp(['VMEDIAN was ' num2str(etime2./etime1) ' faster than loop with MEDIAN.'])

reporttest('VMEDIAN NaNs with row median',aresame([1 nan],vmedian([1 nan; nan nan],1)));
reporttest('VMEDIAN NaNs with column median',aresame([1 nan]',vmedian([1 nan; nan nan],2)));


%% Simplicial Complex in Two Dimensions
%
% We dsecribe the data structure of the simplicial complex associated to a
% two dimensional trianglulation give by |node,elem| . The |node| records
% the coordinates of vertices and |elem| is the pointer from local to
% global incices of vertices. See <meshdoc.html meshdoc> for details.
%
% A brief summary of ordering and orientation
%
% * edge: asecond ordering, i.e. edge(:,1)<edge(:,2)
%
% * elem: positive ordering or ascend ordering. The default one is positive
% ordering and the asecond ordering is used for edge elements.
%
% * local edge:
%   consistent orientation [2 3; 3 1; 1 2];
%   asecond orientation    [2 3; 1 3; 1 2];
% The default one is the consistent orientation which will be
% counter-clockwise if elem is positive ordered. The asecond orientation
% will be used for edge elements.
%
% * elem2edgeSign: records the inconsistency of the edge
%  orientation and the induced orientation.
%
% Local edge is the induced orientation [2 3; 3 1; 1 2].
%  elem2edgeSign(1:NT,1:4) records the elementwise inconsistency of local
%  edge orientation and global edge orientation. It can be obtained from
%  |dofedge|. This is used to correct the consistency of the local basis
%  with the global basis.
%
% Both elem and local edges are ascend ordering. Then elem2edgeSign =
%  [1 -1 1] records the inconsistency of the edge orientation and the
%  induced orientation. This is used to construct differential operators.
%
% Function to call
%
%  [elem2edge,edge,elem2edgeSign,edgeSign] = dofedge(elem);
%
% Functions to read on the usage
%
%  PoissonRT0;

%%
% The basic data structure of a mesh consists of node and elem:
node = [0,0; 1,0; 1,1; 0,1];    % nodes
elem = [2,3,1; 4,1,3];          % elements
%%
% In iFEM, |N, NE, NT| represents the muber of vertice, edges, triangles
% respectively.
%
N = size(node,1); NT = size(elem,1); % NE = size(edge,1);

%%
% The corresponding simplicial complex consists of vertices, edges and
% triangles. We shall discuss the following three issues:
%
% * *Indexing* of simplexes
% * *Ordering* of vertices of simplexes
% * *Orientatoin* of simplexes
%
% The indexing and ordering are related and the ordering and orientation
% are mixed together. However the indexing has nothing to do with the
% orientation. The indexing and ordering are the combinarotry structure,
% i.e. only |elem| is needed, while the orientation also depends on |node|,
% the geometry emembdding of vertices.
%
% For indexing, ordering and orientation, there are always local and global
% version. The relation between the local and global version is the most
% complicated issue.

%% Indexing of Simplexes
%
% The indexing refers to the numbering of simplexes, e.g., which edge is
% numbered as the first one. There are two types of the indexing: local and
% global. Each simplex in the simplicial complex has a unique index which
% is called the global index. In one triangle, the three vertices and three
% edges have their local index from 1:3.
%
% In the assembling procedure of finite element methods, an element-wise
% matrix using local indexing is first computed and then assembled to get a
% big matrix using global indexing. Thus the pointer from the local
% indexing to the global indexing is indispensible. For bases independent of
% the ordering and orientation, e.g., |P1| and |P2| elements, the pointer
% is sufficient, otherwise, the inconsistency of local ordering/orientation
% and global ordering/orientation should be taken into account.
%
% *Indexing pointers of vertices*
%
% The |NT| by 3 matrix |elem| is indeed the pointer from the local to the
% global indices of vertices of triangles. For example |elem(t,1)=25| means
% the first vertex of the triangle t is the 25-th vertex.
%
% Similiary, the |NE| by 2 matrix |edge| records the pointer from the local
% to the global indices of vertices of edges.
%
% *Local indexing of edges*
%
% The triangle constists of three vertices indexed as [1,2,3]. Each
% triangle contains three edges. There are two indexing schemes for edges.
%
% * Opposite indexing     |locEdge = [2,3; 3,1; 1,2]|
%
% In |locEdge|, the |i-th| edge is opposite to the |i-th| verices and thus
% called _opposite indexing_.
%
% * Lexicogrphic indexing |locEdgel = [1,2; 1,3; 2,3]|
%
% In |locEdgel|, the indexing is induced from the lexicographic ordering of
% the three edges.
%
% For 2-D triangulations, *we shall always chose opposite indexing*. The
% lexicographic indexing is mainly used in the construction of |face2edge|
% of 3-D triangulations; see <sc3doc.html Simplicial Complex in Three
% Dimensions> for details. Note that the ordering of vertices of each edge
% will not change the indexing. For example, |locEdge = [2,3; 1,3; 1,2]|
% use the same opposite indexing but different ordering. Chosing |[1 3]| or
% |[3 1]| for the second edge will depend on the consideration of
% orientation and ordering.
%
% *Global indexing of edges*
%
% One can easily collect all edges elementwise. The issue is the
% duplication. For example, each interior edge will be counted twice. The
% |unique| funciton is applied such that each edge has a unique global
% index.
%
totalEdge = uint32([elem(:,[2,3]); elem(:,[3,1]); elem(:,[1,2])]);
sortedTotalEdge = sort(totalEdge,2);
[edge,tempvar,je] = unique(sortedTotalEdge,'rows');
NE = size(edge,1);
%%
% *Edge pointer*
%
% |elem2edge(1:NT,1:3)| records the pointer from the local index to the
% global index of edges. For example, |elem2edge(t,1)| = 10 means the first
% edge of triangle |t| (which is formed by [2 3] vertices of |t|) is the
% |10-th| one in the |edge| array.
%
% Such information is stored in the third output of |unique| function.
%%
elem2edge = uint32(reshape(je,NT,3));
%%
% Note that the pointer |elem2edge| depends on the local indexing of edges
% used in the generation of |totalEdge|. Here the opposite indexing of
% three local edges is used.

%% Ordering of Vertices
%
% We discuss the ordering of vertices of simplexes. Again there are local
% ordering and global ordering. They may not be consistent and a sign array
% is used to record the inconsistency if any.
%
% The local ordering refers to the ordering of local veritces of a simplex.
% The local ordering could be used in the formulation of the local basis
% and thus the ordering does matter.
%
% The global ordering refers to the ordering of the global index of
% vertices of a simplex.
%
% *elem*. The local ordering is always [1,2,3]. Any permutation of three
% veritces of a triangle still represents the same triangle. Such
% freedom provide a room to record more information like:
%
% * global ordering of vertices
% * orientation of triangles
% * refinement rule
%
% Two types of ordering of |elem| is of particular importance
%
% * Positive ordering
% * Ascend ordering
%
% In the positive ordering, the three vertices are ordered such that the
% signed area, det(v12,v13), is positive. If |elem| is not positive
% ordered, |elem = fixorder(node,elem)| will compute the signed area by
% |simplexvolume(node,elem)| and switch the vertices for triangles with
% negative areas.
%
% For 2-D triangulations, three vertices of a triangle in 2-D is sorted
% counter-cloclwise and the first vertex is chosen as the newest vertex.
% Such ordering enables the efficient implementation of local refinement
% and coarsening in 2-D; see <bisectdoc.html Bisection in Two Dimensions>
% and <coarsendoc.html Coarsening in Two Dimensions>. *Such ordering scheme
% is the default choice and used in most places*.
%
%
% In ascend ordering, the vertices of |elem| is sorted such that
% |elem(t,1)<elem(t,2)<elem(t,3)|. This can be easily achieved by |elem =
% sort(elem,2)|. Howevery, one has to rotate the boundary flag
% accordingly using |sortelem|.
bdFlag = setboundary(node,elem,'Dirichlet');
display('Before rotation'); display(elem); display(bdFlag);

[elem,bdFlag] = sortelem(elem,bdFlag);
display('After rotation'); display(elem); display(bdFlag);

%%
% Ascend ordering will benefit the construction of local bases for high
% order basis or basis with orientation.
%
% We may switch the default positive ordering of |elem| to ascend ordering
% when generating data structure for finite element basis. However such
% sorting is always hidden in the subroutines when a finite element basis
% requiring ordering is generated; see |PoissonRT0| and |PoissonBDM1|.

%%
% *edge*. The global ordering of edges is always ascended, i.e.
edge(:,1) < edge(:,2);
%%
% Indeed in the generation of |edge|, the |totalEdge| is sorted to the
% ascend ordering such that |unique| can be applied.
%%
%
% Recall that we always use the opposite indexing of edges. We could use
% _ascend ordering_ , i.e.
locEdge = [2 3; 1 3; 1 2]; % Ascend ordering of local edges
%%
% or the _consistent (orientation) ordering_
locEdge = [2 3; 3 1; 1 2]; % Consistent ordering of local edges
%%
% It is consistent since the local ordering orientation of edges is
% consistent with the induced orientation of the three edges; see the
% discussion on the orientation.
%
% There might be an inconsistency between the local and global ordering
% (even for the consistent orientation ordering). That is
% |edge(elem2edge(t,1),1)| may not be smaller than
% |edge(elem2edge(t,1),2)|. It will be more clear from the discussion of
% the corresponding orientation.

%% Orientation of Simplexes
%
% The orientation of a triangle is either positive or negative, the
% orientation of an edge is given by a tangential or normal vector. A
% normal vector is obtained by rotate a given tangential vector by 90
% degree clockwise. For example, when edges are given counter clockwise
% orientation, the corresponding normal vector is the outwards normal
% vector.
%
% The orientation of a d-simplex will induce an orientation of its d-1
% subcomplex and is called _induced orientation_. For example, a positive
% orientated triangle will induce the counter clockwise orientation of its
% three edges.
%
% The ordering of vertices will naturally introduce an orientation and will
% be called _ordering orientation_. More specifically
%
% * The vector from |edge(:,1)| to |edge(:,2)| defines an orientation of
% edges.
% * The sign of det(v12,v13) defines an orientation of the triangle.
%
% The orientation of a simplex in the simplicial complex should be
% uniquely determined which will be called _global orientation_. It can be
% chosen as the global ordering orientation but not always the case.
%
% Inside one triangle, the local orientation of three edges is more
% involved. The local ordering of edges will introduce a local ordering
% orientation. The orientation of the triangle will also induce an induced
% orientation. The local ordering orientation is used in the computing of
% local bases and the induced orientation is used when computing the
% differential operator locally. They may or may not be consisitent with
% the global orientation of edges.
%
% In general, there will be an inconsistency of the following types of
% orientation and apporipate data structure should be constructured to
% record such inconsistency.
%
% * a global orientation
% * the global ordering orientation
% * the local ordering orientation
% * the local induced orientation
%
% *elem*. The orientation of a triangle is either positive or negative. For
% the global ordering orientation, it is the sign of the signed area
% (output of |simplexvolume|).
%
[Dlambda,area,elemSign] = gradbasis(node,elem);
%%
% In the output of |gradbasis|, |area| is always positive and an additional
% array |elemSign| is used to record the sign of the signed area.
%
% |Dlambda(t,:,k)| is the gradient of $\lambda_k$. Therefore the outward
% normal direction of the kth face can be obtained by |-Dlambda(t,:,k)|
% which is independent of the ordering and orientation.
%
% *edge*. For 2-D triangulations, *we shall always chose global ordering
% orientation*, i.e., from the lower index to the bigger index.
%
% The local ordering orientation is implicitly used when computing finite
% element basis in each element. For example, the edge element on edge |[i j]|
% in |locEdge| is defined as
%
% $$\phi_{i,j} = 2(\lambda_i \nabla \lambda_j - \lambda_j \nabla \lambda_i).$$
%
% Permutation of |[i j]| to |[j i]| will change the sign of the basis. Note
% that this is locally, i.e., element by element.
%
% The global basis associated to an edge, however, depends only on the
% global orientation of this edge. We introduce |elem2edgeSign(1:NT, 1:3)|
% to record the inconsistency of a local ordering orientation and a global
% orientation.
%
% For the consistent local ordering [2 3; 3 1; 1 2] and global ascend
% ordering orientation, the elem2edgeSign can be generated as follows:

elem2edgeSign = ones(NT,3,'int8');
totalEdge = uint32([elem(:,[2,3]); elem(:,[3,1]); elem(:,[1,2])]);
idx = (totalEdge(:,1)>totalEdge(:,2));
elem2edgeSign(idx) = -1;

%%
% There is one more inconsistency between the induced orientation and the
% global orientation of edges. If a triangle is positive orientated, the
% induced edge orientation should be given by the outwards (relative to a
% triangle) normal vector. This induced orientation may not be consistent
% with the global orientation of edges.
%
% It depends on the ordering of |elem| and |locEdge|. When |elem| is
% positive ordered and |locEdge| is consistently ordered, it is.
%
% For ascend ordering of |elem| and |edge|, we denote the direction as +1
% if the direction of an edge is the same with the induced direction in a
% certain elem, and -1 otherwise. Then the consistency is given by
elem2edgeSign = [+1 -1 +1];

%%
% The second sign is -1 because the local edge in ascend ordering is [1
% 3] not [3 1].
%%
% The |elem2edgeSign| will be used when assembling differential operators.
% For example, when computing |div| operators on a positive orientated
% triangle, the edge should have outwards normal direction or equivalently
% the counter clockwise orientation.
%%
% We summarize the two popular ordering and orientation schemes below.

%% Positive Ordering and Orientation
%
% The vertice of the |eleme| is sorted such that the area is always
% positive. i.e. the three vertices of the elem are ordered
% counter-clockwisely. Furthermore the first vertex is always the newest
% vertex of the triangle for the easy of local mesh refinement and
% coarsening.
%
% The local edge is using opposite indexing and consistent ordering
%
% |locEdge = [2,3; 3,1; 1,2]|
%
% The ascend ordering is used for global edges, i.e., |edge| is sorted s.t.
%
% |edge(:,1) < edge(:,2)|
%
% The inconsistency of the orientation is recorded in|elem2edgeSign|.
%
% *Where to use*: This is the default ordering and orientation scheme.
%
%% Ascend Ordering and Orientation
%
% *Ascend ordering*. The vertices of |elem| is sorted such that
%
% |elem(t,1)<elem(t,2)<elem(t,3)|
%
% The local edge is also in the ascend ordering
%
% |locEdge = [2,3; 1,3; 1,2]|
%
% The ascend ordering is used for edges, i.e., |edge| is sorted s.t.
%
% |edge(:,1) < edge(:,2)|
%
% It is easy to see the benefit of ascend ordering: the ordering of local
% edges is consistent with the global ones and so is the corresponding
% orientation.
%
% *Orientation*. We choose the ordering orientation.
%
% |elem: sign(det(v12,v13))|
%
% |edge: from the node with smaller global index to bigger one|
%
% For |edge|, the orientation of the global ordering and the local
% ordering is consistent. But they are not consistent with the induced
% orientation inside one triangle. Such inconsistency is recorded in
%
% |elem2edgeSign = [+1 -1 +1]|
%
%
% *Where to use*: for H(curl) and H(div) elements and high order (cubic and
% above) H(grad) elements.

%% An Example
%
node = [0,0; 1,0; 1,1; 0,1];    % nodes
elem = [2,3,1; 4,1,3];          % elements
% [node,elem] = uniformrefine(node,elem);
%%
% Poistive ordering and orientation
[elem2edge,edge,elem2edgeSign] = dofedge(elem);
% plot
set(gcf,'Units','normal');
set(gcf,'Position',[0,0.25,0.25,0.25]);
showmesh(node,elem);
findnode(node);
findelem(node,elem);
findedge(node,edge);
%%
display(elem);
display(edge);
display(elem2edge);
display(elem2edgeSign);

%%
% Ascend Ordering and Orientation
bdFlag = setboundary(node,elem,'Dirichlet');
[elem,bdFlag] = sortelem(elem,bdFlag);
[elem2edge,edge,elem2edgeSign] = dofedge(elem);
display(elem);
display(edge);
display(elem2edge);
display(elem2edgeSign);
elem2edgeSign = [1,-1,1];


function linpack_c_test04 ( )

%*****************************************************************************80
%
%% TEST04 tests CGBCO.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2009
%
%  Author:
%
%    John Burkardt
%
  ml = 1;
  mu = 1;
  n = 3;
  lda = 2*ml+mu+1;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST04\n' );
  fprintf ( 1, '  For a complex general band storage matrix:\n' );
  fprintf ( 1, '  CGBCO factors the matrix and estimates the\n' );
  fprintf ( 1, '  reciprocal condition number.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The matrix order is N = %d\n', n );
  fprintf ( 1, '  The lower band is ML =  %d\n', ml );
  fprintf ( 1, '  The upper band is MU =  %d\n', mu );
%
%  Set the values of the matrix A.
%
  a_save(1:n,1:n) = 0.0;

  m = ml + mu + 1;

  seed = 123456789;

  for j = 1 : n
    i1 = max ( 1, j - mu );
    i2 = min ( n, j + ml );
    for i = i1 : i2
      k = i - j + m;
      [ a(k,j), seed ] = c4_uniform_01 ( seed );
      a_save(i,j) = a(k,j);
    end
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The matrix A:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    for j = 1 : n
      fprintf ( 1, '  (%8f  %8f)', real ( a_save(i,j) ), imag ( a_save(i,j) ) );
    end
    fprintf ( 1, '\n' );
  end
%
%  Factor the matrix A.
%
  [ a, ipvt, rcond ] = cgbco ( a, lda, n, ml, mu );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Estimated reciprocal condition RCOND = %f\n', rcond );

  return
end


function net = cnnff(net, x)
    n = numel(net.layers);
    net.layers{1}.a{1} = x;
    inputmaps = 1;

    for l = 2 : n   %  for each layer
        if strcmp(net.layers{l}.type, 'c')
            %  !!below can probably be handled by insane matrix operations
            for j = 1 : net.layers{l}.outputmaps   %  for each output map
                %  create temp output map
                z = zeros(size(net.layers{l - 1}.a{1}) - [net.layers{l}.kernelsize - 1 net.layers{l}.kernelsize - 1 0]);
                for i = 1 : inputmaps   %  for each input map
                    %  convolve with corresponding kernel and add to temp output map
                    z = z + convn(net.layers{l - 1}.a{i}, net.layers{l}.k{i}{j}, 'valid');
                end
                %  add bias, pass through nonlinearity
                net.layers{l}.a{j} = sigm(z + net.layers{l}.b{j});
            end
            %  set number of input maps to this layers number of outputmaps
            inputmaps = net.layers{l}.outputmaps;
        elseif strcmp(net.layers{l}.type, 's')
            %  downsample
            for j = 1 : inputmaps
                z = convn(net.layers{l - 1}.a{j}, ones(net.layers{l}.scale) / (net.layers{l}.scale ^ 2), 'valid');   %  !! replace with variable
                net.layers{l}.a{j} = z(1 : net.layers{l}.scale : end, 1 : net.layers{l}.scale : end, :);
            end
        end
    end

    %  concatenate all end layer feature maps into vector
    net.fv = [];
    for j = 1 : numel(net.layers{n}.a)
        sa = size(net.layers{n}.a{j});
        net.fv = [net.fv; reshape(net.layers{n}.a{j}, sa(1) * sa(2), sa(3))];
    end
    %  feedforward into output perceptrons
    net.o = sigm(net.ffW * net.fv + repmat(net.ffb, 1, size(net.fv, 2)));

end


function [bestAlgo, inliers] = prtUtilRansac(data,algo,evalFn,varargin)
%[bestAlgo, inliers] = prtUtilRansac(data,algo,evalFn)
%   Perform RANSAC on prtDataSet data.
%
%[bestAlgo, inliers] = prtUtilRansac(data,algo,evalFn,param1,value1,...)
%   Enables inputs of parameter/value pairs as described below:
%
%   Parameters:
%       nIterations (100)
%       nBootstrapSamples (10) - the number of samples to use at each
%         iteration to fit a model.
%       fitErrorThreshold (0.2) - absolute error between truth and guess
%         should be less than fitErrorThreshold to be included inliers.
%
%   Example usage:
%
%       ds = prtDataGenRansac(100,0.2);
%       [~,inliers] = prtUtilRansac(ds,prtRegressLslr,@(a,b)abs(a-b));
%       plot(ds.X(inliers),ds.Y(inliers),'ro',...
%            ds.X(setdiff(1:100,inliers)),ds.Y(setdiff(1:100,inliers)),'bs')

% Copyright (c) 2014 Patrick Wang, Peter Torrione, Kenneth Morton
%
% 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.

%% parse input options
p = inputParser;

p.addParamValue('nIterations',100);
p.addParamValue('nBootstrapSamples',10);
p.addParamValue('fitErrorThreshold',0.2);

p.parse(varargin{:});
inputStructure = p.Results;

%% iterate
bestNFit = 0;
bestAlgo = [];
for iter = 1:inputStructure.nIterations
	% bootstrap
	indices = randperm(data.nObservations);
	indices = indices(1:inputStructure.nBootstrapSamples);
	dataIn = data.retainObservations(indices);
	dataOut = data.removeObservations(indices);

	% train on bootstrapped samples
	algoTrained = algo.train(dataIn);

	% test on left-out samples
	guessOut = algoTrained.run(dataOut);

	% find how many left-out samples fit model well
	error = evalFn(guessOut.X,dataOut.Y); % dataOut.Y?
	nFit = sum(error<inputStructure.fitErrorThreshold);

	% keep the best model
	if nFit > bestNFit
		bestNFit = nFit;
		bestAlgo = algoTrained;
	end
end

%% find all inliers and retrain model
guess = bestAlgo.run(data);
error = evalFn(guess.X,data.Y); % data.Y?
inliers = find(error<inputStructure.fitErrorThreshold);
bestAlgo = algo.train(data.retainObservations(inliers));


function [M, inliers] = RANSAC(x, fittingfn, distfn, degenfn, s, t)

    maxTrials = 1000;
    maxDataTrials = 100;

    [rows, npts] = size(x);

    % Desired probability of choosing at least one sample free from outliers (probably should be a parameter)
    p = 0.99;

    bestM = NaN;
    trialcount = 0;
    bestscore =  0;
    N = 1;

    while N > trialcount

        % Select at random s datapoints to form a trial model, M.
        degenerate = 1;
        count = 1;
        while degenerate
            % Generate s random indicies in the range 1..npts
            ind = randsample(npts, s);
            % Su Tan
%             while tooDense(x(:, ind), 10)
%                 ind = randsample(npts, s);
%             end
            % Test that these points are not a degenerate configuration.
            degenerate = feval(degenfn, x(:,ind));

            if ~degenerate
                M = feval(fittingfn, x(:,ind));
                if isempty(M)
                    degenerate = 1;
                end
            end

            % Safeguard against being stuck in this loop forever
            count = count + 1;
            if count > maxDataTrials
                disp('Unable to select a nondegenerate data set');
                break
            end
        end

%         if scoreX1 ~= 0
%             l = min([scoreX1(ind) scoreX2(ind)]);
%             t = (l + 0.6) * t;
%         end

        [inliers, score, M] = feval(distfn, M, x, t);

        % Find the number of inliers to this model.
        ninliers = length(inliers);

        if score > bestscore
            bestscore = score;
            bestinliers = inliers;
            bestM = M;

            % Update estimate of N
            fracinliers =  ninliers/npts;
            pNoOutliers = 1 -  fracinliers^s;
            pNoOutliers = max(eps, pNoOutliers);
            pNoOutliers = min(1-eps, pNoOutliers);
            N = log(1-p)/log(pNoOutliers);
        end

        trialcount = trialcount+1;

        % Safeguard against being stuck in this loop forever
        if trialcount > maxTrials
            fprintf('ransac reached the maximum number of %d trials\n', maxTrials);
            break
        end

        %fprintf('Iter: %d \n', trialcount);
    end

    if ~isnan(bestM)
        M = bestM;
        inliers = bestinliers;
    else
        M = [];
        inliers = [];
        disp('ransac was unable to find a useful solution');
    end
end


function r = sqr(a)
%SQR          Hessian (elementwise) square
%

% 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
% modified 08/26/12     S.M. Rump  global variables removed
%

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

  K = prod(size(a.x));
  if K==1                   % scalar hessian

    r.x = sqr(a.x);
    r.dx = (2*a.x) * a.dx;
    r.hx = a.hx * (2*a.x) + reshape(a.dx * a.dx.',size(a.hx));

  else                      % matrix hessian

    N = getappdata(0,'INTLAB_HESSIAN_NUMVAR');
    N2 = N^2;

    r.x = sqr(a.x);
    if issparse(a.hx)               % input sparse

      ax = 2*a.x(:);
      sizeax = length(ax);
      [ia,ja,sa] = find(a.dx);
      % check for emptyness: cures Matlab bug
      % a=sparse([],[],[],2,1), [i,j,s]=find(a), s(i).*s(:)  yields error
      if isempty(ia)
        r.dx = sparse([],[],[],N,sizeax);
        r.hx = sparse([],[],[],N2,sizeax);
      else
        if isa(a.x,'intval')          % sparse intval
          rdx = times(ax(ja),sa(:),0);
          if rdx.complex
            r.dx = intval( sparse(ia,ja,rdx.mid,N,sizeax) , sparse(ia,ja,rdx.rad,N,sizeax) , 'midrad' );
          else
            r.dx = intval( sparse(ia,ja,rdx.inf,N,sizeax) , sparse(ia,ja,rdx.sup,N,sizeax) , 'infsup' );
          end
        else                          % sparse point
          r.dx = sparse(ia,ja,ax(ja).*sa(:),N,sizeax);
        end
        r.hx = adx2rhx(N,sizeax,a.dx);
      end
      [ia,ja,sa] = find(a.hx);        % sparse point or intval
      % check for emptyness: cures Matlab bug
      % a=sparse([],[],[],2,1), [i,j,s]=find(a), s(i).*s(:)  yields error
      if ~isempty(ia)
        if isa(a.x,'intval')
          rhx = times(ax(ja),sa(:),0);
          if rhx.complex
            r.hx = r.hx + intval( sparse(ia,ja,rhx.mid,N2,sizeax) , sparse(ia,ja,rhx.rad,N2,sizeax) , 'midrad' );
          else
            r.hx = r.hx + intval( sparse(ia,ja,rhx.inf,N2,sizeax) , sparse(ia,ja,rhx.sup,N2,sizeax) , 'infsup' );
          end
        else
          r.hx = r.hx + sparse(ia,ja,ax(ja).*sa(:),N2,sizeax);
        end
      end

    else                            % input full

      ax = 2*a.x(:).';
      ax = ax(ones(N2,1),:);
      r.dx = a.dx .* ax(1:N,:);
      r.hx = a.hx .* ax + a.dx(repmat(1:N,N,1),:) .* a.dx(repmat(1:N,1,N),:);

    end

  end

  r = class(r,'hessian');

  if rndold
    setround(rndold)
  end


% VL_NNROIPOOL  CNN region of interest pooling.
%   Y = VL_NNROIPOOL(X, ROIS) pools each feature channel in X in
%   the specified regions of interest ROIS. ROIS is a 5 x K array
%   containing K regions. Each region has five coordinates `[t, u0,
%   v0, u1, v1]` where `u0`, `v0` is the upper-left corner of a ROI,
%   `u1`, `v1` is the bottom-right corner, and `t` is the index of the
%   image that contains the region. Spatial coordiantes start at (1,1),
%   with `u` indexing the horizontal axis and `v` the vertical one.
%   The image indeces ranges from 1 to the number of images stored
%   in the tensor X.
%
%   If X has C feature channels, then the output Y is a 1 x 1 x C x K
%   array, with one image instance per region. Arguments can be SINGLE
%   or DOUBLE and CPU or GPU arrays; however, they must all be of the
%   same type (unless empty).
%
%   DZDX = VL_NNROIPOOL(X, ROIS, DZDY) computes the derivative of
%   the layer projected on DZDY with respect to X.
%
%   VL_NNROIPOOL(___, 'opt', value, ...) accepts the following
%   options:
%
%   `Method`:: `'max'`
%     Choose between `'max'` and `'avg'` (average) pooling.
%
%   `Subdivisions`:: `[1 1]`
%     Specifies the number [SH,SW] of vertical and horizontal tiles of
%     a region. This makes the output a SH x SW x C x K array.
%
%   `Transform`:: `1`
%     Specifies a spatial transformation to apply to region vertices before
%     they are applied to the input tensor. If T is a scalar, then
%     the transformation is a scaling centered at the origin:
%
%        u' = T (u - 1) + 1,
%        v' = T (v - 1) + 1.
%
%     If T is a 2D vector, then different scaling factors for the
%     `u` and `v` can be specified. Finally, if T is a 2 x 2 matrix, then:
%
%        u' = T(1,1) u + T(1,2) v + T(1,3),
%        v' = T(2,1) u + T(2,2) v + T(2,3).
%
%     Note that only the upper-left and bottom-right corners of each
%     rectangular region are transformed. Thus this is mostly useful
%     for axis-aligned transformations; the generality of the expression
%     allows, however, to swap `u` and `v`, which may be needed
%     to match different conventions for the box coordiantes.
%
%   See also: VL_NNPOOL().

% Copyright (C) 2016 Hakan Bilen, Abishek Dutta, and Andrea Vedaldi.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).


function [tri, inds] = triangulateFaces(faces)
%TRIANGULATEFACES Convert face array to an array of triangular faces
%
%   TRI = triangulateFaces(FACES)
%   Returns a 3-columns array of indices, based on the data stored in the
%   argument FACES:
%   - if FACES is a N-by-3 array, returns the same array
%   - if FACES is a N-by-4 array, returns an array with 2*N rows and 3
%       columns, splitting each square into 2 triangles (uses first and
%       third vertex of each square as diagonal).
%   - if FACES is a cell array, split each face into a set of triangles,
%       and returns the union of all triangles. Faces are assumed to be
%       convex.
%
%   [TRI INDS] = triangulateFaces(FACES)
%   Also returns original face index of each new triangular face. INDS has
%   the same number of rows as TRI, and has values between 1 and the
%   number of rows of the original FACES array.
%
%
%   Example
%     % create a basic shape
%     [n e f] = createCubeOctahedron;
%     % draw with plain faces
%     figure;
%     drawMesh(n, f);
%     % draw as a triangulation
%     tri = triangulateFaces(f);
%     figure;
%     patch('vertices', n, 'faces', tri, 'facecolor', 'r');
%
%   See also
%   meshes3d, drawMesh, mergeCoplanarFaces
%

% ------
% Author: David Legland
% e-mail: david.legland@nantes.inra.fr
% Created: 2008-09-08,    using Matlab 7.4.0.287 (R2007a)
% Copyright 2008 INRA - BIA PV Nantes - MIAJ Jouy-en-Josas.

%% Tri mesh case: return original set of faces

if isnumeric(faces) && size(faces, 2) == 3
    tri = faces;
    if nargout > 1
        inds = (1:size(faces, 1))';
    end
    return;
end

%% Square faces: split each square into 2 triangles

if isnumeric(faces) && size(faces, 2) == 4
    nf = size(faces, 1);
    tri = zeros(nf * 2, 3);
    tri(1:2:end, :) = faces(:, [1 2 3]);
    tri(2:2:end, :) = faces(:, [1 3 4]);

    if nargout > 1
        inds = kron(1:size(faces, 1), ones(1,2))';
    end

    return;
end

%% Pentagonal faces (for dodecahedron...): split into 3 triangles

if isnumeric(faces) && size(faces, 2) == 5
    nf = size(faces, 1);
    tri = zeros(nf * 3, 3);
    tri(1:3:end, :) = faces(:, [1 2 3]);
    tri(2:3:end, :) = faces(:, [1 3 4]);
    tri(3:3:end, :) = faces(:, [1 4 5]);

    if nargout > 1
        inds = kron(1:size(faces, 1), ones(1,2))';
    end

    return;
end

%% Faces as cell array

% number of faces
nf  = length(faces);

% compute total number of triangles
ni = zeros(nf, 1);
for i = 1:nf
    % as many triangles as the number of vertices minus 1
    ni(i) = length(faces{i}) - 2;
end
nt = sum(ni);

% allocate memory for triangle array
tri = zeros(nt, 3);
inds = zeros(nt, 1);

% convert faces to triangles
t = 1;
for i = 1:nf
    face = faces{i};
    nv = length(face);
    v0 = face(1);
    for j = 3:nv
        tri(t, :) = [v0 face(j-1) face(j)];
        inds(t) = i;
        t = t + 1;
    end
end


function [clustCent,data2cluster,cluster2dataCell] = mean_shift_algorithm(dataPts,bandwidth,threshold);

[dim_dataset,no_data_dataset] = size(dataPts);
no_cluster = 0;         bandSq = (bandwidth).^2;
dataset_index = 1:no_data_dataset;
threshold_convergence =  threshold;% bandwidth(1,1)*exp(-3);  % this condition is taken from a reference paper on the web
tracking_array= false(1,no_data_dataset); % tracking: if a points been seen already
no_point_4_initial = no_data_dataset; % number of points to posibaly use as initilization points
clusterV = zeros(1,no_data_dataset,'uint16'); % cluster allotment method
clustMembsCell = [];
clustCent = []; % center of clust

while no_point_4_initial
    % choosing random data set for and then converging it
    random_index = ceil( (no_point_4_initial-1e-6)*rand); % pick a random seed point
    random_data_point = dataset_index(random_index); % use this point as start of mean
    mean_cur = dataPts(:,random_data_point);  % intilize mean to this points location
    cluster_members = []; % points that will get added to this cluster
    cluster_mem = zeros(1,no_data_dataset,'uint16');
    % convergence loop
    while true
        squ_Euclidean_distance(1,:) = sum(bsxfun(@minus,mean_cur(1:2,:),dataPts(1:2,:)).^2); % dist squared from mean to all points still active
        squ_Euclidean_distance(2,:) = sum(bsxfun(@minus,mean_cur(3:5,:),dataPts(3:5,:)).^2);
        kernel_range_datapoint_index = find(squ_Euclidean_distance(1,:) < bandSq(1,1)  );% points within bandWidth
        kernel_range_datapoint_index= find(squ_Euclidean_distance(2,:) < bandSq(1,2) );
        cluster_mem(kernel_range_datapoint_index) = cluster_mem(kernel_range_datapoint_index)+1;
        mean_previous = mean_cur; % save the old mean
        mean_cur = gaussian_kernel(dataPts(:,kernel_range_datapoint_index),sqrt(squ_Euclidean_distance(1,kernel_range_datapoint_index)),sqrt(squ_Euclidean_distance(2,kernel_range_datapoint_index)),bandwidth); % compute the new mean
        cluster_members = [cluster_members kernel_range_datapoint_index]; % add any point within bandWidth to the cluster
        tracking_array(cluster_members) = true; % mark that these points have been visited
        % converging condition
        if norm(mean_cur-mean_previous) < threshold_convergence
            join_cluster = 0;
            for cno = 1:no_cluster
                dist1 = norm(mean_cur(1:2)-clustCent(1:2,cno)); % spatial
                dist2 = norm(mean_cur(3:5)-clustCent(3:5,cno)); %range
                if( dist1 < bandwidth(1,1) &&dist2<bandwidth(1,2)) % condition to join the kernel
                    join_cluster = cno;
                    break;
                end
            end

            if join_cluster > 0
                nc = numel(cluster_members);
                no = numel(clustMembsCell{join_cluster});
                nw = [nc;no]/(nc+no);
                clustMembsCell{join_cluster} = unique([clustMembsCell{join_cluster},cluster_members]);
                clustCent(:,join_cluster) = mean_cur*nw(1) + mean_previous*nw(2);
                clusterV(join_cluster,:) = clusterV(join_cluster,:) + cluster_mem;
            else
                no_cluster = no_cluster+1;
                clustCent(:,no_cluster) = mean_cur;
                clustMembsCell{no_cluster} = cluster_members;
                clusterV(no_cluster,:) = cluster_mem;
            end
            break;
        end
    end
    dataset_index = find(~tracking_array);
    no_point_4_initial = length(dataset_index);
end
[~,data2cluster] = max(clusterV,[],1);
if nargout > 2
    cluster2dataCell = cell(no_cluster,1);
    for cno = 1:no_cluster
        cluster_members = find(data2cluster == cno);
        cluster2dataCell{cno} = cluster_members;
    end
end

end