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 [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


% 避免大数吃小数
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 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 [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 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


%==============================================================================
% 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 TriMesh3 < handle
%
% Finite Element Mesh based on triangular subdivision of rectangular mesh.
%
% Each Cell is divided into four triangles:
%
%  o---------o---------o---------o---------o
%  | \     / | \     / | \     / | \     / |
%  |   \ /   |   \ /   |   \ /   |   \ /   |
%  |    X    |    X    |    X    |    X    |
%  |  /  \   |  /  \   |  /  \   |  /  \   |
%  |/      \ |/      \ |/      \ |/      \ |
%  o---------o---------o---------o---------o
%  | \     / | \     / | \     / | \     / |
%  |   \ /   |   \ /   |   \ /   |   \ /   |
%  |    X    |    X    |    X    |    X    |
%  |  /  \   |  /  \   |  /  \   |  /  \   |
%  |/      \ |/      \ |/      \ |/      \ |
%  o---------o---------o---------o---------o
%  | \     / | \     / | \     / | \     / |
%  |   \ /   |   \ /   |   \ /   |   \ /   |
%  |    X    |    X    |    X    |    X    |
%  |  /  \   |  /  \   |  /  \   |  /  \   |
%  |/      \ |/      \ |/      \ |/      \ |
%  o---------o---------o---------o---------o
%
%  To construct an instance of this class type:
%
%  >> Mesh = TriMesh3(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 TriMesh3 < 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 = 3;
        % ===================================
        % function handle to myself
        % ===================================
        me = @TriMesh3
        % ===================================
        % 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
        AvN
        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_
        AvN_
    end

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

            this.xn = [ reshape(getNodalGrid(omega,m),[],2);...
               reshape(getCellCenteredGrid(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,[]);
            % get indices of cell-centered points
            cc    = prod(m+1)+(1:prod(m));
            % specify triangles
            this.tri   = [...
                nodes;          nodes+1;        cc; ...
                nodes+1;        nodes+m(1)+2;   cc; ...
                nodes+m(1)+2;   nodes+m(1)+1;   cc; ...
                nodes+m(1)+1;   nodes;          cc; ...
                ];
            this.tri   = reshape(this.tri,3,[])';

            this.nnodes = prod(m+1)+prod(m);
            this.ntri  = size(this.tri,1);
            this.vol = prod((omega(2:2:end)-omega(1:2:end))./m)/4*ones(this.ntri,1);

        end

        function runMinimalExample(~)
            omega = [0 4 2 6]; m = [3 6];
            Mesh  = feval(mfilename,omega,m);
            figure(3); clf;
            set(gcf,'Name',sprintf('%s, m=[%d,%d]',mfilename,m));
            plotTriMesh(Mesh,Mesh.xn);
        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,4,[]),1);
            else
                x = reshape(x,1,[]);
                x = .25*repmat(x,[1 4]);
                x = x(:);
            end

        end

        % ===================================
        % x = getNodalGridData(this,x)
        %
        % gets values at triangle nodes that belong the underlying nodal
        % grid
        % ===================================
        function x = getNodalGridData(this,x)
            x = reshape(x,prod(this.nnodes),[]);
            x = x(1:prod(this.m+1),:);
            x = x(:);
        end

        % ===================================
        % x = getCCGridData(this,x)
        %
        % gets values at cell-centers that belong the underlying nodal
        % grid
        % ===================================
        function x = getCCGridData(this,x)
            x = reshape(x,prod(this.nnodes),[]);
            x = x(prod(this.m+1)+1:end,:);
            x = x(:);
        end

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

                    Pyn = zeros([2*m+1 2]);
                    Pyc = zeros([2*m 2]);

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

                    % compute cell-centers
                    Pyc(1:2:end-1,1:2:end-1,:) = .5*(yn(1:end-1,1:end-1,:) + yc);
                    Pyc(2:2:end  ,1:2:end-1,:) = .5*(yn(2:end  ,1:end-1,:) + yc);
                    Pyc(1:2:end-1,2:2:end  ,:) = .5*(yn(1:end-1,2:end  ,:) + yc);
                    Pyc(2:2:end  ,2:2:end  ,:) = .5*(yn(2:end  ,2:end  ,:) + yc);

                    Pu = [reshape(Pyn,[],2); reshape(Pyc,[],2)];
                    Pu = Pu(:);
                case 'PTu'
                    yn = reshape(yn,[],2);
                    yc = reshape(yn(prod(m+1)+1:end,:),[m 2]);
                    yn = reshape(yn(1:prod(m+1),:),[m+1 2]);

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

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

                    % distribute cell-centers
                    t1 = .5*yc(1:2:end-1,1:2:end-1,:);
                    t2 = .5*yc(2:2:end  ,1:2:end-1,:);
                    t3 = .5*yc(1:2:end-1,2:2:end  ,:);
                    t4 = .5*yc(2:2:end  ,2:2:end  ,:);
                    Pyc = yn(2:2:end,2:2:end,:) + (t1+t2+t3+t4);
                    Pyn(1:end-1,1:end-1,:) = Pyn(1:end-1,1:end-1,:) + t1;
                    Pyn(2:end  ,1:end-1,:) = Pyn(2:end  ,1:end-1,:) + t2;
                    Pyn(1:end-1,2:end  ,:) = Pyn(1:end-1,2:end  ,:) + t3;
                    Pyn(2:end  ,2:end  ,:) = Pyn(2:end  ,2:end  ,:) + t4;

                    Pu = [reshape(Pyn,[],2); reshape(Pyc,[],2)];
                    Pu = Pu(:);
            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
            indfn = reshape(1:prod(mf+1),mf+1);
            indfc = reshape(prod(mf+1)+(1:prod(mf)),mf);
            indcn = reshape(1:prod(m+1),m+1);
            indcc = reshape(prod(m+1)+(1:prod(m)),m);

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

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

            % include midpoint as node
            ii = indfn(2:2:end,2:2:end);
            jj = indcc(:);
            ww = ones(size(jj));
            I = [I; ii(:)]; J = [J; jj(:)]; W = [W; ww(:)];

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

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

            % prolongate from bottom-left to top-right
            ii   = indfc(1:2:end,1:2:end);
            jj   = [reshape(indcn(1:end-1,1:end-1),[],1); reshape(indcc,[],1)];
            ww   = .5*ones(size(jj));
            I = [I; ii(:); ii(:)]; J = [J; jj(:)]; W = [W; ww(:)];
            ii   = indfc(2:2:end,2:2:end);
            jj   = [reshape(indcn(2:end,2:end),[],1); reshape(indcc,[],1)];
            ww   = .5*ones(size(jj));
            I = [I; ii(:); ii(:)]; J = [J; jj(:)]; W = [W; ww(:)];

            % prolongate from top-left to bottom-right
            ii   = indfc(2:2:end,1:2:end);
            jj   = [reshape(indcn(2:end,1:end-1),[],1); reshape(indcc,[],1)];
            ww   = .5*ones(size(jj));
            I = [I; ii(:); ii(:)]; J = [J; jj(:)]; W = [W; ww(:)];
            ii   = indfc(1:2:end,2:2:end);
            jj   = [reshape(indcn(1:end-1,2:end),[],1); reshape(indcc,[],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(mf),prod(m+1)+prod(m));
        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 AvN = get.AvN(this)
            if isempty(this.AvN_),
                av  = @(n) spdiags(ones(n+1,1)*[0.5 0.5],[0,1],n,n+1);
                this.AvN_ = kron(speye(2), [speye(prod(this.m+1)); kron(av(this.m(2)),av(this.m(1)))]);
            end
            AvN = this.AvN_;
        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 = [P,sparse(size(P,1),prod(this.m))];
                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)+prod(this.m))),
                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 l = lumHK(img)
%
%       l = lum(img)
%
%       This function calculates the Helmholtz-Kohlrausch luminance
%
%
%       input:
%           img: an RGB image
%
%       output:
%           l: normalized Helmholtz-Kohlrausch luminance
%
%     Copyright (C) 2015  Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

check3Color(img);

imgXYZ = ConvertRGBtoXYZ(img, 0);
imgLCh = ConvertXYZtoCIELCh(imgXYZ, 0);

L = imgLCh(:,:,1);
C = imgLCh(:,:,2);
h = imgLCh(:,:,3);

h2 = ((h - 90) / 2) * pi / 360;

l = L + (2.5 - 0.025 * L) .* (0.116 * abs(sin(h2)) + 0.085) .* C;
l = l / max(l(:));

end


function [p sres sres_ns] = ResidScan(res, FWHM)
% Calculates P(M>=t) where M is the max value of the smoothed residuals.
% In this implementation the residuals are smoothed using a Gaussian
% kernel.
%
% :Usage:
% ::
%
%     function [p sres sres_ns] = ResidScan(res, FWHM)
%
% :Inputs:
%
%   **res:**
%        residual time course
%
%   **FWHM:**
%        Full Width Half Maximum (in time units)
%
% :Outputs:
%
%   **p:**
%        pvalues
%
%   **sres:**
%        smoothed residuals
%
%   **sres_ns:**
%        smoothed residuals (non standardized)
%
% ..
%    By Martin Lindquist & Ji-Meng Loh, July 2007
%
%    Edited by ML on 10/02/09
% ..

res_ns = res;
res = res./std(res);
len = length(res);

% Create Gaussian Kernel
sig = ceil(FWHM/(2*sqrt(2*log(2))));
klen = 3*sig;
kern = normpdf((-klen:klen),0,sig);
kern = kern./sqrt(sum(kern.^2));

% Convolve
x = conv(res,kern);
sres = x((klen + 1):(end-klen));

x = conv(res_ns,kern/sum(kern));
sres_ns = x((klen + 1):(end-klen));

% Find Max value
[a,location] = max(abs(sres));

% Find p-values using Gaussian Random Field theory
z = Euler_p(1, a, len, FWHM);
z = 2*z;        %Two-sided test
p = min(1, z);

end

% END MAIN FUNCTION

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Subfunctions
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function pval = Euler_p(myDim, value, N, fwhm)
% function z = Euler_p(myDim, value, N, fwhm)
%
% Finds the p value using the expected Euler characteristic.
%
% This function returns P(M \ge value) using the approximation
% \sum_{d=0}^D R_d(V) \rho_d(value) following Worsley et al's "A Unified
% Statistical Approach for Determining Significant Signals in Images of
% Cerebral Activation".
%
% INPUTS:
%
% myDim - the number of dimensions in the data
% value - the value of the maximum.
% N     - the number of (time) points in that 1 dimension
% fwhm  - the full width half maximum
%
% OUTPUTS:
%
% pval - the p-value

% NOTE: CURRENTLY THIS FUNCTION IS ONLY IMPLEMENTED FOR THE 1D CASE

  % Constants
  myfactor = 4*log(2);
  pi2 = 2*pi;
  exptsq = exp(-(value^2)/2);

  % Euler Characteristc Densties
  rho = zeros(5,1);
  rho(1) = 1-normcdf(value);
  rho(2) = myfactor^(0.5)*exptsq/pi2;
  rho(3) = myfactor * exptsq * value / (pi2 ^ (1.5));
  rho(4) = myfactor ^ (1.5) * exptsq * (value^2-1) / (pi2 ^2);
  rho(5) = myfactor ^2 * exptsq * (value^3-3*value) / (pi2 ^ (5/2));

  % Resel Count
  R0 = 1;
  R1 = N/fwhm;

  % P-value
  pval = R0 * rho(1) + R1 * rho(2);

end


function r = cosh(a)
%COSH         Taylor hyperbolic cosine  cosh(a)
%

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

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

  K = getappdata(0,'INTLAB_TAYLOR_ORDER');

  st = a.t;
  r = a;
  N = size(a.t,2);
  st(1,:) = sinh(a.t(1,:));
  r.t(1,:) = cosh(a.t(1,:));
  for j=2:K
    at_ = a.t(2:j,:);           % some 3 % faster
    st(j,:) = sum( repmat((1:j-1)',1,N).*r.t(j-1:-1:1,:).*at_ , 1 ) ./ (j-1);
    r.t(j,:) = sum( repmat((1:j-1)',1,N).*st(j-1:-1:1,:).*at_ , 1 ) ./ (j-1);
  end
  r.t(K+1,:) = sum( repmat((1:K)',1,N).*st(K:-1:1,:).*a.t(2:K+1,:) , 1 ) ./ K;

  if rndold
    setround(rndold)
  end


function [L, stored_at] = quadtree_laplacian(C,W,CH,D,A)
% QUADTREE_LAPLACIAN
% Builds a finite difference Laplacian on a quadtree following the scheme
% suggested by Bickel et al. "Adaptative Simulation of Electrical
% Discharges". This code is *purposefully* not optimized beyond
% asymptotics for simplicity in understanding its functionality and
% translating it to other programming languages beyond prototyping.
%
% L = octree_laplacian(C,W,CH,D,A)
%
% Inputs:
%   C #nodes by 3 matrix of cell centers
%   W #nodes vector of cell widths (**not** half widths)
%   CH #nodes by 4 matrix of child indeces (-1 if leaf node)
%   D #nodes vector of tree depths
%   A #nodes by #nodes sparse adjacency matrix, where a value of a in the
%       (i,j) entry means that node j is to the a-th direction of i
%       (a=1: left;  a=2: right;  a=3: bottom;  a=4: top).
%
% Outputs:
%   L #num_children by #num_children sparse Laplacian matrix
%   stored_at #num_children by 3 matrix of child cell centers, where the
%       values of L are stored
%
%
% Example:
%
% % Build an octree
% P = 0.5*[cos(th),sin(th)];
% P = [P;[-1,-1];[1,1]];
% [C,W,CH,PAR,D,A] = initialize_quadtree(P,'MaxDepth',8,'Graded',true);
% % This is for plotting
% [V,Q] = bad_quad_mesh_from_quadtree(C,W,CH);
% % Call function to construct Laplacian
% [L, stored_at] = quadtree_laplacian(C,W,CH,D,A);
% % Dummy Laplacian function
% gt_fun = stored_at(:,1).^2.0;
% laplacian_fun = 2 + 0.*stored_at(:,1);
% % Find boundary
% Vmin = min(stored_at,[],1);
% Vmax = max(stored_at,[],1);
% is_boundary = (stored_at(:,1)<=(Vmin(1)+0.2)) + (stored_at(:,2)<=(Vmin(2)+0.2)) + ...
%     (stored_at(:,1)>=(Vmax(1)-0.2)) + (stored_at(:,2)>=(Vmax(2)-0.2));
% bb = find(is_boundary);
% bc = gt_fun(bb);
% % Solve as energy minimization
% u = min_quad_with_fixed(0.5*L,-laplacian_fun,bb,bc);
% % Plot solution
% tsurf(Q,V,falpha(1,1),'FaceVertexCData',u)
% hold on
% sct(stored_at(bb,:)) % Visualize boundary conditions
% set(gcf,'Color','w'); grid off; axis equal; colorbar; caxis([0 1]);
%
%
% See also: initialize_quadtree.m

% We will store Laplacian values at
% child cell indeces
children = find(CH(:,1)==-1);
% map from all cells to children
cell_to_children = -ones(size(W,1),1);
cell_to_children(children) = 1:length(children);

% Vectors for constructing the Laplacian
I = [];
J = [];
vals = [];

for i=1:size(children,1)
    new_I = [];
    new_J = [];
    new_vals = [];
    l = [1,1,1,1];
    new_dirs = [];
    child = children(i);
    d = D(child);
    num_dirs = 0;
    % Let's build d u(child)/dx^2 ~ u(child+W(child)*[1,0])/hr(hl+hr) -
    % 2u(child)/hlhr + u(child-W(child)*[1,0])/hr(hl+hr)
    % So, let's look for the value to the j direction. To do this, we seek the
    % lowest-depth neighbor to the j direction. As a reminder the octree
    % adjacency convention is i->j (1:left-2:right-3:bottom-4:top)
    for j=1:4
        j_neighbors = find(A(child,:)==j);
        if ~isempty(j_neighbors)
            depths_j_neighbors = D(j_neighbors);
            [max_depth_j, max_depth_j_neighbor] = max(depths_j_neighbors);
            max_depth_j_neighbor = j_neighbors(max_depth_j_neighbor);
            % There are two options:
            % One: the leaf node to our j direction has lower or equal depth to
            % us
            if max_depth_j<=d
                l(j) = (W(child) + W(max_depth_j_neighbor))/2;
                % then it's easy, just add this node
                new_I = [new_I;i];
                % THIS HAS TO BE A CHILD !
                assert(cell_to_children(max_depth_j_neighbor)>0);
                new_J = [new_J;cell_to_children(max_depth_j_neighbor)];
                new_vals = [new_vals;-1]; % Todo fix this
                new_dirs = [new_dirs;j];
            else
                % In this case, assuming the grid is graded, there should
                % be two j-neighbors at depth d+1
                nn = j_neighbors(D(j_neighbors)==(d+1));
                assert(length(nn)==2,"Are you sure you are inputting a graded quadtree?")
                assert(all(CH(nn,1)==[-1;-1]))
                % Then we simply average both
                l(j) = (W(child) + W(nn(1)))/2;
                new_I = [new_I;i;i];
                new_J = [new_J;cell_to_children(nn(1));cell_to_children(nn(2))];
                new_vals = [new_vals;-0.5;-0.5];
                new_dirs = [new_dirs;j;j];
            end
            num_dirs = num_dirs + 1;
        end
    end
    new_I = [new_I;i];
    new_J = [new_J;i];
    new_vals = [new_vals; 1.0];
    new_dirs = [new_dirs;5]; % just a hack
    % At this point, we have to divide by the edge-lengths
    new_vals(new_dirs==1) = new_vals(new_dirs==1)/(l(1)*(l(1)+l(2)));
    new_vals(new_dirs==2) = new_vals(new_dirs==2)/(l(2)*(l(1)+l(2)));
    new_vals(new_dirs==3) = new_vals(new_dirs==3)/(l(3)*(l(3)+l(4)));
    new_vals(new_dirs==4) = new_vals(new_dirs==4)/(l(4)*(l(3)+l(4)));
    new_vals(new_dirs==5) = 1/(l(1)*l(2)) + 1/(l(3)*l(4));

    % And add them to the big sparse Laplacian construction vectors
    I = [I;new_I];
    J = [J;new_J];
    vals = [vals;new_vals];
end

% THE LAPLACIAN IS NEGATIVE SEMI DEFINITE!
L = -2*sparse(I,J,vals,length(children),length(children));
stored_at = C(children,:);

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   q = solve_spherical_wrist2(robot, q, T, wrist)
%   Solves the inverse kinematic problem for a spherical wrist. This is for
%   the particular RELATIVE orientation of the last three reference systems in
%   ABB robots
%
%   robot: robot structure.
%   q: vector containing the values of the joints 1, 2 and 3.
%   T: orientation of the last reference system.
%   wrist: select -1 or 1 for two possible solutions (wrist up, wrist down)
%
%	See also DIRECTKINEMATIC.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.

function q = solve_spherical_wrist2(robot, q, T, wrist, method)

switch method

    %algebraic solution
    case 'algebraic'
        A01=dh(robot, q, 1);
        A12=dh(robot, q, 2);
        A23=dh(robot, q, 3);

        Q=inv(A23)*inv(A12)*inv(A01)*T;

        %detect the degenerate case when q(5)=0, this leads to zeros
        % in Q13, Q23, Q31 and Q32 and Q33=1
        thresh=1e-12;
        %detect if q(5)==0
        % this happens when cos(q5) in the matrix Q is close to 1
        if abs(Q(3,3)-1)>thresh
            %normal solution
            if wrist==1 %wrist up
                q(4)=atan2(Q(2,3),Q(1,3));
                q(6)=atan2(-Q(3,2),Q(3,1));
            else %wrist down
                q(4)=atan2(Q(2,3),Q(1,3))+pi;
                q(6)=atan2(-Q(3,2),Q(3,1))+pi;
            end
            if abs(cos(q(6)+q(4)))>thresh
                cq5=(-Q(1,1)-Q(2,2))/cos(q(4)+q(6))-1;
            end
            if abs(sin(q(6)+q(4)))>thresh
                cq5=(Q(1,2)-Q(2,1))/sin(q(4)+q(6))-1;
            end
            if abs(sin(q(6)))>thresh
                sq5=Q(3,2)/sin(q(6));
            end
            if abs(cos(q(6)))>thresh
                sq5=-Q(3,1)/cos(q(6));
            end
            q(5)=atan2(sq5,cq5);
        else %degenerate solution, in this case, q4 cannot be determined,
            % so q(4)=0 is assigned
            if wrist==1 %wrist up
                q(4)=0;
                q(5)=0;
                q(6)=atan2(Q(1,2)-Q(2,1),-Q(1,1)-Q(2,2));
            else %wrist down
                q(4)=-pi;
                q(5)=0;
                q(6)=atan2(Q(1,2)-Q(2,1),-Q(1,1)-Q(2,2))+pi;
            end

        end

        %algebraic solution
    case 'geometric'

        % Obtain the position and orientation of the system 3
        % using the already computed joints q1, q2 and q3
        T01=dh(robot, q, 1);
        T12=dh(robot, q, 2);
        T23=dh(robot, q, 3);
        T03=T01*T12*T23;

        x3=T03(1:3,1);
        y3=T03(1:3,2);
        z3=T03(1:3,3);

        % T= [ nx ox ax Px;
        %     ny oy ay Py;
        %     nz oz az Pz];
        a=T(1:3,3);

        % find z4 normal to the plane formed by z3 and a
        z4=cross(z3, a);	% end effector's vector a: T(1:3,3)

        % in case of degenerate solution,
        % when z3 and z6 are parallel, choose q(4)=0 as solution
        if norm(z4) <= 0.000001
            if wrist == 1 %wrist up
                q(4)=0;
            else
                q(4)=-pi;
            end
        else
            cq4=wrist*dot(z4, -y3);
            sq4=wrist*dot(z4, x3);
            q(4)=atan2(sq4, cq4);
        end

        % solve for q5
        T34=dh(robot, q, 4);
        T04=T03*T34;
        x4=T04(1:3, 1);
        y4=T04(1:3, 2);

        z5=T(1:3, 3); % The vector a T(1:3,3) is coincident with z5

        cq5=dot(z5, y4);
        sq5=dot(z5, -x4);
        q(5)=atan2(sq5, cq5);

        % solve for q6
        x6=T(1:3, 1);

        T45=dh(robot, q, 5);
        T05=T04*T45;
        x5=T05(1:3, 1);
        y5=T05(1:3, 2);

        cq6=dot(x6, -x5);
        sq6=dot(x6, -y5);
        q(6)=atan2(sq6, cq6);

    otherwise
        disp('no method specified in solve_spherical_wrist');
end


function s = findspan(n,p,u,U)
% FINDSPAN  Find the span of a B-Spline knot vector at a parametric point
% -------------------------------------------------------------------------
% ADAPTATION of FINDSPAN from C
% -------------------------------------------------------------------------
%
% Calling Sequence:
%
%   s = findspan(n,p,u,U)
%
%  INPUT:
%
%    n - number of control points - 1
%    p - spline degree
%    u - parametric point
%    U - knot sequence
%
%  RETURN:
%
%    s - knot span
%
%  Algorithm A2.1 from 'The NURBS BOOK' pg68

                                                % int findspan(int n, int p, double u, double *U) {

                                                %   int low, high, mid;
                                                %   // special case
if (u==U(n+2)), s=n; return,  end               %   if (u == U[n+1]) return(n);
                                                %
                                                %   // do binary search
low = p;                                        %   low = p;
high = n + 1;                                   %   high = n + 1;
mid = floor((low + high) / 2);                  %   mid = (low + high) / 2;
while (u < U(mid+1) || u >= U(mid+2))           %   while (u < U[mid] || u >= U[mid+1])  {
    if (u < U(mid+1))                           %     if (u < U[mid])
        high = mid;                             %       high = mid;
    else                                        %     else
        low = mid;                              %       low = mid;
    end
    mid = floor((low + high) / 2);              %     mid = (low + high) / 2;
end                                             %   }
                                                %
s = mid;                                        %   return(mid);
                                                %   }


function [fal,dx0] = aliasing_frequency(x0,conf)
%ALIASING_FREQUENCY aliasing frequency for the given secondary sources
%
%   Usage: [fal,dx0] = aliasing_frequency([x0],conf)
%
%   Input parameters:
%       x0      - secondary sources / m
%       conf    - configuration struct (see SFS_config)
%
%   Output parameters:
%       fal     - aliasing frequency / Hz
%       dx0     - mean distance between secondary sources / m
%
%   ALIASING_FREQUENCY(x0,conf) returns the aliasing frequency for the given
%   secondary sources. First the mean distance dx0 between the secondary sources
%   is calculated, afterwards the aliasing frequency is calculated after Spors
%   (2009) as fal = c/(2*dx0). If no secondary sources x0 are provided, they are
%   first calculated by calling secondary_source_positions().
%   For a calculation that includes the dependency on the listener position have
%   a look at Start (1997).
%
%   See also: sound_field_mono_wfs, secondary_source_positions,
%       secondary_source_distance
%
%   References:
%       Spors and Ahrens (2009) - "Spatial sampling artifacts of wave field
%       synthesis for the reproduction of virtual point sources", 126th
%       Convention of the Audio Engineering Society, Paper 7744,
%       http://www.aes.org/e-lib/browse.cfm?elib=14940
%
%       Start (1997) - "Direct Sound Enhancement by Wave Field Synthesis",
%       PhD thesis, TU Delft,
%       http://resolver.tudelft.nl/uuid:c80d5b58-67d3-4d84-9e73-390cd30bde0d

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

%% ===== Checking of input parameters ====================================
nargmin = 1;
nargmax = 2;
narginchk(nargmin,nargmax);
if nargin<nargmax
    conf = x0;
    x0 = [];
end
isargstruct(conf);

%% ===== Configuration ==================================================
c = conf.c;

%% ===== Computation =====================================================
% If no explicit secondary source distribution is given, calculate one
if isempty(x0)
    x0 = secondary_source_positions(conf);
end
% Get average distance between secondary sources
dx0 = secondary_source_distance(x0);
% Calculate aliasing frequency
fal = c/(2*dx0);


function [f,relres,iter]=frsyniter(F,c,varargin)
%FRSYNITER  Iterative synthesis
%   Usage:  f=frsyniter(F,c);
%           f=frsyniter(F,c,Ls);
%           [f,relres,iter]=frsyniter(F,c,...);
%
%   Input parameters:
%         F       : Frame
%         c       : Array of coefficients.
%         Ls      : length of signal.
%   Output parameters:
%         f       : Signal.
%         relres  : Vector of residuals.
%         iter    : Number of iterations done.
%
%   `f=frsyniter(F,c)` iteratively inverts the analysis operator of *F*, so
%   `frsyniter` always performs the inverse operation of |frana|, even
%   when a perfect reconstruction is not possible by using |frsyn|.
%
%   `[f,relres,iter]=frsyniter(...)` additionally returns the relative
%   residuals in a vector *relres* and the number of iteration steps *iter*.
%
%   **Note:** If it is possible to explicitly calculate the canonical dual
%   frame then this is usually a much faster method than invoking
%   `frsyniter`.
%
%   `frsyniter` takes the following parameters at the end of the line of
%   input arguments:
%
%     'tol',t      Stop if relative residual error is less than the
%                  specified tolerance. Default is 1e-9 (1e-5 for single precision)
%
%     'maxit',n    Do at most n iterations.
%
%     'cg'         Solve the problem using the Conjugate Gradient
%                  algorithm. This is the default.
%
%     'pcg'        Solve the problem using the Preconditioned Conjugate Gradient
%                  algorithm. Please note that preconditioning is not supported
%                  for all frame types.
%
%     'print'      Display the progress.
%
%     'quiet'      Don't print anything, this is the default.
%
%   Algorithms
%   ----------
%
%   The function uses the (Preconditioned) Conjugate Gradient algorithm
%   to solve the following problem::
%
%   ..   FF*f=Fc
%
%   .. math:: FF* f = Fc
%
%   The preconditioning alters the equations such that
%
%   ..   inv(M)FF*f=inv(M)Fc
%
%   .. math:: M^{-1}FF* f = M^{-1}Fc
%
%   Examples
%   --------
%
%   The following example shows how to rectruct a signal without ever
%   using the dual frame:::
%
%      F=frame('dgtreal','gauss',10,20);
%      c=frana(F,bat);
%      [r,relres]=frsyniter(F,c,'tol',1e-14);
%      norm(bat-r)/norm(bat)
%      semilogy(relres);
%      title('Conversion rate of the CG algorithm');
%      xlabel('No. of iterations');
%      ylabel('Relative residual');
%
%   See also: frame, frana, frsyn, franaiter

% AUTHORS: Nicki Holighaus & Peter L. Søndergaard

complainif_notenoughargs(nargin,2,'FRSYNITER');
complainif_notvalidframeobj(F,'FRSYNITER');

tolchooser.double=1e-9;
tolchooser.single=1e-5;

definput.keyvals.Ls=[];
definput.keyvals.tol=tolchooser.(class(c));
definput.keyvals.maxit=100;
definput.keyvals.Fd = [];
definput.flags.alg={'cg','pcg'};
definput.keyvals.printstep=10;
definput.flags.print={'quiet','print'};

[flags,kv,Ls]=ltfatarghelper({'Ls'},definput,varargin);

% if flags.do_auto
%     varargin2 = varargin;
%     varargin2(strcmpi(varargin2,'auto')) = [];
%
%     try
%         varargin2{end+1} = 'pcg';
%         [f,relres,iter]=frsyniter(F,c,varargin2{:});
%     catch
%         if ~flags.do_quiet
%             warning(sprintf('%s: Falling back to regular CG.',upper(mfilename)));
%         end
%         varargin2{end+1} = 'cg';
%         [f,relres,iter]=frsyniter(F,c,varargin2{:});
%     end
%     return;
% end

L=framelengthcoef(F,size(c,1));

Fd = kv.Fd;
% Compute the preconditioner
if flags.do_pcg && isempty(Fd)
    try
       d = cast(1./framediag(F,L),class(c));
    catch
       switch F.type
            case {'filterbank','ufilterbank'}
                Fd = frame(F.type,{'dual',F.g,'forcepainless'},F.a,numel(F.g));
            case {'filterbankreal','ufilterbankreal'}
                Fd = frame(F.type,{'realdual',F.g,'forcepainless'},F.a,numel(F.g));
            otherwise
                error('%s: No preconditioning method available for given frame type.',...
                upper(mfilename));
       end
    end
end

F=frameaccel(F,L);

A=@(x) F.frsyn(F.frana(x));

% It is possible to specify the initial guess, but this is not
% currently done

if flags.do_pcg && isempty(Fd)

      [f,flag,~,iter,relres]=pcg(A,F.frsyn(c),kv.tol,kv.maxit,@(x)d.*x);
elseif flags.do_pcg

      Fd = frameaccel(Fd,L);
      A=@(x) Fd.frsyn(F.frana(x));
      [f,flag,~,iter,relres]=pcg(A,Fd.frsyn(c),kv.tol,kv.maxit);
else

      [f,flag,~,iter,relres]=pcg(A,F.frsyn(c),kv.tol,kv.maxit);
end

if nargout>1
      relres=relres/norm(c(:));
end

% Cut or extend f to the correct length, if desired.
if ~isempty(Ls)
    f=postpad(f,Ls);
else
    Ls=L;
end

if 0
      % This code has been disabled, as the PCG algorithm is so much faster.
    if flags.do_unlocbox

          % Get the upper frame bound (Or an estimation bigger than the bound)
          [~,B]=framebounds(F,L,'a');

          % Set the parameter for the fast projection on a B2 ball
          param.At=@(x) frsyn(F,x);     % adjoint operator
          param.A=@(x)  frana(F,x);     % direct operator
          param.y=c;                    % coefficient
          param.tight=0;                % It's not a tight frame
          param.max_iter=kv.maxit;
          param.tol=kv.tol;
          param.nu=B;

          % Display parameter 0 nothing, 1 summary at convergence, 2 all
          % steps
      if flags.do_print
          param.verbose=1;
      else
          param.verbose=0;
      end

      % Make the projection. Requires UNLocBOX
      [f, ~] = fast_proj_B2(zeros(L,1), 0, param);

      % compute the residue
      res = param.A(f) - param.y; norm_res = norm(res(:), 2);
      relres=norm_res/norm(c(:), 2);

      iter=0; % The code of the fast_proj_B2 is not yet compatible with this
    end

end