function [Rob,Sen,Lmk,Obs,Frm,Fac] = solveGraphCholesky(Rob,Sen,Lmk,Obs,Frm,Fac,options)

% SOLVEGRAPHCHOLESKY Solves the SLAM graph using Cholesky decomposition.
%   [Rob,Sen,Lmk,Obs,Frm,Fac] = solveGraphCholesky(Rob,Sen,Lmk,Obs,Frm,Fac)
%   solves the graph-SLAM problem using Cholesky decomposition of the
%   Hessian matrix.
%
%   IMPORTANT NOTE: This method is illustrative and constitutes the
%   motivation for this toolbox. One can achieve better performances, both
%   in computing time and possibly in robustness and accuracy, by using
%   Matlab's built-in nonlinear optimization tools, such as LSQNONLIN.
%
%   See courseSLAM.pdf in the documentation for details about the Cholesky
%   decomposition for solving the graph-SLAM problem.
%
%   See also ERRORSTATEJACOBIANS, UPDATESTATES, COMPUTEERROR,
%   COMPUTERESIDUAL, COLAMD, '\', MLDIVIDE, LSQNONLIN.

% Copyright 2015-    Joan Sola @ IRI-UPC-CSIC.

global Map

% Control of iterations and exit conditions
n_iter      = options.niterations; % exit criterion of number of iterations
target_dres = options.target_dres; % exit criterion for error variation
target_res  = options.target_res;  % exit criterion for current residual
res_old     = 1e10;                   % last iteration's error

% Map states range
Map.mr = find(Map.used);

for it = 1:n_iter

%     fprintf('----------------\nIteration: %d; \n',it)

    % Compute Jacobians for projection onto the manifold
    [Frm,Lmk] = errorStateJacobians(Frm,Lmk);

    % Build Hessian H and rhs vector b, in global Map
    Fac       = buildProblem(Rob,Sen,Lmk,Obs,Frm,Fac);

    if it == 1 % do this only once:

        % Column permutation
        p  = colamd(Map.H(Map.mr,Map.mr))';

        % Permutated map range
        pr = Map.mr(p);
    end

    % Decomposition
    [Map.R, ill] = chol(Map.H(pr,pr));

    if ill
        error('Ill-conditioned Hessian')
    end

    % Solve for dx and reorder:
    %   - dx is Map.x(mr)
    %   - reordered dx is Map.x(pr)
    y         = -Map.R'\Map.b(pr); % solve for y
    Map.x(pr) = Map.R\y;

    % NOTE: Matlab is able to do all the reordering and Cholesky
    % factorization for you. If you just use the operator '\', as in
    % 'dx = -H\b', Matlab will reorder H, then factor it as R'R, then solve
    % the two subproblems, then reorder back the result into dx. Use the
    % following line to accomplish this, and comment out the code from line
    % 'if it == 1' until here:
    %
    %     Map.x(Map.mr) = -Map.H(Map.mr,Map.mr)\Map.b(Map.mr);

    % Update nominal states
    [Rob,Lmk,Frm] = updateStates(Rob,Lmk,Frm);

    % Check resulting errors
    [res, err_max] = computeResidual(Rob,Sen,Lmk,Obs,Frm,Fac);
    dres           = res - res_old;
    res_old        = res;

    %         fprintf('Residual: %.2e; variation: %.2e \n', res, dres)

    if ( ( -dres <= target_dres ) || (err_max <= target_res) ) %&& ( abs(derr) < target_derr) )
        break;
    end

end

end

function Fac = buildProblem(Rob,Sen,Lmk,Obs,Frm,Fac)

% BUILDPROBLEM Build least squares problem's matrix H and vector b
%   Fac = BUILDPROBLEM(Rob,Sen,Lmk,Obs,Frm,Fac) Builds the least squares
%   problem's matrix H and vector b for a solution using sparse Cholesky
%   factorization of H.

global Map

% Reset Hessian and rhs vector
Map.H(Map.mr,Map.mr) = 0;
Map.b(Map.mr)    = 0;

% Iterate all factors
for fac = find([Fac.used])

    % Extract some pointers
    rob    = Fac(fac).rob;
    sen    = Fac(fac).sen;
    lmk    = Fac(fac).lmk;
    frames = Fac(fac).frames;

    % Compute factor error, info mat, and Jacobians
    [Fac(fac), e, W, ~, J1, J2, r1, r2] = computeError(...
        Rob(rob),       ...
        Sen(sen),       ...
        Lmk(lmk),       ...
        Obs(sen,lmk),   ...
        Frm(frames),    ...
        Fac(fac));

    % Compute sparse Hessian blocks
    H_11 = J1' * W * J1;
    H_12 = J1' * W * J2;
    H_22 = J2' * W * J2;

    % Compute rhs vector blocks
    b1   = J1' * W * e;
    b2   = J2' * W * e;

    % Update H and b
    Map.H(r1,r1) = Map.H(r1,r1) + H_11;
    Map.H(r1,r2) = Map.H(r1,r2) + H_12;
    Map.H(r2,r1) = Map.H(r2,r1) + H_12';
    Map.H(r2,r2) = Map.H(r2,r2) + H_22;

    Map.b(r1,1)  = Map.b(r1,1)  + b1;
    Map.b(r2,1)  = Map.b(r2,1)  + b2;

end

end

% ========== 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 jed = ymdf_to_jed_islamic_b ( y, m, d, f )

%*****************************************************************************80
%
%% YMDF_TO_JED_ISLAMIC_B converts an Islamic B YMDF date to a JED.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Edward Richards,
%    Algorithm E,
%    Mapping Time, The Calendar and Its History,
%    Oxford, 1999, pages 323-324.
%
%  Parameters:
%
%    Input, integer Y, M, D, real F, the YMDF date.
%
%    Output, real JED, the corresponding Julian Ephemeris Date.
%

%
%  Check the date.
%
  [ y, m, d, ierror ] = ymd_check_islamic ( y, m, d );

  if ( ierror ~= 0 )
    jed = -1.0;
    return
  end
%
%  Convert the calendar date to a computational date.
%
  y_prime = y + 5519 - floor ( ( 12 - m ) / 12 );
  m_prime = mod ( m + 11, 12 );
  d_prime = d - 1;
%
%  Convert the computational date to a JED.
%
  j1 = floor ( ( 10631 * y_prime + 14 ) / 30 );

  j2 = floor ( ( 2951 * m_prime + 51 ) / 100 );

  jed = j1 + j2 + d_prime - 7664 - 0.5 + f;

  return
end


 function scArc(Point, LinCol)
 %scArc : Draws an arc (a circle) on the smith chart passing through a given point
%
%  SYNOPSIS:
%     Draws an arc (a circle) on the smith chart passing through a given point.
%     the point in question is defined as an (r,x) pair. This function should only
%     be called when a smith chart figure is already present.
%
%     See also scDraw, scInv, scMove, scMatchCirc
%
%  SYNTAX:
%     scArc(Point, LinCol)
%
%  INPUT ARGUMENTS:
%     Point  : values of r and x defining a point on the smith chart as a vector [r x]
%     LinCol : color of the arc (or circle)
%
%  OUTPUT ARGUMENT:
%     none
%
%  EXAMPLE:
%         The Command sequence
%         scDraw;
%         scArc([2 3]);
%         will draw a blank smith chart and draw a circle having center at the
%         smith chart center [1 0] and passing through the point [2 3]
%         in the smith chart corresponding to an impedance (2+j3)*Z_L. This
%         may be thought of as the locus of impedance along the lossless
%         line of line impedance Z_L.
%
%
%     Mohammad Ashfaq - (31-05-2000)
%     Mohammad Ashfaq - (13-04-2006) Modified (example included)
%

 if nargin == 1
     LinCol = 'm';
 end

 r1 = Point(1);
 x1 = Point(2);

 %x2 = to(2);
 %r2 = to(1);

 [u1, v1] = scPOI(r1, x1);
 %[u2,v2]=point_of_intersection(r2,x2);
 x = linspace(-sqrt(u1^2+v1^2),sqrt(u1^2+v1^2),500);

 plot(x,sqrt(u1^2+v1^2-x.^2),LinCol);
 plot(x,-sqrt(u1^2+v1^2-x.^2),LinCol);

 % MARK POINT
 plot(u1,v1,'r*')


