function [x_g,w_g,phi,p_x,p_y,p_z] = threed_shapeiso(x_local,r,s,t,w)
%-----------------------------------------------------------------------
%  threed_shapeiso.m - computes test functions and derivatives on an
%                      element given element coordinates and Gauss points.
%                      Isoparametric coordinates are used (curved elements)
%
%  Copyright (c) 2002, Jeff Borggaard, Virginia Tech
%  Version: 1.0a
%
%  Usage:    [x_g,w_g,phi,p_x,p_y,p_z] = threed_shapeiso(x_local,r,s,t,w)
%
%  Variables:     x_local
%                        Coordinates of the element nodes
%                 (r,s,t)
%                        Coordinates of Gauss points in unit tetrahedron
%                 w
%                        Gauss weights associated with (r,s,t)
%
%                 x_g
%                        Coordinates of Gauss points in the element
%                 w_g
%                        Gauss weights scaled by the element Jacobian
%                 phi
%                        Value of element shape functions at x_g
%                 p_x
%                 p_y
%                 p_z
%                        First spatial derivatives of phi
%                 p_xx, etc.
%                        Second spatial derivatives of phi
%                        (only applicable with 2nd order or higher)
%                        (currently not implemented)
%-----------------------------------------------------------------------
  x      = x_local;
  [n,t1] = size(x);  % t1 had better be 3
  rule   = length(r);

  if (n == 10)
    % The following assumes isoparametric elements
    c0 =   x( 1,:);                                      % 1
    c1 =-3*x( 1,:) -   x( 2,:) + 4*x( 5,:)            ;  % r
    c2 =-3*x( 1,:) -   x( 3,:) + 4*x( 7,:)            ;  % s
    c3 =-3*x( 1,:) -   x( 4,:) + 4*x(10,:)            ;  % t
    c4 = 2*x( 1,:) + 2*x( 2,:) - 4*x( 5,:)            ;  % r^2
    c5 = 2*x( 1,:) + 2*x( 3,:) - 4*x( 7,:)            ;  % s^2
    c6 = 2*x( 1,:) + 2*x( 4,:) - 4*x(10,:)            ;  % t^2
    c7 = 4*x( 1,:) - 4*x( 5,:) + 4*x( 6,:) - 4*x( 7,:); % rs
    c8 = 4*x( 1,:) - 4*x( 5,:) + 4*x( 8,:) - 4*x(10,:); % rt
    c9 = 4*x( 1,:) - 4*x( 7,:) + 4*x( 9,:) - 4*x(10,:); % st

    x_g(:,1) = c0(1) + c1(1)*r + c2(1)*s + c3(1)*t + c4(1)*r.^2 ...
             + c5(1)*s.^2 + c6(1)*t.^2 + c7(1)*r.*s + c8(1)*r.*t ...
             + c9(1)*s.*t;
    xr = c1(1) + 2*c4(1)*r + c7(1)*s + c8(1)*t;
    xs = c2(1) + 2*c5(1)*s + c7(1)*r + c9(1)*t;
    xt = c3(1) + 2*c6(1)*t + c8(1)*r + c9(1)*s;

    x_g(:,2) = c0(2) + c1(2)*r + c2(2)*s + c3(2)*t + c4(2)*r.^2 ...
             + c5(2)*s.^2 + c6(2)*t.^2 + c7(2)*r.*s + c8(2)*r.*t ...
             + c9(2)*s.*t;
    yr = c1(2) + 2*c4(2)*r + c7(2)*s + c8(2)*t;
    ys = c2(2) + 2*c5(2)*s + c7(2)*r + c9(2)*t;
    yt = c3(2) + 2*c6(2)*t + c8(2)*r + c9(2)*s;

    x_g(:,3) = c0(3) + c1(3)*r + c2(3)*s + c3(3)*t + c4(3)*r.^2 ...
             + c5(3)*s.^2 + c6(3)*t.^2 + c7(3)*r.*s + c8(3)*r.*t ...
             + c9(3)*s.*t;
    zr = c1(3) + 2*c4(3)*r + c7(3)*s + c8(3)*t;
    zs = c2(3) + 2*c5(3)*s + c7(3)*r + c9(3)*t;
    zt = c3(3) + 2*c6(3)*t + c8(3)*r + c9(3)*s;

    % Compute the Jacobian of the (r,s,t) -> (x,y,z) transformation
    jac = ( xr.*ys.*zt + xs.*yt.*zr + xt.*yr.*zs ...
           -xr.*yt.*zs - xs.*yr.*zt - xt.*ys.*zr );

    w_g = jac.*w;

    % Invert derivatives of the mapping
    rx = ( ys.*zt-yt.*zs )./jac;
    ry = ( xt.*zs-xs.*zt )./jac;
    rz = ( xs.*yt-xt.*ys )./jac;

    sx = ( yt.*zr-yr.*zt )./jac;
    sy = ( xr.*zt-xt.*zr )./jac;
    sz = ( xt.*yr-xr.*yt )./jac;

    tx = ( yr.*zs-ys.*zr )./jac;
    ty = ( xs.*zr-xr.*zs )./jac;
    tz = ( xr.*ys-xs.*yr )./jac;

    % Compute shape function and derivatives at Gauss points
    u  = 1 - r  - s  - t ;
    ux =   - rx - sx - tx;
    uy =   - ry - sy - ty;
    uz =   - rz - sz - tz;

    phi = zeros(rule,n);
    phi(:,1)  = u - 2*r.*u - 2*s.*u - 2*t.*u;
    phi(:,2)  = r - 2*r.*u - 2*r.*s - 2*r.*t;
    phi(:,3)  = s - 2*r.*s - 2*s.*u - 2*s.*t;
    phi(:,4)  = t - 2*r.*t - 2*s.*t - 2*t.*u;
    phi(:,5)  = 4*r.*u;
    phi(:,6)  = 4*r.*s;
    phi(:,7)  = 4*s.*u;
    phi(:,8)  = 4*r.*t;
    phi(:,9)  = 4*s.*t;
    phi(:,10) = 4*t.*u;

    p_x = zeros(rule,n);
    p_x(:,1)  = ux - 2*rx.*u - 2*r.*ux - 2*sx.*u ...
                   - 2*s.*ux - 2*tx.*u - 2*t.*ux    ;
    p_x(:,2)  = rx - 2*rx.*u - 2*r.*ux - 2*rx.*s ...
                   - 2*r.*sx - 2*rx.*t - 2*r.*tx    ;
    p_x(:,3)  = sx - 2*rx.*s - 2*r.*sx - 2*sx.*u ...
                   - 2*s.*ux - 2*sx.*t - 2*s.*tx    ;
    p_x(:,4)  = tx - 2*rx.*t - 2*r.*tx - 2*sx.*t ...
                   - 2*s.*tx - 2*tx.*u - 2*t.*ux    ;
    p_x(:,5)  = 4*rx.*u + 4*r.*ux;
    p_x(:,6)  = 4*rx.*s + 4*r.*sx;
    p_x(:,7)  = 4*sx.*u + 4*s.*ux;
    p_x(:,8)  = 4*rx.*t + 4*r.*tx;
    p_x(:,9)  = 4*sx.*t + 4*s.*tx;
    p_x(:,10) = 4*tx.*u + 4*t.*ux;

    p_y = zeros(rule,n);
    p_y(:,1)  = uy - 2*ry.*u - 2*r.*uy - 2*sy.*u ...
                   - 2*s.*uy - 2*ty.*u - 2*t.*uy    ;
    p_y(:,2)  = ry - 2*ry.*u - 2*r.*uy - 2*ry.*s ...
                   - 2*r.*sy - 2*ry.*t - 2*r.*ty    ;
    p_y(:,3)  = sy - 2*ry.*s - 2*r.*sy - 2*sy.*u ...
                   - 2*s.*uy - 2*sy.*t - 2*s.*ty    ;
    p_y(:,4)  = ty - 2*ry.*t - 2*r.*ty - 2*sy.*t ...
                   - 2*s.*ty - 2*ty.*u - 2*t.*uy    ;
    p_y(:,5)  = 4*ry.*u + 4*r.*uy;
    p_y(:,6)  = 4*ry.*s + 4*r.*sy;
    p_y(:,7)  = 4*sy.*u + 4*s.*uy;
    p_y(:,8)  = 4*ry.*t + 4*r.*ty;
    p_y(:,9)  = 4*sy.*t + 4*s.*ty;
    p_y(:,10) = 4*ty.*u + 4*t.*uy;

    p_z = zeros(rule,n);
    p_z(:,1)  = uz - 2*rz.*u - 2*r.*uz - 2*sz.*u ...
                   - 2*s.*uz - 2*tz.*u - 2*t.*uz    ;
    p_z(:,2)  = rz - 2*rz.*u - 2*r.*uz - 2*rz.*s ...
                   - 2*r.*sz - 2*rz.*t - 2*r.*tz    ;
    p_z(:,3)  = sz - 2*rz.*s - 2*r.*sz - 2*sz.*u ...
                   - 2*s.*uz - 2*sz.*t - 2*s.*tz    ;
    p_z(:,4)  = tz - 2*rz.*t - 2*r.*tz - 2*sz.*t ...
                   - 2*s.*tz - 2*tz.*u - 2*t.*uz    ;
    p_z(:,5)  = 4*rz.*u + 4*r.*uz;
    p_z(:,6)  = 4*rz.*s + 4*r.*sz;
    p_z(:,7)  = 4*sz.*u + 4*s.*uz;
    p_z(:,8)  = 4*rz.*t + 4*r.*tz;
    p_z(:,9)  = 4*sz.*t + 4*s.*tz;
    p_z(:,10) = 4*tz.*u + 4*t.*uz;

  else
    error('Only quadratic isoparametric elements are supported\n')
  end


function [m,v,w,g,f,pp,gg]=v_gaussmix(x,c,l,m0,v0,w0,wx)
%V_GAUSSMIX fits a gaussian mixture pdf to a set of data observations [m,v,w,g,f]=(x,c,l,m0,v0,w0,wx)
%
% Usage:
%    (1) [m,v,w]=v_gaussmix(x,[],[],k);    % create GMM with k mixtures and diagonal covariances
%    (2) [m,v,w]=gaussmix(x,[],[],k,'v');    % create GMM with k mixtures and full covariances
%
% Inputs: n data values, k mixtures, p parameters, l loops
%
%     X(n,p)   Input data vectors, one per row.
%     C(1)     Minimum variance of normalized data (Use [] to take default value of 1/n^2)
%     L        The integer portion of l gives a maximum loop count. The fractional portion gives
%              an optional stopping threshold. Iteration will cease if the increase in
%              log likelihood density per data point is less than this value. Thus l=10.001 will
%              stop after 10 iterations or when the increase in log likelihood falls below
%              0.001.
%              As a special case, if L=0, then the first three outputs are omitted.
%              Use [] to take default value of 100.0001
%     M0       Number of mixtures required (or initial mixture means - see below)
%     V0       Initialization mode:
%                'f'    Initialize with K randomly selected data points [default]
%                'p'    Initialize with centroids and variances of random partitions
%                'k'    k-means algorithm ('kf' and 'kp' determine initialization)
%                'h'    k-harmonic means algorithm ('hf' and 'hp' determine initialization) [default]
%                's'    use unscaled data during initialization phase instead of scaling it first
%                'm'    M0 contains the initial centres
%                'v'    full covariance matrices
%              Mode 'hf' [the default] generally gives the best results but 'f' is faster and often OK
%     W0(n,1)  Data point weights
%
%     Alternatively, initial values for M0, V0 and W0 can be given  explicitly:
%
%     M0(k,p)  Initial mixture means, one row per mixture.
%     V0(k,p)  Initial mixture variances, one row per mixture.
%      or V0(p,p,k)  one full-covariance matrix per mixture
%     W0(k,1)  Initial mixture weights, one per mixture. The weights should sum to unity.
%     WX(n,1)  Data point weights
%
% Outputs: (Note that M, V and W are omitted if L==0)
%
%     M(k,p)   Mixture means, one row per mixture. (omitted if L==0)
%     V(k,p)   Mixture variances, one row per mixture. (omitted if L==0)
%       or V(p,p,k) if full covariance matrices in use (i.e. either 'v' option or V0(p,p,k) specified)
%     W(k,1)   Mixture weights, one per mixture. The weights will sum to unity. (omitted if L==0)
%     G       Average log probability of the input data points.
%     F        Fisher's Discriminant measures how well the data divides into classes.
%              It is the ratio of the between-mixture variance to the average mixture variance: a
%              high value means the classes (mixtures) are well separated.
%     PP(n,1)  Log probability of each data point
%     GG(l+1,1) Average log probabilities at the beginning of each iteration and at the end
%
% The fitting procedure uses one of several initialization methods to create an initial guess
% for the mixture centres and then uses the EM (expectation-maximization) algorithm to refine
% the guess. Although the EM algorithm is deterministic, the initialization procedures use
% random numbers and so the routine will not give identical answers if you call it multiple
% times with the same input data. See v_randvec() for generating GMM data vectors.

