%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Copyright (C) 2010, John T. Ramshur, jramshur@gmail.com
%
% This file is part of HRVAS
%
% HRVAS 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.
%
% HRVAS 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 HRVAS.  If not, see <http://www.gnu.org/licenses/>.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function output = freqDomainHRV(ibi,VLF,LF,HF,AR_order,window, ...
    noverlap,nfft,fs,methods,flagPlot)
%freqDomainHRV - calculates freq domain HRV using FFT, AR, and Lomb-Scargle
%methods
%
%Inputs:    ibi = 2Dim array of time (s) and inter-beat interval (s)
%           AR_order = order of AR model
%           window = # of samples in window
%           noverlap = # of samples to overlap
%           fs = cubic spline interpolation rate / resample rate (Hz)
%           nfft = # of points in the frequency axis
%           methods = cell array of strings that defines the methods used to
%               calculate freqDomain. The default is all to use
%               all three methods.
%               methods={'welch','ar','lomb'}
%           flagPlot = flag to tell function to plot PSD. 1=plot,
%           0=don't plot, default is 0.
%Outputs:   output is a structure containg all HRV. One field for each
%           PSD method.
%           Output units include:
%               peakHF,LF,VLF (Hz)
%               aHF,aLF,aVLF (ms^2)
%               pHF,pLF,pVLF (%)
%               nHF,nLF,nVLF (%)
%               PSD (ms^2/Hz)
%               F (Hz)
%Usage:  (1) To compute freq. domain HRV on a ibi data set named dIBI
%        using VLF=[0.0-0.16], LF =[0.16-0.6], HF=[0.6 3],
%        AR model order = 16, welch window width = 256,
%        # of overlap pnts in welch window (50%) = 128, # of pnts in fft = 512,
%        IBI resample rate = 10Hz
%
%        Use: output = freqDomainHRV(sampledata,[0 .16],[.16 .6],[.6 3], ...
%                       16, 256, 128, 512, 10);
%
%        (2) To do the above and also plot all three power
%        spectrum densities (PSD)
%
%        Use: output = freqDomainHRV(sampledata,[0 .16],[.16 .6],[.6 3], ...
%                       16,256,128,512,10,{'welch','ar','lomb'},1);

    %check input
    if nargin<9
        error('Not enough input arguments!')
    elseif nargin<10
        methods={'welch','ar','lomb'};
        flagPlot=false;
    elseif nargin<11
        flagPlot=false;
    end

    flagWelch=false; flagAR=false; flagLomb=false;
    for m=1:length(methods)
        if strcmpi(methods{m},'welch')
            flagWelch=true;
        elseif strcmpi(methods{m},'ar')
            flagAR=true;
        elseif strcmpi(methods{m},'lomb')
            flagLomb=true;
        end
    end

    t=ibi(:,1); %time (s)
    y=ibi(:,2); %ibi (s)

    y=y.*1000; %convert ibi to ms
    %assumes ibi units are seconds

    maxF=fs/2;

    %prepare y
    y=detrend(y,'linear');
    y=y-mean(y);

    %Welch FFT
    if flagWelch
        [output.welch.psd,output.welch.f] = ...
            calcWelch(t,y,window,noverlap,nfft,fs);
        output.welch.hrv = ...
            calcAreas(output.welch.f,output.welch.psd,VLF,LF,HF);
    else
        output.welch=emptyData(nfft,maxF);
    end

    %AR
    if flagAR
        [output.ar.psd,output.ar.f]=calcAR(t,y,fs,nfft,AR_order);
        output.ar.hrv=calcAreas(output.ar.f,output.ar.psd,VLF,LF,HF);
    else
        output.ar=emptyData(nfft,maxF);
    end

    %Lomb
    if flagLomb
        [output.lomb.psd,output.lomb.f]=calcLomb(t,y,nfft,maxF);
        output.lomb.hrv = ...
            calcAreas(output.lomb.f,output.lomb.psd,VLF,LF,HF,true);
    else
        output.lomb=emptyData(nfft,maxF);
    end

    %plot all three psd
    if flagPlot
    figure;
    h1=subplot(3,1,1);
    plotPSD(h1,output.welch.f,output.welch.psd,VLF,LF,HF,[0 0.6],[]);
    legend('welch')
    h2=subplot(3,1,2);
    plotPSD(h2,output.ar.f,output.ar.psd,VLF,LF,HF,[0 0.6],[]);
    legend('AR')
    h3=subplot(3,1,3);
    plotPSD(h3,output.lomb.f,output.lomb.psd,VLF,LF,HF,[0 0.6],[]);
    legend('Lomb-Scargle')
    end
end