function [D, X] = ODL(Y, k, lambda, opts, method)
% * Solving the following problem:
%  (D, X) = \arg\min_{D,X} 0.5||Y - DX||_F^2 + lambda||X||_1
% * Syntax: `[D, X] = ODL(Y, k, lambda, opts, sc_method)`
%   - INPUT:
%     + `Y`: collection of samples.4/7/2016 7:35:39 PM
%     + `k`: number of atoms in the desired dictionary.
%     + `lambda`: norm 1 regularization parameter.
%     + `opts`: option.
%     + `sc_method`: sparse coding method used in the sparse coefficient update. Possible values:
%       * `'fista'`: using FISTA algorithm. See also [`fista`](#fista).
%       * `'spams'`: using SPAMS toolbox [[12]](#fn_spams).
%   - OUTPUT:
%     + `D, X`: as in the problem.
% -----------------------------------------------
% Author: Tiep Vu, thv102@psu.edu, 4/7/2016
%         (http://www.personal.psu.edu/thv102/)
% -----------------------------------------------
	if nargin == 0
		addpath(fullfile('..', 'utils'));
        addpath(fullfile('..', 'build_spams'));
        d      = 50; % data dimension
        N      = 100; % number of samples
        k      = 50; % dictionary size
        lambda = 0.1;
        Y      = normc(rand(d, N));
        %
		opts.max_iter      = 500;
		opts.show_progress = 0;
		opts.check_grad    = false;
		opts.tol           = 1e-8;
		opts.verbose     = true;

	end
	%%
	opts = initOpts(opts);
	%%
	%% ========= initial D ==============================
	D = PickDfromY(Y, [0, size(Y,2)], k);
    X = zeros(size(D,2), size(Y,2));
    if opts.verbose
        fprintf('cost: %f', ODL_cost(Y, D, X, lambda));
    end
    optsX = opts;
	optsX.max_iter = 200;
	optsX.tol      = 1e-8;
    optsD = opts;
	optsD.max_iter = 200;
	optsD.tol      = 1e-8;
	iter = 0;
	while iter < opts.max_iter
		iter = iter + 1;
		%% ========= sparse coding step ==============================
		X = lasso_fista(Y, D, X, lambda, optsX);
       	if opts.verbose
			costX = ODL_cost(Y, D, X, lambda);
			fprintf('iter: %3d, costX = %5f\n', iter, costX)
		end
		%% ========= dictionary update step ==============================
		F = X*X'; E = Y*X';
		D = ODL_updateD(D, E, F, optsD);
		if opts.verbose
			costD = ODL_cost(Y, D, X, lambda);
			fprintf('iter: %3d, costD = %5f\n', iter, costD)
		end
	end
	%%
	if nargin == 0
		pause;
	end
end


function [S, mu, sigma2] = standardize(M, mu, sigma2)
% function S = standardize(M, mu, sigma2)
% Make each column of M be zero mean, std 1.
% Thus each row is scaled separately.
%
% If mu, sigma2 are omitted, they are computed from M

if nargin < 2
  mu = mean(M,2);
  sigma2 = std(M,0,2);
  sigma2 = sigma2 + eps*(sigma2==0);
end

[nrows ncols] = size(M);
S = M - repmat(mu(:), [1 ncols]);
S = S ./ repmat(sigma2, [1 ncols]);


function y = vl_gaussian(x)
% VL_GAUSSIAN  Standard Gaussian density function
%   Y=VL_GAUSSIAN(X) computes the standard (zero mean, unit variance)
%   Gaussian density.
%
%   To obtain the Gaussian density of standard deviation S do
%
%     Y = 1/S * VL_GAUSSIAN(X/S).
%
%   See also: VL_DGAUSSIAN(), VL_DDGAUSSIAN(), VL_HELP().

% Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

y = 1/sqrt(2*pi)*exp(-0.5*x.^2) ;


function [V,F,I] = capsule(n,a)
  % [V,F,I] = capsule(n,a)
  %
  % Inputs:
  %   n  number of faces along latitude of each hemisphere
  %   a  length of cylindrical portion
  % Outputs:
  %   V  #V by 3 list of vertex positions
  %   F  #F by 3 list of triangle indices into rows of V
  %   I  #F list I(f) = 1,2 or 3 indicating bottom hemisphere, cylindrical part,
  %     top hemisphere, respectively.
  %
  % See also: stadium
  R = 1;
  s = 1;
  s = ceil( (2*n)/(2*pi*R)* a);
  [CV,CF] = cylinder_mesh(R,2*n,'Stacks',s);
  CV = CV.*[1 1 a];
  [X,Y,Z] = sphere(2*n);
  [SF,SV] = surf2patch(X,Y,Z,'triangles');
  [SV,~,J] = remove_duplicate_vertices(SV,1e-7);
  SF=J(SF);
  SF = SF(SF(:,1)~=SF(:,2) & SF(:,2)~=SF(:,3) & SF(:,3)~=SF(:,1),:);
  SBC = barycenter(SV,SF);
  T = SBC(:,3)>0;
  TV = SV+[0 0 a];
  TF = SF(T,:);
  BV = SV;
  BF = SF(~T,:);
  V = [BV;CV;TV];
  F = [BF;size(BV,1)+[CF;size(CV,1)+TF]];
  [V,~,J] = remove_duplicate_vertices(V,1e-7);
  F=J(F);
  [V,~,~,F] = remove_unreferenced(V,F);
  I = [repmat(1,size(BF,1),1);repmat(2,size(CF,1),1);repmat(3,size(TF,1),1)];
end


function C = estimateC(W,R,B,C,lam,tol)

% INPUT:
% W: 2-by-P matrix
% R: 2-by-3 matrix
% B: 3K-by-P matrx
% C: 1-by-K vector

% This function estimates c by minimizing
% f(C) + lam*r(C), where
% f(C) = 0.5 * norm(W-R*S,'fro')^2 where S = \sum_i C_i*B_i
% r(C) = \|C\|_1
% It implements proximal gradient + nesterov

P = size(W,2);
K = size(B,1)/3;

C = C'; % transpose to be a column vector
C0 = C; % C0: the previous estimate
t = 1; % auxiliary variable for nesterov
t0 = 1; % auxiliary variable for nesterov
fvalue = inf;

% next we work on the linear system y = X*C
y = W(:); % vectorized W
X = zeros(2*P,K); % each column is a rotated Bk
for k = 1:K
    RBk = R*B(3*k-2:3*k,:);
    X(:,k) = RBk(:);
end

if lam == 0
    C = X\y;
    C = C';
    return
end

% mu is set as the 2-norm of the Hessian of f(C)
mu = norm(X'*X);

for iter = 1:1000

    % Z is an auxiliary variable in nesterov method
    Z = C + (t0-1)/t*(C-C0);

    % gradient descent
    Z = Z + X'*(y-X*Z)/mu;

    % nonegative thresholding
%     Z = Z - lam/mu;
%     Z = max(Z,0);

    % soft thresholding
    Z = sign(Z).*max(abs(Z)-lam/mu,0);

    % update C
    C0 = C;
    C = Z;

    % update t
    t0 = t;
    t = (1+sqrt(1+4*t^2))/2;

    % function value
    fvalue0 = fvalue;
    fvalue = 0.5 * norm(y-X*C,'fro')^2 + lam*sum(abs(C));
    if fvalue > fvalue0
        t0 = 1; % APG with restart
        t = 1;
    end

    % check convergence
    RelChg = norm(C-C0,'fro')/(norm(C0,'fro')+eps) ;
%     fprintf('Iter %d: FunVal = %f, RelChg = %f\n',iter,fvalue,RelChg);
    if RelChg < tol
        break
    end

end

C = C'; % transpose back to be a row vector


%ISLIMIT Test if HAL joint angles are within their limits
%
% Returns a matrix the same size as the matrix of joint angles input
% (where each row is a joint angle set), whose elements are false if
% within joint limits or true if not. The formulae by Lenarcic & Umek
% (1994) are used to calculate the shoulder range of motion, with the
% qlim values representing the limits when other joint angles are zero.
%
% Copyright (C) Bryan Moutrie, 2013-2014
% Licensed under the GNU Lesser General Public License
% see full file for full statement
%
% This file modifies file(s) from The Robotics Toolbox for MATLAB (RTB)
% by Peter Corke (www.petercorke.com), see file for statement
%
% Upper arm range of motion:
%  Lenarcic, J. & Umek, A. 1994, 'Simple Model of Human Arm Reachable
%   Workspace', IEEE Transactions on systems, man, and cybernetics,
%   vol. 24, no. 8, pp. 1239-46.
%
% Syntax:
%  (1) I = hal.islimit(q)
%
% Outputs:
%  I : logical matrix, elements which are true are angles outside limit
%
% Inputs:
%  q : Joint row vector, or trajectory matrix of joint row vectors
%
% See also HAL.reachable

% LICENSE STATEMENT:
%
% This file is part of pHRIWARE.
%
% pHRIWARE 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.
%
% pHRIWARE 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 Lesser General Public
% License along with pHRIWARE.  If not, see <http://www.gnu.org/licenses/>.
%
% RTB LIBRARY:
%
% Copyright (C) 1993-2014, by Peter I. Corke
% http://www.petercorke.com
% Released under the GNU Lesser General Public license,
% Modified 16/6/2014 (HAL is a subclass of SerialLink)

function I = islimit(hal, q)

I = false(size(q));

for i = [1 4 5 6 7]
    I(:,i) = hal.qlim(i,2) <= q(:,i)|q(:,i) <= hal.qlim(i,1);
end

toplim(:,1) = hal.qlim(2,2) - q(:,1)/6;
toplim(:,2) = hal.qlim(3,2) + ...
    4*q(:,1)/9 - 5*q(:,2)/9 + 10*q(:,1).*q(:,2)/(9*pi);

lowlim(:,1) = hal.qlim(2,1) + q(:,1)/3;
lowlim(:,2) = hal.qlim(3,1) + ...
    7*q(:,1)/9 - q(:,2)/9 + 4*q(:,1).*q(:,2)/(9*pi);

I(:,[2 3]) =  toplim <= q(:,[2 3])|q(:,[2 3]) <= lowlim;

end


function imgOut = SchlickTMO(img, s_mode, p, nBit, L0, k)
%
%       imgOut = SchlickTMO(img, s_mode, p, nBit, L0, k)
%
%
%       Input:
%           -img: input HDR image.
%           -s_mode: parameters selection: 'manual', 'automatic', and 'nonuniform'.
%           -p: a model parameter which takes values in [1,+inf].
%           -nBit: number of bit for the quantization step.
%           -L0: lowest value of the LDR monitor that can be perceived
%           by the HVS.
%           -k: a value in [0,1].
%
%       Output
%           -imgOut: tone mapped image
%
%     Copyright (C) 2010-15  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/>.
%
%     The paper describing this technique is:
%     "Quantization Techniques for Visualization of High Dynamic Range Pictures"
% 	  by Christophe Schlick
%     in "Photorealistic Rendering Techniques" 1995
%

checkNegative(img);

check13Color(img);

check3Color(img);

if(~exist('s_mode', 'var'))
    s_mode = 'automatic';
end

if(~exist('nBit', 'var'))
    nBit = 8;
end

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

if(~exist('k','var'))
    k = 0.5;
end

if(~exist('p', 'var'))
    p = 1 / 0.005;
end

%compute the luminance channel
L = lum(img);

%compute the min luminance
LMin = min(L(L > 0.0));

%compute the max luminance
LMax = max(L(L>0.0));

%mode selection
switch s_mode
    case 'manual'
        p = max([p, 1]);

    case 'automatic'
        p = L0 * LMax / (2^nBit * LMin);

    case 'nonuniform'
        p = L0 * LMax / (2^nBit * LMin);
        p = p * (1 - k + k * L / sqrt(LMax * LMin));
end

%dynamic range reduction
Ld = p .* L ./ ((p - 1) .* L + LMax);

%change luminance
imgOut = ChangeLuminance(img, L, Ld);

end


function [output] = ols_reg(Y,X,options)

% this function computes the ols coefficients of Y on X
% Y TxN
% X Txk (interepct at the end)
% Y = XB + E

nindex      = find(sum(isnan([Y,X]),2)==0);
index       = find(sum(isnan([Y,X]),2)>0);
Y(index,:)  = [];
X(index,:)  = [];
[N,K]       = size(X);
robust_se_  = 0;
L           = round(N/4);

if nargin > 2
    if isfield(options,'robust_se_') == 1
        robust_se_ = options.robust_se_;
    end
    if isfield(options,'L') == 1
        L = options.L;
    end
end

ny = size(Y,2);

XX      = (X'*X);
iXX     = XX\eye(K);
Bols    = X\Y;                  % Bols   = (X'*X)\(X'*Y);
err     = Y - X*Bols;
se      = nan(K,ny);
Sols    = zeros(K*ny);

if robust_se_==2 % NW Robust SE
    %-----------------------------------------------------------------------%
    Serror     = (err'*err);
    nwWeights  = (L+1-(1:L))./(L+1);
    for j= 1 : L
        G      = (err(j+1 : N, :)'*err(1 : N-j , :));
        Serror = Serror + nwWeights(j) * (G + G');
    end
    Serror = Serror/(N-K);
    Sols   = kron(diag(diag(Serror)),iXX);
    se     = reshape(sqrt(diag(Sols)), K, ny);

elseif robust_se_ == 1 % Hamilton (1994), Ch 10 pag 282, eq (10.5.20)
    %-----------------------------------------------------------------------%
    for vv = 1: ny % equation by equation
        u= err(:,vv);
        errs=X.*u;
        V0 = [errs'*errs] / N ; %regular weighting matrix
        for ind_i = (1:L)
            S = errs(1:N-ind_i,:)'*errs(1+ind_i:N,:) / N;
            V0 = V0 + (1 - ind_i/(L+1))*(S + S');
        end
        %     D    =   inv((X'*X)/N);
        %     varb = 1/N*D*V1*D;
        Solsj =  N * iXX * V0 * iXX;
        Sols((vv-1)*K+1 : vv*K, (vv-1)*K+1 : K*vv ) = Solsj;
    end
    Serror = (err'*err)/(N-K); % not sure this is the correct Coavariance of the shocks.
    se     = reshape(sqrt(diag(Sols)), K, ny);

elseif robust_se_ == 5 % Matlab HAC function
    %-----------------------------------------------------------------------%
    if  exist('hac') ==2
        % find constant
        index = find(sum(diff(X,1,1),1)~=0);
        indexC = find(sum(diff(X,1,1),1)==0);
        for vv = 1: ny
%             [EstCoeffCov0,se0,~] = hac(X(:,index),Y(:,ny),'display','off','type','HC');
            [EstCoeffCov0,se0,~] = hac(X(:,index),Y(:,ny),'display','off','bandwidth','AR1OLS'); % remove the intercept
            se(index,vv)  = se0(2:end);
            se(indexC,vv) = se0(1); %intercept
            Sols((vv-1)*K+1 : vv*K, (vv-1)*K+1 : K*vv ) = EstCoeffCov0; % order is not correct
        end
        Serror  = 1/(N-K)*(err'*err);
%         output.TtestRobust  = coeff./se;
%         output.pvalueRobust = tpdf(output.TtestRobust,N-K);
    else
        error('Matlab Econ Toolbox missing')
    end
else
    Serror  = 1/(N-K)*(err'*err);
    Sols    = kron(diag(diag(Serror)),iXX);
    se      = reshape(sqrt(diag(Sols)), K, ny);
end

Ttest  = Bols./ reshape(sqrt(diag(Sols)), K, ny);
ESS    = diag((X*Bols - mean(Y))'*(X*Bols - mean(Y)));
RSS    = diag(err' * err);
TSS    = diag((Y - mean(Y))' * (Y - mean(Y)));
R2     = ones(length(ESS),1) - RSS ./ TSS;
adjR2  = ones(length(ESS),1) - (ones(length(ESS),1) - R2)*(N-1)/(N-K);
Ftest  = ESS/(K-1) ./ diag(Serror);

% for v = 1 : ny
% %     output.logl(v,1)   = -N/2*log(2*pi*Serror(v,v)) - RSS(v,1)/(2*Serror(v,v));
%     [output.AIC(v,1), output.SIC(v,1), output.HQIC(v,1)] = IC(output.logl(v,1), N, K);
% end

output.beta   = Bols;                   % OLS estimator
output.error  = err;                   % (TxN) matrix of Residuals
output.e_ols  = err;                   % (TxN) matrix of Residuals
output.Serror = Serror;                 % Covariance matrix of Residuals
output.Sigma_ols = Serror;                 % Covariance matrix of Residuals
output.Sols   = Sols;                   % Covariance matrix of Bols
output.Ttest  = Ttest;                  % t-statistics
output.pvalue = tpdf(Ttest,N-K);        % p-value
output.Ftest  = Ftest;                  % F-test
output.R2     = R2;                     % R2
output.adjR2  = adjR2;                  % Adjusted R2
output.yfit   = X*Bols;                 % Fitted Values
output.N      = N;                      % # of observation used
output.K      = K;                      % # of regressors
output.nindex = nindex;                 % index of missing observation
output.index  = index;                  % index of observation used
output.se     = se;
output.XX     = XX;
output.X      = X;
output.Y      = Y;

for v = 1 : ny
%     output.logl(v,1)   = -N/2*log(2*pi*Serror(v,v)) - RSS(v,1)/(2*Serror(v,v));
%     [output.AIC(v,1), output.SIC(v,1), output.HQIC(v,1)] = IC(output, N, K);
    S = Serror(v,v); E = err(:,v);
    [output.AIC(v,1), output.SIC(v,1), output.HQIC(v,1)] = IC(S, E, N, K);
end

% if nargin > 2
%     [EstCov,se,coeff] = hac(X(:,1:end-1),Y,'display','off'); % remove the intercept
%     output.serob        = se(2:end);
%     output.serob(end+1) = se(1); %intercept
%     output.TtestRobust  = coeff./se;
%     output.pvalueRobust = tpdf(output.TtestRobust,N-K);
% end


function K = covPERard(cov, hyp, x, z, i)

% Stationary periodic covariance function for a stationary covariance function
% k0 such as covMaternard, covPPard, covRQard and covSEard.
% Stationary means that the covariance function k0(x,z) depends on the
% data points x,z only through the squared distance
% dxz = (x-z)'*inv(P)*(x-z) where the P matrix is diagonal with ARD parameters
% ell_1^2,...,ell_D^2, where D is the dimension of the input space.
% The covariance function is parameterized as:
%
% k(x,z) = k0(u(x),u(z)), u(x) = [sin(pi*x/p); cos(pi*x/p)]
%
% where the period p belongs to covPERiso and hyp0 belong to k0:
%
% hyp = [ log(p_1)
%         log(p_2)
%          .
%         log(p_D)
%         hyp0 ]
%
% The first D hyperparameters of k0 are the log lengthscales such that
% hyp0(i) = log(ell(i)) for i=1..D.
% Note that for k0 = covSEard and D = 1, a faster alternative is covPeriodic.
%
% Copyright (c) by Hannes Nickisch, 2013-10-16.
%
% See also COVFUNCTIONS.M.

nocov = false;                   % default case when no cov argument is provided
if nargin==0, cov = {@covSEard}; nocov = true; end                % default case
if isnumeric(cov)       % detect old version where the cov parameter was missing
  % i <- z, z <- x, x <- hyp, hyp <- cov
  if nargin>3, i = z; end
  if nargin>2, z = x; end
  if nargin>1, x = hyp; end
  hyp = cov; cov = {@covSEard}; nocov = true;
end

if nocov && nargin<2 || ~nocov && nargin<3         % report number of parameters
  K = ['(D+',feval(cov{:}),')']; return
end
if nocov && nargin<3 || ~nocov && nargin<4, z = []; end    % make sure, z exists
xeqz = isempty(z); dg = strcmp(z,'diag');                       % determine mode

[n,D] = size(x);
p = exp(hyp(1:D));
lell = hyp(D+(1:D));
hyp0 =  [lell; lell; hyp(2*D+1:end)];

if nocov && nargin<4 || ~nocov && nargin<5
  [x,z] = u(x,z,p,dg);                      % apply the embedding u:IR^D->IR^2*D
  K = feval(cov{:},hyp0,x,z);
else
  if i<=D
    if dg                                                   % compute distance d
      di = zeros([n,1]);
    else
      if xeqz                                             % symmetric matrix Kxx
        di = repmat(reshape(x(:,i),n, 1),[1, n])...
            -repmat(reshape(x(:,i),1, n),[n, 1]);
      else                                               % cross covariances Kxz
        nz = size(z,1);
        di = repmat(reshape(x(:,i),n, 1),[1,nz])...
            -repmat(reshape(z(:,i),1,nz),[n, 1]);
      end
    end
    di = 2*pi*di/p(i); dD2_dlpi = -2*sin(di).*di;   % derivative dD2i/dlog(p(i))
    [x,z] = u(x,z,p,dg);                    % apply the embedding u:IR^D->IR^2*D
    if dg                                            % compute squared distances
      D2 = zeros(n,1);
    else
      if xeqz
        D2 = sq_dist(x(:,[i,i+D])');
      else
        D2 = sq_dist(x(:,[i,i+D])',z(:,[i,i+D])');
      end
    end
    % reconstruct derivative w.r.t. D2i from derivative w.r.t. log(ell(i))
    dK_dD2 = feval(cov{:},hyp0,x,z,i) + feval(cov{:},hyp0,x,z,i+D);
    dK_dD2 = dK_dD2./(-2*D2); dK_dD2(D2<1e-12) = 0;
    K = dK_dD2.*dD2_dlpi;                                     % apply chain rule
  else
    [x,z] = u(x,z,p,dg);                    % apply the embedding u:IR^D->IR^2*D
    if i<=2*D
      K = feval(cov{:},hyp0,x,z,i-D) + feval(cov{:},hyp0,x,z,i);
    else
      K = feval(cov{:},hyp0,x,z,i);
    end
  end
end

function [x,z] = u(x,z,p,dg)                % apply the embedding u:IR^D->IR^2*D
  x = 2*pi*x*diag(1./p); x = [sin(x), cos(x)];
  if numel(z)>0 && ~dg, z = 2*pi*z*diag(1./p); z = [sin(z), cos(z)]; end


function [ Y ] = TMED( X, h )
% TMED  - The fuzzy filter with Triangular function with MEDian center
% x     - input (noisy image)
% y     - output (de-noised image)
% F     - window function, weighting function

if nargin < 2
    h=3;    % local search window size: h=3; h=5; h=7; h=9; h=11;
    f=1;    % padding value for window: f=1; f=2; f=3; f=4; f=5;
else
    f=(h-1)/2;    % padding value for window: f=1; f=2; f=3; f=4; f=5;
end
F = zeros(h*h,1);
Y  = zeros(size(X));

X = double(padarray(X,[f f],'symmetric'));
[m n ~] = size(X);

for i=1+f:1:m-f
   for j=1+f:1:n-f
       x=reshape(X(i-f:i+f, j-f:j+f),[],1);
       xmax = max(x);
       xmin = min(x);
       xmed = median(x);
       xmm = max(xmax-xmed, xmed-xmin);

       F(:,:) = 0;
       if xmm==0
           F(:,:) = 1;
       else
           F = (abs(x-xmed)/xmm);
       end

       Y(i-f,j-f) = sum(sum(F.*x))/sum(sum(F));
       clear xmax xmin xmed xmm;

   end
end
end


function order = r82row_order_type ( n, a )

%*****************************************************************************80
%
%% R82ROW_ORDER_TYPE finds if an R82ROW is (non)strictly ascending/descending.
%
%  Discussion:
%
%    An R82ROW is a (2,N) array of R8's.
%
%    The dictionary or lexicographic ordering is used.
%
%    (X1,Y1) < (X2,Y2)  <=>  X1 < X2 or ( X1 = X2 and Y1 < Y2).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 October 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries of the array.
%
%    Input, real A(2,N), the array to be checked.
%
%    Output, integer ORDER, order indicator:
%    -1, no discernable order;
%    0, all entries are equal;
%    1, ascending order;
%    2, strictly ascending order;
%    3, descending order;
%    4, strictly descending order.
%
  dim_num = 2;
%
%  Search for the first value not equal to A(1,1).
%
  i = 1;

  while ( 1 )

    i = i + 1;

    if ( n < i )
      order = 0;
      return
    end

    if ( ...
         a(1,1) <  a(1,i) || ...
       ( a(1,1) == a(1,i) && a(2,1) < a(2,i) ) )

      if ( i == 2 )
        order = 2;
      else
        order = 1;
      end

      break

    elseif ( ...
        a(1,i) <  a(1,1) || ...
      ( a(1,i) == a(1,1) && a(2,i) < a(2,1) ) )

      if ( i == 2 )
        order = 4;
      else
        order = 3;
      end

      break

    end

  end
%
%  Now we have a "direction".  Examine subsequent entries.
%
  while ( 1 )

    i = i + 1;
    if ( n < i )
      break
    end

    if ( order == 1 )

      if ( ...
          a(1,i) <  a(1,i-1) || ...
        ( a(1,i) == a(1,i-1) && a(2,i) < a(2,i-1) ) )
        order = -1;
        break
      end

    elseif ( order == 2 )

      if ( ...
          a(1,i) <  a(1,i-1) || ...
        ( a(1,i) == a(1,i-1) && a(2,i) < a(2,i-1) ) )
        order = -1;
        break
      elseif ( ...
         a(1,i) == a(1,i-1) && a(2,i) == a(2,i-1) )
        order = 1;
      end

    elseif ( order == 3 )

      if ( ...
          a(1,i-1) <  a(1,i) || ...
        ( a(1,i-1) == a(1,i) && a(2,i-1) < a(2,i) ) )
        order = -1;
        break
      end

    elseif ( order == 4 )

      if ( ...
          a(1,i-1) <  a(1,i) || ...
        ( a(1,i-1) == a(1,i) && a(2,i-1) < a(2,i) ) )
        order = -1;
        break
      elseif ( a(1,i) == a(1,i-1) && a(2,i) == a(2,i-1) )
        order = 3;
      end

    end

  end

  return
end


function value = p32_f ( dim_num, point_num, x )

%*****************************************************************************80
%
%% P32_F evaluates the integrand for problem 32.
%
%  Dimension:
%
%    N arbitrary.
%
%  Region:
%
%    0 <= X(1:DIM_NUM) <= 1
%
%  Integral Parameters:
%
%    The integral depends on vectors C(1:N) and Z(1:N).
%
%    The reference suggests choosing C at random in [0,1]
%    and then multiplying by the normalizing factor sqrt(140/N**(3/2)).
%
%    The default value of C(1:N) is (1/2)^(1/N).
%
%    The default value of Z(1:N) is (1/2)^(1/N).
%
%  Integrand:
%
%    exp ( c(1:n)*x(1:n) ) if all x(1:n) <= z(1:n)
%    0                        otherwise
%
%  Exact Integral:
%
%    product ( g(1:n)(x,z,a,b,c) )
%
%    where g(i)(x,z,a,b,c) =
%
%      0                                         if z(i) <= a(i)
%      ( e^(c(i)*z(i) ) - e^(c(i)*a(i)) ) / c(i) if a(i) <= z(i) <= b(i)
%      ( e^(c(i)*b(i) ) - e^(c(i)*a(i)) ) / c(i) if b(i) <= z(i)
%
%    with obvious modifications when c(i) = 0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 June 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Alan Genz,
%    [Integral #6]
%    A Package for Testing Multiple Integration Subroutines,
%    in Numerical Integration: Recent Developments, Software
%    and Applications,
%    edited by Patrick Keast and Graeme Fairweather,
%    D Reidel, 1987, pages 337-340,
%    LC: QA299.3.N38.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the dimension of the argument.
%
%    Input, integer POINT_NUM, the number of points.
%
%    Input, real X(DIM_NUM,POINT_NUM), the evaluation points.
%
%    Output, real VALUE(POINT_NUM), the integrand values.
%
  c = [];
  c = p32_r8vec ( 'G', 'C', dim_num, c );

  z = [];
  z = p32_r8vec ( 'G', 'Z', dim_num, z );

  value(1:point_num) = 0.0;

  for point = 1 : point_num

    if ( all ( x(1:dim_num,point) <= z(1:dim_num)' ) )
      value(point) = exp ( c(1:dim_num) * x(1:dim_num,point) );
    else
      value(point) = 0.0;
    end

  end

  p32_i4 ( 'I', '#', point_num );

  return
end


function [ a_lu, info ] = r8ge_np_trf ( m, n, a )

%*****************************************************************************80
%
%% R8GE_NP_TRF computes the LU factorization of a R8GE matrix.
%
%  Discussion:
%
%    The R8GE storage format is used for a general M by N matrix.  A storage
%    space is made for each logical entry.  The two dimensional logical
%    array is mapped to a vector, in which storage is by columns.
%
%    R8GE_NP_TRF is a nonpivoting version of R8GE_TRF, and will fail if
%    a zero element is encountered along the diagonal.
%
%    The factorization has the form
%      A = L * U
%    where L is lower triangular with unit diagonal elements (lower
%    trapezoidal if N < M), and U is upper triangular (upper trapezoidal
%    if M < N).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 March 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the number of rows of the matrix A.  0 <= M.
%
%    Input, integer N, the number of columns of the matrix A.  0 <= N.
%
%    Input, real A(M,N), the matrix to be factored.
%
%    Output, integer INFO.
%    = 0: successful exit
%    < 0: if INFO = -K, the K-th argument had an illegal value
%    > 0: if INFO = K, U(K,K) is exactly zero. The factorization
%         has been completed, but the factor U is exactly
%         singular, and division by zero will occur if it is used
%         to solve a system of equations.
%
%    Output, real A_LU(M,N), the factors L and U from the factorization
%    A = L*U; the unit diagonal elements of L are not stored.
%
  info = 0;
  a_lu(1:m,1:n) = a(1:m,1:n);
%
%  Test the input parameters.
%
  if ( m < 0 )
    info = -1;
    return
  elseif ( n < 0 )
    info = -2;
    return
  end

  if ( m == 0 | n == 0 )
    return
  end

  for j = 1 : min ( m, n )
%
%  Compute elements J+1:M of the J-th column.
%
    if ( a(j,j) ~= 0.0E+00 )
      a_lu(j+1:m,j) = a_lu(j+1:m,j) / a_lu(j,j);
    elseif ( info == 0 )
      info = j;
    end
%
%  Update the trailing submatrix.
%
    if ( j < min ( m, n ) )

      for ii = j+1 : m
        a_lu(ii,j+1:n) = a_lu(ii,j+1:n) - a_lu(ii,j) * a_lu(j,j+1:n);
      end

    end

  end

  return
end


function [y,r]=smooth(x,twice)
% SMOOTH will compute the non-linear running medians of
%     the input vector.  First it smooths the data, compute
%     the residuals, smooth the residuals, and add this back
%     to the first smooth.
%
% Usage: y = smooth(x, OPTIONAL twice?)
%  where x         is the input vector of minimum length 3
%        twice?    whether to do this twice (default = 1)
%        y         is the smoothed vector
%
% [y,r] = smooth(x, OPTIONAL twice?)
%     r  is the final residuals where x = y + r
%
% See also: R3R, SR, R3RSR, H

%
% Huy Le,  Massachusetts Institute of Technology.   1997
%
% See John Wilder Tukey, "EXPLORATORY DATA ANALYSIS".
% Addison-Wesley Publishing Co. 1977. Chpt 7, 16.

format long e;
xlen = length(x); x = x(:); y = x; r = x - y; batch = 1;
if nargin<2, twice = 1; end
if nargout<2, res = 0; end
if ~all(isfinite(x)), error('Input must have finite values.'); end
if xlen<3, error('Input vector must be at least 3 elements long.'); end
% first, run the 3RSR of vector x
xtemp1 = R3RSR(x,batch);
% first hanning
xtemp2 = H(xtemp1,batch);
% second hanning
xtemp1 = H(xtemp2,batch);
if twice,
   % compute first residuals
   rtemp = x - xtemp1;
   % run 3RSR on first residuals
   rtemp1 = R3RSR(rtemp,batch);
   % hanning of first residuals
   rtemp2 = H(rtemp1,batch);
   % sum of smooths
   xtemp = xtemp1 + rtemp2;
else
   xtemp = xtemp1;
end

for ii = 1:xlen,
   y(ii) = xtemp(ii);
   r(ii) = x(ii) - y(ii);
end

return


%DEMOPOLYGONCENTROIDS  One-line description here, please.
%
%   output = demoPolygonCentroids(input)
%
%   Example
%   demoPolygonCentroids
%
%   See also
%

% ------
% Author: David Legland
% e-mail: david.legland@inrae.fr
% INRAE - BIA Research Unit - BIBS Platform (Nantes)
% Created: 2022-02-16,    using Matlab 9.9.0.1570001 (R2020b) Update 4
% Copyright 2022 INRAE.

%% Demo polygon

% vertices of a "U"-shaped polygon
poly = [0 0;30 0;30 20;20 20;20 11;10 11;10 20;0 20];

% display the filled polygon and the open polyline
figure; hold on; axis equal; axis([-5 35 -5 30]);
fillPolygon(poly, 'c');
hPoly = drawPolygon(poly, 'linewidth', 2, 'color', 'k');
drawVertices(poly);

%% Computes various centroids

% computes the centroids
vxCentroid = centroid(poly);
lsCentroid = polylineCentroid(poly, 'closed');
pgCentroid = polygonCentroid(poly);

% display the centroids
hVxc = drawPoint(vxCentroid, 'marker', 'o', 'color', [0.8 0 0], 'linewidth', 2, 'MarkerFaceColor', 'w');
hLsc = drawPoint(lsCentroid, 'marker', 'o', 'color', [0 0.8 0], 'linewidth', 2, 'MarkerFaceColor', 'w');
hPgc = drawPoint(pgCentroid, 'marker', 'o', 'color', [0 0 0.8], 'linewidth', 2, 'MarkerFaceColor', 'w');

% decorate graph
legend([hPoly hVxc hLsc hPgc], ...
    {'Polygon', 'Vertex Centroid', 'Polyline Centroid', 'Polygon Centroid'}, ...
    'Location', 'NorthEast');

print(gcf, 'polygon_centroids.png', '-dpng');


function [ grid_num, point_num ] = gl_grids_minmax_size ( dim_num, q_min, ...
  q_max )

%*****************************************************************************80
%
%% GL_GRIDS_MINMAX_SIZE returns sizes for the GL_GRIDS_MINMAX.
%
%  Discussion:
%
%    This routine can be used to determine the necessary size to be
%    allocated to arrays GRID_ORDER and GRID_POINT in a call to
%    GL_GRIDS_MINMAX.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 November 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer Q_MIN, Q_MAX, the minimum and maximum values of
%    Q, the sum of the orders in each spatial coordinate.
%
%    Output, integer GRID_NUM, the number of Gauss-Legendre
%    grids whose Q value is between Q_MIN and Q_MAX.
%
%    Output, integer POINT_NUM, the total number of points in the grids.
%

%
%  Determine the total number of points that will be generated
%  by "going through the motions".
%
  point_num = 0;
  grid_num = 0;

  for q = q_min : q_max

    more = 0;
    order_1d = [];

    while ( 1 )

      [ order_1d, more ] = compnz_next ( q, dim_num, order_1d, more );

      order_nd = prod ( order_1d(1:dim_num) );

      point_num = point_num + order_nd;

      grid_num = grid_num + 1;

      if ( ~more )
        break
      end

    end

  end

  return
end


function out = simbin(mat1,mat2,type,mask)

% Computes 106 measures of similarity and dissimilarity (distance) between
% two binary matrices. Matrices can be any dimensions, but must have the
% same dimensions. A mask can be used to indicate the relevant elements of
% the matrices (needed when the measure takes into account elements present
% in neither binary object). See further information below (including next
% to each measure).
%
% IMPORTANT: naming conventions for certain measures (e.g., the 5 Sokal and
% Sneath measures) are inconsistent across different researchers, different
% formulas for the same 'measure' exist in the literature, and different
% measures have similar names (e.g., Anderberg's vs. Anderberg's D). Thus,
% you should check that the formula used below corresponds to the
% particular measure that you wish to compute.
%
% Usage: out = overlap_similarity(mat1,mat2,type,mask)
%           mat1: 1st binary matrix
%           mat2: 2nd binary matrix
%           type: measure of (dis)similarity to be used
%           mask: binary mask indicating relevant matrix elements
%
%
% Further Information:
%
% All measures are symmetric (i.e., it does not matter which matrix is mat1
% vs. mat 2) except:
%                    AD, AMPLE, BUB1, BUB2, cole, diceasym1, diceasym2,
%                    digby, fleiss, forbes, GKlambda, GKtau, GK2, Ko2, LH,
%                    MP, Pcs, Pmsc, Pei1, Pei2, PHI, PH, Q0, RDeprob, SS4,
%                    SS5, tarantula, YQ, YQD, YY
%
% Computation of these measures takes into account (in some way) the
% elements present in neither binary object (i.e., d) (which may not be
% optimal if you want a straightforward measure of overlap):
%                    AD, AMPLE, baulieu, bc, Bshape, BUB1, BUB2, CK, cole,
%                    dennis, digby, disper, eyraud, faith, FD, fleiss,
%                    GKlambda, GKtau, GK1, GK2, GK3, GK4, GLS, goodall,
%                    gower, GW, hamann, HD, inprod, Kcam, Kpoai, LH,
%                    meanman, michael, MK, MP, pattern, Pcs, Pmsc, Pei1,
%                    Pei2, PHI, PH, Q0, RDeprob, Rpattern, RR, RT, scott,
%                    SM, Spattern, SS1, SS3, SS4, SS5, Stau, stiles,
%                    tarantula, tarwid, var, YQ, YQD, YY
%
% Positive monotomic relationship: bc, pattern
%                                  BSeuc, meanman, mirkin, var
%                                  BUB1, BUB2
%                                  DK, johnson
%                                  disper, MK, Stau
%                                  int, RR
%                                  hamann, inprod, SM
%                                  chord, hellinger
%                                  faith, tversky
%
% Negative monotomic relationship: BB with savage
%                                  BLWMN with dice
%                                  BSeuc, meanman, mirkin, and var with hamann, inprod, and SM
%                                  Bpattern with bc and pattern
%                                  Beuc with RT
%                                  steffensen with int and RR
%                                  YQ with YQD
%
% TAG:       MEASURE NAME:
% AMPLE      Analyzing Method Patterns to Locate Errors (Dallmeier et al, 2005) (similarity)
% AD         Anderberg's D (similarity)
% anderberg  Anderberg 1973 (similarity)
% baulieu    Baulieu 1989 (dissimilarity) (also called size difference)
% BB         Braun and Blanquet 1932 (similarity)
% bc         bc (dissimilarity)
% benini     Benini 1901 (similarity)
% Beuc       Binary euclidean distance (dissimilarity)
% BLWMN      Binary Lance and Williams nonmetric (dissimilarity) (also called Sokal and Sneath nonmetric or Bray and Curtis)
% Bpattern   Browsing pattern (similarity)
% BSeuc      Binary squared euclidean distance (dissimilarity) (also called Hamming, city block, Manhattan, or Filkov)
% Bshape     Binary shape (dissimilarity)
% BUB1       Baroni-Urbani and Buser 1976 1 (similarity)
% BUB2       Baroni-Urbani and Buser 1976 2 (similarity)
% chiY       Chi Square with correction of Yates
% chord      Chord (dissimilarity)
% CK         Cohen's kappa (similarity)
% cole       Cole 1949 (similarity)
% dennis     Dennis (similarity)
% dice       Dice (similarity) (also called Nei and Lei's genetic coefficient, Czekanowski, or Sorenson, or Bray)
% diceasym1  Dice asymmetric 1 (similarity)
% diceasym2  Dice asymmetric 2 (similarity)
% digby      Digby 1983 (similarity)
% disper     Dispersion (similarity)
% DK         Driver and Kroeber 1932 (similarity) (also called Kulczynski 1927 2)
% eyraud     Eyraud 1936 (similarity)
% fager      Fager (similarity)
% faith      Faith (similarity)
% FaMc       Fager and McGowan 1963 (similarity)
% FD         Forbes 1907's D (similarity)
% fleiss     Fleiss 1975 (similarity)
% forbes     Forbes 1925 (similarity)
% fossum     Fossum (similarity) (also called Jones and Curtis)
% gilbert    Gilbert's coefficient (similarity)
% gini       Gini 1912 index (similarity)
% GK1        Goodman and Kruskal 1954 1 (similarity)
% GK2        Goodman and Kruskal 1954 2 (similarity)
% GK3        Goodman and Kruskal 1954 3 (similarity)
% GK4        Goodman and Kruskal 1954 4 (similarity)
% GKlambda   Goodman and Kruskal's lambda (similarity)
% GKtau      Goodman and Kruskal's tau (similarity)
% goodall    Goodall 1967's angular transformation of simple matching coefficient (similarity) (also called Austin and Colwell)
% gower      Gower (similarity)
% GW         Gilbert and Wells 1966 (similarity)
% hamann     Hamann 1961 (similarity)
% HD         Hawkins and Dotson 1975 (similarity)
% hellinger  Hellinger (dissimilarity)
% inprod     Inner product (similarity)
% int        Intersection of two masks (similarity)
% jaccard    Jaccard 1908 (similarity) (also called similarity ratio or Tanimioto)
% johnson    Johnson 1967 (similarity) (also called McConnaughey 1964)
% Kcam       Kuhns 1965's coefficient of arithmetic means (similarity)
% Ko1        Koppen 1884 (similarity)
% Ko2        Koppen 1870 (similarity)
% Kpoai      Kuhns 1965's proportion of overlap above independence (similarity)
% kulczynski Kulczynski 1927 1 (similarity)
% meanman    Mean Manhattan (dissimilarity) (also called Canberra or Sneath total difference)
% michael    Michael 1920 (similarity)
% mirkin     Mirkin (dissimilarity)
% MK         Maron and Kuhns 1960 (similarity)
% modgini    Modified gini index (similarity)
% mountford  Mountford 1962 (similarity)
% MP         Maxwell and Pilliner 1968 (similarity)
% ochiai     Ochiai 1957 (similarity) (also called Driver and Kroeber or Otsuka)
% pattern    Pattern (dissimilarity)
% Pcs        Pearson 1905 chi-square (similarity)
% Pei1       Peirce 1884 1 (similarity)
% Pei2       Peirce 1884 2 (similarity)
% PH         Pearson and Heron 1913 (similarity)
% PHI        Fourfold point correlation (similarity) (also called Pearson and Heron 1913)
% Pmsc       Pearson 1905's coefficient of mean square contingency (similarity)
% Q0         Q0 (Batagelj and Bren 1995) (dissimilarity)
% Rcost      R cost
% RDeprob    Relative decrease of error probability (similarity)
% Rpattern   Retrieval pattern (similarity)
% RR         Russell and Rao 1940 (similarity)
% RT         Rogers and Tanimoto 1960 (similarity)
% savage     Savage (dissimilarity)
% Scost      S cost
% scott      Scott 1955 (similarity)
% simpson    Simpson 1943's ecological coexistence coefficient (similarity) (also called overlap)
% SM         Sokal and Michener 1958's simple matching (similarity) (also called Rand or Kendall)
% soergel    Soergel distance
% sorensen   Sorensen 1948 (similarity)
% sorgenfrei Sorgenfrei 1958 (similarity) (also called Fowlkes-Mallows)
% Spattern   Sneath pattern difference (dissimilarity)
% SS1        Sokal and Sneath 1963 1 (similarity)
% SS2        Sokal and Sneath 1963 2 (similarity)
% SS3        Sokal and Sneath 1963 3 (similarity)
% SS4        Sokal and Sneath 1963 4 (similarity) (also called Anderberg)
% SS5        Sokal and Sneath 1963 5 (similarity) (also called Ochiai 2)
% Stau       Stuart's tau (similarity)
% steffensen Steffensen 1934 (dissimilarity)
% stiles     Stiles (similarity)
% Tcost      T combined cost
% tarantula  Tarantula (Jones et al., 2002 & 2005) (similarity)
% tarwid     Tarwid 1960 (similarity)
% tversky    Tversky 1977's feature contrast model (similarity)
% Ucost      U cost
% unigram    Unigram subtuples
% US         Upholt 1977's S
% US         Upholt 1977's F
% var        Variance (dissimilarity)
% YQ         Yule 1911's Q coefficient of association (similarity)
% YQD        Yule's Q (Yule and Kendall, 1957) distance (dissimilarity)
% YY         Yule 1912's Y (or omega) coefficient of colligation (similarity)
%
%
% Author: Jeffrey M. Spielberg (jspielb2@gmail.com)
% Version: 05.06.16
%
% WARNING: This is a beta version. There no known bugs, but only limited
% testing has been perfomed. This software comes with no warranty (even the
% implied warranty of merchantability or fitness for a particular purpose).
% Therefore, USE AT YOUR OWN RISK!!!
%
% Copyleft 2014-2016. Software can be modified and redistributed, but
% modifed, redistributed versions must have the same rights

mat1 = double(mat1>0);
mat2 = double(mat2>0);

if nargin==2||isempty(type)
    type = 'dice';
    mask = ones(size(mat1));
elseif nargin<4
    mask = ones(size(mat1));
elseif nargin==4
    mask = double(mask>0);
end

mat1  = mat1.*mask;
mat2  = mat2.*mask;
nmat1 = not(mat1).*mask;
nmat2 = not(mat2).*mask;

a = sumn(mat1.*mat2);
b = sumn(nmat1.*mat2);
c = sumn(mat1.*nmat2);
d = sumn(nmat1.*nmat2);
tot = a+b+c+d;
q = (a+b)*(a+c)*(b+d)*(c+d);

switch type
    case 'AD'           % Anderberg's D (similarity) (measures the reduction in error probability when one item is used to predict the other) (range: 0:1)
        t1  = max(a,b)+max(c,d)+max(a,c)+max(b,d);
        t2  = max((a+c),(b+d))+max((a+d),(c+d));
        out = (t1-t2)/(2*tot);
    case 'AMPLE'        % Analyzing Method Patterns to Locate Errors (Dallmeier et al, 2005) (similarity) (undefined when a+b and/or c+d are 0)
        out = abs((a/(a+b))-(c/(c+d)));
    case 'anderberg'    % Anderberg 1973 (similarity)
        out = (8*a)/((8*a)+b+c);
    case 'baulieu'      % Baulieu 1989 (dissimilarity) (also called size difference) (range: 0:Inf)
        out = ((b-c)^2)/(tot^2);
    case 'BB'           % Braun and Blanquet 1932 (similarity) (range: 0:1)
        out = a/max((a+b),(a+c));
    case 'bc'           % bc (dissimilarity)
        out = (4*b*c)/(tot^2);
    case 'benini'       % Benini 1901 (similarity)
        out = (a-((a+b)*(a+c)))/(a+min(b,c)-((a+b)*(a+c)));
    case 'Beuc'         % Binary euclidean distance (dissimilarity) (range: 0:Inf)
        out = sqrt(b+c);
    case 'BLWMN'        % Binary Lance and Williams nonmetric (dissimilarity) (also called Sokal and Sneath nonmetric or Bray and Curtis) (range: 0:1)
        out = (b+c)/((2*a)+b+c);
    case 'Bpattern'     % Browsing pattern (similarity)
        out = a-(b*c);
    case 'BSeuc'        % Binary squared euclidean distance (dissimilarity) (also called Hamming, city block, Manhattan, or Filkov) (range: 0:Inf)
        out = b+c;
    case 'Bshape'       % Binary shape (dissimilarity) (range: no limits)
        out = ((tot*(b+c))-((b-c)^2))/(tot^2);
    case 'BUB1'         % Baroni-Urbani and Buser 1976 1 (similarity) (range: 0:1)
        out = (sqrt(a*d)+a)/(sqrt(a*d)+a+b+c);
    case 'BUB2'         % Baroni-Urbani and Buser 1976 2 (similarity)
        out = (sqrt(a*d)+a-b-c)/(sqrt(a*d)+a+b+c);
    case 'chiY'         % Chi Square with correction of Yates
        out = (tot*(abs((a*d)-(b*c))-(tot/2))^2)/q;
    case 'chord'        % Chord (dissimilarity)
        out = sqrt(2*(1-(a/sqrt((a+b)*(a+c)))));
    case 'CK'           % Cohen's kappa (similarity)
        out = (2*((a*d)-(b*c)))/(((a+b)*(b+d))+((a+c)*(c+d)));
    case 'cole'         % Cole 1949 (similarity) (undefined when a+b, b+d, a+c, and/or c+d are 0)
        out = ((a*d)-(b*c))/min(((a+b)*(a+c)),((b+d)*(c+d)));
    case 'dennis'       % Dennis (similarity)
        out = ((a*d)-(b*c))/sqrt(tot*(a+b)*(a+c));
    case 'dice'         % Dice (similarity) (also called Nei and Lei's genetic coefficient, Czekanowski, Sorenson, Bray, Upholt's F, Gower and Legendre's T, or percent positive agreement) (range: 0:1)
        out = (2*a)/((2*a)+b+c);
    case 'diceasym1'    % Dice asymmetric 1 (similarity)
        out = a/(a+c);
    case 'diceasym2'    % Dice asymmetric 2 (similarity)
        out = a/(a+b);
    case 'digby'        % Digby 1983 (similarity) (undefined when a+b, b+d, a+c, and/or c+d are 0)
        out = (((a*d)^(3/4))-((b*c)^(3/4)))/(((a*d)^(3/4))+((b*c)^(3/4)));
    case 'disper'       % Dispersion (similarity) (range: -1:1)
        out = ((a*d)-(b*c))/(tot^2);
    case 'DK'           % Driver and Kroeber 1932 (similarity) (also called Kulczynski 1927 2) (measures the conditional probability that both items are positive) (range: 0:1)
        out = (a/2)*((1/(a+b))+(1/(a+c)));
    case 'eyraud'       % Eyraud 1936 (similarity) (undefined when a+b, b+d, a+c, and/or c+d are 0)
        out = (a-((a+b)*(a+c)))/q;
        %out = ((tot^2)*((tot*a)-((a+b)*(a+c))))/q; % Alternative formula
    case 'fager'        % Fager (similarity)
        out = (a/sqrt((a+b)*(a+c)))-(1/(2*sqrt(min((a+b),(a+c)))));
        %out = (a/(((a+b)*(a+c))^2))-(max(b,c)/2); % Alternative formula
    case 'faith'        % Faith (similarity)
        out = (a+(0.5*d))/tot;
    case 'FaMc'         % Fager and McGowan 1963 (similarity)
        out = (a/sqrt((a+b)*(a+c)))-((a+max(b,c))/2);
    case 'FD'           % Forbes 1907's D (similarity) (also called Kocher and Wong)
        out = (a*tot)/((a+b)*(a+c));
    case 'fleiss'       % Fleiss 1975 (similarity) (undefined when a+b, b+d, a+c, and/or c+d are 0)
        out = (((a*d)-(b*c))*(((a+b)*(b+d))+((a+c)*(c+d))))/(2*q);
    case 'forbes'       % Forbes 1925 (similarity) (also called Loevinger's H) (undefined when a+b, b+d, a+c, and/or c+d are 0)
        out = ((tot*a)-((a+b)*(a+c)))/((tot*min((a+b),(a+c)))-((a+b)*(a+c)));
        %out = ((tot*a)-((a+b)*(a+c)))/((tot*min(b,c))-((a+b)*(a+c))); % Alternative formula
    case 'fossum'       % Fossum (similarity) (also called Jones and Curtis)
        out = (tot*((a-0.5)^2))/((a+b)*(a+c));
    case 'gilbert'      % Gilbert's coefficient (similarity) (undefined under certain circumstances)
        out = (a-((a+b)*(a+c)))/(a+b+c-((a+b)*(a+c)));
    case 'gini'         % Gini 1912 index (similarity) (undefined under certain circumstances)
        out = (a-((a+b)*(a+c)))/sqrt((1-((a+b)^2))*(1-((a+c)^2)));
    case 'GKlambda'     % Goodman and Kruskal's lambda (similarity) (measures the proportional reduction in error using one item to predict another) (range: 0:1)
        t1  = max(a,b)+max(c,d)+max(a,c)+max(b,d);
        t2  = max((a+c),(b+d))+max((a+d),(c+d));
        out = (t1-t2)/((2*tot)-t2);
    case 'GKtau'        % Goodman and Kruskal's tau (similarity) (undefined when a+b and/or c+d are 0)
        out = (((((a-((a+b)*(a+c)))^2)+((b-((a+b)*(b+d)))^2))/(a+b))/(1-((a+c)^2)-((b+d)^2)))+(((((c-((a+c)*(c+d)))^2)+((d-((b+d)*(c+d)))^2))/(c+d))/(1-((a+c)^2)-((b+d)^2)));
    case 'GK1'          % Goodman and Kruskal 1 1954 (similarity)
        out = ((2*min(a,d))-b-c)/((2*min(a,d))+b+c);
    case 'GK2'          % Goodman and Kruskal 2 1954 (similarity)
        out = (max(a,c)+max(b,d)-max((a+b),(c+d)))/(1-max((a+b),(c+d)));
    case 'GK3'          % Goodman and Kruskal 3 1954 (similarity)
        out = (a+d-max(a,d)-((b+c)/2))/(1-max(a,d)-((b+c)/2));
    case 'GK4'          % Goodman and Kruskal 4 1954 (similarity)
        out = ((max(a,b)+max(c,d)+max(a,c)+max(b,d))/(2-max((a+c),(b+d))-max((a+b),(c+d))))-((max((a+c),(b+d))+max((a+b),(c+d)))/(2-max((a+c),(b+d))-max((a+b),(c+d))));
    case 'goodall'      % Goodall 1967's angular transformation of simple matching coefficient (similarity) (also called Austin and Colwell)
        out = asin(sqrt((a+d)/tot))/(50*pi);
    case 'gower'        % Gower (similarity) (undefined when a+b, b+d, a+c, and/or c+d are 0)
        out = (a+d)/sqrt(q);
    case 'GW'           % Gilbert and Wells 1966 (similarity) (undefined when a+b and/or a+c are 0)
        out = log((a*tot)/((a+b)*(a+c)));
    case 'hamann'       % Hamann 1961 (similarity) (range: -1:1)
        out = ((a+d)-(b+c))/tot;
    case 'HD'           % Hawkins and Dotson 1975 (similarity)
        out = 0.5*((a/(a+b+c))+(d/(b+c+d)));
    case 'hellinger'    % Hellinger (dissimilarity)
        out = 2*sqrt(1-(a/sqrt((a+b)*(a+c))));
    case 'inprod'       % Inner product (similarity)
        out = a+d;
    case 'int'          % Intersection of two masks (similarity)
        out = a;
    case 'jaccard'      % Jaccard 1908 (similarity) (also called similarity ratio or Tanimioto) (range: 0:1)
        out = a/(a+b+c);
    case 'johnson'      % Johnson 1967 (similarity) (also called McConnaughey 1964)
        out = (a/(a+b))+(a/(a+c));
    case 'Kcam'         % Kuhns 1965's coefficient of arithmetic means (similarity)
        out = (2*((a*d)-(b*c)))/(tot*((2*a)+b+c));
    case 'Ko1'          % Koppen 1884 (similarity)
        out = a+((b+c)/2);
    case 'Ko2'          % Koppen 1870 (similarity) (undefined when a+b and/or 1-a-b are 0)
        out = (((a+b)*(1-a-b))-c)/((a+b)*(1-a-b));
    case 'Kpoai'        % Kuhns 1965's proportion of overlap above independence (similarity) (undefined when a+c and/or a+c are 0)
        out = ((a*d)-(b*c))/(tot*(a/((a+b)*(a+c)))*((2*a)+b+c-(((a+b)*(a+c))/tot)));
    case 'kulczynski'   % Kulczynski 1927 1 (similarity) (range: 0:Inf)
        out = a/(b+c);
    case 'meanman'      % Mean Manhattan (dissimilarity) (also called Canberra or Sneath total difference)
        out = (b+c)/tot;
    case 'michael'      % Michael 1920 (similarity) (range: 0:1)
        out = (4*((a*d)-(b*c)))/(((a+d)^2)+((b+c)^2));
    case 'mirkin'       % Mirkin (dissimilarity)
        out = 2*(b+c);
    case 'MK'           % Maron and Kuhns 1960 (similarity)
        out = ((a*d)-(b*c))/tot;
    case 'modgini'      % Modified gini index (similarity)
        out = (a-((a+b)*(a+c)))/(1-(abs(b-c)/2)-((a+b)*(a+c)));
    case 'mountford'    % Mountford 1962 (similarity)
        out = a/((0.5*((a*b)+(a*c)))+(b*c));
    case 'MP'           % Maxwell and Pilliner 1968 (similarity) (undefined when a+b, b+d, a+c, and/or c+d are 0)
        out = (2*((a*d)-(b*c)))/q;
    case 'ochiai'       % Ochiai 1957 (similarity) (also called Driver and Kroeber or Otsuka) (binary form of cosine similarity) (range: 0:1)
        out = a/sqrt((a+b)*(a+c));
    case 'pattern'      % Pattern (dissimilarity) (range: 0:1)
        out = (b*c)/(tot^2);
    case 'Pcs'          % Pearson 1905 chi-square (similarity) (undefined when a+b, b+d, a+c, and/or c+d are 0)
        out = (tot*(((a*d)-(b*c))^2))/q;
    case 'Pmsc'         % Pearson 1905's coefficient of mean square contingency (similarity) (undefined when a+b, b+d, a+c, and/or c+d are 0)
        ch2 = (tot*(((a*d)-(b*c))^2))/q;
        out = sqrt(ch2/(tot+ch2));
    case 'Pei1'         % Peirce 1884 1 (similarity) (undefined when a+c and/or b+d are 0)
        out = ((a*b)-(b*c))/((a+c)*(b+d));
    case 'Pei2'         % Peirce 1884 2 (similarity) (undefined when a+c and/or b+d are 0)
        out = ((a*b)+(b*c))/((a*b)+(2*b*c)+(c*d));
    case 'PHI'          % Fourfold point correlation (similarity) (also called Pearson and Heron 1913) (binary form of the Pearson product-moment correlation) range: 0:1) (undefined when a+b, b+d, a+c, and/or c+d are 0)
        out = ((a*d)-(b*c))/sqrt(q);
    case 'PH'           % Pearson and Heron 1913 (similarity) (undefined when a+b, b+d, a+c, and/or c+d are 0)
        out = cos((180*sqrt(b*c))/(sqrt(a*d)+sqrt(b*c)));
    case 'Q0'           % Q0 (Batagelj and Bren 1995) (dissimilarity) (range: 0:Inf) (undefined when a+b, b+d, a+c, and/or c+d are 0)
        out = (b*c)/(a*d);
    case 'Rcost'        % R cost
        out = log(1+(a/(a+b)))*log(1+(a/(a+c)));
    case 'RDeprob'      % Relative decrease of error probability (similarity)
        out = (max(a,b)+max(c,d)-max((a+c),(b+d)))/(1-max((a+c),(b+d)));
    case 'Rpattern'     % Retrieval pattern (similarity)
        out = a*d;
    case 'RR'           % Russell and Rao 1940 (similarity) (range: 0:1)
        out = a/tot;
    case 'RT'           % Rogers and Tanimoto 1960 (similarity) (binary dot product) (range: 0:1)
        out = (a+d)/(a+d+(2*(b+c)));
    case 'savage'       % Savage (dissimilarity)
        out = 1-(a/(a+max(b,c)));
    case 'Scost'        % S cost
        out = log(1+min(b,c)/(a+1))^-.5;
    case 'scott'        % Scott 1955 (similarity)
        out = ((4*((a*d)-(b*c)))-((b-c)^2))/(((2*a)+b+c)+(b+c+(2*d)));
    case 'simpson'      % Simpson 1943's ecological coexistence coefficient (similarity) (also called overlap) (range: 0:1)
        out = a/min((a+b),(a+c));
    case 'SM'           % Sokal and Michener 1958's simple matching (similarity) (also called Rand or Kendall) (range: 0:1)
        out = (a+d)/tot;
    case 'soergel'      % Soergel distance
        out = (b+c)/(b+c+d);
    case 'sorensen'     % Sorensen 1948 (similarity)
        out = (4*a)/((4*a)+b+c);
    case 'sorgenfrei'   % Sorgenfrei 1958 (similarity) (also called Fowlkes-Mallows)
        out = (a^2)/((a+b)*(a+c));
    case 'Spattern'     % Sneath pattern difference (dissimilarity)
        out = (2*sqrt(b*c))/tot;
    case 'SS1'          % Sokal and Sneath 1963 1 (similarity) (also called Gower and Legendre's S)
        out = (2*(a+d))/((2*(a+d))+b+c);
    case 'SS2'          % Sokal and Sneath 1963 2 (similarity)
        out = a/(a+(2*(b+c)));
    case 'SS3'          % Sokal and Sneath 1963 3 (similarity) (range: 0:Inf)
        out = (a+d)/(b+c);
    case 'SS4'          % Sokal and Sneath 1963 4 (similarity) (also called Anderberg) (measures the conditional probability that both items are in the same state [present or absent]) (range: 0:1) (undefined when a+b, b+d, a+c, and/or c+d are 0)
        out = ((a/(a+b))+(a/(a+c))+(d/(b+d))+(d/(c+d)))/4;
    case 'SS5'          % Sokal and Sneath 1963 5 (similarity) (also called Ochiai 2) (range: 0:1) (undefined when a+b, b+d, a+c, and/or c+d are 0)
        out = (a*d)/sqrt(q);
    case 'Stau'         % Stuart's tau (similarity)
        out = 2*((a*d)-(b*c));
    case 'steffensen'   % Steffensen 1934 (dissimilarity)
        out = (a-((a+b)*(a+c)))/2;
    case 'stiles'       % Stiles (similarity) (undefined when a+b, b+d, a+c, and/or c+d are 0 and other circumstances)
        out = log10((tot*((abs((a*d)-(b*c))-(tot/2))^2))/q);
    case 'tarantula'    % Tarantula (Jones et al., 2002 & 2005) (similarity) (undefined when a+b and/or c+d are 0)
        out = (a/(a+b))/((a/(a+b))+(c/(c+d)));
    case 'tarwid'       % Tarwid 1960 (similarity)
        out = ((tot*a)-((a+b)*(a+c)))/((tot*a)+((a+b)*(a+c)));
    case 'Tcost'        % T combined cost
        R = log(1+(a/(a+b)))*log(1+(a/(a+c)));
        S = log(1+min(b,c)/(a+1))^-.5;
        U = log(1+((min(b,c)+a)/(max(b,c)+a)));
        out = sqrt(U*S*R);
    case 'tversky'      % Tversky 1977's feature contrast model (similarity)
        out = a-b-c;
    case 'Ucost'        % U cost
        out = log(1+((min(b,c)+a)/(max(b,c)+a)));
    case 'unigram'      % Unigram subtuples
        out = log((a*d)/(b*c))-(3.29*sqrt((1/a)+(1/b)+(1/c)+(1/d)));
    case 'UF'           % Upholt 1977's F (also called Gower and Legendre's T)
        out = (2*a)/((2*a)+b+c);
    case 'US'           % Upholt's S 1977
        F = (2*a)/((2*a)+b+c);
        out = (0.5*(-F+sqrt((F^2)+(8*F))))^(1/tot);
    case 'var'          % Variance (dissimilarity) (range: 0:Inf)
        out = (b+c)/(4*tot);
    case 'YQ'           % Yule 1911's Q coefficient of association (similarity) (undefined when a+b, b+d, a+c, and/or c+d are 0)
        out = ((a*d)-(b*c))/((a*d)+(b*c));
    case 'YQD'          % Yule's Q distance (dissimilarity) (range: -1:1) (undefined when a+b, b+d, a+c, and/or c+d are 0)
        out = (b*c)/((a*d)+(b*c));
    case 'YY'           % Yule 1912's Y (or omega) coefficient of colligation (similarity) (range: -1:1) (undefined when a+b, b+d, a+c, and/or c+d are 0)
        out = (sqrt(a*d)-sqrt(b*c))/(sqrt(a*d)+sqrt(b*c));
end

function out = sumn(in)
out = sum(in(:));


function [ r, info ] = r8pp_fa ( n, a )

%*****************************************************************************80
%
%% R8PP_FA factors a R8PP matrix.
%
%  Discussion:
%
%    The R8PP storage format is appropriate for a symmetric positive
%    definite matrix.  Only the upper triangle of the matrix is stored,
%    by successive partial columns, in an array of length (N*(N+1))/2,
%    which contains (A11,A12,A22,A13,A23,A33,A14,...,ANN)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 February 2004
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real A((N*(N+1))/2), a R8PP matrix.
%
%    Output, real R((N*(N+1))/2), an upper triangular matrix R, stored
%    in packed form, so that A = R'*R.
%
%    Output, integer INFO, error flag.
%    0, for normal return.
%    K, if the leading minor of order K is not positive definite.
%
  info = 0;

  r(1:(n*(n+1))/2) = a(1:(n*(n+1))/2);

  jj = 0;

  for j = 1 : n

    s = 0.0E+00;
    kj = jj;
    kk = 0;

    for k = 1 : j-1

      kj = kj + 1;
      t = r(kj);
      for i = 1 : k-1
        t = t - r(kk+i) * r(jj+i);
      end
      kk = kk + k;
      t = t / r(kk);
      r(kj) = t;
      s = s + t * t;

    end

    jj = jj + j;
    s = r(jj) - s;

    if ( s <= 0.0E+00 )
      info = j;
      return;
    end

    r(jj) = sqrt ( s );

  end

  return
end


function stroud_test326 ( )

%*****************************************************************************80
%
%% TEST326 tests SPHERE_CAP_VOLUME_3D, SPHERE_CAP_VOLUME_ND.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 April 2009
%
%  Author:
%
%    John Burkardt
%
  dim_num = 3;
  ntest = 12;
  r = 1.0;
  center = [ 0.0, 0.0, 0.0 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST326\n' );
  fprintf ( 1, '  SPHERE_CAP_VOLUME_3D computes the volume of a\n' );
  fprintf ( 1, '    spherical cap, defined by a plane that cuts the\n' );
  fprintf ( 1,'    sphere to a thickness of H units.\n' );
  fprintf ( 1, '  SPHERE_CAP_VOLUME_ND does the same operation,\n' );
  fprintf ( 1, '    but in N dimensions.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Using a radius R = %f\n', r );

  volume1 = sphere_volume_3d ( r );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Volume of the total sphere in 3D = %f\n', volume1 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '        H           Cap        Cap\n' );
  fprintf ( 1, '                    volume_3d  volume_nd\n' );
  fprintf ( 1, '\n' );

  for i = 0 : ntest + 1

    h = 2.0 * r * i / ntest;

    volume1 = sphere_cap_volume_3d ( r, h )

    volume2 = sphere_cap_volume_nd ( dim_num, r, h );

    fprintf ( 1, '  %12.6f  %12.6f  %12.6f\n', h, volume1, volume2 );

  end

  return
end


function ldQ = VBA_logDet(Q,indIn,t)
% computes the log-determinant of matrix Q
% function ldQ = VBA_logDet(Q,indIn,t)
% IN:
%   - Q: nxn matrix
%   - indIn: vector of indices that defines a submatrix of Q, whose
%   determinant is computed {1:n}
%   - t: threshold on the eigenvalues of the submatrix {eps}
% OUT:
%   - ldQ: log-determinant of the submatrix of Q

if nargin < 3
    t = eps;
end
if nargin < 2 || isempty(indIn)
    dq = diag(Q);
    indIn = find(~isinf(dq)&dq~=0);
end
subQ = full(Q(indIn,indIn));
if isequal(subQ,eye(length(indIn))) % identity matrix
    ldQ = 0;
elseif isequal(subQ,diag(diag(subQ))) % diagonal matrix
    dQ = diag(subQ);
    ldQ = sum(log(dQ(abs(dQ)>t)));
else % full matrix
    if ~ VBA_isWeird (subQ)
        ev = eig(subQ);
    else
        ev = 0;
    end
    ev = ev(abs(ev)>t);
    ldQ = sum(log(ev));
end
ldQ = real(ldQ);


function otr = transform(itr,tmx)

%
% FUNCTION
% otr = transform(itr,tmx)
%
% DESCRIPTION
% transform 3D-tensor (Euclidean or Cartesion tensor) of any order (>0) to another coordinate system
%
% PARAMETERS
% otr = output tensor, after transformation; has the same dimensions as the input tensor
% itr = input tensor, before transformation; should be a 3-element vector, a 3x3 matrix, or a 3x3x3x... multidimensional array, each dimension containing 3 elements
% tmx = transformation matrix, 3x3 matrix that contains the direction cosines between the old and the new coordinate system
%

ne = numel(itr);                % number of tensor elements
nd = ndims(itr);                % number of tensor dimensions, i.e. order of tensor
if (ne==3), nd = 1; end         % order of tensor is 1 in case of a 3x1 or 1x3 vector

otr = itr;                      % create output tensor
otr(:) = 0;                     % fill output tensor with zeros; this way a symbolic tensor remains symbolic

iie = zeros(nd,1);              % initialise vector with indices of input tensor element
ioe = zeros(nd,1);              % initialise vector with indices of output tensor element
cne = cumprod(3*ones(nd,1))/3;  % vector with cumulative number of elements for each dimension (divided by three)

for oe = 1:ne,                                 % loop over all output elements
   ioe = mod(floor((oe-1)./cne),3)+1;          % calculate indices of current output tensor element
   for ie = 1:ne,                              % loop over all input elements
      pmx = 1;                                 % initialise product of transformation matrices
      iie = mod(floor((ie-1)./cne),3)+1;       % calculate indices of current input tensor element
      for id = 1:nd,                           % loop over all dimensions
         pmx = pmx * tmx( ioe(id), iie(id) );  % create product of transformation matrices
      end
      otr(oe) = otr(oe) + pmx * itr(ie);       % add product of transformation matrices and input tensor element to output tensor element
   end
end


		% Generarea retelei de noduri pe domeniului considerat
[X,Y]=meshgrid(-3.75:0.05:3.5);
		% Evaluarea functiei de doua variabile de reprezentat
Z=peaks(X,Y);
		% Incarcarea nivelelor negative de reprezentat
v_neg=-3:0.5:-1;
		% Incarcarea nivelelor pozitive de reprezentat
v_poz=1:0.5:7;
		% Desenarea liniilor de contur corespunzatoare
		%  - nivelelor negative cu linii intrerupte albastre
contour(X,Y,Z,v_neg,'b--');
hold on
		%  - nivelelor pozitive cu linii continue rosii
contour(X,Y,Z,v_poz,'r-');
hold off


function [upsilonap, upsilon] = lfmapComputeUpsilonMatrix(gamma, sigma2, t1, t2, mode)

% LFMAPCOMPUTEUPSILONMATRIX Upsilon matrix acce. pos. with t1, t2 limits
% FORMAT
% DESC computes a portion of the LFM kernel.
% ARG gamma : Gamma value for system.
% ARG sigma2 : length scale of latent process.
% ARG t1 : first time input (number of time points x 1).
% ARG t2 : second time input (number of time points x 1).
% ARG mode : operation mode, according to the derivative (mode 0,
% derivative wrt t1, mode 1 derivative wrt t2)
% RETURN upsilon : result of this subcomponent of the kernel for the given values.
%
% COPYRIGHT : Mauricio Alvarez, 2010
%
% SEEALSO : lfmComputeUpsilonMatrix.F, lfmComputeH3.m

% KERN

sigma = sqrt(sigma2);
gridt1 = repmat(t1, 1, length(t2));
gridt2 = repmat(t2', length(t1), 1);
timeGrid = gridt1 - gridt2;

if mode==0
    if nargout > 1
        upsilon = lfmComputeUpsilonMatrix(gamma, sigma2, t1, t2);
        upsilonap = gamma^2*upsilon - (2/(sqrt(pi)*sigma))*exp(-(timeGrid.^2)./sigma2).*(gamma + 2*timeGrid/sigma2);
    else
        upsilonap = gamma^2*lfmComputeUpsilonMatrix(gamma, sigma2, t1, t2) ...
            - (2/(sqrt(pi)*sigma))*exp(-(timeGrid.^2)./sigma2).*(gamma + 2*timeGrid/sigma2);
    end
else
    if nargout > 1
        upsilon = lfmComputeUpsilonMatrix(gamma, sigma2, t1, t2);
        upsilonap = gamma^2*upsilon - (2/(sqrt(pi)*sigma))*exp(-(timeGrid.^2)./sigma2).*(gamma + 2*timeGrid/sigma2) ...
            + (2/(sqrt(pi)*sigma))*exp(-gamma*t1)*((gamma - 2*t2/sigma2).*exp(-(t2.^2)/sigma2)).';
    else
        upsilonap = gamma^2*lfmComputeUpsilonMatrix(gamma, sigma2, t1, t2) ...
            - (2/(sqrt(pi)*sigma))*exp(-(timeGrid.^2)./sigma2).*(gamma + 2*timeGrid/sigma2) ...
            + (2/(sqrt(pi)*sigma))*exp(-gamma*t1)*((gamma - 2*t2/sigma2).*exp(-(t2.^2)/sigma2)).';
    end
end


% Finite phase difference and Kay frequency estimation
%
% Estimates the instantaneous frequency of the input signal using
% General Phase Difference (FFD, CFD, 4th, 6th) and Kay Frequency
% Estimation
%
% Usage:
%
%     ife = pde( signal, order [, window_length]);
%
% Inputs
%
%    signal
%
%	  Input one dimensional signal to be analysed.
%
%    order
%
%	  Order of the finite phase difference estimator. Available
%	  estimator orders are: 1, 2, 4, 6.
%
%    window_length
%
%	  Kay smoothing window length in the case of weighted phase
%	  difference estimator.  NB Set order = 1 for Kay's method.
%
%
%     For General Phase	Difference:
%
%     window_length:0;
%     order:1, 2, 4 or 6;
%
%     e.g. ife = pdeIF( signal, order);
%
%     for Kay smoothing weighted phase difference:
%
%     window_length: desired window length;
%     order=1;
%
%     e.g. ife = pde( signal, 0, window_length );
%
%
% TFSAP 7.0
% Copyright Prof. B. Boashash
% Qatar University, Doha
% email: tfsap.research@gmail.com


function L = gaussianNoiseExpectationLogLikelihood(noise, mu, varsigma, y)

% GAUSSIANNOISEEXPECTATIONLOGLIKELIHOOD Likelihood of the data under the GAUSSIAN noise model.
% FORMAT
% DESC returns the likelihoods for data points under the  Gaussian noise model.
% ARG noise : the noise structure for which the likelihood is required.
% ARG mu : input mean locations for the likelihood.
% ARG varSigma : input variance locations for the likelihood.
% ARG y : target locations for the likelihood.
%
% SEEALSO : gaussianNoiseParamInit, gaussianNoiseLogLikelihood, noiseLikelihood
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005

N = size(y, 1);
D = size(y, 2);
varsigma = varsigma + noise.sigma2;
for i = 1:D
  mu(:, i) = mu(:, i) + noise.bias(i);
end
arg = (mu - y)./sqrt(varsigma);
L = (2*pi*varsigma).^(-1/2).*exp( - .5*arg.*arg);


function [ mD ] = CalcDistanceMatrixCRows( mX )
% ----------------------------------------------------------------------------------------------- %
% [ mD ] = CalcDistanceMatrixARows( mX )
%   Calculates the distance matrix for the input data. The distance matrix
%   is a symmetric matrix where 'mD(ii, jj) = dist(mX(ii, :), mX(jj, :));'.
%   This function uses the squared Euclidean Distance for the distance
%   metric.
% Input:
%   - mX            -   Data Matrix.
%                       Each data sample is a row of the matrix.
%                       Structure: Matrix (numVars x varDim).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
% Output:
%   - mD            -   Distance Matrix.
%                       A symmetric matrix where 'mD(ii, jj) = dist(mX(ii,
%                       :), mX(jj, :));'.
%                       Structure: Matrix (numVars x numVars).
%                       Type: 'Single' / 'Double'.
%                       Range: [0, inf).
% References
%   1.  A
% Remarks:
%   1.  B
% TODO:
%   1.  C
% Release Notes:
%   -   1.0.000     01/01/2021  Royi Avital	RoyiAvital@yahoo.com
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

FALSE   = 0;
TRUE    = 1;

OFF     = 0;
ON      = 1;

mG = mX * mX.';
vG = diag(mG);

mD = vG.' + vG - (2 * mG);

end


function pass = test_paramODE_nonlin_US(pref)
% Test solving a parameter dependent ODE.
% Nick Hale, August 2011

% NOTE: Taken from V4 test chebop_paramODE.

if ( nargin == 0 )
    pref = cheboppref();
end
tol = 1e3*pref.bvpTol;

% Nonlinear parameter dependent problem (ULTRAS)
pref.discretization = @ultraS;

% Natural setup
x = chebfun('x', [-1 1]);
N = chebop(@(x, u, a) (1-x.^2).*u + .1*diff(u,2) + a.*exp(u));
% N = chebop(@(x, u, a) x.*u + .001*diff(u,2) + a.*diff(u));
N.lbc = @(u, a) [u + a + 1 ; diff(u)];
N.rbc = @(u, a) u - 1;
N.init = [x ; -1];
u = mldivide(N, chebfun(0), pref);
res = N(x, u);
Nlbc = N.lbc(u{1},u{2});
Nrbc = N.rbc(u{1},u{2});
err = norm(res) + Nlbc{1}(-1) + Nlbc{2}(-1) + Nrbc(1);

pass = err < tol;

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   demo script to insert new nodes to a tetrahedral mesh
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

load rat_head.mat

[node,elem,face]=vol2mesh(volimage>0.05,1:size(volimage,1),1:size(volimage,2),...
                           1:size(volimage,3),2,2,1);

plotmesh(node,face);

dt=pi/10;
r=3;
x0=30;
y0=28;
theta=dt:dt:2*pi;
x=x0+r*cos(theta);
y=y0+r*sin(theta);

p0=[x;y;ones(1,length(theta))*50]';
v0=[0,0,-1];

[t,u,v,idx,xnode]=raysurf(p0,v0,node,face(:,1:3));

hold on
plotmesh(p0,'r+');

goodpt=find(~isnan(xnode(:,1)) & ~isnan(xnode(:,2)) & ~isnan(xnode(:,3)));
[newnode,newelem,newface]=meshrefine(node,elem,face,xnode(goodpt,:));

figure
plotmesh(newnode,newface);
hold on
plotmesh(p0,'r+');
plotmesh(xnode,'r.')


%NS1
% Numerical solution of 2-D instationary Navier-Stokes equations.
% Rectangular domain; nonuniform Cartesian staggered grid.
% Time discretization with omega-scheme.
% Pressure-correction method.
% Central or upwind scheme for inertia terms.

% Copyright 2001 P. Wesseling
% This program and its subprograms may be freely used, modified and distributed
% under the GNU General Public License: http://www.gnu.org/copyleft/gpl.html

% Theory is given in Chapter 6 of
% 	Principles of Computational Fluid Dynamics, by P. Wesseling
% 	Springer-Verlag, Berlin etc., 2001. ISBN 3-540-67853-0
% See http://dutita0.twi.tudelft.nl/nw/users/wesseling/

% Theory is also given in Chapter 5 of
%	Computational Fluid Dynamics
%	Lecture Notes by P. Wesseling
%	Department of Applied Mathematical Analysis, ITS, TU Delft
% See http://ta.twi.tudelft.nl/nw/users/wesseling/

% This program generates Figures  and  in the Lecture Notes.

% Functions called: startup,problem_specification, grid_generation, draw_grid,
%		    viscous_matrix, grad_and_div, initial_condition,
%		    right_hand_side, inertia_matrix, errornorm, isobarplot,
%		    streamlineplot, velocity_profile, relchangeplot

startup, format compact, clear all
time0 = clock;
global geval n u0 v0 J K yu yv yseglen alpha
% ........................................Input..............................
geval = 3;	% Enter 1 for horizontal Poiseuille flow to the right
		%  or 2 for vertical Poiseuille flow
		%  or 3 for backward facing step
		%  or 4 for driven cavity
		%  or 5 for uniform flow under angle alpha without segmentation
		%  or 6 for uniform flow under angle alpha with segmentation
		%  or 7 for horizontal Poiseuille flow to the left

problem_specification	% Prepare file problem_specification.m as part of input
% User must prepare files ubd.m, vbd.m, pbd.m to specify boundary values

% .................................End of input......................

grid_generation
draw_grid
h = waitbar(0,'Computing...');
viscous_matrix		% Generate viscous matrices Bu, Bv and Masku, Maskv
grad_and_div		% Generate pressure gradient matrices Pu, Pv and
			%  divergence matrices Du, Dv
initial_condition	% Generate initial u0, v0, p0
tic
[Lp,Up] = lu([Du Dv]*[Pu;Pv]);	% LU decomposition of pressure correction matrix
tijd = toc; disp(['Pres. corr. matrix LU time = ',num2str(tijd)])
if max(abs(sum(([Du Dv]*[Pu;Pv])'))) < 10*eps
  % Do consistency check
end

nstep = floor(tend/dt);	% Number of time steps
%nstep = 2;
relchange = zeros(nstep,1);	% For plotting relative change per time step
t = 0; n = 0;
right_hand_side		% Generate right-hand sides ru, rv; has to be moved
			% inside time loop if boundary conditions time-dependent
u1 = u0; v1 = v0;
for n = 1:nstep		% Time-stepping with omega scheme
  t = n*dt;
  u0 = u1; v0 = v1;
%  right_hand_side	% Generate right-hand sides ru, rv; can be moved outside
			% time loop if boundary conditions not time-dependent

  % Navier-Stokes, predictor:
  inertia_matrix	% Generate inertia matrices and adapt right-hand sides
  rum = (ru + rum)'; rvm = (rv + rvm)'; rum = rum(:); rvm = rvm(:);
  if n == 1, tic, end
  Cu = dt*(Bu + Cu); Cv = dt*(Bv + Cv);
  u1 = (speye(length(u0)) + omega*Cu)\...
        (Masku*u0 + dt*(rum - Pu*p0) - (1-omega)*Cu*u0);
  v1 = (speye(length(v0)) + omega*Cv)\...
        (Maskv*v0 + dt*(rvm - Pv*p0) - (1-omega)*Cv*v0);

  % End of Navier-Stokes predictor

  % Stokes:
  %rum = ru'; rum = rum(:); rvm = rv'; rvm = rvm(:);
  %if n == 1, tic, end
  %u1 = Bu\(dt*(rum - Pu*p0) + Masku*u0);
  %v1 = Bv\(dt*(rvm - Pv*p0) + Maskv*v0);
  % End of Stokes

  if n == 1, tijd = toc; disp(['velocity solve time = ',num2str(tijd)]), tic,end
  dp = Up\(Lp\(Du*u1 + Dv*v1));	p0 = p0 + dp/dt;
  u1 = u1 - Pu*dp;	v1 = v1 - Pv*dp;
  if n == 1, tijd = toc; disp(['press. corr. time = ',num2str(tijd)]),end
  mm = max([norm(u1,inf),norm(v1,inf)]); relchange(n) = max([norm(u1-u0,inf)...
  /mm,norm(v1-v0,inf)/mm, norm(dp,inf)/(norm(p0,inf) + 0.5*mm^2)]);
  waitbar(n/nstep)
end

disp(['Relative change = ',num2str(relchange(end))])
divergence = norm(Du*u1 + Dv*v1,inf);
if (geval==1)|(geval==2)
  errornorm		% Maximum norm of error in outflow profile if exact
			% solution is available
end
close(h)
tic
isobarplot		% Plot of isobars and velocity vectors
streamlineplot 		% Plot of streamlines and velocity vectors
velocity_profile	% Plot of velocity profiles
relchangeplot		% Plot of relative change per time step
tijd = toc; disp(['Plotting time = ',num2str(tijd)])
disp(['Total time = ',num2str(etime(clock,time0))])


function [H] = rotate(D);

% ROTATE returns the homogenous coordinate transformation matrix
% corresponding to a rotation around the x, y and z-axis. The direction of
% the rotation is according to the right-hand rule.
%
% Use as
%   [H] = rotate(R)
% where
%   R		[rx, ry, rz] in degrees
%   H		corresponding homogenous transformation matrix
%
% Note that the order in which the rotations are performs matters. The
% rotation is first done around the z-axis, then the y-axis and finally the
% x-axis.

% Copyright (C) 2000-2005, Robert Oostenveld
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

% $Log: rotate.m,v $
% Revision 1.1  2009/01/30 04:02:11  arno
% *** empty log message ***
%
% Revision 1.5  2006/09/12 13:35:28  roboos
% convert input rotation from degrees (according to documentation) into radians (needed for computations)
%
% Revision 1.4  2005/08/15 08:15:33  roboos
% reimplemented the rotate function, which contained an error (the error is in the AIR technical reference)
% changed all functions to be dependent on the rotate, translate and scale function
% all functions now behave consistenly, which also means that they are not compleetly backward compatible w.r.t. the order of the rotations
%
% Revision 1.3  2004/05/19 09:57:07  roberto
% added GPL copyright statement, added CVS log item
%

% convert degrees to radians
R = D*pi/180;

% get the individual angles (in radians)
rx = R(1);
ry = R(2);
rz = R(3);

% precompute the sin/cos values of the angles
cX = cos(rx);
cY = cos(ry);
cZ = cos(rz);
sX = sin(rx);
sY = sin(ry);
sZ = sin(rz);

% according to Roger Woods' http://bishopw.loni.ucla.edu/AIR5/homogenous.html
% it should be this, but I cannot reproduce his rotation matrix
% H = eye(4,4);
% H(1,1) = cZ*cY + sZ*sX*sY;
% H(1,2) = sZ*cY - cZ*sX*sY;
% H(1,3) =            cX*sY;
% H(2,1) = -sZ*cX;
% H(2,2) =  cZ*cX;
% H(2,3) =     sX;
% H(3,1) =  sZ*sX*cY - cZ*sY;
% H(3,2) = -cZ*sX*cY - sZ*sY;
% H(3,3) =             cX*cY;

% instead, the following rotation matrix does work according my
% expectations. It rotates according to the right hand rule and first
% rotates around z, then y and then x axis
H = [
           cZ*cY,          -sZ*cY,              sY,               0
  cZ*sY*sX+sZ*cX, -sZ*sY*sX+cZ*cX,          -cY*sX,               0
 -cZ*sY*cX+sZ*sX,  sZ*sY*cX+cZ*sX,           cY*cX,               0
               0,               0,               0,               1
];

if 0
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% The following code can be used to construct the combined rotation matrix
% for either xyz or zyx ordering (using the Matlab symbolic math toolbox)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  syms sX sY sZ cX cY cZ
  % this is for only rotating around x
  Rx = [
    1   0    0   0
    0   cX  -sX  0
    0   sX   cX  0
    0   0    0   1
    ];
  % this is for only rotating around y
  Ry = [
    cY  0   sY  0
    0   1   0   0
    -sY  0   cY  0
    0   0   0   1
    ];
  % this is for only rotating around z
  Rz = [
    cZ -sZ  0   0
    sZ  cZ  0   0
    0   0   1   0
    0   0   0   1
    ];
  % combine them
  Rzyx = Rz * Ry * Rx  % rotate around x, y, then z
  Rxyz = Rx * Ry * Rz  % rotate around z, y, then x
end


function [indTheta, Xi, numterms] = ADMinitvary(nT,lambda,MaxIter,tol, plotflag)

% normalize the collumns of the null space of Theta to use in the
% initial conditoin for ADM search
for ii = 1:size(nT,2)
    nTn(:,ii) = nT(:,ii)/mean(nT(:,ii));
end

% run ADM algorithm on each row of nTn
for jj = 1:size(nTn,1)
    jj;
    qinit = nTn(jj,:)'; % intial conditions
    q(:,jj) = ADM(nT, qinit, lambda, MaxIter, tol); % algrorithm for
    % finding coefficients resutling in  sparsest vector in null space
    out(:,jj) = nT*q(:,jj); % compose sparsets vectors
    nzeros(jj)= length(find(abs(out(:,jj))<lambda)); % chech how many zeros each
    % of the found sparse vectors have
end

indsparse = find(nzeros==max(nzeros)); % find the vector with the largest number of zeros
indTheta = find(abs(out(:,indsparse(1)))>=lambda); % save the indices of non zero coefficients
% Xi = out(indTheta, indsparse(1)); % get those coefficients
Xi = out(:, indsparse(1)); % get sparsest vector
smallinds =(abs(out(:,indsparse(1)))<lambda);
Xi(smallinds)=0; % set thresholded coefficients to zero

% check that the solution found by ADM is unique.
if length(indsparse)>1
    Xidiff= (out(indTheta, indsparse(1))-out(indTheta,indsparse(2)));
    if Xidiff>tol
        warning('ADM has discovered two different sparsest vectors')
    end
end
% calculate how many terms are in the sparsest vector
numterms = length(indTheta);

if plotflag == 2
    figure(121)
    semilogy(size(nT,1)-nzeros, 'o');
    drawnow
end


function [L,S,obj,err,iter] = igc(A,C,lambda,opts)

% Reference: Chen, Yudong, Sujay Sanghavi, and Huan Xu. Improved graph clustering.
% IEEE Transactions on Information Theory 60.10 (2014): 6440-6455.
%
% min_{L,S} ||L||_*+lambda*||C \cdot S||_1, s.t. A=L+S, 0<=L<=1.
%
% ---------------------------------------------
% Input:
%       A       -    d*n matrix
%       C       -    d*n matrix
%       lambda  -    >0, parameter
%       opts    -    Structure value in Matlab. The fields are
%           opts.tol        -   termination tolerance
%           opts.max_iter   -   maximum number of iterations
%           opts.mu         -   stepsize for dual variable updating in ADMM
%           opts.max_mu     -   maximum stepsize
%           opts.rho        -   rho>=1, ratio used to increase mu
%           opts.DEBUG      -   0 or 1
%
% Output:
%       L       -    d*n matrix
%       S       -    d*n matrix
%       obj     -    objective function value
%       err     -    residual
%       iter    -    number of iterations
%
% version 1.0 - 19/06/2016
%
% Written by Canyi Lu (canyilu@gmail.com)
%

tol = 1e-8;
max_iter = 500;
rho = 1.1;
mu = 1e-4;
max_mu = 1e10;
DEBUG = 0;

if ~exist('opts', 'var')
    opts = [];
end
if isfield(opts, 'tol');         tol = opts.tol;              end
if isfield(opts, 'max_iter');    max_iter = opts.max_iter;    end
if isfield(opts, 'rho');         rho = opts.rho;              end
if isfield(opts, 'mu');          mu = opts.mu;                end
if isfield(opts, 'max_mu');      max_mu = opts.max_mu;        end
if isfield(opts, 'DEBUG');       DEBUG = opts.DEBUG;          end

C = abs(C);
[d,n] = size(A);

L = zeros(d,n);
S = L;
Z = L;
Y1 = L;
Y2 = L;

iter = 0;
for iter = 1 : max_iter
    Lk = L;
    Sk = S;
    Zk = Z;
    % first super block {L,S}
    [L,nuclearnormL] = prox_nuclear(Z-Y2/mu,1/mu);
    S = prox_l1(-Z+A-Y1/mu,C*(lambda/mu));

    % second super block {Z}
    Z = project_box((-S+A+L+(Y2-Y1)/mu)/2,0,1);

    dY1 = Z+S-A;
    dY2 = L-Z;
    chgL = max(max(abs(Lk-L)));
    chgS = max(max(abs(Sk-S)));
    chgZ = max(max(abs(Zk-Z)));
    chg = max([chgL chgS chgZ max(abs(dY1(:))) max(abs(dY2(:)))]);
    if DEBUG
        if iter == 1 || mod(iter, 10) == 0
            obj = nuclearnormL+lambda*sum(sum(C.*abs(S)));
            err = sqrt(norm(dY1,'fro')^2+norm(dY2,'fro')^2);
            disp(['iter ' num2str(iter) ', mu=' num2str(mu) ...
                    ', obj=' num2str(obj) ', err=' num2str(err)]);
        end
    end

    if chg < tol
        break;
    end
    Y1 = Y1 + mu*dY1;
    Y2 = Y2 + mu*dY2;
    mu = min(rho*mu,max_mu);
end
obj = nuclearnormL+lambda*sum(sum(C.*abs(S)));
err = sqrt(norm(dY1,'fro')^2+norm(dY2,'fro')^2);