%  Bugs/Suggestions
%     (1) Allow processing in chunks by outputting/reinputting an array of sufficient statistics
%     (2) Other initialization options:
%              'l'    LBG algorithm
%              'm'    Move-means (dog-rabbit) algorithm
%     (3) Allow updating of weights-only, not means/variances
%     (4) Allow freezing of means and/or variances

%      Copyright (C) Mike Brookes 2000-2009
%      Version: $Id: v_gaussmix.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[n,p]=size(x); % n = number of training values, p = dimension of data vector
wn=ones(n,1);
memsize=v_voicebox('memsize');    % set memory size to use
if isempty(c)
    c=1/n^2;
else
    c=c(1);         % just to prevent legacy code failing
end
fulliv=0;           % initial variance is not full
if isempty(l)
    l=100+1e-4;         % max loop count + stopping threshold
end
if nargin<5 || isempty(v0) || ischar(v0)             % no initial values specified for m0, v0, w0
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %  No initialvalues given, so we must use k-means or equivalent
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    if nargin<6
        if nargin<5 || isempty(v0)
            v0='hf';                 % default initialization mode: hf
        end
        wx=wn;                      % no data point weights
    else
        wx=w0(:);                   % data point weights
    end
    wx=wx/sum(wx);
    if any(v0=='m')
        k=size(m0,1);
    else
        k=m0;
    end
    fv=any(v0=='v');                % full covariance matrices requested
    mx0=wx'*x;         % calculate mean of input data in each dimension
    vx0=wx'*x.^2-mx0.^2; % calculate variance of input data in each dimension
    sx0=sqrt(vx0);
    sx0(sx0==0)=1;      % do not divide by zero when scaling
    if n<=k                         % each data point can have its own mixture
        xs=(x-mx0(wn,:))./sx0(wn,:);          % scale the data
        m=xs(mod((1:k)-1,n)+1,:);   % just include all points several times
        v=zeros(k,p);               % will be set to floor later
        w=zeros(k,1);
        w(1:n)=1/n;
        if l>0
            l=0.1;                  % no point in iterating
        end
    else                            % more points than mixtures
        if any(v0=='s')
            xs=x;                   % do not scale data during initialization
        else
            xs=(x-mx0(wn,:))./sx0(wn,:);  % else scale now
            if any(v0=='m')
                m=(m0-mx0(ones(k,1),:))./sx0(ones(k,1),:);  % scale specified means as well
            end
        end
        w=repmat(1/k,k,1);                      % all mixtures equally likely
        if any(v0=='k')                         % k-means initialization
            if any(v0=='m')
                [m,e,j]=v_kmeans(xs,k,m);
            elseif any(v0=='p')
                [m,e,j]=v_kmeans(xs,k,'p');
            else
                [m,e,j]=v_kmeans(xs,k,'f');
            end
        elseif any(v0=='h')                     % k-harmonic means initialization
            if any(v0=='m')
                [m,e,j]=v_kmeanhar(xs,k,[],4,m);
            else
                if any(v0=='p')
                    [m,e,j]=v_kmeanhar(xs,k,[],4,'p');
                else
                    [m,e,j]=v_kmeanhar(xs,k,[],4,'f');
                end
            end
        elseif any(v0=='p')                     % Initialize using a random partition
            j=ceil(rand(n,1)*k);                % allocate to random clusters
            j(v_rnsubset(k,n))=1:k;               % but force at least one point per cluster
            for i=1:k
                m(i,:)=mean(xs(j==i,:),1);
            end
        else
            if any(v0=='m')
                m=m0;                           % use specified centres
            else
                m=xs(v_rnsubset(k,n),:);          % Forgy initialization: sample k centres without replacement [default]
            end
            [e,j]=v_kmeans(xs,k,m,0);             % find out the cluster allocation
        end
        if any(v0=='s')
            xs=(x-mx0(wn,:))./sx0(wn,:);      % scale data now if not done previously
        end
        v=zeros(k,p);                   % diagonal covariances
        w=zeros(k,1);
        for i=1:k
            ni=sum(j==i);               % number assigned to this centre
            w(i)=(ni+1)/(n+k);          % weight of this mixture
            if ni
                v(i,:)=sum((xs(j==i,:)-repmat(m(i,:),ni,1)).^2,1)/ni;
            else
                v(i,:)=zeros(1,p);
            end
        end
    end