function [PSD,F]=calcWelch(t,y,window,noverlap,nfft,fs)
%calFFT - Calculates the PSD using Welch method.
%
%Inputs:
%Outputs:

    %Prepare y
    t2 = t(1):1/fs:t(length(t));%time values for interp.
    y=interp1(t,y,t2','spline')'; %cubic spline interpolation
    y=y-mean(y); %remove mean

    %Calculate Welch PSD using hamming windowing
    [PSD,F] = pwelch(y,window,noverlap,(nfft*2)-1,fs,'onesided');

end

function [PSD,F]=calcAR(t,y,fs,nfft,AR_order)
%calAR - Calculates the PSD using Auto Regression model.
%
%Inputs:
%Outputs:

    %Prepare y
    t2 = t(1):1/fs:t(length(t)); %time values for interp.
    y=interp1(t,y,t2,'spline')'; %cubic spline interpolation
    y=y-mean(y); %remove mean
    y = y.*hamming(length(y)); %hamming window

    %Calculate PSD
    %Method 1
%     [A, variance] = arburg(y,AR_order); %AR using Burg method
%     [H,F] = freqz(1,A,nfft,fs);
%     PSD=(abs(H).^2).*(variance/fs); %malik, p.67
    %Method 2
    [PSD,F]=pburg(y,AR_order,(nfft*2)-1,fs,'onesided');
    %Method 3
%      h=spectrum.burg;
%      hpsd = psd(h, y, 'NFFT', nfft, 'Fs', 2);
%      F=hpsd.Frequencies;
%      PSD=hpsd.Data;

end

function [PSD,F]=calcLomb(t,y,nfft,maxF)
%calLomb - Calculates the PSD using Lomb-Scargle method.
%
%Inputs:
%Outputs:

    %Calculate PSD
    deltaF=maxF/nfft;
    F = linspace(0.0,maxF-deltaF,nfft);
    PSD=lomb2(y,t,F,false); %calc lomb psd
end

function output=calcAreas(F,PSD,VLF,LF,HF,flagNorm)
%calcAreas - Calulates areas/energy under the PSD curve within the freq
%bands defined by VLF, LF, and HF. Returns areas/energies as ms^2,
%percentage, and normalized units. Also returns LF/HF ratio.
%
%Inputs:
%   PSD: PSD vector
%   F: Freq vector
%   VLF, LF, HF: array containing VLF, LF, and HF freq limits
%   flagNormalize: option to normalize PSD to max(PSD)
%Output:
%
%Usage:
%
%
%   Modified from Gary Clifford's ECG Toolbox: calc_lfhf.m

    if nargin<6
       flagNorm=false;
    end

    %normalize PSD if needed
    if flagNorm
        PSD=PSD/max(PSD);
    end

    % find the indexes corresponding to the VLF, LF, and HF bands
    iVLF= (F>=VLF(1)) & (F<=VLF(2));
    iLF = (F>=LF(1)) & (F<=LF(2));
    iHF = (F>=HF(1)) & (F<=HF(2));

    %Find peaks
      %VLF Peak
      tmpF=F(iVLF);
      tmppsd=PSD(iVLF);
      [pks,ipks] = zipeaks(tmppsd);
      if ~isempty(pks)
        [tmpMax i]=max(pks);
        peakVLF=tmpF(ipks(i));
      else
        [tmpMax i]=max(tmppsd);
        peakVLF=tmpF(i);
      end
      %LF Peak
      tmpF=F(iLF);
      tmppsd=PSD(iLF);
      [pks,ipks] = zipeaks(tmppsd);
      if ~isempty(pks)
        [tmpMax i]=max(pks);
        peakLF=tmpF(ipks(i));
      else
        [tmpMax i]=max(tmppsd);
        peakLF=tmpF(i);
      end
      %HF Peak
      tmpF=F(iHF);
      tmppsd=PSD(iHF);
      [pks,ipks] = zipeaks(tmppsd);
      if ~isempty(pks)
        [tmpMax i]=max(pks);
        peakHF=tmpF(ipks(i));
      else
        [tmpMax i]=max(tmppsd);
        peakHF=tmpF(i);
      end

    % calculate raw areas (power under curve), within the freq bands (ms^2)
    aVLF=trapz(F(iVLF),PSD(iVLF));
    aLF=trapz(F(iLF),PSD(iLF));
    aHF=trapz(F(iHF),PSD(iHF));
    aTotal=aVLF+aLF+aHF;

    %calculate areas relative to the total area (%)
    pVLF=(aVLF/aTotal)*100;
    pLF=(aLF/aTotal)*100;
    pHF=(aHF/aTotal)*100;

    %calculate normalized areas (relative to HF+LF, n.u.)
    nLF=aLF/(aLF+aHF);
    nHF=aHF/(aLF+aHF);

    %calculate LF/HF ratio
    lfhf =aLF/aHF;

    %create output structure
    if flagNorm
        output.aVLF=round(aVLF*1000)/1000;
        output.aLF=round(aLF*1000)/1000;
        output.aHF=round(aHF*1000)/1000;
        output.aTotal=round(aTotal*1000)/1000;
    else
        output.aVLF=round(aVLF*100)/100; % round
        output.aLF=round(aLF*100)/100;
        output.aHF=round(aHF*100)/100;
        output.aTotal=round(aTotal*100)/100;
    end
    output.pVLF=round(pVLF*10)/10;
    output.pLF=round(pLF*10)/10;
    output.pHF=round(pHF*10)/10;
    output.nLF=round(nLF*1000)/1000;
    output.nHF=round(nHF*1000)/1000;
    output.LFHF=round(lfhf*1000)/1000;
    output.peakVLF=round(peakVLF(1)*100)/100;
    output.peakLF=round(peakLF(1)*100)/100;
    output.peakHF=round(peakHF(1)*100)/100;
end

function plotPSD(aH,F,PSD,VLF,LF,HF,limX,limY)

    color.vlf=[.5 .5 1];    %vlf color
    color.lf=[.7 .5 1];     %lf color
    color.hf=[.5 1 1];      %hf color

    % find the indexes corresponding to the VLF, LF, and HF bands
    iVLF= find( (F>=VLF(1)) & (F<VLF(2)) );
    iLF = find( (F>=LF(1)) & (F<LF(2)) );
    iHF = find( (F>=HF(1)) & (F<HF(2)) );

    %plot area under PSD curve
    area(aH,F(:),PSD(:),'FaceColor',[.8 .8 .8]);
    hold(aH);
    area(aH,F(iVLF(1):iVLF(end)+1),PSD(iVLF(1):iVLF(end)+1), ...
        'FaceColor',color.vlf);
    area(aH,F(iLF(1):iLF(end)+1),PSD(iLF(1):iLF(end)+1), ...
        'FaceColor',color.lf);
    area(aH,F(iHF(1):iHF(end)+1),PSD(iHF(1):iHF(end)+1), ...
        'FaceColor',color.hf);

    if ~isempty(limX)
        set(aH,'xlim',limX)
    else
        limX=[min(F) max(F)];
    end
    if ~isempty(limY)
        set(aH,'ylim',limY)
    else
        limY=[min(PSD) max(PSD)];
    end

    %draw vertical lines around freq bands
    line1=line([VLF(2) VLF(2)],[limY(1) limY(2)]);
    set(line1,'color',[1 0 0],'parent',aH);
    line2=line([LF(2) LF(2)],[limY(1) limY(2)]);
    set(line2,'color',[1 0 0],'parent',aH);
    line3=line([HF(2) HF(2)],[limY(1) limY(2)]);
    set(line3,'color',[1 0 0],'parent',aH);

    hold(aH)

end

function output=emptyData(nfft,maxF)
%create output structure of zeros

    output.hrv.aVLF=0;
    output.hrv.aLF=0;
    output.hrv.aHF=0;
    output.hrv.aTotal=0;
    output.hrv.pVLF=0;
    output.hrv.pLF=0;
    output.hrv.pHF=0;
    output.hrv.nLF=0;
    output.hrv.nHF=0;
    output.hrv.LFHF=0;
    output.hrv.peakVLF=0;
    output.hrv.peakLF=0;
    output.hrv.peakHF=0;

    %PSD with all zeros
    deltaF=maxF/nfft;
    output.f = linspace(0.0,maxF-deltaF,nfft);
    output.psd=zeros(length(output.f),1);

end

function [pks locs]=zipeaks(y)
%zippeaks: finds local maxima of input signal y
%Usage:  peak=zipeaks(y);
%Returns 2x(number of maxima) array
%pks = value at maximum
%locs = index value for maximum
%
%Reference:  2009, George Zipfel (Mathworks File Exchange #24797)

%check dimentions
if isempty(y)
    Warning('Empty input array')
    pks=[]; locs=[];
    return
end
[rows cols] = size(y);
if cols==1 && rows>1 %all data in 1st col
    y=y';
elseif cols==1 && rows==1
    Warning('Short input array')
    pks=[]; locs=[];
    return
end

%Find locations of local maxima
%yD=1 at maxima, yD=0 otherwise, end point maxima excluded
    N=length(y)-2;
    yD=[0 (sign(sign(y(2:N+1)-y(3:N+2))-sign(y(1:N)-y(2:N+1))-.1)+1) 0];
%Indices of maxima and corresponding values of y
    Y=logical(yD);
    I=1:length(Y);
    locs=I(Y);
    pks=y(Y);
end


% Compute null space vanilla
function [qd_null] = null_space_7dof(robot, q)
%compute nil-space!!
J = manipulator_jacobian(robot, q);
I = eye(robot.DOF);
Jp=pinv(J);
%null space projector
%n_space_projector = (I-Jp*J);
%for an arbitrary vector
%do not use [1 1 1 1]
qd3 = [0 0 1 0 0 0 0]';
%qd3 = q;
qd_null = project(J, Jp, I, qd3);

function qd_null = project(J, Jp, I, qd)
%q2 est� calculado a trav�s de un proyector (I-Jp*J),
%, de tal manera que q2 pertenece al null space de J
qd_null = (I-Jp*J)*qd;


function c = tapas_hgf_binary_pu_config
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Contains the configuration for the Hierarchical Gaussian Filter (HGF)
% for binary inputs in the *presence* of perceptual uncertainty.
%
% The HGF is the model introduced in
%
% Mathys C, Daunizeau J, Friston, KJ, and Stephan KE. (2011). A Bayesian foundation
% for individual learning under uncertainty. Frontiers in Human Neuroscience, 5:39.
%
% The binary HGF model has since been augmented with a positive factor kappa1 which
% scales the second level with respect to the first, i.e., the relation between the
% first and second level is
%
% p(x1=1|x2) = s(kappa1*x2), where s(.) is the logistic sigmoid.
%
% By default, kappa1 is fixed to 1, leading exactly to the model introduced in
% Mathys et al. (2011).
%
% This file refers to BINARY inputs (Eqs 1-3 in Mathys et al., (2011));
% for continuous inputs, refer to tapas_hgf_config.
%
% This file refers to UNCERTAIN inputs (Eqs 45-47 in Mathys et al., (2011));
% for inputs without uncertainty, refer to tapas_hgf_binary_config.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% The HGF configuration consists of the priors of parameters and initial values. All priors are
% Gaussian in the space where the quantity they refer to is estimated. They are specified by their
% sufficient statistics: mean and variance (NOT standard deviation).
%
% Quantities are estimated in their native space if they are unbounded (e.g., the omegas). They are
% estimated in log-space if they have a natural lower bound at zero (e.g., the sigmas).
%
% Parameters can be fixed (i.e., set to a fixed value) by setting the variance of their prior to
% zero. Aside from being useful for model comparison, the need for this arises whenever the scale
% and origin at the j-th level are arbitrary. This is the case if the observation model does not
% contain the representations mu_j and sigma_j. A choice of scale and origin is then implied by
% fixing the initial value mu_j_0 of mu_j and either kappa_j-1 or omega_j-1.
%
% Fitted trajectories can be plotted by using the command
%
% >> tapas_hgf_binary_plotTraj(est)
%
% where est is the stucture returned by tapas_fitModel. This structure contains the estimated
% perceptual parameters in est.p_prc and the estimated trajectories of the agent's
% representations (cf. Mathys et al., 2011). Their meanings are:
%
%         est.p_prc.mu_0       row vector of initial values of mu (in ascending order of levels)
%         est.p_prc.sa_0       row vector of initial values of sigma (in ascending order of levels)
%         est.p_prc.rho        row vector of rhos (representing drift; in ascending order of levels)
%         est.p_prc.ka         row vector of kappas (in ascending order of levels)
%         est.p_prc.om         row vector of omegas (in ascending order of levels)
%         est.p_prc.al         scalar alpha (perceptual uncertainty)
%         est.p_prc.eta0       scalar eta0 (mean of first input category)
%         est.p_prc.eta1       scalar eta1 (mean of second input category)
%
% Note that the first entry in all of the row vectors will be NaN because, at the first level,
% these parameters are either determined by the second level (mu_0 and sa_0) or undefined (rho,
% kappa, and omega).
%
%         est.traj.mu          mu (rows: trials, columns: levels)
%         est.traj.sa          sigma (rows: trials, columns: levels)
%         est.traj.muhat       prediction of mu (rows: trials, columns: levels)
%         est.traj.sahat       precisions of predictions (rows: trials, columns: levels)
%         est.traj.v           inferred variance of random walk (rows: trials, columns: levels)
%         est.traj.w           weighting factors (rows: trials, columns: levels)
%         est.traj.da          volatility prediction errors  (rows: trials, columns: levels)
%         est.traj.ud          updates with respect to prediction  (rows: trials, columns: levels)
%         est.traj.psi         precision weights on prediction errors  (rows: trials, columns: levels)
%         est.traj.epsi        precision-weighted prediction errors  (rows: trials, columns: levels)
%         est.traj.wt          full weights on prediction errors (at the first level,
%                                  this is the learning rate) (rows: trials, columns: levels)
%
% Note that in the absence of sensory uncertainty (which is the assumption here), the first
% column of mu, corresponding to the first level, will be equal to the inputs. Likewise, the
% first column of sa will be 0 always.
%
% Tips:
% - When analyzing a new dataset, take your inputs u and use
%
%   >> est = tapas_fitModel([], u, 'tapas_hgf_binary_pu_config', 'tapas_bayes_optimal_binary_config');
%
%   to determine the Bayes optimal perceptual parameters (given your current priors as defined in
%   this file here, so choose them wide and loose to let the inputs influence the result). You can
%   then use the optimal parameters as your new prior means for the perceptual parameters.
%
% - When analyzing a new dataset, take your inputs u and use
%
% - If you get an error saying that the prior means are in a region where model assumptions are
%   violated, lower the prior means of the omegas, starting with the highest level and proceeding
%   downwards.
%
% - Alternatives are lowering the prior means of the kappas, if they are not fixed, or adjusting
%   the values of the kappas or omegas, if any of them are fixed.
%
% - If the log-model evidence cannot be calculated because the Hessian poses problems, look at
%   est.optim.H and fix the parameters that lead to NaNs.
%
% - Your guide to all these adjustments is the log-model evidence (LME). Whenever the LME increases
%   by at least 3 across datasets, the adjustment was a good idea and can be justified by just this:
%   the LME increased, so you had a better model.
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2012-2017 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Config structure
c = struct;

% Model name
c.model = 'hgf_binary_pu';

% Number of levels (minimum: 3)
c.n_levels = 3;

% Input intervals
% If input intervals are irregular, the last column of the input
% matrix u has to contain the interval between inputs k-1 and k
% in the k-th row, and this flag has to be set to true
c.irregular_intervals = false;

% Sufficient statistics of Gaussian parameter priors

% Initial mus and sigmas
% Format: row vectors of length n_levels
% For all but the first two levels, this is usually best
% kept fixed to 1 (determines origin on x_i-scale). The
% first level is NaN because it is determined by the second,
% and the second implies neutrality between outcomes when it
% is centered at 0.
c.mu_0mu = [NaN, 0, 1];
c.mu_0sa = [NaN, 0, 0];

c.logsa_0mu = [NaN,   log(0.1), log(1)];
c.logsa_0sa = [NaN,          0,      0];

% Rhos
% Format: row vector of length n_levels.
% Undefined (therefore NaN) at the first level.
% Fix this to zero to turn off drift.
c.rhomu = [NaN, 0, 0];
c.rhosa = [NaN, 0, 0];

% Kappas
% Format: row vector of length n_levels-1.
% Fixing log(kappa1) to log(1) leads to the original HGF model.
% Higher log(kappas) should be fixed (preferably to log(1)) if the
% observation model does not use mu_i+1 (kappa then determines the
% scaling of x_i+1).
c.logkamu = [log(1), log(1)];
c.logkasa = [     0,      0];

% Omegas
% Format: row vector of length n_levels.
% Undefined (therefore NaN) at the first level.
c.ommu = [NaN,  -3,  -6];
c.omsa = [NaN, 4^2, 4^2];

% Alpha
% Format: scalar.
c.logalmu = log(0.5);
c.logalsa = 1;

% Eta0
% Format: scalar.
c.eta0mu = 0;
c.eta0sa = 0;

% Eta1
% Format: scalar.
c.eta1mu = 1;
c.eta1sa = 0;

% Gather prior settings in vectors
c.priormus = [
    c.mu_0mu,...
    c.logsa_0mu,...
    c.rhomu,...
    c.logkamu,...
    c.ommu,...
    c.logalmu,...
    c.eta0mu,...
    c.eta1mu,...
         ];

c.priorsas = [
    c.mu_0sa,...
    c.logsa_0sa,...
    c.rhosa,...
    c.logkasa,...
    c.omsa,...
    c.logalsa,...
    c.eta0sa,...
    c.eta1sa,...
         ];

% Check whether we have the right number of priors
expectedLength = 3*c.n_levels+2*(c.n_levels-1)+4;
if length([c.priormus, c.priorsas]) ~= 2*expectedLength;
    error('tapas:hgf:PriorDefNotMatchingLevels', 'Prior definition does not match number of levels.')
end

% Model function handle
c.prc_fun = @tapas_hgf_binary_pu;

% Handle to function that transforms perceptual parameters to their native space
% from the space they are estimated in
c.transp_prc_fun = @tapas_hgf_binary_pu_transp;

return;


%==============================================================================
% This code is part of the Finite Element Method app for the Matlab-based toolbox
%  FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR/FAIRFEM
%==============================================================================
%
% classdef TriMesh2 < handle
%
% Finite Element Mesh based on triangular subdivision of rectangular mesh.
%
% Each Cell is divided into two triangles:
%
%  o---------o---------o---------o---------o
%  | \       | \       | \       | \       |
%  |   \     |   \     |   \     |   \     |
%  |     \   |     \   |     \   |     \   |
%  |       \ |       \ |       \ |       \ |
%  o---------o---------o---------o---------o
%  | \       | \       | \       | \       |
%  |   \     |   \     |   \     |   \     |
%  |     \   |     \   |     \   |     \   |
%  |       \ |       \ |       \ |       \ |
%  o---------o---------o---------o---------o
%  | \       | \       | \       | \       |
%  |   \     |   \     |   \     |   \     |
%  |     \   |     \   |     \   |     \   |
%  |       \ |       \ |       \ |       \ |
%  o---------o---------o---------o---------o
%
%  To construct an instance of this class type:
%
%  >> Mesh = TriMesh2(omega,m)
%
% Input:
% 	omega - description of spatial domain
%   m     - number of cells
%
% Properties:
%   xn     - node list
%   tri    - triangle list
%   dim    - space dimension
%   omega  - description of spatial domain
%   m      - number of cells
%   type   - type of partition
%   vol    - volume of triangles
%   nnodes - number of nodes
%   ntri   - number of triangles
%   dx1    - partial derivative operator
%   dx2    - partial derivative operator
%   GRAD   - gradient operator
%   P1     - projection operator for node 1
%   P2     - projection operator for node 2
%   P3     - projection operator for node 3
%   PC     - projection operator for Barycentrum
%   P      - prolongation operator
%   Pt     - restriction operator
%
%  Methods:
%   mfPu   - matrix free prolongation/restriction
%   mfPi   - matrix free edge projector
%   getP   - builds prolongation operator
%   tri2cc - averaging
%
%
% see also
% =========================================================================
classdef TriMesh2 < handle

    properties
        % ===================================
        % node list
        % ===================================
        xn
        % ===================================
        % triangle list
        % ===================================
        tri
        % ===================================
        % space dimension
        % ===================================
        dim  = 2;
        % ===================================
        % description of computational domain
        % ===================================
        omega
        % ===================================
        % number of cells
        % ===================================
        m
        % ===================================
        % type of partition
        % ===================================
        type = 1;
        % ===================================
        % function handle to myself
        % ===================================
        me = @TriMesh2
        % ===================================
        % volume of triangles
        % ===================================
        vol
        % ===================================
        % number of nodes
        % ===================================
        nnodes
        % ===================================
        % number of triangles in mesh
        % ===================================
        ntri
    end

    properties (Access = public, Dependent) % These will be created when first callend and stores persistently
        % ===================================
        % dx1  - partial derivative operator
        % ===================================
        dx1
        % ===================================
        % dx2  - Partial derivative operator
        % ===================================
        dx2
        % ===================================
        % GRAD - Gradient operator
        %
        %         | dx1 |
        %  GRAD = |     |
        %         | dx2 |
        %
        % ===================================
        GRAD
        % ===================================
        % B - Vector gradient operator
        %
        %         | GRAD  0    |
        %  B =    |            |
        %         | 0     GRAD |
        %
        % ===================================
        B
        % ===================================
        % P1 - Projection operator on Node 1
        % ===================================
        P1
        % ===================================
        % P2 - Projection operator on Node 2
        % ===================================
        P2
        % ===================================
        % P3 - Projection operator on Node 3
        % ===================================
        P3
        % ===================================
        % PC - Projection operator on Barycenter
        % ===================================
        PC
        % ===================================
        % P  - Prolongation operator
        % ===================================
        P
        % ===================================
        % Pt - Restriction operator
        % ===================================
        Pt
        % ===================================
        % Boundary indices
        % ===================================
        boundaryIdx
        % ===================================
        % Boundary projector
        % ===================================
        boundaryProj
        mfdx1
        mfdx2
        mfGRAD
    end

    properties (Access = private)
        % These are where the dependent data is actually stored
        dx1_
        dx2_
        GRAD_
        B_
        P1_
        P2_
        P3_
        PC_
        P_
        Pt_
        mfdx1_
        mfdx2_
        mfGRAD_
        boundaryIdx_
        boundaryProj_
    end

    methods
        function this = TriMesh2(omega,m)
            if nargin==0,
                help(mfilename);
                this.runMinimalExample;
                return;
            end
            this.omega = omega;
            this.m     = m;

            this.xn = reshape(getNodalGrid(omega,m),[],2);
            % get indices of bottom left vertices
            nodes = reshape(1:prod(m+1),m+1);
            nodes = reshape(nodes(1:end-1,1:end-1),1,[]);
            % specify triangles
            this.tri   = [nodes; nodes+1; nodes+m(1)+1; nodes+m(1)+2; nodes+m(1)+1; nodes+1];
            this.tri   = reshape(this.tri,3,[])';
            this.nnodes = prod(m+1);
            this.ntri  = size(this.tri,1);
            this.vol = prod((omega(2:2:end)-omega(1:2:end))./m)/2*ones(this.ntri,1);
        end

        function runMinimalExample(~)
            omega = [0 4 2 6]; m = [8 16];
            Mesh  = feval(mfilename,omega,m);
        end

        function x = mfPi(this,x,i)
            % =============================================================
            % function x = mfPi(this,x,i)
            %
            % matrix free edge projector
            % =============================================================
            switch i
                case 1
                    P = this.P1;
                case 2
                    P = this.P2;
                case 3
                    P = this.P3;
                case 'C'
                    P = this.PC;
            end
            if size(x,1) == this.ntri,
                % ajoint
                x = P'*x;
            else
                x = P * x;
            end
        end

        function x = tri2cc(this,x)
            % =============================================================
            % function x = tri2cc(x)
            %
            % averaging or adjoint
            % =============================================================
            if numel(x)==this.ntri,
                x = mean(reshape(x,2,[]),1);
            else
                x = reshape(x,1,[]);
                x = .5*[x;x];
                x = x(:);
            end

        end

        function Pu = mfPuNodal(~,yn,m,flag)
            % =============================================================
            % function Pu = mfPuNodal(~,yn,m,flag)
            %
            % matrix free prolongation/restriction for nodal quantities
            % =============================================================
            d  = numel(yn)/prod(m+1);
            yn = reshape(yn,[m+1, d]);
            switch flag
                case 'Pu' % coarse --> fine
                    yn = reshape(yn,[m+1 2]);
                    Pu = zeros([2*m+1 2]);

                    % include existing nodes
                    Pu(1:2:end,1:2:end,:) = yn;
                    % prolongate in x direction
                    Pu(2:2:end-1,:,:) =  .5*(Pu(1:2:end-2,:,:) + Pu(3:2:end,:,:));
                    % prolongate in y direction
                    Pu(1:2:end,2:2:end-1,:) =  .5*(Pu(1:2:end,1:2:end-2,:) + Pu(1:2:end,3:2:end,:));
                    % prolongate diagonal
                    Pu(2:2:end,2:2:end,:) = .5* (Pu(3:2:end,1:2:end-2,:) + Pu(1:2:end-2,3:2:end,:));

                    Pu = reshape(Pu,[],1);
                case 'PTu'
                    yn = reshape(yn,[m+1 2]);

                    % include parent nodes
                    Pu = yn(1:2:end,1:2:end,:);

                    % distribute in x direction
                    t = .5* yn(2:2:end-1,1:2:end,:);
                    Pu(1:end-1,:,:) = Pu(1:end-1,:,:) + t;
                    Pu(2:end,:,:)   = Pu(2:end,:,:) + t;

                    % distribute in y direction
                    t = .5* yn(1:2:end,2:2:end-1,:);
                    Pu(:,1:end-1,:) = Pu(:,1:end-1,:) + t;
                    Pu(:,2:end,:) = Pu(:,2:end,:) + t;

                    % distribute diagonal
                    t = .5* yn(2:2:end-1,2:2:end-1,:);
                    Pu(2:end,1:end-1,:) = Pu(2:end,1:end-1,:) + t;
                    Pu(1:end-1,2:end,:) = Pu(1:end-1,2:end,:) + t;

                    Pu = reshape(Pu,[],1);
            end
        end

        function P = getPuNodal(~,m)
            % =============================================================
            % function P = getPuNodal(~,m)
            %
            % returns prolongation operator for input of cell-width m
            % =============================================================

            mf = 2*m;
            % indices of fine and coarse grid nodes
            indf = reshape(1:prod(mf+1),mf+1);
            indc = reshape(1:prod(m+1),m+1);

            % allocate space
            I = []; J = []; W = [];

            % include existing nodes
            ii = indf(1:2:end,1:2:end);
            jj = indc(:);
            ww = ones(size(jj));
            I = [I; ii(:)]; J = [J; jj(:)]; W = [W; ww(:)];

            % prolongate in x direction
            ii   = indf(2:2:end-1,1:2:end);
            jj   = [reshape(indc(1:end-1,:),[],1); reshape(indc(2:end,:),[],1)];
            ww = .5*ones(size(jj));
            I = [I; ii(:);ii(:)]; J = [J; jj(:)]; W = [W; ww(:)];

            % prolongate in y direction
            ii   = indf(1:2:end,2:2:end-1);
            jj   = [reshape(indc(:,1:end-1),[],1); reshape(indc(:,2:end),[],1)];
            ww   = .5*ones(size(jj));
            I = [I; ii(:); ii(:)]; J = [J; jj(:)]; W = [W; ww(:)];

            % prolongate diagonally
            ii   = indf(2:2:end-1,2:2:end-1);
            jj   = [reshape(indc(1:end-1,2:end),[],1); reshape(indc(2:end,1:end-1),[],1)];
            ww   = .5*ones(size(jj));
            I = [I; ii(:);ii(:)]; J = [J; jj(:)]; W = [W; ww(:)];

            P = sparse(I,J,W,prod(mf+1),prod(m+1));

        end

        % ========== get methods ========================================
        function dx1 = get.dx1(this)
            if isempty(this.dx1_),
                [this.dx1_, this.dx2_] = getGradientMatrixFEM(this,0);
            end
            dx1 = this.dx1_;
        end

        function dx2 = get.dx2(this)
            if isempty(this.dx2_),
                [this.dx1_, this.dx2_] = getGradientMatrixFEM(this,0);
            end
            dx2 = this.dx2_;
        end

        function GRAD = get.GRAD(this)
            if isempty(this.GRAD_),
                this.GRAD_ = [this.dx1;this.dx2];
            end
            GRAD = this.GRAD_;
        end
        function B = get.B(this)
            if isempty(this.B_),
                this.B_ = blkdiag(this.GRAD,this.GRAD);
            end
            B = this.B_;
        end

        function P1 = get.P1(this)
            if isempty(this.P1_),
                A = speye(this.nnodes);
                this.P1_ = A(this.tri(:,1),:);
            end
            P1 = this.P1_;
        end
        function P2 = get.P2(this)
            if isempty(this.P2_),
                A = speye(this.nnodes);
                this.P2_ = A(this.tri(:,2),:);
            end
            P2 = this.P2_;
        end

        function P3 = get.P3(this)
            if isempty(this.P3_),
                A = speye(this.nnodes);
                this.P3_ = A(this.tri(:,3),:);
            end
            P3 = this.P3_;
        end
        function PC = get.PC(this)
            if isempty(this.PC_),
                this.PC_ = (this.P1+this.P2+this.P3)/3;
            end
            PC = this.PC_;
        end
        function P = get.P(this)
            if isempty(this.P_),
                this.P_ = this.getPuNodal(this.m);
            end
            P = this.P_;
        end
        function Pt = get.Pt(this)
            if isempty(this.Pt_),
                this.Pt_ = this.getPuNodal(this.m/2);
            end
            Pt = this.Pt_;
        end

        function mfdx1 = get.mfdx1(this)
            if isempty(this.mfdx1_),
                [this.mfdx1_, this.mfdx2_] = getGradientMatrixFEM(this,1);
            end
            mfdx1 = this.mfdx1_;
        end

        function mfdx2 = get.mfdx2(this)
            if isempty(this.mfdx2_),
                [this.mfdx1_, this.mfdx2_] = getGradientMatrixFEM(this,1);
            end
            mfdx2 = this.mfdx2_;
        end

        function mfGRAD = get.mfGRAD(this)
            if isempty(this.mfGRAD_),
                this.mfGRAD_ = getGradientMatrixFEM(this,1);
            end
            mfGRAD = this.mfGRAD_;
        end

        function idx = get.boundaryIdx(this)
            if isempty(this.boundaryIdx_),
                id = reshape(1:prod(this.m+1),this.m+1);
                idx = [reshape(id([1,end],:),[],1);reshape(id(2:end-1,[1,end]),[],1)] ;
                this.boundaryIdx_ = idx;
            end
            idx = this.boundaryIdx_;
        end

        function idx = get.boundaryProj(this)
            if isempty(this.boundaryProj_),
                idx = this.boundaryIdx;

                P = speye(prod(this.m+1));
                P = P(idx,:);
                P = kron(speye(2),P);

                this.boundaryProj_ = P;
            end
            idx = this.boundaryProj_;
        end

        % ========== set methods ========================================
        function set.xn(this,xn)
            if numel(xn)~=(2*prod(this.m+1)),
                error('Invalid number of nodes');
            end
            if isempty(this.xn),
                this.xn = xn;
            else

                this.xn = xn;
                % delete operators that are sensitive to nodes
                this.dx1_  = [];
                this.dx2_  = [];
                this.GRAD_ = [];
                this.B_    = [];

                this.vol = volTetraGrid(this,this.xn,'matrixFree',1);
            end
        end
    end
end


function p_ = ViewRanking(X,p,Lower,Upper)

[J,N]=size(X);
K=length(Lower);

% constrain probabilities to sum to one...
Aeq = ones(1,J);
beq=1;

% ...constrain the expectations...
V=X(:,Lower) - X(:,Upper);

A = V';
b = 0;

% ...compute posterior probabilities
p_ = EntropyProg(p,A,b,Aeq ,beq);


clear;
clc;
close all;

RP = [ -2175464.65976786          4387261.15978363          4072912.71678943]';
SP = [  15987741.24878          3203328.88432069          21122447.2672636]';

this_TOW = 225119;

[lat,  lon, h] = ch_ECEF2LLA(RP);
[az, el] = satellite_az_el(SP, RP);

el = linspace(0.1, pi*0.9, 100);

 dtrop = tropo_correction(el, 1*1000);

plot(dtrop)
title("对流层误差");
ylabel("m");


function [Q_matrix] = adapt_noise_covariance(Phi_matrix, P_new, P_old, Q_matrix, n, GNSS_epoch, corrections)
%% Adapt Q matrix to system noise
% Adjust measurement noise covariance matrix Q according to state correction sequence
% Adam Werries 2016, see Apache 2.0 license.

%% Mohamed and Schwarz method, 1999
k = GNSS_epoch-1;
if k > n
    C = zeros(15,15);
    for j = k-n+1:k
        C = C + corrections(:,j)*corrections(:,j)';
    end
    Q_matrix = C./n + P_new - Phi_matrix*P_old*Phi_matrix';
    Q_matrix = diag(diag(Q_matrix));
else

end

%% Ding and Wang method, 2007
% TODO

end


function varargout = cosh(varargin)
%COSH (overloaded)

switch class(varargin{1})

    case 'double'
        error('Overloaded SDPVAR/COSH CALLED WITH DOUBLE. Report error')

    case 'sdpvar'
        varargout{1} = InstantiateElementWise(mfilename,varargin{:});

    case 'char'

        operator = struct('convexity','none','monotonicity','none','definiteness','positive','model','callback');
        operator.convexhull = [];
        operator.bounds = @bounds;
        operator.derivative = @(x)(sinh(x));

        varargout{1} = [];
        varargout{2} = operator;
        varargout{3} = varargin{3};

    otherwise
        error('SDPVAR/COSH called with CHAR argument?');
end

function [L,U] = bounds(xL,xU)
if xL<0 & xU>0
    L = 0;
    U = max([cosh(xL) cosh(xU)]);
elseif xL<0
    L = cosh(xU);
    U = cosh(xL);
else
    L = cosh(xL);
    U = cosh(xU);
end


function marginal = marginal_nodes(engine, query, add_ev)
% MARGINAL_NODES Compute the marginal on the specified query nodes (cond_gauss)
% marginal = marginal_nodes(engine, query, add_ev)
%
% 'query' must be a singleton set
% add_ev is an optional argument; if 1, we will "inflate" the marginal of observed nodes
% to their original size, adding 0s to the positions which contradict the evidence

if nargin < 3, add_ev = 0; end

if length(query) ~= 1
  error('cond_gauss_inf_engine can only handle marginal queries on single nodes')
end
j = query;
bnet = bnet_from_engine(engine);

if myismember(j, bnet.cnodes)
  if ~myismember(j, engine.onodes)
    [m, C] = collapse_mog(engine.mu{j}, engine.Sigma{j}, engine.T);
    marginal.mu = m;
    marginal.Sigma = C;
    marginal.T = 1.0; % single mixture component
  else
    marginal.mu = engine.evidence{j};
    k = bnet.node_sizes(j);
    marginal.Sigma = zeros(k,k);
    marginal.T = 1.0; % since P(E|E)=1
  end
else
  marginal = pot_to_marginal(marginalize_pot(engine.joint_dmarginal, j));
  if add_ev
    marginal = add_ev_to_dmarginal(marginal, engine.evidence, bnet.node_sizes);
  end
end

marginal.domain = query;


function L = obslikebernoulli(X,hmm)
%
% Evaluate likelihood of data given observation model, for one continuous trial
%
% INPUT
% X          N by ndim data matrix
% hmm        hmm data structure
%
% OUTPUT
% B          Likelihood of N data points
%
% Author: Cam Higgins, OHBA, University of Oxford

K = hmm.K;
[T,ndim] = size(X);

L = zeros(T,K);

for k=1:K
    % expectation of log(p) is psi(a) - psi(a+b):
    pterm = X .* repmat( psi(hmm.state(k).W.a) - psi(hmm.state(k).W.a + hmm.state(k).W.b) ,T,1);
    qterm = (~X) .* repmat(psi(hmm.state(k).W.b) - psi(hmm.state(k).W.a + hmm.state(k).W.b) ,T,1);
    L(:,k) = sum(pterm+qterm,2);
end

L = exp(L);
end


function y = nbBernPred(model, X)
% Prediction of naive Bayes classifier with independent Bernoulli.
% input:
%   model: trained model structure
%   X: d x n data matrix
% output:
%   y: 1 x n predicted class label
% Written by Mo Chen (sth4nth@gmail.com).
mu = model.mu;
w = model.w;
[~,y] = max(log(mu)'*X+log(1-mu)'*(1-X)+log(w(:)),[],1);


function pde = jumpmgdata1
%% Data of JUMPMG1
%
% Copyright (C)  Long Chen. See COPYRIGHT.txt for details.

pde = struct('f',@f,'g_D',@g_D,'d',@omega);

    function s = f(p) % load data (right hand side function)
    s = zeros(size(p,1),1);
    end

    function s = g_D(p) % Dirichlet boundary condition
    s = zeros(size(p,1),1);
    x = p(:,1);
    idx = (abs(x-1)<eps);
    s(idx) = 1;
    s(~idx) = 0;
    end

    function c = omega(p) % diffusion constant
    global epsilon
    c = zeros(size(p,1),1);
    x = p(:,1); y = p(:,2); z = p(:,3);
    idx = (x>0) & (x<1) & (y>0) & (y<1) & (z>0) & (z<1);
    c(idx) = 1/epsilon;
    c(~idx) = 1;
    end
end


function value = r8vec_any_nonzero ( n, a )

%*****************************************************************************80
%
%% R8VEC_ANY_NONZERO: ( any A nonzero ) for R8VEC's.
%
%  Discussion:
%
%    An R8VEC is a vector of R8's.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 December 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries.
%
%    Input, real A(N), the vector.
%
%    Output, logical R8VEC_ANY_NONZERO is TRUE if any entry is nonzero.
%
  value = any ( a(1:n) ~= 0.0 );

  return
end


function Population = EnvironmentalSelection(Population,W,N)
% The environmental selection of MOEA/IGD-NS

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

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

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

function Remain = LastSelection(PopObj,W,K)
% Select part of the solutions in the last front

    N  = size(PopObj,1);
    NW = size(W,1);

    %% Calculate the distance between each solution and point
    Distance = pdist2(PopObj,W);
    Con      = min(Distance,[],2);

    %% Delete the solution which has the smallest metric contribution one by one
    [dis,rank] = sort(Distance,1);
    Remain     = true(1,N);
    while sum(Remain) > K
        % Calculate the fitness of outliers
        Outliers = Remain;
        Outliers(rank(1,:)) = false;
        METRIC   = sum(dis(1,:)) + sum(Con(Outliers));
        Metrics  = inf(1,N);
        Metrics(Outliers) = METRIC - Con(Outliers);
        % Calculate the fitness of other solutions
        for p = find(Remain & ~Outliers)
            temp = rank(1,:) == p;
            outliers = false(1,N);
            outliers(rank(2,temp)) = true;
            outliers = outliers & Outliers;
            Metrics(p) = METRIC - sum(dis(1,temp)) + sum(dis(2,temp)) - sum(Con(outliers));
        end
        % Delete the worst solution and update the variables
        [~,del] = min(Metrics);
        temp = rank ~= del;
        dis  = reshape(dis(temp),sum(Remain)-1,NW);
        rank = reshape(rank(temp),sum(Remain)-1,NW);
        Remain(del) = false;
    end
end


function [x, infos] = palm_sparse_smooth_nmf(V, rank, in_options)
% PALM framework with smoothness and sparsity constraints for non-negative
% matrix factorization (PALM-Sparse-Smooth-NMF)
%
% The problem of interest is defined as
%
%           min || WH - V ||_F^2 + lambda * || W ||_1 + eta || HT ||_F^2
%                                   + betaW ||W||_F^2 + betaH ||H||_F^2,
%           where
%           {V, W, H} >= 0,
%
%           Algorithm for NMF with eucidian norm as objective function and
%           L1 constraint on W for sparse paterns and Tikhonov regularization
%           for smooth activation coefficients.
%
% Inputs:
%       matrix      V
%       rank        rank
%       options     options
%           lambda      weight for the L1 sparsity penalty (default: 0)
%           eta         weight for the smoothness constraint.
%           gamma1: 	constant > 1 for the gradient descend step of W.
%           gamma2:     constant > 1 for the gradient descend step of W.
%           betaH:      constant. L-2 constraint for H.
%           betaW:      constant. L-2 constraint for W.
% Output:
%       w           solution of w
%       infos       information
%
% References:
%
%
% This file is part of NMFLibrary.
%
% This file has been ported from
%       palm_nmf.m at https://github.com/raimon-fa/palm-nmf
%
%{
The MIT License
Copyright (c) 2017 Raimon Fabregat
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.
%}
%
% Ported by M.Horie and H.Kasai on June 21, 2022 for NMFLibrary
%
% Change log:
%

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

    % set local options
    local_options = [];
    local_options.lambda = 0;   % sparsity = lambda
    local_options.eta   = 0;    % smoothness = eta
    local_options.betaW = 0.1;
    local_options.betaH = 0.1;
    local_options.gamma1 = 1.001;
    local_options.gamma2 = 1.001;
    local_options.sub_mode = 'std';

    % 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);
    W = init_factors.W;
    H = init_factors.H;

    % initialize for PAML-NMF
    TTp_norm = 0;
    TTp = zeros(n);
    if options.lambda == 0 && options.eta == 0
        % NMF
        % In the case without constraints it can be shown that
        % the gammas can be divided by 2 (Bolte 2014)
        options.gamma1 = options.gamma1 / 2;
        options.gamma2 = options.gamma2 / 2;
        options.betaW = 0;
        options.betaH = 0;
    elseif options.lambda > 0 && options.eta == 0
        % sparse NMF
        options.betaW = 0;
        options.sub_mode = 'sparse';
    elseif options.lambda == 0 && options.eta > 0
        % smooth NMF
        % Tikhonov regularization matrix
        T = eye(n) - diag(ones(n-1, 1),-1);
        T = T(:, 1:end-1);
        TTp = T * T';
        TTp_norm = norm(TTp, 'fro');
        options.betaH = 0;
        options.sub_mode = 'smooth';
    elseif options.lambda > 0 && options.eta > 0
        % smooth and sparse NMF
        % Tikhonov regularization matrix
        T = eye(n) - diag(ones(n-1, 1),-1);
        T = T(:, 1:end-1);
        TTp = T * T';
        TTp_norm = norm(TTp, 'fro');
        options.sub_mode = 'smooth & sparse';
    else
        error('Give positive values to the parameters')
    end

    % initialize
    method_name = sprintf('PALM-Sparse-Smooth (%s)', options.sub_mode);
    epoch = 0;
    grad_calc_count = 0;

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

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

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

    % set start time
    start_time = tic();

    % main loop
    while true

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

        % update W
        c = options.gamma1 * 2 * (norm(H * H', 'fro') + options.betaW);
        z1 = W - (1 / c) * 2 * ((W * H - V) * H' + options.betaW * W);
        W = max(z1 - 2 * options.lambda / c, 0);

        % update H
        d = options.gamma2 * 2 * (norm(W*W', 'fro') + options.eta * TTp_norm + options.betaH);
        z2 = H - (1 / d) * 2 * (W' * (W * H - V) + options.eta * (H * TTp) + options.betaH * H);
        H = max(z2, 0);

        % measure elapsed time
        elapsed_time = toc(start_time);

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

        % update epoch
        epoch = epoch + 1;

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

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

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

end


function y = VBA_sample(form,suffStat,N)
% legacy code
s = warning ('on');
warning ('*** The function `VBA_sample` is now deprecated. Please see `VBA_random` for an alternative.')
warning (s);

if nargin < 3
    N = 1;
end

switch form
    case 'gaussian'
        if isscalar(suffstat.mu)
            N = {1, N};
        else
            N = {N};
        end

        y = VBA_random ('Gaussian', suffStat.mu, suffStat.Sigma, N{:});

    case 'gamma'
        y = VBA_random ('Gamma', suffStat.a, suffStat.b, 1, N);

    case 'dirichlet'
        y = VBA_random ('Dirichlet', suffStat.d, N);

     case 'bernoulli'
        y = VBA_random ('Bernoulli', suffStat.p, 1, N);

    case 'binomial'
        y = VBA_random ('Binomial', suffStat.n, suffstat.p, 1, N);

    case 'multinomial'
        y = VBA_random ('Multinomial', suffStat.n, suffstat.p, N);

end


function [ xy, line_pointer, line_data ] = xyl_example ( point_num, ...
  line_num, line_data_num )

%*****************************************************************************80
%
%% XYL_EXAMPLE sets data suitable for a pair of XY and XYL files.
%
%  Discussion:
%
%    There are 13 points.
%    There are 3 lines.
%    There are 15 line data items.
%
%         4 12-11
%         /\ | |
%        /  \| |
%       /   13 |
%      /      \10
%     /        \
%    5          3
%    |          |
%    |     9--8 |
%    |     |  | |
%    |     |  | |
%    |     6--7 |
%    |          |
%    1----------2
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 January 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer POINT_NUM, the number of points.
%
%    Input, integer LINE_NUM, the number of lines.
%
%    Input, integer LINE_DATA_NUM, the number of line items.
%
%    Output, real XY(2,POINT_NUM), the point coordinates.
%
%    Output, integer LINE_POINTER(LINE_NUM+1), pointers to the
%    first line item for each line.
%
%    Output, integer LINE_DATA(LINE_DATA_NUM), indices
%    of points that form lines.
%
  xy(1:2,1:point_num) = [ ...
     0.0,   0.0; ...
     6.0,   0.0; ...
     6.0,   7.0; ...
     3.0,  10.0; ...
     0.0,   7.0; ...
     4.0,   1.0; ...
     5.0,   1.0; ...
     5.0,   4.0; ...
     4.0,   4.0; ...
     5.0,   8.0; ...
     5.0,  11.0; ...
     4.0,  11.0; ...
     4.0,   9.0 ]';

  line_pointer(1:line_num+1) = [ 1, 7, 12, 16 ];

  line_data(1:line_data_num) = [ ...
     1,  2,  3,  4,  5,  1, ...
     6,  7,  8,  9,  6, ...
    10, 11, 12, 13 ];

  return
end


function value = c8_mul ( z1, z2 )

%*****************************************************************************80
%
%% C8_MUL multiplies two C8's.
%
%  Discussion:
%
%    A C8 is a complex value.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, complex Z1, Z2, the values to multiply.
%
%    Output, complex VALUE, the function value.
%
  value = z1 * z2;

  return
end


function ttt=im2col_inds(a, block)
%IM2COL Rearrange image blocks into columns.
%   B = IM2COL(A,[M N],'distinct') rearranges each distinct
%   M-by-N block in the image A into a column of B. IM2COL pads A
%   with zeros, if necessary, so its size is an integer multiple
%   of M-by-N. If A = [A11 A12; A21 A22], where each Aij is
%   M-by-N, then B = [A11(:) A21(:) A12(:) A22(:)].
%
%   B = IM2COL(A,[M N],'sliding') converts each sliding M-by-N
%   block of A into a column of B, with no zero padding. B has
%   M*N rows and will contain as many columns as there are M-by-N
%   neighborhoods in A. If the size of A is [MM NN], then the
%   size of B is (M*N)-by-((MM-M+1)*(NN-N+1). Each column of B
%   contains the neighborhoods of A reshaped as NHOOD(:), where
%   NHOOD is a matrix containing an M-by-N neighborhood of
%   A. IM2COL orders the columns of B so that they can be
%   reshaped to form a matrix in the normal way. For example,
%   suppose you use a function, such as SUM(B), that returns a
%   scalar for each column of B. You can directly store the
%   result in a matrix of size (MM-M+1)-by-(NN-N+1) using these
%   calls:
%
%        B = im2col(A,[M N],'sliding');
%        C = reshape(sum(B),MM-M+1,NN-N+1);
%
%   B = IM2COL(A,[M N]) uses the default block type of
%   'sliding'.
%
%   B = IM2COL(A,'indexed',...) processes A as an indexed image,
%   padding with zeros if the class of A is uint8 or uint16, or
%   ones if the class of A is double.
%
%   Class Support
%   -------------
%   The input image A can be numeric or logical. The output matrix
%   B is of the same class as the input image.
%
%   Example
%   -------
%   Calculate the local mean using a [2 2] neighborhood with zero padding.
%
%       A = reshape(linspace(0,1,16),[4 4])'
%       B = im2col(A,[2 2])
%       M = mean(B)
%       newA = col2im(M,[1 1],[3 3])
%
%   See also BLOCKPROC, COL2IM, COLFILT, NLFILTER.

%   Copyright 1993-2016 The MathWorks, Inc.

[ma,na] = size(a);
m = block(1); n = block(2);

if any([ma na] < [m n]) % if neighborhood is larger than image
    b = zeros(m*n,0);
    return
end

% Create Hankel-like indexing sub matrix.
mc = block(1); nc = ma-m+1; nn = na-n+1;
cidx = (0:mc-1)'; ridx = 1:nc;
t = cidx(:,ones(nc,1)) + ridx(ones(mc,1),:);    % Hankel Subscripts
tt = zeros(mc*n,nc);
rows = 1:mc;
for i=0:n-1,
    tt(i*mc+rows,:) = t+ma*i;
end
ttt = zeros(mc*n,nc*nn);
cols = 1:nc;
for j=0:nn-1,
    ttt(:,j*nc+cols) = tt+ma*j;
end

%%%
%%% Function parse_inputs
%%%
function [a, block, kind, padval] = parse_inputs(varargin)

narginchk(2,4);

switch nargin
    case 2
        if (strcmp(varargin{2},'indexed'))
            error(message('images:im2col:tooFewInputs'))
        else
            % IM2COL(A, [M N])
            a = varargin{1};
            block = varargin{2};
            kind = 'sliding';
            padval = 0;
        end

    case 3
        if (strcmp(varargin{2},'indexed'))
            % IM2COL(A, 'indexed', [M N])
            a = varargin{1};
            block = varargin{3};
            kind = 'sliding';
            padval = 1;
        else
            % IM2COL(A, [M N], 'kind')
            a = varargin{1};
            block = varargin{2};
            kind = validatestring(varargin{3},{'sliding','distinct'},mfilename,'kind',3);
            padval = 0;
        end

    case 4
        % IM2COL(A, 'indexed', [M N], 'kind')
        a = varargin{1};
        block = varargin{3};
        kind = validatestring(varargin{4},{'sliding','distinct'},mfilename,'kind',4);
        padval = 1;

end

if (isa(a,'uint8') || isa(a, 'uint16'))
    padval = 0;
end


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

%%
clear; close all; clc;

%% Syntax
% |[Es,Vs,Cs]=element2HexLattice(E,V,cPar);|

%% Description
% This function converts an element description (elements and vertices i.e
% nodes into a lattice structure. The lattice structure is returned as a
% hexahederal mesh.

%% Examples
%

%%
% Plot settings
cMap=gjet(4);
fontSize=15;

%% Example 1 Creating a lattice structure on hexahedral meshes

%%
% Creating example geometry.
[V,~]=platonic_solid(2,1); %Vertices of cube
E=1:8; %Element description of the 8-node cube (hexahedral element)
C=(1:size(E,1))'; %color (e.g. material) labels for all elements
[F,~]=element2patch(E,C); %Patch data for plotting

%%
% Create lattice structure
controlParameter.growSteps=0; %0 is normal, positive or negative integers increase or decrease the edge lattice thickness respectively
controlParameter.latticeSide=[]; %Empty outputs both, 1=side 1 the edge lattice, 2=side 2 the dual lattice to the edge lattice
[Es,Vs,Cs]=element2HexLattice(E,V,controlParameter); %Get lattice structure

%%
% Visualizing input mesh and lattic structures

% Create patch Data for visualization
[Fs,CsF]=element2patch(Es,Cs); %Patch data for plotting
[Fs1,CsF1]=element2patch(Es(Cs==1,:),Cs(Cs==1,:)); %Patch data for plotting
[Fs2,CsF2]=element2patch(Es(Cs==2,:),Cs(Cs==2,:)); %Patch data for plotting

cFigure;
hs=subplot(2,2,1);
title('The input mesh','fontSize',fontSize)
hold on;
gpatch(F,V,0.5*ones(1,3),'k',0.25,3);
axisGeom(gca,fontSize);
camlight headlight; lighting flat;
ha=axis; axis off;

subplot(2,2,2);
title('The two complementary lattice structures','fontSize',fontSize)
hold on;
gpatch(F,V,0.5*ones(1,3),'k',0.25,3);
gpatch(Fs,Vs,CsF);
colormap(cMap);
cLim=caxis; caxis([1 2]);
axisGeom(gca,fontSize);
camlight headlight; lighting flat;
axis(ha); axis off;

subplot(2,2,3);
title('Lattice side 1','fontSize',fontSize)
hold on;
gpatch(F,V,0.5*ones(1,3),'k',0.25,3);
gpatch(Fs1,Vs,cMap(1,:));
colormap(cMap);
caxis(cLim);
axisGeom(gca,fontSize);
camlight headlight; lighting flat;
axis(ha); axis off;

subplot(2,2,4);
title('Lattice side 2','fontSize',fontSize)
hold on;
gpatch(F,V,0.5*ones(1,3),'k',0.25,3);
gpatch(Fs2,Vs,CsF2);
colormap(cMap);
caxis(cLim);
axisGeom(gca,fontSize);
camlight headlight; lighting flat;
axis(ha); axis off;

drawnow;

%% Example 2 Creating a lattice structure on tetrahedral meshes

%%
% Creating example geometry.

[V,~]=platonic_solid(1,1); %Vertices of tetrahedron
E=1:4; %Element description of the 4-node tetrahedron
C=(1:size(E,1))'; %color (e.g. material) labels for all elements
[F,~]=element2patch(E,C); %Patch data for plotting

%%
% Create lattice structure
controlParameter.growSteps=0; %0 is normal, positive or negative integers increase or decrease the edge lattice thickness respectively
controlParameter.latticeSide=[]; %Empty outputs both, 1=side 1 the edge lattice, 2=side 2 the dual lattice to the edge lattice
[Es,Vs,Cs]=element2HexLattice(E,V,controlParameter); %Get lattice structure

%%
% Visualizing input mesh and lattic structures

% Create patch Data for visualization
[Fs,CsF]=element2patch(Es,Cs); %Patch data for plotting
[Fs1,CsF1]=element2patch(Es(Cs==1,:),Cs(Cs==1,:)); %Patch data for plotting
[Fs2,CsF2]=element2patch(Es(Cs==2,:),Cs(Cs==2,:)); %Patch data for plotting

cFigure;
hs=subplot(2,2,1);
title('The input mesh','fontSize',fontSize)
hold on;
gpatch(F,V,0.5*ones(1,3),'k',0.25,3);
axisGeom(gca,fontSize);
camlight headlight; lighting flat;
ha=axis; axis off;

subplot(2,2,2);
title('The two complementary lattice structures','fontSize',fontSize)
hold on;
gpatch(F,V,0.5*ones(1,3),'k',0.25,3);
gpatch(Fs,Vs,CsF);
colormap(cMap);
cLim=caxis; caxis([1 2]);
axisGeom(gca,fontSize);
camlight headlight; lighting flat;
axis(ha); axis off;

subplot(2,2,3);
title('Lattice side 1','fontSize',fontSize)
hold on;
gpatch(F,V,0.5*ones(1,3),'k',0.25,3);
gpatch(Fs1,Vs,cMap(1,:));
colormap(cMap);
caxis(cLim);
axisGeom(gca,fontSize);
camlight headlight; lighting flat;
axis(ha); axis off;

subplot(2,2,4);
title('Lattice side 2','fontSize',fontSize)
hold on;
gpatch(F,V,0.5*ones(1,3),'k',0.25,3);
gpatch(Fs2,Vs,CsF2);
colormap(cMap);
caxis(cLim);
axisGeom(gca,fontSize);
camlight headlight; lighting flat;
axis(ha); axis off;

drawnow;

%% Example 3 Changing lattice structure thickness

%%
% Creating example geometry.

[V,~]=platonic_solid(1,1); %Vertices of tetrahedron
E=1:4; %Element description of the 4-node tetrahedron
C=(1:size(E,1))'; %color (e.g. material) labels for all elements
[F,~]=element2patch(E,C); %Patch data for plotting

%%
% Create lattice structure
controlParameter.latticeSide=[]; %Empty outputs both, 1=side 1 the edge lattice, 2=side 2 the dual lattice to the edge lattice

%%
% lattice structure for 0 growth steps
controlParameter.growSteps=0; %0 is normal, positive or negative integers increase or decrease the edge lattice thickness respectively
[Es_0,Vs_0,Cs_0]=element2HexLattice(E,V,controlParameter); %Get lattice structure

controlParameter.growSteps=1; %0 is normal, positive or negative integers increase or decrease the edge lattice thickness respectively
[Es_1,Vs_1,Cs_1]=element2HexLattice(E,V,controlParameter); %Get lattice structure

controlParameter.growSteps=-1; %0 is normal, positive or negative integers increase or decrease the edge lattice thickness respectively
[Es_n1,Vs_n1,Cs_n1]=element2HexLattice(E,V,controlParameter); %Get lattice structure

%%
% Visualizing input mesh and lattic structures

% Create patch Data for visualization
[Fs_0,CsF_0]=element2patch(Es_0,Cs_0); %Patch data for plotting
[Fs_1,CsF_1]=element2patch(Es_1,Cs_1); %Patch data for plotting
[Fs_n1,CsF_n1]=element2patch(Es_n1,Cs_n1); %Patch data for plotting

cFigure;
subplot(1,3,1);
title('growSteps=1','fontSize',fontSize)
hold on;
gpatch(F,V,0.5*ones(1,3),'k',0.25,3);
gpatch(Fs_1,Vs_1,CsF_1);
colormap(cMap);
cLim=caxis; caxis([1 2]);
axisGeom(gca,fontSize);
camlight headlight; lighting flat;
axis off;

subplot(1,3,2);
title('growSteps=0','fontSize',fontSize)
hold on;
gpatch(F,V,0.5*ones(1,3),'k',0.25,3);
gpatch(Fs_0,Vs_0,CsF_0);
colormap(cMap);
cLim=caxis; caxis([1 2]);
axisGeom(gca,fontSize);
camlight headlight; lighting flat;
axis off;

subplot(1,3,3);
title('growSteps=-1','fontSize',fontSize)
hold on;
gpatch(F,V,0.5*ones(1,3),'k',0.25,3);
gpatch(Fs_n1,Vs_n1,CsF_n1);
colormap(cMap);
cLim=caxis; caxis([1 2]);
axisGeom(gca,fontSize);
camlight headlight; lighting flat;
axis off;

drawnow;

%% Example 4 Lattices on multiple elements, adjusting lattice type

%%
% Creating example geometry.
[V,~]=platonic_solid(2,1); %Vertices of cube
E=1:8; %Element description of the 8-node cube (hexahedral element)
[E,V,C]=subHex(E,V); %Subdevide into 8 sub-cubes
[E,V,C]=hex2tet(E,V,C,1); %Convert to tetrahedral elements

[F,~]=element2patch(E,C); %Patch data for plotting

%%
% Create lattice structure
controlParameter.growSteps=-1; %0 is normal, positive or negative integers increase or decrease the edge lattice thickness respectively
controlParameter.latticeSide=1; %Empty outputs both, 1=side 1 the edge lattice, 2=side 2 the dual lattice to the edge lattice
[Es,Vs,Cs]=element2HexLattice(E,V,controlParameter); %Get lattice structure

%%
% Visualizing input mesh and lattic structures

% Create patch Data for visualization
[Fs,CsF]=element2patch(Es,Cs); %Patch data for plotting

cFigure;
hs=subplot(1,2,1);
title('The input mesh','fontSize',fontSize)
hold on;
gpatch(F,V,0.5*ones(1,3),'k',0.25,3);
axisGeom(gca,fontSize);
camlight headlight; lighting flat;

subplot(1,2,2);
title('Lattice side 1','fontSize',fontSize)
hold on;
gpatch(Fs,Vs,cMap(1,:));
colormap(cMap);
caxis(cLim);
axisGeom(gca,fontSize);
camlight headlight; lighting flat;

drawnow;

%%
% Create lattice structure
controlParameter.growSteps=1; %0 is normal, positive or negative integers increase or decrease the edge lattice thickness respectively
controlParameter.latticeSide=2; %Empty outputs both, 1=side 1 the edge lattice, 2=side 2 the dual lattice to the edge lattice
[Es,Vs,Cs]=element2HexLattice(E,V,controlParameter); %Get lattice structure

%%
% Visualizing input mesh and lattic structures

% Create patch Data for visualization
[Fs,CsF]=element2patch(Es,Cs); %Patch data for plotting

cFigure;
hs=subplot(1,2,1);
title('The input mesh','fontSize',fontSize)
hold on;
gpatch(F,V,0.5*ones(1,3),'k',0.25,3);
axisGeom(gca,fontSize);
camlight headlight; lighting flat;

subplot(1,2,2);
title('Lattice side 2','fontSize',fontSize)
hold on;
gpatch(Fs,Vs,cMap(1,:));
colormap(cMap);
caxis(cLim);
axisGeom(gca,fontSize);
camlight headlight; lighting flat;

drawnow;

%% Example 5 Hierarchical lattice structures

%%
% Creating example geometry.

% [V,~]=platonic_solid(2,1); %Vertices of cube
% E=1:8; %Element description of the 8-node cube (hexahedral element)
% C=(1:size(E,1))'; %color (e.g. material) labels for all elements
% [F,~]=element2patch(E,C); %Patch data for plotting

[V,~]=platonic_solid(1,1); %Vertices of tetrahedron
E=1:4; %Element description of the 4-node tetrahedron
C=(1:size(E,1))'; %color (e.g. material) labels for all elements
[F,~]=element2patch(E,C); %Patch data for plotting

%%
% Create first order lattice structure
controlParameter.growSteps=-1; %0 is normal, positive or negative integers increase or decrease the edge lattice thickness respectively
controlParameter.latticeSide=1; %Empty outputs both, 1=side 1 the edge lattice, 2=side 2 the dual lattice to the edge lattice
[Es1,Vs1,Cs1]=element2HexLattice(E,V,controlParameter); %Get lattice structure

%%
% Uncomment to create more complex structure
[Es1,Vs1,Cs1]=hex2tet(Es1,Vs1,Cs1,5); %Convert to tetrahedral elements
% [Es1,Vs1,Cs1]=tet2hex(Es1,Vs1,1); %Convert to tetrahedral elements

%%
% Create second order lattice structure
controlParameter.growSteps=-1; %0 is normal, positive or negative integers increase or decrease the edge lattice thickness respectively
controlParameter.latticeSide=1; %Empty outputs both, 1=side 1 the edge lattice, 2=side 2 the dual lattice to the edge lattice
[Es2,Vs2,Cs2]=element2HexLattice(Es1,Vs1,controlParameter); %Get lattice structure

%%
% Visualizing input mesh and lattic structures

% Create patch Data for visualization
[Fs1,CsF1]=element2patch(Es1,Cs1); %Patch data for plotting
[Fs2,CsF2]=element2patch(Es2,Cs2); %Patch data for plotting

[indBounary]=tesBoundary(Fs2);
Fs2_b=Fs2(indBounary,:);

[indBounary]=tesBoundary(Fs1);
Fs1_b=Fs1(indBounary,:);

%%

cFigure;
hs=subplot(1,3,1);
title('The input mesh','fontSize',fontSize)
hold on;
gpatch(F,V,0.5*ones(1,3),'k',0.25,3);
axisGeom(gca,fontSize);
camlight headlight; lighting flat;
ha=axis; axis off;

subplot(1,3,2);
title('Lattice order 1','fontSize',fontSize)
hold on;
gpatch(F,V,0.5*ones(1,3),'k',0,3);
gpatch(Fs1_b,Vs1,cMap(1,:),'none');
colormap(cMap);
caxis(cLim);
axisGeom(gca,fontSize);
camlight headlight; lighting flat;
axis(ha); axis off;

subplot(1,3,3);
title('Lattice order 2','fontSize',fontSize)
hold on;
gpatch(F,V,0.5*ones(1,3),'k',0,3);
gpatch(Fs2_b,Vs2,cMap(1,:),'none');
colormap(cMap);
caxis(cLim);
axisGeom(gca,fontSize);
camlight headlight; lighting flat;
axis(ha); axis off;

drawnow;

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

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


% 避免大数吃小数
clear;

% 计算 (10^9+10^-9-10^9)/10^-9
S = (10^9 + 10^-9 - 10^9) / 10 ^ -9;
fprintf('直接计算的结果为: %.20e\n',S);
% S1 = S;

% 改变顺序，再次计算
S = (10^9 - 10^9 + 10^-9) / 10 ^ -9;
fprintf('改变顺序计算的结果为: %.20e\n',S);


function normal_01_cdf_inv_test ( )

%*****************************************************************************80
%
%% NORMAL_01_CDF_INV_TEST tests NORMAL_01_CDF_INV.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 February 2015
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'NORMAL_01_CDF_INV_TEST\n' );
  fprintf ( 1, '  NORMAL_01_CDF_INV inverts the Normal 01 CDF;\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      CDF             X                         X\n' );
  fprintf ( 1, '                     (exact)                   (computed)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x1, cdf ] = normal_01_cdf_values ( n_data );

    if ( n_data == 0 )
      break
    end

    x2 = normal_01_cdf_inv ( cdf );

    fprintf ( 1, '  %14.6g  %24.16g  %24.16g\n', cdf, x1, x2 );

  end

  return
end


%SOLVEPNPRANSAC  Finds an object pose from 3D-2D point correspondences using the RANSAC scheme
%
%     [rvec, tvec, success, inliers] = cv.solvePnPRansac(objectPoints, imagePoints, cameraMatrix)
%     [...] = cv.solvePnPRansac(..., 'OptionName', optionValue, ...)
%
% ## Input
% * __objectPoints__ Array of object points in the object coordinate space,
%   1xNx3/Nx1x3 or Nx3 array, where `N` is the number of points, or cell
%   array of length `N` of 3-element vectors can be also passed here
%   `{[x,y,z], ...}`.
% * __imagePoints__ Array of corresponding image points, 1xNx2/Nx1x2 or Nx2
%   array, where `N` is the number of points, or cell array of length `N` of
%   2-element vectors can be also passed here `{[x,y], ...}`.
% * __cameraMatrix__ Input camera matrix `A = [fx 0 cx; 0 fy cy; 0 0 1]`.
%
% ## Output
% * __rvec__ Output rotation vector (see cv.Rodrigues) that, together with
%   `tvec`, brings points from the model coordinate system to the camera
%   coordinate system.
% * __tvec__ Output translation vector.
% * __success__ success logical flag.
% * __inliers__ Output vector that contains indices (zero-based) of inliers in
%   `objectPoints` and `imagePoints`.
%
% ## Options
% * __DistCoeffs__ Input vector of distortion coefficients
%   `[k1,k2,p1,p2,k3,k4,k5,k6,s1,s2,s3,s4,taux,tauy]` of 4, 5, 8, 12 or 14
%   elements. If the vector is empty, the zero distortion coefficients are
%   assumed. default empty.
% * __Rvec__ Initial `rvec`. Not set by default.
% * __Tvec__ Initial `tvec`. Not set by default.
% * __UseExtrinsicGuess__ Parameter used for `Method='Iterative'`. If true,
%   the function uses the provided `rvec` and `tvec` values as initial
%   approximations of the rotation and translation vectors, respectively, and
%   further optimizes them. default false.
% * __IterationsCount__ Number of iterations. default 100.
% * __ReprojectionError__ Inlier threshold value used by the RANSAC procedure.
%   The parameter value is the maximum allowed distance between the observed
%   and computed point projections to consider it an inlier. default 8.0.
% * __Confidence__ The probability that the algorithm produces a useful result.
%   default 0.99
% * __Method__ Method for solving the PnP problem. See cv.solvePnP.
%   default 'Iterative'
%
% The function estimates an object pose given a set of object points, their
% corresponding image projections, as well as the camera matrix and the
% distortion coefficients. This function finds such a pose that minimizes
% reprojection error, that is, the sum of squared distances between the
% observed projections `imagePoints` and the projected (using cv.projectPoints)
% `objectPoints`. The use of RANSAC makes the function resistant to outliers.
%
% Note: The default method used to estimate the camera pose for the Minimal
% Sample Sets step is `EPnP`. Exceptions: if you choose `P3P` or `AP3P`, these
% methods will be used; if the number of input points is equal to 4, `P3P` is
% used.
%
% The method used to estimate the camera pose using all the inliers is defined
% by the flags parameters unless it is equal to `P3P` or `AP3P`. In this case,
% the method `EPnP` will be used instead.
%
% See also: cv.solvePnP, estimateWorldCameraPose
%


function r = taylor(a,str)
%TAYLOR       Taylor class constructor
%
%  r = taylor(a)
%
%An explicit call of the constructor is only necessary to initialize
%  a constant to be of type taylor. Otherwise, any operation
%  with a dependent variable produces a result of type taylor.
%
%For more details try
%
%  help taylorinit
%
%and demotaylor.
%

%taylor.size is size of input
%taylor.t stored as column vector of length INTLAB_TAYLOR_ORDER; in case of
%vector or matrix input, columns of taylor.t are the Taylor coefficients
%

% written  05/21/09     S.M. Rump
% modified 02/28/10     S.M. Rump  rounding
% modified 08/26/12     S.M. Rump  global variables removed
%

  superiorto('intval');

  if nargin==0
    r.size = [];
    r.t = [];
    r = class(r,'taylor');
    return
  end

  INTLAB_TAYLOR_ORDER = getappdata(0,'INTLAB_TAYLOR_ORDER');

  if INTLAB_TAYLOR_ORDER==0
    error('no dependent variables initialized for use of Taylor')
  end

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

  if nargin==1

    if isa(a,'taylor')
      r = a;
    else
      r.size = size(a);
      len = prod(r.size);
      r.t = [ a(:).' ; zeros(INTLAB_TAYLOR_ORDER,len) ];
      r = class(r,'taylor');
    end

  elseif nargin==2

    if ischar(str)

      if isequal(str,'taylorinit')         % call by taylorinit

        r.size = size(a.init);
        len = prod(r.size);
        r.t = [ a.init(:).' ; ones(1,len) ; zeros(INTLAB_TAYLOR_ORDER-1,len) ];
        r = class(r,'taylor');

      elseif isequal(str,'taylor')         % call by @intval\taylor

        r.size = size(a.init);
        len = prod(r.size);
        r.t = intval([ a.init(:).' ; zeros(INTLAB_TAYLOR_ORDER,len) ]);
        r = class(r,'taylor');

      elseif isequal(str,'taylorintval')   % call by @intval\intval

        r = a;
        r.t = intval(r.t);

      elseif isequal(str,'random')          % generates .t randomly, only for test purposes

        if isa(a,'struct')                  % input interval
          a = a.init;
        end
        r.size = size(a);
        len = prod(r.size);
        r.t = [ a(:).' ; randn(INTLAB_TAYLOR_ORDER,len) ];
        r = class(r,'taylor');

      else
        error('invalid call of taylor constructor')
      end

    end

  else

    error('invalid call of constructor taylor')

  end

  if rndold
    setround(rndold)
  end


function [nodeNo,nodeChildIdx] = depthIndex2NodeNo(d,k,wt)
%DEPTHINDEX2NODENO Get node from depth and index in the tree
%   Usage: [nodeNo,nodeChildIdx] = depthIndex2NodeNo(d,k,wt)
%
%   `[nodeNo,nodeChildIdx] = depthIndex2NodeNo(d,k,wt)` returns node
%   *nodeNo* and an array of its children nodes *nodeChildIdx* positioned
%   in depth *g* and index *k* in the tree *wt*.
%
if(d==0)
    nodeNo=0;
    nodeChildIdx=0;
    return;
end

% find ordered nodes at depth d-1
nodesNo = getNodesInDepth(d,wt);
if(isempty(nodesNo))
   error('%s: Depth of the tree is less than given d.',mfilename);
end

% k is index in children of ordered nodes at depth d

nodeNo = zeros(numel(k),1);
nodeChildIdx = zeros(numel(k),1);
chNo = cumsum(cellfun( @(nEl) length(nEl.g),wt.nodes(nodesNo)));
chNoZ = [0;chNo(:)];

for kIdx=1:numel(k)
    ktmp = k(kIdx);
    idx = find(chNo>ktmp,1);
    if isempty(idx)
       error('%s: Index k=%i out of bounds.',mfilename,ktmp);
    end
    nodeNo(kIdx) = nodesNo(idx);
    nodeChildIdx(kIdx) = ktmp-chNoZ(idx)+1;
end

function nodd = getNodesInDepth(d,wt)
% find all nodes with d steps to the root ordered
if d==1
    % return root
    nodd = find(wt.parents==0);
    return;
end

nbf = nodeBForder(0,wt);
nbfTmp = nbf;
tempd = 0;
while tempd<d
    nbf(nbfTmp==0) = [];
    nbfTmp(nbfTmp==0) = [];
    nbfTmp = wt.parents(nbfTmp);
    tempd = tempd+1;
end
nodd = nbf(nbfTmp==0);


%tstoolbox/mex/amutual
%   Fast, but crude auto mutual information of a scalar timeseries for the
%   timelags from zero to maxtau. The input time series should be much
%   longer than maximal timelag maxtau. The algorithm uses equidistant
%   histogram boxes, so results are bad in a mathematical sense. However,
%   a fast algorithm based on ternary search trees to store only nonempty
%   boxes is used.
%
%   Syntax:
%
%     * a = amutual(ts, maxtau, partitions)
%
%   Input arguments:
%
%     * ts - vector holding time series data
%     * maxtau - maximal time lag
%     * partitions - number of partitions for the one-dimensional
%       histogram
%
%   Output arguments:
%
%     * a - vector of length maxtau+1, holding auto mutual information
%
% Copyright 1997-2001 DPI Goettingen, License http://www.physik3.gwdg.de/tstool/gpl.txt


function C = calcConc(s,p,s0)
% This function converts a DCE signal intensity time course to the time course of
% tissue concentration of contrast agent
%
% INPUTS
% s : Signal time course vector.
% p : Parameter array.
%     p(1) = R1 (mmolXsec), p(2) = TR (s), p(3) = FA, p(4) = T10 (s)
% Optional -
% s0: Signal at time t=0. If not input, s(1) is used.
%
% OUTPUT
% C : Time course of Gd concentration (mmoles/L) in blood.
% --------------------------------------------------------------------------------------
% Knowing the relaxivity of the contrast agent, the T1 of blood without contrast, and
% the blood signal value prior to contrast, the concentration of contrast
% agent in the blood is:
%                             1/T1  = 1/T10 + R1* C(t)
% Given S/S0, 1/T1 (for each time point) can be obtained by substituting in from the
% partial saturation equation:
%                        S/S0 = (1-exp(-TR/T1)sin(theta))/(1-exp(-TR/T1)cos(theta)
% Then you go back and solve for C(t).
%
% This function returns a vector (C) containing the blood or tissue Gd concentration time course
% units are mmoles/L
% --------------------------------------------------------------------------------------
%
% Kristen Zakian

%Get inputs
R1 = p(1); TR = p(2); FA = p(3); T10 = p(4);
a = FA*pi/180;       % calculate flip angle in radians

if nargin == 2       % if no s0 specified, then use the first point
    s0 = s(1);
end

if s0 ~= 0
    v = s./s0;
else
    s0 = 0.001;      % fudge
    v = s./s0;       % fudge
    errordlg('concformula: s0 = 0!','ERROR');  %Got this error on Acevedo RT-BL
end

if T10 ~= 0
    TT = TR/T10;
    E10 = exp(-TT);
else
    error('concformula: T10 = 0!')
end

u = 1 - E10;
w = 1 - E10*cos(a);

num = w - u.*v.*cos(a);
denom = w - u.*v;
if denom ~= 0
    L = log(num./denom);
    %y = 1/R1*1/TR*(L - TT)*1000;
    C = 1/R1*1/TR*(L - TT);
else
    error('calcconc: denom = 0');
end

for i=1:size(L,1)
    if ~isreal(L(i,:))
        C(i,:) = double(0);
    end
end

C(C <=0) = 0;

end


function r4mat_uniform_ab_test ( )

%*****************************************************************************80
%
%% R4MAT_UNIFORM_AB_TEST tests R4MAT_UNIFORM_AB.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 Deember 2014
%
%  Author:
%
%    John Burkardt
%
  m = 5;
  n = 4;
  a = -1.0;
  b = +5.0;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R4MAT_UNIFORM_AB_TEST\n' );
  fprintf ( 1, '  R4MAT_UNIFORM_AB computes a random R4MAT.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  %g <= x <= %g\n', a, b );
  fprintf ( 1, '  Initial seed is %d\n', seed );

  [ v, seed ] = r4mat_uniform_ab ( m, n, a, b, seed );

  r4mat_print ( m, n, v, '  Uniform R4MAT:' );

  return
end


function varargout = grMean(varargin)
%GRMEAN Compute mean value from neighbour nodes.
%
%   LBL2 = grMean(EDGES, LBL1)
%   new label for each node of the graph is computed as the mean of the
%   values of neighbours and of old value.
%
%   Example
%   grMean
%
%   See also
%   grMedian, grDilate, grErode
%

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2006-01-20
% Copyright 2006-2022 INRA - CEPIA Nantes - MIAJ (Jouy-en-Josas)

if length(varargin) == 2
    edges   = varargin{1};
    values 	= varargin{2};
elseif length(varargin) == 3
    edges   = varargin{2};
    values  = varargin{3};
else
    error('Wrong number of arguments in "grMean"');
end

res = zeros(size(values));

uni = unique(edges(:));
for n = 1:length(uni)
    neigh = grAdjacentNodes(edges, uni(n));
    res(uni(n)) = mean(values([uni(n); neigh]));
end

varargout{1} = res;


% LRR | ROSL | Robust Orthonormal Subspace Learning (Shu et al. 2014)
% process_video('LRR', 'ROSL', 'dataset/demo.avi', 'output/demo_LRR-ROSL.avi');

K = 1; % The initialiation of the subspace dimension
tol = 1e-5;
maxIter = 30;
lambda = 1e-1; %2e-3;
[~,~,E_hat,A_hat] = inexact_alm_rosl(M,K,lambda,tol,maxIter);
L = A_hat;
S = E_hat;


function [mResult, fMls, fMc, fMu, fSigma, mDatPredBest, vPredBest, fBvalue, fAvalue] = calc_McCdflognormal(mCatalog, fBinning)
% function [mResult, fMls, fMc, fMu, fSigma, mDatPredBest, vPredBest, fBvalue, fAvalue] = calc_McCdflognormal(mCatalog, fBinning);
% -----------------------------------------------------------------------------------------------------------------------------
% Determine Mc using maximum likelihood score
% Fitting non-cumulative frequency magnitude distribution above and below Mc:
% below: Cumulative LOGNORMAL density function
% above: Gutenberg-Richter law
%
% Incoming variables:
% mCatalog   : EQ catalog
% fBinning   : Binning interval, usually 0.1
%
% Outgoing variables:
% mResult     : Solution matrix including
%               vProbability: maximum likelihood score
%               vMc         : Mc values
%               vX_res      : mu (of normal CDF), sigma (of normal CDF), residuum, exitflag
%               vNmaxBest   : Number of events in lowest magnitude bin considered complete
%               vABValue    : a and b-value
% fMls       : minimum maximum likelihood score --> best Mc
% fMc        : Best estimated magnitude of completeness
% mDatPredBest   : Matrix of non-cumulative FMD [Prediction, magnitudes, original distribution]
% vPredBest      : Matrix of non-cumulative FMD below Mc [magnitude, prediction, uncertainty of prediction]
% fBvalue        : b-value
%
% J. Woessner: woessner@seismo.ifg.ethz.ch
% last update: 03.11.03

% Initialize
vProbability = [];
vMc = [];
vABValue =[];
mFitRes = [];
vX_res = [];
vNCumTmp = [];
mDataPred = [];
vPredBest = [];
vDeltaBest = [];
vX_res = [];
vNmaxBest = [];
mResult=[];
mDatPredBest = [];

% Determine exact time period
fPeriod1 = max(mCatalog(:,3)) - min(mCatalog(:,3));

% Determine max. and min. magnitude
fMaxMag = ceil(10 * max(mCatalog(:,6))) / 10;

% Set starting value for Mc loop and LSQ fitting procedure
fMcTry= calc_Mc(mCatalog,1);
fSmu = abs(fMcTry/2);
fSSigma = abs(fMcTry/4);
if (fSmu > 1)
    fSmu = fMcTry/10;
    fSSigma = fMcTry/20;
end
fMcBound = fMcTry;

% Calculate FMD for original catalog
[vFMDorg, vNonCFMDorg] = calc_FMD(mCatalog);
fMinMag = min(vNonCFMDorg(1,:));

%% Shift to positive values
% if fMinMag ~= 0
%     fMcBound = fMcTry-fMinMag;
% end
% Loop over Mc-values
for fMc = fMcBound-0.6:0.1:fMcBound+0.6
    fMc = round(fMc*10)/10;
    vFMD = vFMDorg;
    vNonCFMD = vNonCFMDorg;
    vNonCFMD = fliplr(vNonCFMD);
    % Calculate a and b-value for GR-law and distribution vNCum
    [nIndexLo, fMagHi, vSel, vMagnitudes] = fMagToFitBValue(mCatalog, vFMD, fMc);
    if (length(mCatalog(vSel,1)) >= 20)
        [fMeanMag, fBValue, fStdDev, fAValue] =  calc_bmemag(mCatalog(vSel,:), fBinning);
        % Normalize to time period
        vFMD(2,:) = vFMD(2,:)./fPeriod1; % ceil taken out
        vNonCFMD(2,:) = vNonCFMD(2,:)./fPeriod1; % ceil removed
        % Compute quantity of earthquakes by power law
        fMaxMagFMD = max(vNonCFMD(1,:));
        fMinMagFMD = min(vNonCFMD(1,:));
        vMstep = [fMinMagFMD:0.1:fMaxMagFMD];
        vNCum = 10.^(fAValue-fBValue.*vMstep); % Cumulative number

        % Compute non-cumulative numbers vN
        fNCumTmp = 10^(fAValue-fBValue*(fMaxMagFMD+0.1));
        vNCumTmp  = [vNCum fNCumTmp ];
        vN = abs(diff(vNCumTmp));

        % Normalize vN
        vN = vN./fPeriod1;
        % Data selection
        % mData = Non-cumulative FMD values from GR-law and original data
        mData = [vN' vNonCFMD'];
        vSel = (mData(:,2) >= fMc);
        mDataTest = mData(~vSel,:);
        mDataTmp = mData.subset(vSel);
        % Choices of normalization
        fNmax = mDataTmp(1,3); % Frequency of events in Mc bin
        %fNmax = max(mDataTest(:,3));  % Use maximum frequency of events in bins below Mc
        %fNmax = mDataTest(length(mDataTest(:,1)),3); % Use frequency of events at bin Mc-0.1 -> best fit
        if (~isempty(fNmax) & ~isnan(fNmax) & fNmax ~= 0 & length(mDataTest(:,1)) > 4)
            mDataTest(:,3) = mDataTest(:,3)/fNmax; % Normalize datavalues for fitting with CDF
            % Move to M=0 to fit with lsq-algorithm
            fMinMagTmp = min(mDataTest(:,2));
            mDataTest(:,2) = mDataTest(:,2)-fMinMagTmp;
            % Curve fitting: Non cumulative part below Mc
            options = optimset;
            %options = optimset('Display','off','Tolfun',1e-7,'TolX',0.0001,'MaxFunEvals', 100000,'MaxIter',10000);
            options = optimset('Display','off','Tolfun',1e-5,'TolX',0.001,'MaxFunEvals', 1000,'MaxIter',1000);
            [vX, resnorm, resid, exitflag, output, lambda, jacobian]=lsqcurvefit(@calc_lognormal,[fSmu  fSSigma], mDataTest(:,2), mDataTest(:,3),[],[],options);
            mDataTest(:,1) = logncdf(mDataTest(:,2), vX(1), vX(2))*fNmax;
            if (length(mDataTest(:,2)) > length(vX(1,:)))
                %% Confidence interval determination
                % vPred : Predicted values of lognormal function
                % vPred+-delta : 95% confidence level of true values
                [vPred,delta] = nlpredci(@calc_lognormal,mDataTest(:,2),vX, resid, jacobian);
            else
                vPred = NaN;
                delta = NaN;
            end; % END: This section is due for errors produced with datasets less long than amount of parameters in vX
            % Results of fitting procedure
            mFitRes = [mFitRes; vX resnorm exitflag];
            % Move back to original magnitudes
            mDataTest(:,2) = mDataTest(:,2)+fMinMagTmp;
            % Set data together
            mDataTest(:,3) = mDataTest(:,3)*fNmax;
            mDataPred = [mDataTest; mDataTmp];
            % Denormalize to calculate probabilities
            mDataPred(:,1) = round(mDataPred(:,1).*fPeriod1);
            mDataPred(:,3) = mDataPred(:,3).*fPeriod1;
            vProb_ = calc_log10poisspdf2(mDataPred(:,3), mDataPred(:,1)); % Non-cumulative

            % Sum the probabilities
            fProbability = (-1) * sum(vProb_);
            vProbability = [vProbability; fProbability];
            % Move magnitude back
            mDataPred(:,2) = mDataPred(:,2)+fMinMag;
            vMc = [vMc; fMc];
            vABValue = [vABValue; fAValue fBValue];

            % Keep values
            vDeltaBest = [vDeltaBest; delta];
            vX_res = [vX_res; vX resnorm exitflag];
            vNmaxBest = [vNmaxBest; fNmax];

            % Keep best fitting model
            if (fProbability == min(vProbability))
                vDeltaBest = delta;
                vPredBest = [mDataTest(:,2) vPred*fNmax*fPeriod1 delta*fNmax*fPeriod1]; % Gives back uncertainty
                %fMc+fMinMag : Test procedure
                mDatPredBest = [mDataPred];
           end
        else
            %disp('Not enough data');
            % Setting values
            fProbability = NaN;
            fMc = NaN;
            vX(1) = NaN;
            vX(2) = NaN;
            resnorm = NaN;
            exitflag = NaN;
            delta = NaN;
            vPred = [NaN NaN NaN];
            fNmax = NaN;
            fAValue = NaN;
            fBValue = NaN;
            vProbability = [vProbability; fProbability];
            vMc = [vMc; fMc];
            vX_res = [vX_res; vX resnorm exitflag];
%             vDeltaBest = [vDeltaBest; NaN];
%             vPredBest = [vPredBest; NaN NaN NaN];
            vNmaxBest = [vNmaxBest; fNmax];
            vABValue = [vABValue; fAValue fBValue];
        end; % END of IF fNmax
    end; % END of IF length(mCatalog(vSel,1))

    % Clear variables
    vNCumTmp = [];
    mModelDat = [];
    vNCum = [];
    vSel = [];
    mDataTest = [];
    mDataPred = [];
end; % END of FOR fMc
% Result matrix
mResult = [mResult; vProbability vMc vX_res vNmaxBest vABValue];

% Find best estimate, excluding the case of mResult all NAN
if  ~isempty(nan(mResult))
    if ~isnan(nan(mResult(:,1)))
        vSel = find(nan(mResult(:,1)) == mResult(:,1));
        fMc = min(mResult(vSel,2));
        fMls = min(mResult(vSel,1));
        fMu = min(mResult(vSel,3));
        fSigma = min(mResult(vSel,4));
        fAvalue = min(mResult(vSel,8));
        fBvalue = min(mResult(vSel,9));
    else
        fMc = NaN;
        fMls = NaN;
        fMu = NaN;
        fSigma = NaN;
        fAvalue = NaN;
        fBvalue = NaN;
    end
else
    fMc = NaN;
    fMls = NaN;
    fMu = NaN;
    fSigma = NaN;
    fAvalue = NaN;
    fBvalue = NaN;
end


function [sv,vis] = visibleSegment(s,d,imSize,mrg,lmin)

% VISIBLESEGMENT  Visible segment.
%   VISIBLESEGMENT(S,D,IMSIZE) returns the segment portion of segment S
%   that is visible in the image defined by IMSIZE. D is a vector of depths
%   of the two segment's endpoints.
%
%   VISIBLESEGMENT(...,MRG) restricts the image size to be smaller in MRG
%   pixels at its four borders. The default is MRG = 0 pix.
%
%   VISIBLESEGMENT(...,LMIN) sets all segments shorter than LMIN pixels to
%   be invisible. The default is LMIN = 1 pix.
%
%   [SV,VIS] = VISIBLESEGMENT(...) returns the visible segment SV and a
%   flag of visibility. In case of non visibility, the flag is set to false
%   and the output segment is SV = [0;0;0;0].
%
%   The function works for segment matrices S = [S1 S2 ... Sn] and D = [D1
%   D2 ... Dn], giving a segments matrix SV and a visibility vector VIS.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

% input options and defaults
if nargin < 5
    lmin = 1;
    if nargin < 4
        mrg = 0;
    end
end

% init output arrays
n   = size(s,2);
sv  = zeros(4,n);
vis = false(1,n);

% loop all segments
for i = 1:n
    a = s(1:2,i); % endpoints
    b = s(3:4,i);
    ad = d(1,i);  % depths
    bd = d(2,i);

    if ad<0 && bd<0 % both depths negative -> not visible
        sv(:,i) = zeros(4,1);
        vis(i) = false;

    else

        u = normvec(b-a); % uncorrected direction
        if ad<0           % endpoint A is behind the camera
            a = b + 1e6*u;
        elseif bd<0       % endpoint B is behind the camera
            b = a - 1e6*u;
        end

        % trim segment at image borders, with margin
        ss = trimSegment([a;b],imSize,mrg);

        % check visibility and assign output
        if ...  % conditions for visibility (add with AND if needed)
                ~isempty(ss) ...           % no-null vector
                && (segLength(ss) >= lmin) % minimum length

            sv(:,i) = ss; % visible
            vis(i) = true;
        end
    end
end

return

%% test - generate 2 line handles
lmin = 10;
mrg = 0;
imSize = [100;100];
cla
axis([0,imSize(1),0,imSize(2)])
lh = line('color','r','linestyle','--')
yh = line('color','c','linewidth',3);

%% test - plot random lines
s = 300+randn(4,1)*400
% d = randn(2,1)

s = [-20;50;120;50]
d = [-1;-1]

[sv,vis] = visibleSegment(s,d,imSize,mrg,lmin)

set(lh,'xdata',s([1,3]),'ydata',s([2,4]))
set(yh,'xdata',sv([1,3]),'ydata',sv([2,4]))

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


% function [xpad, nup, n1, n2] = padsignal(x, padtype, padlength)
%
% Pads signal and returns indices of original signal
%
% Input:
%	x: original signal
%	padtype (optional): either 'symmetric' (default) or 'replicate'
%   padlength (optional): number of samples to pad on each side; default is nearest power of 2
% Output:
%	x: padded signal
%   nup: next power of 2
%   n1: length on left
%   n2: length on right
%
%---------------------------------------------------------------------------------
%    Synchrosqueezing Toolbox
%    Authors: Eugene Brevdo, Gaurav Thakur
%---------------------------------------------------------------------------------
function [xpad, nup, n1, n2] = padsignal(x, padtype, padlength)

[nup, n1, n2] = p2up(length(x));
if nargin<3
	[nup, n1, n2] = p2up(length(x));	%if padlength not given, pad up to nearest power of 2
else
	nup = length(x)+2*padlength;
	n1 = padlength-1;
	n2 = padlength;
end

%xl = padarray(x(:), n1, padtype, 'pre');
%xr = padarray(x(:), n2, padtype, 'post');
%xpad = [xl(1:n1); x(:); xr(end-n2+1:end)];
%padarray needs image processing toolbox; below is equivalent code to avoid that

x=x(:);
n=length(x);
if strcmpi(padtype,'symmetric')
	xl = repmat([x;flipud(x)],[ceil(n1/(2*n)),1]);
	xr = repmat([flipud(x);x],[ceil(n2/(2*n)),1]);
elseif strcmpi(padtype,'replicate')
	xl = x(1)*ones(n1,1);
	xr = x(end)*ones(n2,1);
end

xpad = [xl(end-n1+1:end); x; xr(1:n2)];


function b = cpbsl ( abd, lda, n, m, b )

%*****************************************************************************80
%
%% CPBSL solves a complex hermitian positive definite band system.
%
%  Discussion:
%
%    The system matrix must have been factored by CPBCO or CPBFA.
%
%    A division by zero will occur if the input factor contains
%    a zero on the diagonal.  Technically this indicates
%    singularity but it is usually caused by improper subroutine
%    arguments.  It will not occur if the subroutines are called
%    correctly and INFO == 0.
%
%    To compute inverse(A) * C where C is a matrix with P columns:
%
%      call cpbco(abd,lda,n,rcond,z,info)
%
%      if (rcond is too small .or. info /= 0) then
%        error
%      end if
%
%      do j = 1, p
%        call cpbsl(abd,lda,n,c(1,j))
%      end do
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 May 2007
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1,
%    LC: QA214.L56.
%
%  Parameters:
%
%    Input, complex ABD(LDA,N), the output from CPBCO or CPBFA.
%
%    Input, integer LDA, the leading dimension of ABD.
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer M, the number of diagonals above the main diagonal.
%
%    Input, complex B(N), the right hand side.
%
%    Output, complex B(N), the solution.
%

%
%  Solve hermitian(R) * Y = B.
%
  for k = 1 : n
    lm = min ( k - 1, m );
    la = m + 1 - lm;
    lb = k - lm;
    t = abd(la:la+lm-1,k)' * transpose ( b(lb:lb+lm-1) );
    b(k) = ( b(k) - t ) / abd(m+1,k);
  end
%
%  Solve R * X = Y.
%
  for k = n : -1 : 1
    lm = min ( k - 1, m );
    la = m + 1 - lm;
    lb = k - lm;
    b(k) = b(k) / abd(m+1,k);
    t = -b(k);
    b(lb:lb+lm-1) = b(lb:lb+lm-1) + t * transpose ( abd(la:la+lm-1,k) );
  end

  return
end