else
    %%%%%%%%%%%%%%%%%%%%%%%%
    % use initial values given as input parameters
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    if nargin<7
        wx=wn;              % no data point weights
    end
    wx=wx(:)/sum(wx); % normalize weights and force a column vector
    mx0=wx'*x;         % calculate mean of input data in each dimension
    vx0=wx'*x.^2-mx0.^2; % calculate variance of input data in each dimension
    sx0=sqrt(vx0);
    sx0(sx0==0)=1;      % do not divide by zero when scaling
    [k,p]=size(m0);
    xs=(x-mx0(wn,:))./sx0(wn,:);          % scale the data
    m=(m0-mx0(ones(k,1),:))./sx0(ones(k,1),:);          % and the means
    v=v0;
    w=w0;
    fv=ndims(v)>2 || size(v,1)>k;                       % full covariance matrix is supplied
    if fv
        mk=eye(p)==0;                                    % off-diagonal elements
        fulliv=any(v(repmat(mk,[1 1 k]))~=0);            % check if any are non-zero
        if ~fulliv
            v=reshape(v(repmat(~mk,[1 1 k])),p,k)'./repmat(sx0.^2,k,1);   % just pick out and scale the diagonal elements for now
        else
            v=v./repmat(sx0'*sx0,[1 1 k]);              % scale the full covariance matrix
        end
    end
end
if length(wx)~=n
    error('%d datapoints but %d weights',n,length(wx));
end
lsx=sum(log(sx0));
xsw=xs.*repmat(wx,1,p); % weighted data points
if ~fulliv          % initializing with diagonal covariance
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Diagonal Covariance matrices  %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    v=max(v,c);         % apply the lower bound
    xs2=xs.^2.*repmat(wx,1,p);          % square and weight the data for variance calculations

    % If data size is large then do calculations in chunks

    nb=min(n,max(1,floor(memsize/(8*p*k))));    % chunk size for testing data points
    nl=ceil(n/nb);                  % number of chunks
    jx0=n-(nl-1)*nb;                % size of first chunk

    im=repmat(1:k,1,nb); im=im(:);
    th=(l-floor(l))*n;
    sd=(nargout > 3*(l~=0)); % = 1 if we are outputting log likelihood values
    lp=floor(l)+sd;   % extra loop needed to calculate final G value

    lpx=zeros(1,n);             % log probability of each data point
    wk=ones(k,1);
    wp=ones(1,p);
    wnb=ones(1,nb);
    wnj=ones(1,jx0);

    % EM loop

    g=0;                            % dummy initial value for comparison
    gg=zeros(lp+1,1);
    ss=sd;                          % initialize stopping count (0 or 1)
    for j=1:lp
        g1=g;                       % save previous log likelihood (2*pi factor omitted)
        m1=m;                       % save previous means, variances and weights
        v1=v;
        w1=w;
        vi=-0.5*v.^(-1);                % data-independent scale factor in exponent
        lvm=log(w)-0.5*sum(log(v),2);   % log of external scale factor (excluding -0.5*p*log(2pi) term)

        % first do partial chunk (of length jx0)

        jx=jx0;
        ii=1:jx;                        % indices of data points in this chunk
        kk=repmat(ii,k,1);              % kk(jx,k): one row per data point, one column per mixture
        km=repmat(1:k,1,jx);            % km(jx,k): one row per data point, one column per mixture
        py=reshape(sum((xs(kk(:),:)-m(km(:),:)).^2.*vi(km(:),:),2),k,jx)+lvm(:,wnj); % py(k,jx) pdf of each point with each mixture
        mx=max(py,[],1);                % mx(1,jx) find normalizing factor for each data point to prevent underflow when using exp()
        px=exp(py-mx(wk,:));            % find normalized probability of each mixture for each datapoint
        ps=sum(px,1);                   % total normalized likelihood of each data point
        px=px./ps(wk,:);                % relative mixture probabilities for each data point (columns sum to 1)
        lpx(ii)=log(ps)+mx;
        pk=px*wx(ii);                   % pk(k,1) effective fraction of data points for each mixture (could be zero due to underflow)
        sx=px*xsw(ii,:);
        sx2=px*xs2(ii,:);
        for il=2:nl                     % process the data points in chunks
            ix=jx+1;
            jx=jx+nb;                   % increment upper limit
            ii=ix:jx;                   % indices of data points in this chunk
            kk=repmat(ii,k,1);
            py=reshape(sum((xs(kk(:),:)-m(im,:)).^2.*vi(im,:),2),k,nb)+lvm(:,wnb);
            mx=max(py,[],1);            % find normalizing factor for each data point to prevent underflow when using exp()
            px=exp(py-mx(wk,:));        % find normalized probability of each mixture for each datapoint
            ps=sum(px,1);               % total normalized likelihood of each data point
            px=px./ps(wk,:);            % relative mixture probabilities for each data point (columns sum to 1)
            lpx(ii)=log(ps)+mx;
            pk=pk+px*wx(ii);            % pk(k,1) effective fraction of data points for each mixture (could be zero due to underflow)
            sx=sx+px*xsw(ii,:);
            sx2=sx2+px*xs2(ii,:);
        end
        g=lpx*wx;                       % total log probability summed over all data points
        gg(j)=g;                        % save log prob at each iteration
        w=pk;                           % normalize to get the weights
        if pk                           % if all elements of pk are non-zero
            m=sx./pk(:,wp);             % calculate mixture means
            v=sx2./pk(:,wp);            % and variances
        else
            wm=pk==0;                   % mask indicating mixtures with zero weights
            nz=sum(wm);              	% number of zero-weight mixtures
            [vv,mk]=sort(lpx);          % find the lowest probability data points
            m=zeros(k,p);               % initialize means and variances to zero (variances are floored later)
            v=m;
            m(wm,:)=xs(mk(1:nz),:); 	% set zero-weight mixture means to worst-fitted data points
            w(wm)=1/n;               	% set these weights non-zero
            w=w*n/(n+nz);            	% normalize so the weights sum to unity
            wm=~wm;                 	% mask for non-zero weights
            m(wm,:)=sx(wm,:)./pk(wm,wp);  % recalculate means and variances for mixtures with a non-zero weight
            v(wm,:)=sx2(wm,:)./pk(wm,wp);
        end
        v=max(v-m.^2,c);                % apply floor to variances
        if g-g1<=th && j>1
            if ~ss, break; end  %  stop
            ss=ss-1;       % stop next time
        end
    end
    if sd && ~fv  % we need to calculate the final probabilities
        pp=lpx'-0.5*p*log(2*pi)-lsx;   % log of total probability of each data point
        gg=gg(1:j)-0.5*p*log(2*pi)-lsx;    % average log prob at each iteration
        g=gg(end);
        m=m1;       % back up to previous iteration
        v=v1;
        w=w1;
        mm=sum(m,1)/k;
        f=(m(:)'*m(:)-k*mm(:)'*mm(:))/sum(v(:));
    end
    if ~fv
        m=m.*sx0(ones(k,1),:)+mx0(ones(k,1),:);	% unscale means
        v=v.*repmat(sx0.^2,k,1);                % and variances
    else
        v1=v;
        v=zeros(p,p,k);
        mk=eye(p)==1;                           % mask for diagonal elements
        v(repmat(mk,[1 1 k]))=v1';              % set from v1
    end
end
if fv              % check if full covariance matrices were requested
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Full Covariance matrices  %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    pl=p*(p+1)/2;
    lix=1:p^2;
    cix=repmat(1:p,p,1);
    rix=cix';
    lix(cix>rix)=[];                                        % index of lower triangular elements
    cix=cix(lix);                                           % index of lower triangular columns
    rix=rix(lix);                                           % index of lower triangular rows
    dix=find(rix==cix);
    lixi=zeros(p,p);
    lixi(lix)=1:pl;
    lixi=lixi';
    lixi(lix)=1:pl;                                        % reverse index to build full matrices
    v=reshape(v,p^2,k);
    v=v(lix,:)';                                            % lower triangular in rows

    % If data size is large then do calculations in chunks

    nb=min(n,max(1,floor(memsize/(24*p*k))));    % chunk size for testing data points
    nl=ceil(n/nb);                  % number of chunks
    jx0=n-(nl-1)*nb;                % size of first chunk
    %
    th=(l-floor(l))*n;
    sd=(nargout > 3*(l~=0)); % = 1 if we are outputting log likelihood values
    lp=floor(l)+sd;   % extra loop needed to calculate final G value
    %
    lpx=zeros(1,n);             % log probability of each data point
    wk=ones(k,1);
    wp=ones(1,p);
    wpl=ones(1,pl);             % 1 index for lower triangular matrix
    wnb=ones(1,nb);
    wnj=ones(1,jx0);

    % EM loop

    g=0;                        % dummy initial value for comparison
    gg=zeros(lp+1,1);
    ss=sd;                      % initialize stopping count (0 or 1)
    vi=zeros(p*k,p);            % stack of k inverse cov matrices each size p*p
    vim=zeros(p*k,1);       	% stack of k vectors of the form inv(v)*m
    mtk=vim;                  	% stack of k vectors of the form m
    lvm=zeros(k,1);
    wpk=repmat((1:p)',k,1);
    for j=1:lp
        g1=g;               	% save previous log likelihood (2*pi factor omitted)
        m1=m;                	% save previous means, variances and weights
        v1=v;
        w1=w;
        for ik=1:k

            % these lines added for debugging only
            %             vk=reshape(v(k,lixi),p,p);
            %             condk(ik)=cond(vk);
            %%%%%%%%%%%%%%%%%%%%
            [uvk,dvk]=eig(reshape(v(ik,lixi),p,p));	% convert lower triangular to full and find eigenvalues
            dvk=max(diag(dvk),c);                	% apply variance floor to eigenvalues
            vik=-0.5*uvk*diag(dvk.^(-1))*uvk';      % calculate inverse
            vi((ik-1)*p+(1:p),:)=vik;               % vi contains all mixture inverses stacked on top of each other
            vim((ik-1)*p+(1:p))=vik*m(ik,:)';       % vim contains vi*m for all mixtures stacked on top of each other
            mtk((ik-1)*p+(1:p))=m(ik,:)';           % mtk contains all mixture means stacked on top of each other
            lvm(ik)=log(w(ik))-0.5*sum(log(dvk));       % vm contains the weighted sqrt of det(vi) for each mixture
        end
        %
        %         % first do partial chunk
        %
        jx=jx0;
        ii=1:jx;
        xii=xs(ii,:).';
        py=reshape(sum(reshape((vi*xii-vim(:,wnj)).*(xii(wpk,:)-mtk(:,wnj)),p,jx*k),1),k,jx)+lvm(:,wnj);
        mx=max(py,[],1);                % find normalizing factor for each data point to prevent underflow when using exp()
        px=exp(py-mx(wk,:));            % find normalized probability of each mixture for each datapoint
        ps=sum(px,1);                   % total normalized likelihood of each data point
        px=px./ps(wk,:);                % relative mixture probabilities for each data point (columns sum to 1)
        lpx(ii)=log(ps)+mx;
        pk=px*wx(ii);                       % effective fraction of data points for each mixture (could be zero due to underflow)
        sx=px*xsw(ii,:);
        sx2=px*(xsw(ii,rix).*xs(ii,cix));	% accumulator for variance calculation (lower tri cov matrix as a row)
        for il=2:nl
            ix=jx+1;
            jx=jx+nb;        % increment upper limit
            ii=ix:jx;
            xii=xs(ii,:).';
            py=reshape(sum(reshape((vi*xii-vim(:,wnb)).*(xii(wpk,:)-mtk(:,wnb)),p,nb*k),1),k,nb)+lvm(:,wnb);
            mx=max(py,[],1);                % find normalizing factor for each data point to prevent underflow when using exp()
            px=exp(py-mx(wk,:));            % find normalized probability of each mixture for each datapoint
            ps=sum(px,1);                   % total normalized likelihood of each data point
            px=px./ps(wk,:);                % relative mixture probabilities for each data point (columns sum to 1)
            lpx(ii)=log(ps)+mx;
            pk=pk+px*wx(ii);                    % effective fraction of data points for each mixture (could be zero due to underflow)
            sx=sx+px*xsw(ii,:);             % accumulator for mean calculation
            sx2=sx2+px*(xsw(ii,rix).*xs(ii,cix));	% accumulator for variance calculation
        end
        g=lpx*wx;                   % total log probability summed over all data points
        gg(j)=g;                    % save convergence history
        w=pk;               	% w(k,1) normalize to get the column of weights
        if pk                       % if all elements of pk are non-zero
            m=sx./pk(:,wp);         % find mean and mean square
            v=sx2./pk(:,wpl);
        else
            wm=pk==0;                       % mask indicating mixtures with zero weights
            nz=sum(wm);                  % number of zero-weight mixtures
            [vv,mk]=sort(lpx);             % find the lowest probability data points
            m=zeros(k,p);                   % initialize means and variances to zero (variances are floored later)
            v=zeros(k,pl);
            m(wm,:)=xs(mk(1:nz),:);                % set zero-weight mixture means to worst-fitted data points
            w(wm)=1/n;                      % set these weights non-zero
            w=w*n/(n+nz);                   % normalize so the weights sum to unity
            wm=~wm;                         % mask for non-zero weights
            m(wm,:)=sx(wm,:)./pk(wm,wp);  % recalculate means and variances for mixtures with a non-zero weight
            v(wm,:)=sx2(wm,:)./pk(wm,wpl);
        end
        v=v-m(:,cix).*m(:,rix);                 % subtract off mean squared
        if g-g1<=th && j>1
            if ~ss, break; end  %  stop
            ss=ss-1;       % stop next time
        end
    end
    if sd  % we need to calculate the final probabilities
        pp=lpx'-0.5*p*log(2*pi)-lsx;   % log of total probability of each data point
        gg=gg(1:j)-0.5*p*log(2*pi)-lsx;    % average log prob at each iteration
        g=gg(end);
        %             gg' % *** DEBUG ONLY ***
        m=m1;                                           % back up to previous iteration
        v=zeros(p,p,k);                                 % reserve spave for k full covariance matrices
        trv=0;                                          % sum of variance matrix traces
        for ik=1:k                                      % loop for each mixture to apply variance floor
            [uvk,dvk]=eig(reshape(v1(ik,lixi),p,p));	% convert lower triangular to full and find eigenvectors
            dvk=max(diag(dvk),c);                       % apply variance floor to eigenvalues
            v(:,:,ik)=uvk*diag(dvk)*uvk';               % reconstitute full matrix
            trv=trv+sum(dvk);                           % add trace to the sum
        end
        w=w1;
        mm=sum(m,1)/k;
        f=(m(:)'*m(:)-k*mm(:)'*mm(:))/trv;
    else
        v1=v;                                           % lower triangular form
        v=zeros(p,p,k);                                 % reserve spave for k full covariance matrices
        for ik=1:k                                      % loop for each mixture to apply variance floor
            [uvk,dvk,]=eig(reshape(v1(ik,lixi),p,p));	% convert lower triangular to full and find eigenvectors
            dvk=max(diag(dvk),c);                       % apply variance floor
            v(:,:,ik)=uvk*diag(dvk)*uvk';               % reconstitute full matrix
        end
    end
    m=m.*sx0(ones(k,1),:)+mx0(ones(k,1),:);  % unscale means
    v=v.*repmat(sx0'*sx0,[1 1 k]);
end
if l==0         % suppress the first three output arguments if l==0
    m=g;
    v=f;
    w=pp;
end


function F = spm_Tcdf(x,v)
% Cumulative Distribution Function (CDF) of Students t distribution
% FORMAT p = spm_Tcdf(x,v)
%
% x - T-variate (Student's t has range (-Inf,Inf)
% v - degrees of freedom (v>0, non-integer d.f. accepted)
% F - CDF of Student's t-distribution with v degrees of freedom at points x
%__________________________________________________________________________
%
% spm_Tcdf implements the Cumulative Distribution of the Students t-distribution.
%
% Definition:
%--------------------------------------------------------------------------
% The CDF F(x) of the Student's t-distribution with v degrees of
% freedom is the probability that a realisation of a t random variable
% X has value less than x; F(x)=Pr{X<x} for X~G(h,c). Student's
% t-distribution is defined for real x and positive integer v (See
% Evans et al., Ch37).
%
% This implementation is not restricted to whole (positive integer) df
% v, rather it will compute for any df v>0.
%
% Variate relationships: (Evans et al., Ch37 & 7)
%--------------------------------------------------------------------------
% The Student's t distribution with 1 degree of freedom is the Standard
% Cauchy distribution, which has a simple closed form CDF.
%
% Algorithm:
%--------------------------------------------------------------------------
% The CDF of the Student's t-distribution with v degrees of freedom
% is related to the incomplete beta function by:
%       Pr(|X|<x) = betainc(v/(v+x^2),v/2,1/2)
% so
%              {     betainc(v/(v+x^2),v/2,1/2) / 2      for x<0
%       F(x) = |   0.5                                   for x=0
%              { 1 - betainc(v/(v+x^2),v/2,1/2) / 2      for x>0
%
% See Abramowitz & Stegun, 26.5.27 & 26.7.1; Press et al., Sec6.4 for
% definitions of the incomplete beta function. The relationship is
% easily verified by substituting for v/(v+x^2) in the integral of the
% incomplete beta function.
%
% MATLAB's implementation of the incomplete beta function is used.
%
%
% References:
%--------------------------------------------------------------------------
% Evans M, Hastings N, Peacock B (1993)
%       "Statistical Distributions"
%        2nd Ed. Wiley, New York
%
% Abramowitz M, Stegun IA, (1964)
%       "Handbook of Mathematical Functions"
%        US Government Printing Office
%
% Press WH, Teukolsky SA, Vetterling AT, Flannery BP (1992)
%       "Numerical Recipes in C"
%        Cambridge
%
%__________________________________________________________________________
% Copyright (C) 1992-2011 Wellcome Trust Centre for Neuroimaging

% Andrew Holmes
% $Id: spm_Tcdf.m 4182 2011-02-01 12:29:09Z guillaume $

%-Format arguments, note & check sizes
%--------------------------------------------------------------------------
if nargin<2, error('Insufficient arguments'), end

ad = [ndims(x);ndims(v)];
rd = max(ad);
as = [[size(x),ones(1,rd-ad(1))];...
      [size(v),ones(1,rd-ad(2))]];
rs = max(as);
xa = prod(as,2)>1;
if all(xa) && any(diff(as(xa,:)))
    error('non-scalar args must match in size');
end

%-Computation
%--------------------------------------------------------------------------
%-Initialise result to zeros
F = zeros(rs);

%-Only defined for strictly positive v. Return NaN if undefined.
md = ( ones(size(x))  &  v>0 );
if any(~md(:))
    F(~md) = NaN;
    warning('Returning NaN for out of range arguments');
end

%-Special case: f is 0.5 when x=0 (where betainc involves log of zero)
F( md  &  x==0 ) = 0.5;

%-Special case: Standard Cauchy distribution when v=1
ml = ( md  &  v==1 ); if xa(1), mlx=ml; else mlx=1; end
F(ml) = 0.5 + atan(x(mlx))/pi;

%-Compute where defined & not special cases
Q  = find( md  &  x~=0  &  v~=1 );
if isempty(Q), return, end
if xa(1), Qx=Q; else Qx=1; end
if xa(2), Qv=Q; else Qv=1; end

%-Compute
xQxPos = x(Qx)>0;
F(Q) = xQxPos -(xQxPos*2-1).*0.5.*betainc(v(Qv)./(v(Qv)+x(Qx).^2),v(Qv)/2,1/2);


function [dir,x0,y0] = boundarydir(x,y,orderout)
%BOUNDARYDIR Determine the direction of a sequence of planar points.
%   DIR = BOUNDARYDIR(X,Y) determines the direction of travel of a
%   closed, nonintersecting sequence of planar points with coordinates
%   contained in column vectors X and Y. Values of DIR are 'cw'
%   (clockwise) and 'ccw' (counterclockwise). The direction of travel is
%   with respect to the image coordinate system defined in Chapter 2 of
%   the book.
%
%   [DIR,X0,Y0] = BOUNDARYDIR(X,Y,ORDEROUT) determines the direction DIR
%   of the input sequence, and also outputs the sequence with its
%   direction of travel as specified in ORDEROUT. Valid values of this
%   parameter as 'cw' and 'ccw'. The coordinates of the output sequence
%   are column vectors X0 and Y0.
%
%   The input sequence is assumed to be nonintersecting, and it cannot
%   have duplicate points, with the exception of the first and last
%   points possibly being the same, a condition often resulting from
%   boundary-following functions, such as bwboundaries.
%
%   Copyright 2002-2020 Gatesmark
%
%   This function, and other functions in the DIPUM Toolbox, are based
%   on the theoretical and practical foundations established in the
%   book Digital Image Processing Using MATLAB, 3rd ed., Gatesmark
%   Press, 2020.
%
%   Book website: http://www.imageprocessingplace.com
%   License: https://github.com/dipum/dipum-toolbox/blob/master/LICENSE.txt

% Preliminaries.
% Make sure coordinates are column vectors.
x = x(:);
y = y(:);

% If the first and last points are the same, delete the last point.
% The point will be restored later.
restore = false;
if x(1) == x(end) && y(1) == y(end)
   x = x(1:end-1);
   y = y(1:end-1);
   restore = true;
end
% Check for duplicate points.
if length([x y]) ~= length(unique([x y],'rows'))
   error('No duplicate points except first and last are allowed.')
end

% The topmost, leftmost point in the sequence is always a convex
% vertex.
x0 = x;
y0 = y;
cx = find(x0 == min(x0));
cy = find(y0 == min(y0(cx)));
x1 = x0(cx(1));
y1 = y0(cy(1));
% Scroll data so that the first point in the sequence is (x1, y1),
% the guaranteed convex point.
I = find(x0 == x1 & y0 == y1);
x0 = circshift(x0, [-(I - 1), 0]);
y0 = circshift(y0, [-(I - 1), 0]);

% Form the matrix needed to check for travel direction. Only three
% points are needed: (x1, y1), the point before it, and the point
% after it.
A = [x0(end) y0(end) 1; x0(1) y0(1) 1; x0(2) y0(2) 1];
dir = 'cw';
if det(A) > 0
   dir = 'ccw';
end

% Prepare outputs.
if nargin == 3
   x0 = x; % Reuse x0 and y0.
   y0 = y;
   if ~strcmp(dir,orderout)
      x0(2:end) = flipud(x0(2:end)); % Reverse order of travel.
      y0(2:end) = flipud(y0(2:end));
   end
   if restore
      x0(end + 1) = x0(1);
      y0(end + 1) = y0(1);
   end
end


function W = compute_mesh_weight(vertices,faces,type,options)

% compute_mesh_weight - compute a weight matrix
%
%   W = compute_mesh_weight(vertices,faces,type,options);
%
%   W is sparse weight matrix and W(i,j)=0 is vertex i and vertex j are not
%   connected in the mesh.
%
%   todo:
%       1. validate spring weights
%       2. add Mean_curvature weights
%   type is either
%       'combinatorial': W(i,j)=1 is vertex i is conntected to vertex j.
%       'distance': W(i,j) = 1/d_ij^2 where d_ij is distance between vertex
%           i and j.
%       'spring': W(i,j) = 1/d_ij where d_ij is distance between vertex
%           i and j.
%       'conformal' or 'dcp': W(i,j) = cot(alpha_ij)+cot(beta_ij) where alpha_ij and
%           beta_ij are the adjacent angle to edge (i,j). Refer to Skeleton
%           Extraction by Mesh Extraction_08, and Intrinsic Parameterizations of Surface Meshes_02.
%       'Laplace-Beltrami': W(i,j) = (cot(alpha_ij)+cot(beta_ij))/2 where alpha_ij and
%           beta_ij are the adjacent angle to edge (i,j). Refer to Computing discrete minimal surfaces
%           andtheir conjugates_93, Lemma 2 of On the convergence of metric and geometric properties of
%           polyhedral surfaces_06, and Characterizing Shape Using Conformal Factors_08, and ,
%       'Mean_curvature',W(i,j) = (1/area_i)*(cot(alpha_ij)+cot(beta_ij))/2 where alpha_ij and
%           beta_ij are the adjacent angle to edge (i,j), area_i is the
%           area of vertex i's Voroni vicinity. Refer to ??
%       'mvc': W(i,j) = [tan(/_kij/2)+tan(/_jil/2)]/d_ij where /_kij and /_jil
%           are angles at i
%
%   If options.ring is offered, the computation of it can be avoided.
%
%   Add spring and mvc weight (JJCAO, 2009)
%   Add options.ring (JJCAO, 2009)
%   Copyright (c) 2007 Gabriel Peyre

options.null = 0;
[vertices,faces] = check_face_vertex(vertices,faces);

n = max(max(faces));

if isfield(options, 'verb')
    verb = options.verb;
else
    verb = n>5000;
end

if nargin<3
    type = 'dcp';
end

switch lower(type)
    case 'combinatorial'
        W = triangulation2adjacency(faces);
    case 'distance'
        W = my_euclidean_distance_2(triangulation2adjacency(faces),vertices);
        W(W>0) = 1./W(W>0);
    case 'spring'
        W = sqrt(my_euclidean_distance(triangulation2adjacency(faces),vertices));
        W(W>0) = 1./W(W>0);
    case {'conformal','dcp'} % conformal laplacian
        W = compute_mesh_weight_dcp(vertices, faces, verb);
    case 'laplace-beltrami' %
        W = compute_mesh_weight_dcp(vertices, faces, verb)*0.5;
    case 'mvc'% mvc laplacian
        if isfield(options, 'rings')
            rings = options.rings;
        else
            rings = compute_vertex_face_ring(faces);
        end
        W = sparse(n,n);
        for i = 1:n
            if verb
                progressbar(i,n);
            end
            for b = rings{i}
                % b is a face adjacent to a
                bf = faces(:,b);
                % compute complementary vertices
                if bf(1)==i
                    v = bf(2:3);
                elseif bf(2)==i
                    v = bf([1 3]);
                elseif bf(3)==i
                    v = bf(1:2);
                else
                    error('Problem in face ring.');
                end
                j = v(1); k = v(2);
                vi = vertices(:,i);
                vj = vertices(:,j);
                vk = vertices(:,k);
                % angles
                alpha = myangle(vi-vk,vi-vj);
                % add weight
                W(i,j) = W(i,j) + tan( 0.5*alpha )/sqrt(sum((vi-vj).^2));
                W(i,k) = W(i,k) + tan( 0.5*alpha )/sqrt(sum((vi-vk).^2));
            end
        end
    otherwise
        error('Unknown type.')
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function W = compute_mesh_weight_dcp(verts, faces, verb)
n = size(verts,2);
W = sparse(n,n);
% new implementation
for i = 1:size(faces,2)
    c = faces(:,i);

    v1 = verts(:,c(1));
    v2 = verts(:,c(2));
    v3 = verts(:,c(3));

    cot1 = dot(v2-v1,v3-v1)/norm(cross(v2-v1,v3-v1));
    cot2 = dot(v3-v2,v1-v2)/norm(cross(v3-v2,v1-v2));
    cot3 = dot(v1-v3,v2-v3)/norm(cross(v1-v3,v2-v3));

    W(c(2),c(3)) = W(c(2),c(3)) + cot1;
    W(c(3),c(2)) = W(c(3),c(2)) + cot1;
    W(c(3),c(1)) = W(c(3),c(1)) + cot2;
    W(c(1),c(3)) = W(c(1),c(3)) + cot2;
    W(c(1),c(2)) = W(c(1),c(2)) + cot3;
    W(c(2),c(1)) = W(c(2),c(1)) + cot3;
end

%%
% old implementation
% for i = 1:n
%     if verb
%         progressbar(i,n);
%     end
%     for b = rings{i}
%         % b is a face adjacent to a
%         bf = faces(:,b);
%         % compute complementary vertices
%         if bf(1)==i
%             v = bf(2:3);
%         elseif bf(2)==i
%             v = bf([1 3]);
%         elseif bf(3)==i
%             v = bf(1:2);
%         else
%             error('Problem in face ring.');
%         end
%         j = v(1); k = v(2);
%         vi = verts(:,i);
%         vj = verts(:,j);
%         vk = verts(:,k);
%
%         u = vk-vi; v = vk-vj;
%         W(i,j) = W(i,j) + dot(u,v)/norm(cross(u,v));
%         u = vj-vi; v = vj-vk;
%         W(i,k) = W(i,k) + dot(u,v)/norm(cross(u,v));
%     end
% end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function beta = myangle(u,v)
du = sqrt( sum(u.^2) );
dv = sqrt( sum(v.^2) );
du = max(du,eps); dv = max(dv,eps);
beta = acos( sum(u.*v) / (du*dv) );

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function W = my_euclidean_distance_2(A,vertex)
% square euclidean distance
if size(vertex,1)<size(vertex,2)
    vertex = vertex';
end

[i,j,s] = find(sparse(A));
d = sum( (vertex(i,:) - vertex(j,:)).^2, 2);
W = sparse(i,j,d);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function W = my_euclidean_distance(A,vertex)
% euclidean distance
if size(vertex,1)<size(vertex,2)
    vertex = vertex';
end

[i,j,s] = find(sparse(A));
d = sum( (vertex(i,:) - vertex(j,:)).^2, 2).^0.5;
W = sparse(i,j,d);


function x = r8sto_yw_sl ( n, b, x )

%*****************************************************************************80
%
%% R8STO_YW_SL solves the Yule-Walker equations for a R8STO matrix.
%
%  Discussion:
%
%    The R8STO storage format is used for a symmetric Toeplitz matrix.
%    It stores the N elements of the first row.
%
%    The matrix is also required to be positive definite.
%
%    This implementation of the algorithm assumes that the diagonal element
%    is 1.
%
%    The real symmetric Toeplitz matrix can be described by N numbers, which,
%    for convenience, we will label B(0:N-1).  We assume there is one more
%    number, B(N).  If we let A be the symmetric Toeplitz matrix whose first
%    row is B(0:N-1), then the Yule-Walker equations are:
%
%      A * X = -B(1:N)
%
%  Example:
%
%    To solve
%
%     1.0 0.5 0.2    x1   0.5
%     0.5 1.0 0.5 *  x2 = 0.2
%     0.2 0.5 1.0    x3   0.1
%
%    we input:
%
%      N = 3
%      B(1:3) = (/ 0.5, 0.2, 0.1 /)
%
%    with output:
%
%      X(1:3) = (/ -75, 12, -5 /) / 140
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 March 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Gene Golub and Charles Van Loan,
%    Section 4.7.2, "Solving the Yule-Walker Equations",
%    Matrix Computations,
%    Third Edition,
%    Johns Hopkins, 1996.
%
%  Parameters:
%
%    Input, integer N, the order of the system.
%
%    Input, real B(N), defines the linear system.  The first entry of A is
%    a 1, followed by B(1) through B(N-1).  The right hand side of the
%    system is -B(1:N).
%
%    Output, real X(N), the solution of the linear system.
%
  x(1) = -b(1);
  beta = 1.0E+00;
  alpha = -b(1);

  for i = 1 : n-1
    beta = ( 1.0E+00 - alpha * alpha ) * beta;
    alpha = - ( b(i+1) + b(i:-1:1) * x(1:i)' ) / beta;
    x(1:i) = x(1:i) + alpha * x(i:-1:1);
    x(i+1) = alpha;
  end

  return
end


function kpu_sparse_test ( )

%*****************************************************************************80
%
%% KPU_SPARSE_TEST uses the KPU function to build a sparse grid.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 April 2012
%
%  Author:
%
%    Original MATLAB version by Florian Heiss, Viktor Winschel.
%    This MATLAB version by John Burkardt.
%
%  Local parameters:
%
%    Local, integer D, the spatial dimension.
%
%    Local, integer MAXK, the maximum level to check.
%
  d = 10;
  maxk = 4;
  func = 'prod( exp(-(x/2).^2/2)/2/sqrt(2*pi), 2)';
  trueval = fu_integral ( d );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'KPU_SPARSE_TEST:\n' );
  fprintf ( 1, '  KPU sparse grid:\n' );
  fprintf ( 1, '  Sparse nested, unweighted quadrature over [0,1].\n' );
  fprintf ( 1, '  Exact integral is %g\n', trueval );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   D  Level   Nodes    SG error    MC error\n' );
  fprintf ( 1, '\n' );

  for k = 2 : maxk
%
%  Compute sparse grid estimate.
%
    [ x, w ] = nwspgr ( 'kpu', d, k );
    fx = eval ( func );
    SGappr = w' * fx;
    SGerror = sqrt((SGappr - trueval).^2)/trueval;
%
%  Average 1000 Monte Carlo estimates.
%
    numnodes = length ( w );
    sim = zeros(1000,1);
    for r = 1 : 1000
      x = rand ( numnodes, d );
      fx = eval ( func );
      sim(r) = mean ( fx );
    end
    simerror = sqrt ( mean( ( sim - trueval).^2) ) / trueval;

    fprintf( '  %2d     %2d  %6d  %10.5g  %10.5g\n', ...
      d, k, numnodes, SGerror, simerror )

  end

  return
end


%This Matlab script can be used to reproduce Figure 1.18 in the monograph:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.01 (Last edited: 2019-04-17)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

%Empty workspace and close figures
close all;
clear;

%Define the SNR
SNR = 1;

%Define betabar (strength of inter-cell interference)
betabar = 1e-1;

%Define the range of number of UEs
K = 1:20;

%Define range of antenna-UE ratios
c = [1 2 4 8];

%Extract the maximal number of UEs and BS antennas
Kmax = max(K);
Mmax = Kmax*max(c);

%Select number of Monte Carlo realizations for the line-of-sight (LoS)
%angles and of the non-line-of-sight (NLoS) Rayleigh fading
numberOfRealizations = 10000;

%Generate NLoS channels using uncorrelated Rayleigh fading
H_NLoS_desired = sqrt(1/2)*(randn(Mmax,Kmax,numberOfRealizations)+1i*randn(Mmax,Kmax,numberOfRealizations));
H_NLoS_interfering = sqrt(betabar/2)*(randn(Mmax,Kmax,numberOfRealizations)+1i*randn(Mmax,Kmax,numberOfRealizations));

%Preallocate matrices for storing the simulation results
SE_MMSE_NLoS_montecarlo = zeros(length(K),length(c));
SE_MMSE_NLoS_nonlinear = zeros(length(K),length(c));

%% Go through all Monte Carlo realizations
for n = 1:numberOfRealizations

    %Output simulation progress
    disp([num2str(n) ' realizations out of ' num2str(numberOfRealizations)]);

    %Go through the range of number of UEs
    for kindex = 1:length(K)

        %Go through the range of antenna-UE ratios
        for cindex = 1:length(c)

            %Compute the number of antennas
            M = K(kindex)*c(cindex);

            %Compute the SE with non-linear processing under NLoS propagation
            %for one realization of the Rayleigh fading. We use the classic
            %log-det formula for the uplink sum SE, when treating the
            %inter-cell interference as colored noise
            SE_MMSE_NLoS_nonlinear(kindex,cindex) = SE_MMSE_NLoS_nonlinear(kindex,cindex) + real(log2(det(eye(M) + SNR* H_NLoS_desired(1:M,1:K(kindex),n)*H_NLoS_desired(1:M,1:K(kindex),n)' +  SNR*H_NLoS_interfering(1:M,1:K(kindex),n)*H_NLoS_interfering(1:M,1:K(kindex),n)' )) - log2(det(eye(M)+SNR*H_NLoS_interfering(1:M,1:K(kindex),n)*H_NLoS_interfering(1:M,1:K(kindex),n)')))/numberOfRealizations;

            %Compute the SE with M-MMSE under NLoS propagation for one
            %realization of the Rayleigh fading

            %Compute the M-MMSE combining vectors
            MMMSEfilter = ( SNR* H_NLoS_desired(1:M,1:K(kindex),n)*H_NLoS_desired(1:M,1:K(kindex),n)' + SNR* H_NLoS_interfering(1:M,1:K(kindex),n)*H_NLoS_interfering(1:M,1:K(kindex),n)' + eye(M) ) \ (SNR*H_NLoS_desired(1:M,1:K(kindex),n));

            %Compute the intra-cell channel powers after M-MMSE combining
            channelgainsIntracell = abs(MMMSEfilter'*H_NLoS_desired(1:M,1:K(kindex),n)).^2;

            %Extract the desired signal power for each UE
            signalpowers = diag(channelgainsIntracell);

            %Extract and compute interference powers for each UE
            interferencepowers = sum(channelgainsIntracell,2) - signalpowers + sum(abs(MMMSEfilter'*H_NLoS_interfering(1:M,1:K(kindex),n)).^2,2);

            %Compute the effective 1/SNR after noise amplification
            scalednoisepower = (1/SNR)*sum(abs(MMMSEfilter').^2,2);

            %Compute the uplink SE with M-MMSE combining
            SE_MMSE_NLoS_montecarlo(kindex,cindex) = SE_MMSE_NLoS_montecarlo(kindex,cindex) + sum(log2(1 + signalpowers./(interferencepowers+scalednoisepower)))/numberOfRealizations;

        end

    end

end

%% Plot the simulation results
figure(1);
hold on; box on;

plot(K,SE_MMSE_NLoS_montecarlo(:,4)./SE_MMSE_NLoS_nonlinear(:,4),'r-','LineWidth',1);
plot(K,SE_MMSE_NLoS_montecarlo(:,3)./SE_MMSE_NLoS_nonlinear(:,3),'k-.','LineWidth',1);
plot(K,SE_MMSE_NLoS_montecarlo(:,2)./SE_MMSE_NLoS_nonlinear(:,2),'b--','LineWidth',1);
plot(K,SE_MMSE_NLoS_montecarlo(:,1)./SE_MMSE_NLoS_nonlinear(:,1),'k:','LineWidth',1);

xlabel('Number of UEs (K)');
ylabel('Fraction of non-linear performance');

legend('M/K=8','M/K=4','M/K=2','M/K=1','Location','SouthWest');
ylim([0.5 1]);


function value = r8_epsilon ( )

%*****************************************************************************80
%
%% R8_EPSILON returns the R8 roundoff unit.
%
%  Discussion:
%
%    The roundoff unit is a number R which is a power of 2 with the
%    property that, to the precision of the computer's arithmetic,
%      1 < 1 + R
%    but
%      1 = ( 1 + R / 2 )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 August 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real VALUE, the roundoff unit.
%
  value = eps;

  return
end


function [I J K]=sphere_index(r,Ic,Jc,Kc)

% function [I J K]=sphere_index(r)
% ------------------------------------------------------------------------
% This function generates the indices 'I',  'J' and 'K' of voxel centers
% found within a sphere of radius 'r' (in voxels). The indices are not
% 'real' indices but are 'centered around zero'. They can be used to
% generate the indices of voxels found inside a sphere around point 'n' by
% adding In,Jn,Kn to I,J and K.
%
%
% Kevin Mattheus Moerman
% kevinmoerman@hotmail.com
% 07/08/2008
% ------------------------------------------------------------------------

if nargin == 1
    Ic=0; Jc=0; Kc=0;
end

%Set-up mesh calculate radius
[X,Y,Z] = meshgrid((-round(r+1)):1:(round(r+1)));
X=X+Jc; Y=Y+Ic; Z=Z+Kc;
radius = hypot(hypot(X,Y),Z);
[I,J,K]=ind2sub(size(X),find(radius<=r));
IJK_middle=round(size(X)/2);
I=I-IJK_middle(1);
J=J-IJK_middle(2);
K=K-IJK_middle(3);

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


function value = p16_f ( dim_num, point_num, x )

%*****************************************************************************80
%
%% P16_F evaluates the integrand for problem 16.
%
%  Discussion:
%
%    The integrand can be regarded as the L1 norm of X - Z.
%
%    It would be nice to allow the use to specify several
%    base points Z, to make the function more jagged more places%
%
%  Dimension:
%
%    DIM_NUM arbitrary.
%
%  Region:
%
%    0 <= X(1:DIM_NUM) <= 1
%
%  Integral Parameters:
%
%    The integrand can be regarded as the L1 norm of X - Z.
%
%    There is a basis point Z associated with the integrand.
%    Z(1:DIM_NUM) defaults to ( 0.5, 0.5, ..., 0.5 ).
%    The user can set, get, or randomize this value by calling
%    P16_R8VEC.
%
%  Integrand:
%
%    sum ( abs ( x(1:dim_num) - z(1:dim_num) ) )
%
%  Exact Integral:
%
%    The integral is separable into
%
%       Int ( A(1) <= X(1) <= B(1) ) abs ( X(1) - Z(1) )
%     * Product ( B(1:N)-A(1:N), skip index 1 )
%     + Int ( A(2) <= X(2) <= B(2) ) abs ( X(2) - Z(2) )
%     * Product ( B(1:N)-A(1:N), skip index 2 )
%     ...
%     + Int ( A(N) <= X(N) <= B(N) ) abs ( X(N) - Z(N) )
%     * Product ( B(1:N)-A(1:N), skip index N )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 June 2007
%
%  Author:
%
%    John Burkardt
%
%  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.
%
  z = [];
  z = p16_r8vec ( 'G', 'Z', dim_num, z );

  value(1:point_num) = 0.0;

  for point = 1 : point_num

    value(point) = sum ( abs ( x(1:dim_num,point) - z(1:dim_num)' ) );

  end

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

  return
end


function [L,E] = facet_laplacian(V,F)
  % FACET_LAPLACIAN Builds an "edge-based" Laplacian L, which is an #E by #V
  % rectangular matrix which maps scalar functions living at vertices to
  % Laplacian values living at edges. For tets, edges are now facets.
  %
  % [L,E] = facet_laplacian(V,F)
  %
  % Inputs:
  %   V  #V by dim list of vertex positions
  %   F  #F by element-size list of triangle indices
  % Outputs:
  %   L  #E by #V edge-based Laplacian
  %   E  #E by 2 list of edges
  %
  % Examples:
  %   % load some non-convex shape (V,F)
  %   [L,E] = facet_laplacian(V,F);
  %   % find boundary edges
  %   B = is_boundary_edge(E,F);
  %   % Construct mass matrix
  %   [M,mE] = crouzeix_raviart_massmatrix(V,F);
  %   % Be sure same edges are being used.
  %   assert(all(E(:)==mE(:)));
  %   % "Kill" boundary edges
  %   L(B,:) = 0;
  %   % Linear functions are now in the spectrum
  %   [~,~,U] = svd(full(L));
  %   tsurf(F,[V(:,1:2) U(:,end-1)])
  %
  %   % mesh in (V,F)
  %   [Le] = facet_laplacian(V,F);
  %   [Lcr,E,EMAP] = crouzeix_raviart_cotmatrix(V,F);
  %   V2E = sparse(E(:),repmat(1:size(E,1),1,size(E,2))',1,size(V,1),size(E,1))';
  %   V2E = bsxfun(@rdivide,V2E,sum(V2E,2));
  %   LcrV2E = Lcr*V2E;
  %   max(max(abs(LcrV2E--2*Le)))
  %
  %
  % See also: crouzeix_raviart_massmatrix, is_boundary_edge
  %

  %% check for non-manifold edges
  %S = statistics(V,F,'Fast',true);
  %if S.num_nonmanifold_edges > 0
  %  error(sprintf('There are %d non-manifold edges',S.num_nonmanifold_edges));
  %end

  switch size(F,2)
  case 3
    % number of vertices
    n = size(V,1);
    % number of faces
    m = size(F,1);
    % Compute cotangents
    C = cotangent(V,F);
    % Map each face-edge to a unique edge
    F2E = reshape(1:3*m,m,3);
    % Assemble entries
    %             R S S R S S R S S
    LI = [ F2E(:,[1 1 1 2 2 2 3 3 3]) ];
    LJ = [   F(:,[1 2 3 2 3 1 3 1 2]) ];
    LV = [ ...
      C(:,2)+C(:,3), -C(:,3), -C(:,2) ...
      C(:,3)+C(:,1), -C(:,1), -C(:,3) ...
      C(:,1)+C(:,2), -C(:,2), -C(:,1)];

    %warning('only spokes');
    %LI = LI(:,[2 3 5 6 8 9]);
    %LJ = LJ(:,[2 3 5 6 8 9]);
    %LV = LV(:,[2 3 5 6 8 9]);
    %warning('only rims');
    %LI = LI(:,[1 4 7]);
    %LJ = LJ(:,[1 4 7]);
    %LV = LV(:,[1 4 7]);

    assert(all(size(LI)==size(LJ)));
    assert(all(size(LI)==size(LV)));
    % Throw contribution at each edge
    L = sparse(LI,LJ,LV,3*m,n);

    allE = [F(:,[2 3]);F(:,[3 1]);F(:,[1 2])];
    % Map duplicate edges to first instance
    [E,~,EMAP] = unique(sort(allE,2),'rows');

    L = sparse(EMAP,F2E(:),1,size(E,1),3*m) * L;

    % Q: What's going on for boundary edges?
    % A: Interior edges are integrating around butterly. Boundary edges are only
    % integrating around half what would be their butterfly. They "should" also
    % integrate along themselves to enclose an area. Since they don't they
    % amount to computing minus the normal derivative.
  case 4
    [Lcr,E] = crouzeix_raviart_cotmatrix(V,F);
    A = sparse(E(:),repmat(1:size(E,1),1,3)',1,size(V,1),size(E,1))';
    Df = diag(sparse(sum(A,2)));
    % Legacy factor of 2 to match triangle version
    L = 0.5*3*Lcr*(Df\A);
    % Lv == 0.5*A'*Lf;
  end

end


function Problem2_26
%Problem 2.26 Ternary condensation inside a vertical tube.
% Uses the function BVP4C to numerically solve the boundary-value problem.
% BVPINIT is used to form an initial guess for the solution on
% a mesh of ten equally spaced points. A guess for unknown parameters
% (the two fluxes) is the last argument of BVPINIT.
% BVP4C returns the solution as the structure 'sol'. The computed fluxes are
% returned in the field sol.parameters.
% The calculated concentration profiles are plotted.

% Calculate the mass-transfer coefficients
Re = 9574;
density = 0.882;
viscosity = 0.00001602;
diameter = 0.0254;
% MS diffusion coefficients
D = [1.6*10^-5 1.444*10^-5 3.873*10^-5];
% Calculate Schmidt numbers
Sc(1) = viscosity/(density*D(1));
Sc(2) = viscosity/(density*D(2));
Sc(3) = viscosity/(density*D(3));
% Calculate Sherwood numbers
Sh(1) = 0.023*Re^0.83*Sc(1)^0.44;
Sh(2) = 0.023*Re^0.83*Sc(2)^0.44;
Sh(3) = 0.023*Re^0.83*Sc(3)^0.44;
% Molecular weight of pure components
M = [60.1 18 28];
y1 = [0.1123
      0.4246
      0.4631];
Mav1 = M*y1;
y2 = [0.1457
      0.1640
      0.6903];
Mav2 = M*y2;
Mav = (Mav1+Mav2)/2;
% Calculate total molar density
c = density/Mav;
F12 = Sh(1)*c*D(1)*1000/diameter;
F13 = Sh(2)*c*D(2)*1000/diameter;
F23 = Sh(3)*c*D(3)*1000/diameter;
solinit = bvpinit(linspace(0,1,10),[1, 1], [1, 1]);
options = bvpset('stats','on');
sol = bvp4c(@prob226ode,@prob226bc,solinit,options,F12,F13,F23);
x = sol.x;
y = sol.y;
y(3,:) = 1-y(1,:) -y(2,:);
fprintf('\n');
fprintf('Computed Mass-Transfer Coefficients:');
fprintf('\n');
fprintf('F12, mole/m2-s = %7.4f.\n',F12);
fprintf('\n');
fprintf('F13, mole/m2-s = %7.4f.\n',F13);
fprintf('\n');
fprintf('F23, mole/m2-s = %7.4f.\n',F23);
fprintf('\n');
fprintf(' Computed N1 in mole/m2-s =%7.4f.\n',sol.parameters(1))
fprintf('\n')
fprintf(' Computed N2 in mole/m2-s=%7.4f.\n',sol.parameters(2))
fprintf('\n')
clf reset
plot(x,y(1,:),'k-*',x,y(2,:),'k:+',x,y(3,:),'k-.o')
axis([0 1 0 0.7])
title('Concentration profiles in Problem 2.26')
xlabel('Distance along the diffusion path, cm.')
ylabel('Mole fraction')
legend('Isopropanol', 'Water', 'Nitrogen')
shg

% --------------------------------------------------------------------------

function dydx = prob226ode(x,y,N,F12,F13,F23)
N3 = 0;

%F12 = 0.933;
%F13 = 0.880;
%F23 = 1.530;
dydx = [ (y(1)*N(2)-y(2)*N(1))/F12+(y(1)*N3-(1-y(1)-y(2))*N(1))/F13
         (y(2)*N(1)-y(1)*N(2))/F12+(y(2)*N3-(1-y(1)-y(2))*N(2))/F23 ];

% --------------------------------------------------------------------------

function res = prob226bc(ya,yb,N,F12,F13,F23)
res = [ya(2)-0.4246
       yb(2) - 0.1640
       ya(1) - 0.1123
       yb(1) - 0.1457 ];

% --------------------------------------------------------------------------


function psnr= PSNR(X,Y)
%Calculates the Peak-to-peak Signal to Noise Ratio of two images X and Y
[M,N]=size(X);
m=double(0);
X=cast(X,'double');
Y=cast(Y,'double');
for i=1:M
    for j=1:N
        m=m+((X(i,j)-Y(i,j))^2);
    end
end
m=m/(M*N);
psnr=10*log10(255*255/m);


function out = DN_Spread(y,spreadMeasure)
% DN_Spread     Measure of spread of the input time series.
%
% Returns the spread of the raw data vector, as the standard deviation,
% inter-quartile range, mean absolute deviation, or median absolute deviation.
%
%---INPUTS:
% y, the input data vector
%
% spreadMeasure, the spead measure:
%               (i) 'std': standard deviation
%               (ii) 'iqr': interquartile range
%               (iii) 'mad': mean absolute deviation
%               (iv) 'mead': median absolute deviation

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function 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/>.
% ------------------------------------------------------------------------------

% ------------------------------------------------------------------------------
% Check Inputs
% ------------------------------------------------------------------------------
if nargin < 2 || isempty(spreadMeasure)
    spreadMeasure = 'std'; % return std by default
end

% ------------------------------------------------------------------------------
% Evaluate the spread measure
% ------------------------------------------------------------------------------
switch spreadMeasure
	case 'std'
        % Standard deviation
		out = std(y);

	case 'iqr'
        % Interquartile range
		out = iqr(y);

	case 'mad'
        % Mean absolute deviation
		out = mad(y,0);

    case 'mead'
        % Median absolute deviation
        out = mad(y,1);

    otherwise
        error('Unknown spread measure ''%s''',spreadMeasure)
end

end


function [fAlpha, fDmax] = calc_KStestGR(mCatalog, fBinning, fBValue, fMc)
% [mAlphaVal] = calc_KStestGR(mCatalog, fBinning, fBValue)
% ------------------------------------------------------------------------
% Calculate Koglomorov-Smirnov-test with constant b-value
% Reference for equations to calculate the statistics nD:
% Y.Y. Kagan, Accuracy of modern global earthquake catalogs, Physics of the Earth and Planetary Interiors
% 4179, 1-37, 2002
%
% Incoming variables:
% mCatalog : Earthquake catalog
% fBinning : Binning interval
% fBValue  : Fix b-value
%
% Outgoing variables:
% mAlphaVal(:,1) : Significance level Alpha according to ascending magnitudes
% mAlphaVal(:,2) : Ascending magnitudes
%
% Author: J. Woessner
% last update: 25.03.03

% Initialize
mAlphaVal = [];
vD = [];

% Select data above Mc
vSel = (mCatalog(:,6) >= fMc);
mCatalog = mCatalog(vSel,:);

% Set fix values
fMaxMag = ceil(10 * max(mCatalog(:,6))) / 10;
fMinMag = (round(min(mCatalog(:,6)*10)))/10;
vMag = fMinMag:0.1:fMaxMag;

% Using Bath's law : see Reference Console
% Beta-value
fBeta = log(10)*fBValue;

% Probability density function
% vPdf = fBeta*exp(-fBeta*(vMag-fMc))
%
% % Cumulative density function
% vCdf = cumsum(vPdf);
% %vCdf = vCdf/max(vCdf);
% vCdf = [vCdf; vMag];
% vCdf = vCdf';

% CDF theoretically
vCdf = (1-exp(-fBeta*(vMag-fMc)))./(1-exp(-fBeta*(fMaxMag-2-fMc)));
vCdf = [vCdf; vMag];
vCdf = vCdf';

% Use Kagan 2002 approach
% Sort catalog with ascending magnitude
[mCatSorted,vIndex] = sort(mCatalog(:,6));
mCat = mCatalog(vIndex(:,1),:);

% Calulate statistic fD = max_(1=<k=<N)((k/N-F(M_k)
nEvents = length(mCat(:,1));
vkEvent = 1:length(mCat(:,1));
vkEvent = vkEvent/nEvents;
vkEvent = vkEvent';
for nE = 1:nEvents
    % Find probability of theoretical distribution for event k with magnitude Mk
    try
        vSel = (vCdf(:,2) == (round(mCat(nE,6)*10))/10);
        fD = max(nE/nEvents-vCdf(vSel,1));
        vD = [vD; fD];
    catch
        fD = nan;
        vD = [vD; fD];
    end
end
fDmax = max(vD);
fAlpha = exp(-2*nEvents*(fDmax+1/(6*nEvents))^2);
% vAlpha = exp(-2*nEvents.*(vD+1/(6*nEvents)).^2)
% disp('v')


function Q = mg_ns_smooth(A,sweeps,smooth,stype)
%mg_ns_smooth   smoothers for GMG iteration (Navier-Stokes)
%   Q = mg_ns_smooth(A,sweeps,smooth,stype)
%   input
%          A         coefficient matrix
%          sweeps    number of directions for multidirectional Gauss-Seidel
%          smooth    smoother (1, Jacobi, 2, Gauss-Seidel, 3 ILU)
%          stype     type of smoother (1 point, 2 line)
%   output
%          Q         structure, smoothing operators in factored form
%
%   IFISS function: HCE; 18 March 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage

N=size(A,1);
n = sqrt(size(A,1));
if stype==2
   % line Gauss-Seidel
   Q1 = tril(A,1);
   [L1,U1] = lu(Q1);
   if sweeps>=2
      Q2 = diag(diag(A,0)) + diag(diag(A,-n),-n) + diag(diag(A,n),n) ...
         + diag(diag(A,-n-1),-n-1) + diag( diag(A,-1),-1)        ...
         + diag(diag(A,n-1),n-1);
      [L2,U2] = lu(Q2);
      if sweeps>=3
         Q3 = triu(A,-1);
         [L3,U3] = lu(Q3);
      if sweeps==4
         Q4 = diag(diag(A,0)) + diag(diag(A,-n),-n) + diag(diag(A,n),n) ...
            + diag(diag(A,n+1),n+1) + diag( diag(A,1),1)            ...
            + diag(diag(A,-n+1),-n+1);
         [L4,U4] = lu(Q4);
         else
            L4=sparse(N,N); U4=L4;
         end
      else
         L3=sparse(N,N); U3=L3; L4=L3; U4=L3;
      end
   else
      L2=sparse(N,N); U2=L2; L3=L2; U3=L2; L4=L2; U4=L2;
   end
else
   % point smoothers
   if smooth==3,
      % ILU
      [L1,U1]=ilu0(A);
      L2=sparse(size(L1)); U2=L2; L3=L2; U3=L2; L4=L2; U4=L2;
   elseif smooth==2,
      % point Gauss-Seidel
      Q1 = tril(A,0); [L1,U1] = lu(Q1);
      L2=sparse(size(L1)); U2=L2; L3=L2; U3=L2; L4=L2; U4=L2;
   else
      % point damped Jacobi
      omega=8/9; % relaxation factor for damped Jacobi
      Q1 = (1/omega)*spdiags(diag(A),0,n*n,n*n);[L1,U1] = lu(Q1);
      L2=sparse(size(L1)); U2=L2; L3=L2; U3=L2; L4=L2; U4=L2;
   end
end
Q = struct('L1',L1,'L2',L2,'L3',L3,'L4',L4, ...
           'U1',U1,'U2',U2,'U3',U3,'U4',U4);


function [ortirfmatrix]=irfsim_new(beta,D,n,m,p,k,horizon)

% [irfmatrix ortirfmatrix]=bear.irfsim(beta,D,n,m,p,k,horizon)
% computes IRF matrices and orthogonalised IRF matrices
% inputs:  - vector 'beta': vectorised form of VAR coefficients (defined in 1.1.12)
%          - matrix 'D': structural matrix for the OLS model (defined in 2.3.3)
%          - integer 'n': number of endogenous variables in the BVAR model (defined p 7 of technical guide)
%          - integer 'm': number of exogenous variables in the BVAR model (defined p 7 of technical guide)
%          - integer 'p': number of lags included in the model (defined p 7 of technical guide)
%          - integer 'k': number of coefficients to estimate for each equation in the BVAR model (defined p 7 of technical guide)
%          - integer 'horizon': number of IRF periods
% outputs: - matrix 'irfmatrix': record of the series of irf matrices
%          - matrix 'ortirfmatrix': record of the series of orthogonalised irf matrices

% first reshape beta to obtain B
B=reshape(beta,k,n);

% deal with shocks in turn
for ii=1:n

% create a matrix of zeros of dimension p*n
Y=zeros(p,n);
% set the value of the last row, column i, equal to 1
Y(p,ii)=1;

   % repeat the algorithm from period T+1 to period T+h
   for jj=1:horizon-1

   % step 1
   % use the function lagx to obtain the matrix temp, containing the endogenous regressors
   temp=bear.lagx(Y,p-1);

   % step 2
   % define the vector X
   X=[temp(end,:) zeros(1,m)];

   % step 3
   % obtain the predicted value for T+jj
   yp=X*B;

   % step 4
   % concatenate yp at the top of Y
   Y=[Y;yp];

   % repeat until values are obtained for T+h
   end

% consider 'Y' and trim the (p-1) initial periods: what remains is the series of IRFs for period T to period T+h-1
Y=Y(p:end,:);

% record the results in the matrix irfmatrix

   % loop over periods
   for jj=1:horizon

      % loop over variables
      for kk=1:n
      irfmatrix(kk,ii,jj)=Y(jj,kk);
      end

   end

% conduct the same process with shocks in other variables
end

% obtain now orthogonalised IRFs
% loop over periods
for ii=1:horizon
ortirfmatrix(:,:,ii)=irfmatrix(:,:,ii)*D;
end


function result = ball_f1_nd ( func, n, xc, r )

%*****************************************************************************80
%
%% BALL_F1_ND approximates an integral inside a ball in ND.
%
%  Integration region:
%
%    Points X(1:N) such that:
%
%      Sum ( X(1:N) - XC(1:N) )**2 <= R**2.
%
%  Discussion:
%
%    An (N+1)*2**N point 5-th degree formula is used, Stroud number SN:5-6.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 May 2004
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Arthur H Stroud,
%    Approximate Calculation of Multiple Integrals,
%    Prentice Hall, 1971.
%
%  Parameters:
%
%    Input, external FUNC, the name of the user supplied
%    function which evaluates F at the N-vector X, of the form
%      function value = func ( n, x )
%
%    Input, integer N, the dimension of the space.
%
%    Input, real XC(N), the center of the ball.
%
%    Input, real R, the radius of the ball.
%
%    Output, real RESULT, the approximate integral of the function.
%
  if ( r == 0.0E+00 )
    result = 0.0E+00;
    return
  end

  u2 = ( 1.0E+00 - 2.0E+00 * sqrt ( 1.0E+00 / ( n + 4 ) ) ) / ( n + 2 );
  u = sqrt ( u2 );
  x(1:n) = xc(1:n) - r * u;

  w = 1.0E+00 / ( ( n + 1 ) * 2^n );

  quad = 0.0E+00;
  ihi = 2^n;

  for i = 1 : ihi

    itemp = i - 1;

    for j = 1 : n

      u = ( xc(j) - x(j) ) / r;

      if ( mod ( itemp, 2 ) == 1 )
        x(j) = xc(j) - abs ( x(j) - xc(j) );
      else
        x(j) = xc(j) + abs ( x(j) - xc(j) );
      end

      itemp = floor ( itemp / 2 );

    end

    quad = quad + w * feval ( func, n, x );

  end

  temp = sqrt ( n + 4 );

  t = sqrt ( 2.0E+00 * ( n + 1 ) / ( n + 2 ) ) / ( n * temp );

  y = ( 1.0E+00 + 2.0E+00 / ( n * temp ) ) / ( n + 2 );
  v = sqrt ( y - t );
  u = sqrt ( y + ( n - 1 ) * t );

  khi = 2^n;

  for i = 1 : n

    x(1:n) = xc(1:n) - r * v;

    x(i) = xc(i) - r * u;

    for k = 1 : khi

      ktemp = k - 1;

      for j = 1 : n

        if ( mod ( ktemp, 2 ) == 1 )
          x(j) = xc(j) - abs ( x(j) - xc(j) );
        else
          x(j) = xc(j) + abs ( x(j) - xc(j) );
        end

        ktemp = floor ( ktemp / 2 );

      end

      quad = quad + w * feval ( func, n, x );

    end

    x(i) = xc(i) - r * v;

  end

  volume = ball_volume_nd ( n, r );
  result = quad * volume;

  return
end


function F = spm_Ncdf_jdw(x,u,v)
% Cumulative Distribution Function (CDF) for univariate Normal distributions: J.D.  Williams aproximation
% FORMAT F = spm_Ncdf_jdw(x,u,v)
%
% x - ordinates
% u - mean              [Defaults to 0]
% v - variance  (v>0)   [Defaults to 1]
% F - pdf of N(u,v) at x (Lower tail probability)
%__________________________________________________________________________
%
% spm_Ncdf implements the Cumulative Distribution Function (CDF) for
% the Normal (Gaussian) family of distributions.
%
% References:
%--------------------------------------------------------------------------
% An Approximation to the Probability Integral
% J. D. Williams
% The Annals of Mathematical Statistics, Vol. 17, No. 3. (Sep., 1946), pp.
% 363-365.
%
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_Ncdf_jdw.m 4836 2012-08-10 15:55:21Z karl $

%-Format arguments
%--------------------------------------------------------------------------
if nargin < 3, v = 1; end
if nargin < 2, u = 0; end

%-Approximate integral
%--------------------------------------------------------------------------
x    = (x - u)./sqrt(abs(v));
F    = sqrt(1 - exp(-(2/pi)*x.^2))/2;
i    = x < 0;
F(i) = -F(i);
F    = F + 1/2;


function varargout = transformPoint(varargin)
%TRANSFORMPOINT Apply an affine transform to a point or a point set.
%
%   PT2 = transformPoint(PT1, TRANSFO);
%   Returns the result of the transformation TRANSFO applied to the point
%   PT1. PT1 has the form [xp yp], and TRANSFO is either a 2-by-2, a
%   2-by-3, or a 3-by-3 matrix,
%
%   Format of TRANSFO can be one of :
%   [a b]   ,   [a b c] , or [a b c]
%   [d e]       [d e f]      [d e f]
%                            [0 0 1]
%
%   PT2 = transformPoint(PT1, TRANSFO);
%   Also works when PTA is a N-by-2 array representing point coordinates.
%   In this case, the result PT2 has the same size as PT1.
%
%   [X2, Y2] = transformPoint(X1, Y1, TRANS);
%   Also works when PX1 and PY1 are two arrays the same size. The function
%   transforms each pair (PX1, PY1), and returns the result in (X2, Y2),
%   which has the same size as (PX1 PY1).
%
%
%   See also
%     points2d, transforms2d, translation, rotation
%

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2004-04-06
% Copyright 2004-2022 INRA - TPV URPOI - BIA IMASTE

% parse input arguments
if length(varargin) == 2
    var = varargin{1};
    px = var(:,1);
    py = var(:,2);
    trans = varargin{2};
elseif length(varargin) == 3
    px = varargin{1};
    py = varargin{2};
    trans = varargin{3};
else
    error('wrong number of arguments in "transformPoint"');
end

% apply linear part of the transform
px2 = px * trans(1,1) + py * trans(1,2);
py2 = px * trans(2,1) + py * trans(2,2);

% add translation vector, if exist
if size(trans, 2) > 2
    px2 = px2 + trans(1,3);
    py2 = py2 + trans(2,3);
end

% format output arguments
if nargout < 2
    varargout{1} = [px2 py2];
elseif nargout
    varargout{1} = px2;
    varargout{2} = py2;
end


function [mu, muw]= ViscosityCO2Water(T, p, x_CO2)
% This function calculates the viscosity of the pure water, brine, and
% water-CO2, and brine-CO2 systems
% p is in Pa, T in K, x_CO2 is the mole fraction of CO2, mu in Pa.s
% Written by Ali A. Eftekhari at TU Delft
% See the license file
Mwater = 0.0180153; % kg/mol
% MCO2 = 0.04401; % kg/mol

P = p/1e6; % convert Pa to MPa
m_CO2 = DuanSun(T, p, 0); % CO2 equilibrium molality (mol CO2/kg water)
x_CO2_eq = m_CO2/(m_CO2+1/Mwater);
muw = IAPWS_IF97('mu_pT', P, T); % pure water viscosity in Pa.s
vis_ratio = 1+(-4.069e-3*(T-273.15)+0.2531)*x_CO2/x_CO2_eq;
mu = muw*vis_ratio;


function [varargout]=rigidTransformationMatrixDirect(V1,V2)

%%
%Force input to 3D
if size(V1,2)==2
    V1(:,3)=0;
end

if size(V2,2)==2
    V2(:,3)=0;
end

[Q]=kabschRotationMatrix(V1,V2);

V1_m=mean(V1,1);
V2_m=mean(V2,1);

T1=eye(4,4);
T1(1:3,end)=V2_m(:);

R=eye(4,4);
R(1:3,1:3)=Q;

T2=eye(4,4);
T2(1:3,end)=-V1_m;

T=T1*R*T2;

varargout{1}=T;
varargout{2}=R(1:3,1:3);

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


function boolVal=linTargetIsUntrackable(targetParams,PD,lambda,AbsTol,RelTol,maxIter)
%%LINTARGETISUNTRACKABLE Assuming that an asymptotic inoovation covariance
%           is available or that a target follows a particular specified
%           linear dynamic model (e.g. position, velocity, and
%           acceleration, and it could have correlation terms like a Gauss-
%           Markov model) with known process noise covariance and Cartesian
%           measurement covariance, and assuming that a probability of
%           detection is known, determine whether a target is untrackable.
%           A target is deemed untrackable if the cost function used in a
%           multiple hypothesis tracker (MHT)  is always greater than or
%           equal to  the null hypothesis than for any track hypothesis
%           when given asymptotic track accuracy parameters. If a track is
%           untrackable, even an MHT of infinite length would not be able
%           to track the target. This assumes Cartesian measurements. The
%           use of range rate, complex target amplitude, micro-Doppler and
%           other information could potentially render untrackable targets
%           trackable.
%
%INPUTS: targetParams This can be one of two types of inputs. If an
%          asymptotic innovation covariance matrix is known, then this is
%          the innovation covariance matrix. Otherwise, this is a structure
%          holding parameters for a linear system so that an asymptotic
%          innovation covariance matrix can be computed. The members of the
%          structure are:
%           H The zDim X xDim measurement matrix such that H*x+w is the
%             measurement, where x is the state and w is zero-mean Gaussian
%             noise with covariance matrix R.
%           F The xDim X xDim state transition matrix The state at
%             discrete-time k+1 is modeled as F times the state at time k
%             plus zero- mean Gaussian process noise with covariance matrix
%             Q.
%           R The zDim X zDim measurement covariance matrix.
%           Q The xDim X xDim process noise covariance matrix. This can
%             not be a singular matrix.
%       PD The optional detection probability of the target at each scan.
%          PD should not be 1.
%   lambda The false alarm density in Cartesian coordinates. For example,
%          this might be 1e-8 false alarms per unit volume in cubic meters.
%   RelTol The maximum relative error tolerance allowed when computing the
%          asymptotic predictive covariance matrix. This is a positive
%          scalar. If omitted or an empty matrix is passed, the default
%          value of 1e-13 is used. This value is only used if targetParams
%          is a structure.
%   AbsTol The absolute error tolerance allowed, a positive scalar. When
%          computing the asymptotic predictive covariance matrix. This is a
%          positive scalar. If omitted or an empty matrix is passed, the
%          default value of 1e-10 is used. This value is only used if
%          targetParams is a structure.
%  maxIter An optional integer specifying the maximum number of iterations
%          to use when cmputing the asymptotic predictive covariance
%          matrix. By default, if omitted, this is 5000. This value is only
%          used if targetParams is a structure.
%
%As discussed in Chapter 7.5 of [1], track oriented MHTs declare target
%existence based on finite sums of log-likelihood ratios. These ratios are
%typically made into dimensionless score functions as in Chapter 7.5 of [1]
%and in [2]. For the probability that a measurement z is from a particular
%target, the contribution to the score function is f(z)*PD/lambda, where
%f(z) is the likelihood of the measurement conditioned on the predicted
%state estimate. The null hypothesis receives a score of 1-PD. If the null
%hypothesis is always greater than or equal to the track association
%hypothesis then a target is untrackable.
%
%Given a Cartesian measurement model (z=H*x+w where x is the target state,
%w is measurement noise with covariance matrix R and H is the measurement
%matrix) and assuming a linear dynamic model (xpredicted=F*xPrior+v where v
%is noise with covariance matrix Q) where the state transition matrix F and
%the process noise covariance matrix Q are constant (implying a uniform
%revisit rate) as well as having a known detection probability of PD,  one
%determine an asymptotic lower bound on the estimation accuracy of a
%predicted state measurement using the RiccatiPredNoClutter function. Given
%PPred, a lower bound on the state precision covariance, the score function
%contribution for a target-originated measurement is maximum when the
%predicted measurement equals the measurement value. From Chapter 7.5.3 of
%[1], for a linear measurement model, this is
%PD/(lambda*sqrt(det(2*pi*SPred))) where SPred is the innovation covariance
%matrix, which as given in the derivation of the Kalman filter in Chapter 5
%of [3] is H*PPred*H'+R. Thus, this function determines whether this
%maximum score for a track-originated measurement is greater than the
%alternative (1-PD). If not, then the target is untrackable.
%
%The trackability bound here can be considered something of a "stealth"
%bound, because for a fixed false alarm level in a system, a target can
%become untrackable either by lowing its detection probability sufficiently
%(becoming "stealth") and/or by increasing its maneuverability, which,
%here, is reflected in increasign the process noise covariance.
%
%Note that if the innovation covariance matrix is passed for targetParams,
%this function just uses its size and determiniant.
%
%EXAMPLE:
% T=1;
% targetParams=[];
% targetParams.F=FPolyKal(T,6,1);
% q0=1;
% targetParams.Q=QPolyKal(T,6,1,q0);
% targetParams.R=diag([10;10;10]);
% targetParams.H=[1,0,0,0,0,0;
%    0,1,0,0,0,0;
%    0,0,1,0,0,0];
% PD=0.5;
% lambda=1e-8;
% boolVal=linTargetIsUntrackable(targetParams,PD,lambda)
% %The above example is trackable (boolVal=false). However, if the
% %detection probability decreases/ the false alarm rate increases, then
% %one gets an untrackable target. Consider:
% PD=0.1;
% lambda=1e-6;
% boolVal1=linTargetIsUntrackable(targetParams,PD,lambda)
%
%REFERENCES:
%[1] Y. Bar-Shalom, P. K. Willett, and X. Tian, Tracking and Data Fusion.
%    Storrs, CT: YBS Publishing, 2011.
%[2] Y. Bar-Shalom, S. S. Blackman, and R. J. Fitzgerald, "Dimensionless
%    score function for multiple hypothesis tracking," IEEE Transactions on
%    Aerospace and Electronic Systems, vol. 43, no. 1, pp. 392-400, Jan.
%    2007.
%[3] Y. Bar-Shalom, X. R. Li, and T. Kirubarajan, Estimation with
%    Applications to Tracking and Navigation. New York: John Wiley and
%    Sons, Inc, 2001.
%
%January 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<4||isempty(AbsTol))
    AbsTol=1e-13;
end

if(nargin<5||isempty(RelTol))
    RelTol=1e-10;
end

if(nargin<6||isempty(maxIter))
   maxIter=5000;
end

if(isstruct(targetParams))
    H=targetParams.H;
    F=targetParams.F;
    R=targetParams.R;
    Q=targetParams.Q;
    m=size(H,1);

    %Asymptotic predicted state covariance matrix with given PD assuming
    %completely correct association hypotheses.
    PPred=RiccatiPredNoClutter(H,F,R,Q,PD,AbsTol,RelTol,maxIter);

    %Asymptotic innovation covariance matrix.
    detSPred=det(H*PPred*H'+R);
else
    S=targetParams;
    m=size(S,1);
    detSPred=det(S);
end

maxIsTargetScore=PD/(lambda*sqrt((2*pi)^m*detSPred));
missedDetectScore=1-PD;

boolVal=maxIsTargetScore<=missedDetectScore;
end

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


%[ P, inls ] = ht_simple_ransac_p3p( u, X, rthr, maxiter )
%u: 3 x n image points
%X: 3 x n 3D points
%rthr: inlier threshold
%maxiter: default=1000

function [ P, inls ] = ht_simple_ransac_p3p( u, X, rthr, max_iter )
if nargin < 4
    max_iter = 1000;
end

%initialization
u = bsxfun(@rdivide, u, sqrt(sum(u.^2, 1)));
Npts = size(u, 2);
rthr = cos(rthr);
max_inlsnum = 3;
no_iter = 0;
P = [];
inls = false(1, Npts);
%ransac
while no_iter < max_iter
    no_iter = no_iter + 1;

    idx = randperm(Npts, 3);
    P_cand = P3PSolver([u(:, idx); X(:, idx)]);
    [inls_cand, inls_cand_num] = calculate_inls_angular(P_cand, u, X, rthr);
    if length(P_cand) > 1
        [inls_cand_num, inls_cand_idx] = max(inls_cand_num);
        inls_cand = inls_cand{inls_cand_idx};
        P_cand = P_cand{inls_cand_idx};
    else
        inls_cand_num = inls_cand_num(1);
        inls_cand = inls_cand{1};
        P_cand = P_cand{1};
    end

    if inls_cand_num >= max_inlsnum
        max_inlsnum = inls_cand_num;
        P = P_cand;
        inls = inls_cand;
        max_iter = min([max_iter, nsamples(max_inlsnum, Npts, 3, 0.95)]);
    end

end

end

function [SampleCnt, q] = nsamples(ni, ptNum, pf, conf)
    q  = prod (((ni-pf+1) : ni) ./ ((ptNum-pf+1) : ptNum));
    if q < eps
      SampleCnt = Inf;
    else
      %   SampleCnt  = log(1 - conf) / log(1 - q);
      if q > conf
        SampleCnt = 1;
      else
        SampleCnt  = log(1 - conf) / log(1 - q);
      end
    end
end

function [inls, inls_num] = calculate_inls_angular(Pcand, u, X, rthr)
    inls = cell(1, length(Pcand));
    inls_num = zeros(1, length(Pcand));
    for ii = 1:1:length(Pcand)
        X_reproj = Pcand{ii} * [X; ones(1, size(X, 2))];
        X_reproj = bsxfun(@rdivide, X_reproj, sqrt(sum(X_reproj.^2, 1)));

        res = sum(u .* X_reproj, 1);
        inls{ii} = res > rthr;
        inls_num(ii) = sum(inls{ii});
    end

end


function raeq = eqxra (tjd, k)

% this function computes the intermediate right ascension
% of the equinox at julian date tjd, using an analytical expression
% for the accumulated precession in right ascension.  for the
% true equinox the result is the equation of the origins.

% input

%  tjd = tdb julian date

%  k = equinox selection code

%      set k = 0 for mean equinox
%      set k = 1 for true equinox (equation of the origins)

% output

%  raeq = intermediate right ascension of the equinox, in hours (+ or -)

% NOTE: this function computes the accumulated precession in right
%       ascension using the analytic equation in capitaine et al. (2003),
%       astronomy and astrophysics 412, 567-586, eq. (42).

% ported from NOVAS 3.0

%%%%%%%%%%%%%%%%%%%%%%%

% t0 = tdb julian date of epoch j2000.0 (tt)

t0 = 2451545.0d0;

t = (tjd - t0) / 36525.0d0;

% for the true equinox, obtain the equation of the equinoxes in time seconds

if (k == 1)

    [a, a, ee, a, a] = etilt (tjd);

    eqeq = ee;

else

    eqeq = 0.0d0;

end

% precession in ra in arcseconds taken from capitaine et al. (2003),
% astronomy and astrophysics 412, 567-586, eq. (42)

precra = 0.014506d0 + ...
    (((( -0.0000000368d0 * t ...
    - 0.000029956d0) * t ...
    - 0.00000044d0) * t ...
    + 1.3915817d0) * t ...
    + 4612.156534d0) * t;

raeq = -(precra / 15.0d0 + eqeq) / 3600.0d0;


function transm = transformmatrix(s,r,t)
% *** Homogeneous transformation matrix ***
% Function transformmatrix returns transformation matrix from object to image space.
%
% transm = transformmatrix (s,r,t)
%
% inputs,
%   s = Zoom factor
%   r = Rotation vector (rotx,roty,rotz)
%   t = Translation Vector [x,y,z]
%
% transm = Rotation Matrix
%

S=[s 0 0 0;
   0 s 0 0;
   0 0 s 0;
   0 0 0 1];

Rx=[1 0 0 0;
    0 cos(r(1)) -sin(r(1)) 0;
    0 sin(r(1)) cos(r(1)) 0;
    0 0 0 1];

Ry=[cos(r(2)) 0 sin(r(2)) 0;
    0 1 0 0;
    -sin(r(2)) 0 cos(r(2)) 0;
    0 0 0 1];

Rz=[cos(r(3)) -sin(r(3)) 0 0;
    sin(r(3)) cos(r(3)) 0 0;
    0 0 1 0;
    0 0 0 1];

R=Rx*Ry*Rz;

T=[1 0 0 t(1);
   0 1 0 t(2);
   0 0 1 t(3);
   0 0 0 1];

transm = S*R*T;


function [fx,fy,ft] = computeDerivatives2(im1,im2)
%
% function [fx,fy,fz] = computeDerivatives2(im1,im2)
%
% im1 and im2 are images
%
% [fx,fy,ft] are images, derivatives of the input images

filter = [0.03504 0.24878 0.43234 0.24878 0.03504];
dfilter = [0.10689 0.28461 0.0  -0.28461  -0.10689];

dx1 = conv2sep(im1,dfilter,filter,'valid');
dy1 = conv2sep(im1,filter,dfilter,'valid');
blur1 = conv2sep(im1,filter,filter,'valid');
dx2 = conv2sep(im2,dfilter,filter,'valid');
dy2 = conv2sep(im2,filter,dfilter,'valid');
blur2 = conv2sep(im2,filter,filter,'valid');

fx=(dx1+dx2)/2;
fy=(dy1+dy2)/2;
ft=(blur2-blur1);


% 15.7.02
%
% Unscented Kalman Filter (UKF) applied to FitzHugh-Nagumo neuron dynamics.
% Voltage observed, currents and inputs estimated.
%
% FitzHughNagumo() is the main program and calls the other programs.
%
% A detailed description is provided in
% H.U. Voss, J. Timmer & J. Kurths, Nonlinear dynamical system identification from uncertain and indirect measurements, Int. J. Bifurcation and Chaos 14, 1905-1933 (2004).
% I will be happy to email this paper on request. It contains a tutorial about the estimation of hidden states and unscented Kalman filtering.
%
% For commercial use and questions, please contact me.
%
% ++++++++++++++++++++++++++++++++++++++++++++++
% Henning U. Voss, Ph.D.
% Associate Professor of Physics in Radiology
% Citigroup Biomedical Imaging Center
% Weill Medical College of Cornell University
% 516 E 72nd Street
% New York, NY 10021
% Tel. 001-212 746-5216, Fax. 001-212 746-6681
% Email: hev2006@med.cornell.edu
% ++++++++++++++++++++++++++++++++++++++++++++++

function FitzHughNagumo()

global dT

orient tall

% Dimensions: dq for param. vector, dx augmented state, dy observation
dq=1; dx=dq+2; dy=1;

fct='FitzHughNagumo_fct'; % model function F(x)
obsfct='FitzHughNagumo_obsfct';   % observation function G(x)

ll=800; % number of data samples
dT=0.2; % sampling time step (global variable)

% Simulating data: %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

x0=zeros(2,ll); x0(:,1)=[0; 0]; % true trajectory
dt=.1*dT; nn=fix(dT/dt); % the integration time step is smaller than dT

% External input, estimated as parameter p later on:
z=[1:ll]/250*2*pi; z=-.4-1.01*abs(sin(z/2));

% 4th order Runge-Kutta integrator:

for n=1:ll-1;
  xx=x0(:,n);
  for i=1:nn
    k1=dt*FitzHughNagumo_int(xx,z(n));
    k2=dt*FitzHughNagumo_int(xx+k1/2,z(n));
    k3=dt*FitzHughNagumo_int(xx+k2/2,z(n));
    k4=dt*FitzHughNagumo_int(xx+k3,z(n));
    xx=xx+k1/6+k2/3+k3/3+k4/6;
  end;
  x0(:,n+1)=xx;
end;

x=[z; x0]; % augmented state vector (notation a bit different to paper)

R=.2^2*var(FitzHughNagumo_obsfct(x))*eye(dy,dy); % observation noise covariance matrix
randn('state',0); y=feval(obsfct,x)+sqrtm(R)*randn(dy,ll); % noisy data

% Initial conditions %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

xhat=zeros(dx,ll);
xhat(:,2)=x(:,2); % first guess of x_1 set to observation

Q=.015; % process noise covariance matrix

Pxx=zeros(dx,dx,ll);
Pxx(:,:,1)=blkdiag(Q,R,R);

errors=zeros(dx,ll); % not so important
Ks=zeros(dx,dy,ll);  % Kalman gains

% Main loop for recursive estimation %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for k=2:ll

[xhat(:,k),Pxx(:,:,k),Ks(:,:,k)]=ut(xhat(:,k-1),Pxx(:,:,k-1),y(:,k),fct,obsfct,dq,dx,dy,R);

Pxx(1,1,k)=Q;

errors(:,k)=sqrt(diag(Pxx(:,:,k)));

end; % k

% Results %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

chisq=mean((x(1,:)-xhat(1,:)).^2+(x(2,:)-xhat(2,:)).^2+(x(3,:)-xhat(3,:)).^2)
est=xhat(1:dq,ll)'
error=errors(1:dq,ll)'

figure(1)

subplot(2,1,1)
plot(y,'bd','MarkerEdgeColor','blue', 'MarkerFaceColor','blue','MarkerSize',3);
hold on;
plot(x(dq+1,:),'black','LineWidth',2);
%plot(xhat(dq+1,:),'r','LineWidth',2);
xlabel(texlabel('t'));
ylabel(texlabel('x_1, y'));
hold off;
axis tight
title('(a)')
drawnow

subplot(2,1,2)
plot(x(dq+2,:),'black','LineWidth',2);
hold on
plot(xhat(dq+2,:),'r','LineWidth',2);
plot(x(1,:),'black','LineWidth',2);
for i=1:dq; plot(xhat(i,:),'m','LineWidth',2); end;
for i=1:dq; plot(xhat(i,:)+errors(i,:),'m'); end;
for i=1:dq; plot(xhat(i,:)-errors(i,:),'m'); end;
xlabel(texlabel('t'));
ylabel(texlabel('z, estimated z, x_2, estimated x_2'));
hold off
axis tight
title('(b)')

function r=FitzHughNagumo_int(x,z)
% Function for modeling data, not for UKF execution
a=.7; b=.8; c=3.;
r=[c*(x(2)+x(1)-x(1)^3/3+z); -(x(1)-a+b*x(2))/c];

function r=FitzHughNagumo_obsfct(x)
% Observation function
r=x(2,:);

function r=FitzHughNagumo_fct(dq,x)
% Runge-Kutta integrator for FitzHugh-Nagumo system with parameters

global dT
dt=.02; % local integration step
nn=fix(dT/dt);

p=x(1:dq,:);
xnl=x(dq+1:size(x(:,1)),:);
for n=1:nn
k1=dt*fc(xnl,p);
k2=dt*fc(xnl+k1/2,p);
k3=dt*fc(xnl+k2/2,p);
k4=dt*fc(xnl+k3,p);
xnl=xnl+k1/6+k2/3+k3/3+k4/6;
end
r=[x(1:dq,:); xnl];

function r=fc(x,p);
a=.7; b=.8; c=3.;
r=[c*(x(2,:)+x(1,:)-x(1,:).^3/3+p); -(x(1,:)-a+b*x(2,:))/c];

function [xhat,Pxx,K]=ut(xhat,Pxx,y,fct,obsfct,dq,dx,dy,R);
% Unscented transformation. Not specific to FitzHugh-Nagumo model

  N=2*dx;

  xsigma=chol( dx*Pxx )'; % Pxx=root*root', but Pxx=chol'*chol
  Xa=xhat*ones(1,N)+[xsigma, -xsigma];
  X=feval(fct,dq,Xa);

  xtilde=sum(X')'/N;

  Pxx=zeros(dx,dx);
  for i=1:N;
    Pxx=Pxx+(X(:,i)-xtilde)*(X(:,i)-xtilde)'/N;
  end;

  Y=feval(obsfct,X);

  ytilde=sum(Y')'/N;
  Pyy=R;
  for i=1:N;
    Pyy=Pyy+(Y(:,i)-ytilde)*(Y(:,i)-ytilde)'/N;
  end;
  Pxy=zeros(dx,dy);
  for i=1:N;
    Pxy=Pxy+(X(:,i)-xtilde)*(Y(:,i)-ytilde)'/N;
  end;

  K=Pxy*inv(Pyy);
  xhat=xtilde+K*(y-ytilde);
  Pxx=Pxx-K*Pxy';