function [t,a,k]=BSplineInterpDeriv(t,a,k,numDerivs)
%%BSPLINEINTERPDERIV Given a set of b-spline interpolation weights a and
%            knots t for scalar interpolation, compute the weights and
%            knots to interpolate the numDerivs derivative value using
%            b-splines interpolation.
%
%INPUTS: t The NX1 or 1XN set of knots for the interpolation function. The
%          first and last k-1 knots are outside of the ends of the region
%          with data or mark the ends of the region with data. These vslues
%          must be real.
%        a The npXnumSets collection of b-spline coefficients for
%          interpolating over a certain region for numSets sets of
%          interpolation problems. Such coefficients could be obtained from
%          the function BSplinePolyCoeffs along with t if one wishes to
%          interpolate over gridded data. These values can be complex.
%        k The order of the B-spline polynomials. The order of the
%          interpolation accuracy is k-numDerivs-1.
% numDerivs The number of derivatives to take. This must be less than k-1.
%
%OUTPUTS: a The (np-numDerivs)XnumSets modified coefficients.
%         t The (N-2*numDerivs)X1 modified knots.
%         k The modified B-spline order.
%
%This function implements Equation 12b in Chapter X of [1]. The
%modifications to t simply reflect the modification to the overall length
%of a.
%
%The first and last elements in t are not used in this function. However,
%the equations in [1] assume that those elements are present as they end up
%being used when computing integrals.
%
%EXAMPLE 1:
%Here, we approximate the derivative of a fourth-order function using
%piecewise third-order b-splines and plot the results.
% f=@(t)(t.^4-2*t.^2+t);
% tau=[-2;-1;0;0.5;1;1.5;2];
% y=f(tau);
% k=4;%k-1=3 is the order....
% [a,t]=BSplinePolyFit(tau,y,k);
%
% x=linspace(-2,2,500);
% numDerivs=1;
% [t,a,k]=BSplineInterpDeriv(t,a,k,numDerivs);
% ypDeriv=BSplineInterpVal(x,t,a,k);
% yDeriv=1-4*x+4*x.^3;%First derivative
%
% figure(1)
% clf
% hold on
% plot(x,yDeriv,'-k','linewidth',4)
% plot(x,ypDeriv,'--r','linewidth',2)
%
%EXAMPLE 2:
%This is similar to the previous example, except multiple sets of
%coefficients are computed and evaluated at once.
% f1=@(t)(t.^4-2*t.^2+t);
% f2=@(t)(t.^3-2*t.^2+t);
% tau=[-2;-1;0;0.5;1;1.5;2];
% y=[f1(tau),f2(tau)];
% k=4;%k-1=3 is the order....
% [a,t]=BSplinePolyFit(tau,y,k);
%
% x=linspace(-2,2,500).';
% numDerivs=1;
% [t,a,k]=BSplineInterpDeriv(t,a,k,numDerivs);
% ypDeriv=BSplineInterpVal(x,t,a,k);
% yDeriv=[1-4*x+4*x.^3,1-4*x+3*x.^2];%First derivative
%
% figure(1)
% clf
% hold on
% plot(x,yDeriv(:,1),'-k','linewidth',4)
% plot(x,ypDeriv(:,1),'--r','linewidth',2)
%
% figure(2)
% clf
% hold on
% plot(x,yDeriv(:,2),'-k','linewidth',4)
% plot(x,ypDeriv(:,2),'--r','linewidth',2)
%
%EXAMPLE 3:
%This is an example where a complex function parameterized by a single real
%value is differentiated and interpolated.
% numPoints=300;
% xMin=0;
% xMax=5;
% x=linspace(xMin,xMax,numPoints);
% f=@(x)exp(-1j*2*x).*(4*x.^3+x.*exp(1j*2*x.^2)+12*x+x.^3.*exp(-1j*2*x.^2));
%
% fDx=@(x)exp(-2*1j*x.*(1+x)).*(exp(4*1j*x.^2).*(1+2*1j*x.*(-1+2*x))+x.^2.*(3-2*1j*x.*(1+2*x))+4*exp(2*1j*x.^2).*(3+x.*(-6*1j+(3-2*1j*x).*x)));
%
% fx=f(x);
% k=5;
% [a,t]=BSplinePolyFit(x,fx(:),k);
% numDerivs=1;
% [t,a,k]=BSplineInterpDeriv(t,a,k,numDerivs);
%
% %Interpolate on a finer grid.
% numPoints=2000;
% x=linspace(xMin,xMax,numPoints);
% fx=fDx(x);
% fxInterp=BSplineInterpVal(x,t,a,k);
%
% %Plot the function values and the interpolated values for the real and
% %complex parts.
% figure(1)
% clf
% hold on
% plot(x,real(fx),'-k','linewidth',2)
% plot(x,imag(fx),'--k','linewidth',2)
% plot(x,real(fxInterp),'-r')
% plot(x,imag(fxInterp),'--r')
%
% %Plot the interpolation error.
% figure(2)
% clf
% hold on
% plot(x,real(fxInterp(:))-real(fx(:)))
% plot(x,imag(fxInterp(:))-imag(fx(:)))
%
%REFERENCES:
%[1] C. de Boor, A Practical Guide to Splines. New York: Springer-Verlag,
%    1978.
%
%April 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

numA=size(a,1);

for curDeriv=1:numDerivs
    aNext=a;
    for idxA=(curDeriv+1):numA
        denom=(t(idxA+k-curDeriv)-t(idxA))/(k-curDeriv);

        if(denom==0)
            aNext(idxA,:)=0;
        else
            aNext(idxA,:)=(a(idxA,:)-a(idxA-1,:))/denom;
        end
    end

    a=aNext;
end
a=a((1+numDerivs):end,:);
t=t((1+numDerivs):(end-numDerivs));
k=k-numDerivs;
end

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


function [suspicious_index lof] = LOF(A, k)
%#####################################################################
%# Local Outlier Factor                                              #
%# Authors: Markus M. Breunig, Hans-Peter Kriegel,                   #
%#          Raymond T. Ng, J?rg Sander                               #
%# Oringinal paper :                                                 #
%# LOF: Identifying Density-Based Local Outliers                     #
%# e-mail : { breunig | kriegel | sander }                           #
%#          @dbs.informatik.uni-muenchen.de                          #
%#          rng@cs.ubc.ca                                            #
%# Programmer: Yi-Ren Yeh(yirenyeh@gmail.com)                        #
%# modified by: Zi-Wen Gui(evan176@hotmail.com)                      #
%#                                                                   #
%#                                                                   #
%# Inputs                                                            #
%#   A: the data matrix, each row reprent an instance                #
%#   k: the number of nearest neighbors                              #
%#                                                                   #
%# Outputs                                                           #
%#   lof: the local outlier factor for each instance                 #
%#   suspicious_index: the ranking of instances according to their   #
%#                     suspiciuous score                             #
%#                     For example, suspicious_index(i)=j means the  #
%#                     ith instance is in jth position in the ranking#
%#####################################################################

%Find the nearest neighbors by "KDTree" for each elements
[k_index, k_dist] = knnsearch(A,A,'k',k+1,'nsmethod','kdtree');
%Ignore first element(itself) at nearest neighbors
k_index = k_index(:,2:end);
%Get k-distance
k_dist1 = k_dist(:,end);
%Get row length of matrix A
n = length(A(:,1));
%Initializa lrd_value vector
lrd_value = zeros(n,1);
%Calculate lrd for each elements
for i = 1:n
    lrd_value(i) = lrd(A, i, k_dist1,k_index, k);
end
%Initializa lof vector
lof = zeros(n,1);
%Calculate LOF
for i = 1:n
    lof(i) = sum(lrd_value(k_index(i,:))/lrd_value(i))/k;
end
%Sort lof factor
[non,suspicious_index]=sort(lof,'descend');

%=========================================================================
function lrd_value = lrd(A, index_p, k_dist,k_index, k)
%Calculate the reachability distance for nearest neighbors
Temp = repmat(A(index_p,:),k,1) - A(k_index(index_p,:),:);
Temp = sqrt(sum(Temp.^2,2));
%max{k-distance(b), d(a, b)}
rd_dsit = max([Temp k_dist(k_index(index_p,:))],[],2);
%Calculate the local reachability density for each elements
lrd_value = k/sum(rd_dsit);


function [ point_coord, face_order, face_point ] = ...
  tetrahedron_rhombic_shape_3d ( point_num, face_num, face_order_max )

%*****************************************************************************80
%
%% TETRAHEDRON_RHOMBIC_SHAPE_3D describes a rhombic tetrahedron in 3D.
%
%  Discussion:
%
%    Call TETRAHEDRON_RHOMBIC_SIZE_3D first, to get dimension information.
%
%    The tetrahedron is described using 10 nodes.  If we label the vertices
%    P0, P1, P2 and P3, then the extra nodes lie halfway between vertices,
%    and have the labels P01, P02, P03, P12, P13 and P23.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 January 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Anwei Liu, Barry Joe,
%    Quality Local Refinement of Tetrahedral Meshes Based
%    on 8-Subtetrahedron Subdivision,
%    Mathematics of Computation,
%    Volume 65, Number 215, July 1996, pages 1183-1200.
%
%  Parameters:
%
%    Input, integer POINT_NUM, the number of points in the shape.
%
%    Input, integer FACE_NUM, the number of faces in the shape.
%
%    Input, integer FACE_ORDER_MAX, the maximum number of vertices per face.
%
%    Output, real POINT_COORD(3,POINT_NUM), the vertices.
%
%    Output, integer FACE_ORDER(FACE_NUM), the number of vertices
%    for each face.
%
%    Output, integer FACE_POINT(FACE_ORDER_MAX,FACE_NUM); FACE_POINT(I,J)
%    contains the index of the I-th point in the J-th face.  The
%    points are listed in the counter clockwise direction defined
%    by the outward normal at the face.
%
  dim_num = 3;

  a =        1.0   / sqrt ( 3.0 );
  b = sqrt ( 2.0 ) / sqrt ( 3.0 );
  c = sqrt ( 3.0 ) /        6.0;
  d =        1.0   / sqrt ( 6.0 );
  z = 0.0;
%
%  Set the point coordinates.
%
  point_coord(1:dim_num,1)  = [ -b,  z,  z ]';
  point_coord(1:dim_num,2)  = [  z, -a,  z ]';
  point_coord(1:dim_num,3)  = [  z,  a,  z ]';
  point_coord(1:dim_num,4)  = [  z,  z,  b ]';
  point_coord(1:dim_num,5)  = [ -d, -c,  z ]';
  point_coord(1:dim_num,6)  = [ -d,  c,  z ]';
  point_coord(1:dim_num,7)  = [ -d,  z,  d ]';
  point_coord(1:dim_num,8)  = [  z,  z,  z ]';
  point_coord(1:dim_num,9)  = [  z, -c,  d ]';
  point_coord(1:dim_num,10) = [  z,  c,  d ]';
%
%  Set the face orders.
%
  face_order(1:face_num) = [ 6, 6, 6, 6 ];
%
%  Set faces.
%
  face_point(1:face_order_max,1:face_num) = [ ...
     1,  5,  2,  9,  4,  7; ...
     2,  8,  3, 10,  4,  9; ...
     3,  6,  1,  7,  4, 10; ...
     1,  6,  3,  8,  2,  5 ]';

  return
end


% LLE ALGORITHM (using K nearest neighbors)
%
% [Y] = lle(X,K,dmax)
%
% X = data as D x N matrix (D = dimensionality, N = #points)
% K = number of neighbors
% dmax = max embedding dimensionality
% Y = embedding as dmax x N matrix

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

function [Y] = lle(X,K,d)

[D,N] = size(X);
fprintf(1,'LLE running on %d points in %d dimensions\n',N,D);

% STEP1: COMPUTE PAIRWISE DISTANCES & FIND NEIGHBORS
fprintf(1,'-->Finding %d nearest neighbours.\n',K);

X2 = sum(X.^2,1);
distance = repmat(X2,N,1)+repmat(X2',1,N)-2*X'*X;

[sorted,index] = sort(distance);
neighborhood = index(2:(1+K),:);

% STEP2: SOLVE FOR RECONSTRUCTION WEIGHTS
fprintf(1,'-->Solving for reconstruction weights.\n');

if(K>D)
  fprintf(1,'   [note: K>D; regularization will be used]\n');
  tol=1e-3; % regularlizer in case constrained fits are ill conditioned
else
  tol=0;
end

W = zeros(K,N);
for ii=1:N
   z = X(:,neighborhood(:,ii))-repmat(X(:,ii),1,K); % shift ith pt to origin
   C = z'*z;                                        % local covariance
   C = C + eye(K,K)*tol*trace(C);                   % regularlization (K>D)
   W(:,ii) = C\ones(K,1);                           % solve Cw=1
   W(:,ii) = W(:,ii)/sum(W(:,ii));                  % enforce sum(w)=1
end;

% STEP 3: COMPUTE EMBEDDING FROM EIGENVECTS OF COST MATRIX M=(I-W)'(I-W)
fprintf(1,'-->Computing embedding.\n');

% M=eye(N,N); % use a sparse matrix with storage for 4KN nonzero elements
M = sparse(1:N,1:N,ones(1,N),N,N,4*K*N);
for ii=1:N
   w = W(:,ii);
   jj = neighborhood(:,ii);
   M(ii,jj) = M(ii,jj) - w';
   M(jj,ii) = M(jj,ii) - w;
   M(jj,jj) = M(jj,jj) + w*w';
end;

% CALCULATION OF EMBEDDING
options.disp = 0; options.isreal = 1; options.issym = 1;
[Y,eigenvals] = eigs(M,d+1,0,options);
Y = Y(:,2:d+1)'*sqrt(N); % bottom evect is [1,1,1,1...] with eval 0

fprintf(1,'Done.\n');

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

% other possible regularizers for K>D
%   C = C + tol*diag(diag(C));                       % regularlization
%   C = C + eye(K,K)*tol*trace(C)*K;                 % regularlization


function Y = spe(X, no_dims, varargin)
%SPE Perform the Stochastic Proximity Embedding algorithm
%
%   Y = spe(X, no_dims, 'Global')
%   Y = spe(X, no_dims, 'Local', k)
%
% Perform the Stochastic Proximity Embedding algorithm. The SPE algorithm
% can be compared to the MDS algorithm, although it is much more efficient.
% The total number of updates necessary after the random initialization is
% usually less than n^2 (where n is the number of datapoints). X is the set
% of datapoints on which SPE has to be applied, and no_dims the number of
% dimensions in the embedding space. If the method is used with the 'Global'
% switch, a minimization of the traditional MDS raw stress function is
% performed (the default). Execution with the 'Local' switch only retains
% Euclidean distances in the local neighborhood of the datapoints. The size
% of the neighborhood is defined by the variable k (default = 12).
%
%

% This file is part of the Matlab Toolbox for Dimensionality Reduction.
% The toolbox can be obtained from http://homepage.tudelft.nl/19j49
% You are free to use, change, or redistribute this code in any way you
% want for non-commercial purposes. However, it is appreciated if you
% maintain the name of the original author.
%
% (C) Laurens van der Maaten, Delft University of Technology

    if nargin <= 2
        variant = 'Global';
    else
        variant = varargin{1};
    end
    if strcmp(variant, 'Local')
        if nargin > 3, k = varargin{2};
        else k = 12; end
    end
    if ~strcmp(variant, 'Global') && ~strcmp(variant, 'Local')
        error('Unknown parameter.');
    end
    if exist('k', 'var') && ischar(k)
        error('Adaptive neighborhood selection is not yet supported in SPE.');
    end

    % Initialize parameters
    lambda = 1;                                         % initial learning parameter
    s = 100;                                            % number of updates per iteration
    max_iter = 20000 + round(.04 * size(X, 1) ^ 2);     % number of iterations
    tol = 1e-5;                                         % regularlization parameter
    n = size(X, 1);                                     % number of datapoints
    if strcmp(variant, 'Local')
        max_iter = max_iter * 3;
    end

    % Compute proximity matrix in original space
    if strcmp(variant, 'Global')
        R = L2_distance(X', X');
        R = R / max(max(R)) * sqrt(2);
    else
        [R, n_ind] = find_nn(X, k);
    end

    % Initialize datapoints randomly
    Y = rand(n, no_dims);

    % Perform SPE
    for i=1:max_iter
        if rem(i, 10000) == 0
            disp(['Iteration ' num2str(i) ' of ' num2str(max_iter) '...']);
        end

        % Select points that should be updated
        J = randperm(n);
        ind1 = J(1:s);
        if strcmp(variant, 'Global')
            ind2 = J(s+1:2*s);
        else
            ind2 = double(n_ind(ind1,:))';
            J = round(rand(1, size(ind2, 2)) * (k - 1)) + 1;
            J = J + ((0:length(J) - 1) * k);
            ind2 = ind2(J);
        end

        % Compute distances between points in embedded space
        D = sqrt(sum((Y(ind1,:) - Y(ind2,:)) .^ 2, 2));

        % Get corresponding distances in real space
        Rt = R((ind1 - 1) * size(R, 1) + ind2)';

        % Update locations of points
        Y(ind1,:) = Y(ind1,:) + lambda * (1/2) * repmat(((Rt - D) ./ (D + tol)), 1, no_dims) .* (Y(ind1,:) - Y(ind2,:));
        Y(ind2,:) = Y(ind2,:) + lambda * (1/2) * repmat(((Rt - D) ./ (D + tol)), 1, no_dims) .* (Y(ind2,:) - Y(ind1,:));

        % Update learning parameter
        lambda = lambda - (lambda / max_iter);
    end


function llh=local2llh(xy,origin)
%local2llh     llh=local2llh(xy,origin)
%
%Converts from local coorindates to longitude and latitude
%given the [lon, lat] of an origin. 'origin' should be in
%decimal degrees. Note that heights are ignored and that
%xy is in km.  Output is [lon, lat, height] in decimal
%degrees. This is an iterative solution for the inverse of
%a polyconic projection.

%-------------------------------------------------------------
%   Record of revisions:
%
%   Date          Programmer            Description of Change
%   ====          ==========            =====================
%
%   Aug 23, 2001  Jessica Murray        Clarification to help.
%
%   Apr 4, 2001   Peter Cervelli        Added failsafe to avoid
%                                       infinite loop because of
%                                       covergence failure.
%   Sep 7, 2000   Peter Cervelli		Original Code
%
%-------------------------------------------------------------

%Set ellipsoid constants (WGS84)

   a=6378137.0;
   e=0.08209443794970;

%Convert to radians / meters

%   xy=xy*1000;
%   origin=origin*pi/180;
   xy=double(xy)*1000;
   origin=double(origin)*pi/180;

%Iterate to perform inverse projection

   M0=a*((1-e^2/4-3*e^4/64-5*e^6/256)*origin(2) - ...
        (3*e^2/8+3*e^4/32+45*e^6/1024)*sin(2*origin(2)) + ...
        (15*e^4/256 +45*e^6/1024)*sin(4*origin(2)) - ...
        (35*e^6/3072)*sin(6*origin(2)));

   z=xy(2,:)~=-M0;

   A=(M0+xy(2,z))/a;
   B=xy(1,z).^2./a^2+A.^2;

   llh(2,z)=A;

   delta=Inf;

   c=0;

   while max(abs(delta))>1e-8

      C=sqrt((1-e^2*sin(llh(2,z)).^2)).*tan(llh(2,z));

      M=a*((1-e^2/4-3*e^4/64-5*e^6/256)*llh(2,z) - ...
           (3*e^2/8+3*e^4/32+45*e^6/1024)*sin(2*llh(2,z)) + ...
           (15*e^4/256 +45*e^6/1024)*sin(4*llh(2,z)) - ...
           (35*e^6/3072)*sin(6*llh(2,z)));

      Mn=1-e^2/4-3*e^4/64-5*e^6/256 - ...
         -2*(3*e^2/8+3*e^4/32+45*e^6/1024)*cos(2*llh(2,z)) + ...
         4*(15*e^4/256 +45*e^6/1024)*cos(4*llh(2,z)) + ...
         -6*(35*e^6/3072)*cos(6*llh(2,z));

      Ma=M/a;

      delta=-(A.*(C.*Ma+1)-Ma-0.5*(Ma.^2+B).*C)./ ...
           (e^2*sin(2*llh(2,z)).*(Ma.^2+B-2*A.*Ma)./(4*C)+(A-Ma).*(C.*Mn-2./sin(2*llh(2,z)))-Mn);

      llh(2,z)=llh(2,z)+delta;

      c=c+1;
      if c>100
          error('Convergence failure.')
      end
   end

   llh(1,z)=(asin(xy(1,z).*C/a))./sin(llh(2,z))+origin(1);

%Handle special case of latitude = 0

   llh(1,~z)=xy(1,~z)/a+origin(1);
   llh(2,~z)=0;

%Convert back to decimal degrees

   llh=llh*180/pi;


function [param]=zn2pd(input)
% [param]=zn2pd(input)
% Ziegler-Nichols PD controller for processes of 2nd order.
% Controller uses reference variable (w) just in proportional component.
% This function computes parameters of the controller (r0, q0, q1, q2, p1, p2).
% Output of the controller is calculated follows:
%
%                  r0                            q0 + q1*z^-1 + q2*z^-2
% U(z^-1) = ----------------------- * W(z^-1) - ------------------------ * Y(z^-1)
%            1 + p1*z^-1 + p2*z^-2                1 + p1*z^-1 + p2*z^-2
%
% where p1=0, p2=0, q2=0
%
% Transfer function of the controlled system is:
%
%               b1*z^-1 + b2*z^-2
% Gs(z^-1) = -----------------------
%             1 + a1*z^-1 + a2*z^-2
%
% Input: input ... input parameters
%                  input(1) ... a1
%                  input(2) ... b1
%                  input(3) ... a2
%                  input(4) ... b2
%                  input(5) ... sample time T0
% Output: param ... controller parameters
%                   param(1) ... r0
%                   param(2) ... q0
%                   param(3) ... q1
%                   param(4) ... q2 (0)
%                   param(5) ... p1 (0)
%                   param(6) ... p2 (0)

a1 = input(1);
b1 = input(2);
a2 = input(3);
b2 = input(4);
T0 = input(5);

% compute ultimate gain and frequency
[Kpu, Tu] =  ultim([b1 b2],[a1 a2],T0);

Kp = 0.4*Kpu;
Td = Tu/20;

r0 = Kp;
q0 = Kp*(1+Td/T0);
q1 = -Kp*(Td/T0);
q2 = 0;
p1 = 0;
p2 = 0;

param=[r0; q0; q1; q2; p1; p2];


function p = normpdfln(x, m, S, V)
% NORMPDFLN    log of multivariate normal density.
%   See NORMPDF for argument description.

log2pi = 1.83787706640935;
[d, n] = size(x);
if nargin == 1
  dx = x;
elseif isempty(m)
  dx = x;
else
  % m specified
  sz = size(m);
  if sz(1) ~= d
    error('rows(m) ~= rows(x)')
  end
  nm = sz(2);
  if nm == 1
    dx = x - repmat(m,1,n);
  elseif n == 1
    dx = repmat(x,1,nm) - m;
  elseif nm == n
    dx = x - m;
  else
    error('incompatible number of columns in x and m')
  end
end
if nargin < 3
  % unit variance
  p = -0.5*(d*log2pi + col_sum(dx.*dx));
  return
end
have_inv = 0;
if nargin == 3
  % standard deviation given
  if d == 1
    dx = dx./S;
    p = (-log(S) -0.5*log2pi) - 0.5*(dx.*dx);
    return;
  end
  if S(2,1) ~= 0
    error('S is not upper triangular')
  end
  if any(size(S) ~= [d d])
    error('S is not the right size')
  end
else
  if ischar(V)
    if strcmp(V,'inv')
      % inverse stddev given
      iS = S;
      have_inv = 1;
    else
      error('unknown directive')
    end
  elseif ischar(S)
    if strcmp(S,'inv')
      % inverse variance given
      if d == 1
	iS = sqrt(V);
      else
	iS = chol(V);
      end
      have_inv = 1;
    else
      error('unknown directive')
    end
  else
    % variance given
    if d == 1
      S = sqrt(V);
    else
      S = chol(V);
    end
  end
end
if have_inv
  if d == 1
    dx = iS .* dx;
    logdetiS = log(iS);
  else
    dx = iS*dx;
    logdetiS = sum(log(diag(iS)));
  end
else
  if d == 1
    dx = dx./S;
    logdetiS = -log(S);
  else
    dx = solve_tril(S',dx);
    %dx = S'\dx;
    logdetiS = -sum(log(diag(S)));
  end
end
p = (logdetiS -0.5*d*log2pi) -0.5*col_sum(dx.*dx);


function res = polynomialTransform2d(pts, coeffs)
%POLYNOMIALTRANSFORM2D Apply a polynomial transform to a set of points.
%
%   RES = polynomialTransform2d(PTS, COEFFS)
%   Transforms the input points PTS given as a N-by-2 array of coordinates
%   using the polynomial transform defined by PARAMS.
%   PARAMS given as [a0 b0 a1 b1 ... an bn]
%
%   Example
%   coeffs = [0 0  1 0  0 1   0.1 0  0 0  0 0.1];
%       %     cte   x    y     x^2   x*y   y^2
%   pts = rand(200, 2) * 2 - 1;
%   pts2 = polynomialTransform2d(pts, coeffs);
%   figure; hold on;
%   drawPoint(pts);
%   drawPoint(pts2, 'g');
%
%   See also
%     transformPoint, fitPolynomialTransform2d

% ------
% Author: David Legland
% E-mail: david.legland@grignon.inra.fr
% Created: 2013-09-04, using Matlab 7.9.0.529 (R2009b)
% Copyright 2013-2022 INRA - Cepia Software Platform

x = pts(:,1);
y = pts(:,2);
nPoints = length(x);

xCoeffs = coeffs(1:2:end);
yCoeffs = coeffs(2:2:end);
nCoeffs = length(xCoeffs);

% allocate memory for result
x2 = zeros(nPoints, 1);
y2 = zeros(nPoints, 1);

% degree from coefficient number
degree = sqrt(9/4 - 4*(1 - nCoeffs)/2) - 1.5;

% iterate over degrees
iCoeff = 0;
for iDegree = 0:degree

    % iterate over binomial coefficients of a given degree
    for k = 0:iDegree
        iCoeff = iCoeff + 1;
        tmp = power(x, iDegree-k) .* power(y, k);
        x2 = x2 + xCoeffs(iCoeff) .* tmp;
        y2 = y2 + yCoeffs(iCoeff) .* tmp;
    end
end

res = [x2 y2];


function value = wedge01_integral ( e )

%*****************************************************************************80
%
%% WEDGE01_INTEGRAL returns the integral of a monomial in the unit wedge in 3D.
%
%  Discussion:
%
%    This routine returns the integral of
%
%      product ( 1 <= I <= 3 ) X(I)^E(I)
%
%    over the unit wedge.
%
%    The integration region is:
%
%      0 <= X
%      0 <= Y
%      X + Y <= 1
%      -1 <= Z <= 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 August 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Arthur Stroud,
%    Approximate Calculation of Multiple Integrals,
%    Prentice Hall, 1971,
%    ISBN: 0130438936,
%    LC: QA311.S85.
%
%  Parameters:
%
%    Input, integer E(3), the exponents.
%
%    Output, real VALUE, the integral of the monomial.
%
  value = 1.0;

  k = e(1);

  for i = 1 : e(2)
    k = k + 1;
    value = value * i / k;
  end

  k = k + 1;
  value = value / k;

  k = k + 1;
  value = value / k;
%
%  Now account for integration in Z.
%
  if ( e(3) == - 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'WEDGE01_INTEGRAL - Fatal error!\n' );
    fprintf ( 1, '  E(3) = -1 is not a legal input.\n' );
    error ( 'WEDGE01_INTEGRAL - Fatal error!' );
  elseif ( mod ( e(3), 2 ) == 1 )
    value = 0.0;
  else
    value = value * 2.0 / ( e(3) + 1 );
  end

  return
end


function jac = compute_jacobian(v,u,w)
%
% To compute Jacobian determinant for the image deformation field
% Usage: jac = compute_jacobian(vy,vx,vz);
%
[a11,a12,a13] = gradient_3d_by_mask(u); clear u;
[a21,a22,a23] = gradient_3d_by_mask(v); clear v;
[a31,a32,a33] = gradient_3d_by_mask(w); clear w;
a11 = a11+1;
a22 = a22+1;
a33 = a33+1;

jac = a11.*a22.*a33-a11.*a23.*a32-a21.*a12.*a33+a21.*a13.*a32+a31.*a12.*a23-a31.*a13.*a22;


function [lp,dlp] = priorInvGauss(mu,lam,x)

% Univariate Inverse Gaussian hyperparameter prior distribution.
% Compute log-likelihood and its derivative or draw a random sample.
% The prior distribution is parameterized as:
%
%   p(x) = exp(-lam*(x-mu)^2/(2*mu^2*x)) / sqrt(2*pi*x^3/lam)
%
% where mu(1x1) is the mean parameter, lam(1x1) is the scale parameter
% and x(1xN) contains query hyperparameters for prior evaluation.
%
% For more help on design of priors, try "help priorDistributions".
%
% Copyright (c) by Roman Garnett and Hannes Nickisch, 2014-09-08.
%
% See also PRIORDISTRIBUTIONS.M.

if nargin<2, error('mu and lam parameters need to be provided'), end
if ~(isscalar(mu)&&isscalar(lam))
  error('mu and lam parameters need to be scalars'),end
if nargin<3                                                    % return a sample
  n = randn; y = n*n;
  r = mu + mu*(mu*y-sqrt(4*mu*lam*y+mu^2*y^2))/(2*lam);
  z = rand;
  if z<=mu/(mu+r)
    lp = r;
  else
    lp = mu^2/r;
  end
  return
end

lp  = -lam*(x-mu).^2./(2*mu^2*x) - log(2*pi*x.^3/lam)/2;
q = (x-mu)./x;
dlp = -lam*q.*(2-q)/(2*mu^2) - 3./(2*x);
lp(x<0) = -inf; dlp(x<0) = 0;


function val = gamma_multivariate_ln(x,p);
% function val = gamma_multivariate_ln(x,p);
%
% x: array(1,K)
% p: scalor
%
% x must be more than (p-1)/2
% x should be more than p/2
%
% Gamma_p(x) = pi^(p(p-1)/4) prod_(j=1)^p Gamma(x+(1-j)/2)
% log Gamma_p(x) = p(p-1)/4 log pi + sum_(j=1)^p log Gamma(x+(1-j)/2)

K = length(x);
gammaln_val = gammaln(repmat(x,p,1)+0.5*(1-repmat([1:p]',1,K))); % p by K
val = p*(p-1)*0.25 * log(pi) + sum(gammaln_val,1);

% Local Variables: ***
% mode: matlab ***
% End: ***


function D = sqdistance(A, B, M)
% Square Euclidean or Mahalanobis distances between all sample pairs
% A: d x n1 data matrix
% B: d x n2 data matrix
% M: d x d  Mahalanobis matrix
% D: n1 x n2 pairwise square distance matrix
% Written by Michael Chen (sth4nth@gmail.com). July 2009.

if nargin == 1
    A = bsxfun(@minus,A,mean(A,2));
    S = full(sum(A.^2,1));
    D = full((-2)*(A'*A));
    D = bsxfun(@plus,D,S);
    D = bsxfun(@plus,D,S');
elseif nargin == 2
    assert(size(A,1)==size(B,1));

    m = (sum(A,2)+sum(B,2))/(size(A,2)+size(B,2));
    A = bsxfun(@minus,A,m);
    B = bsxfun(@minus,B,m);
    D = full((-2)*(A'*B));
    D = bsxfun(@plus,D,full(sum(B.^2,1)));
    D = bsxfun(@plus,D,full(sum(A.^2,1)'));
elseif nargin == 3
    assert(size(A,1)==size(B,1));

    m = (sum(A,2)+sum(B,2))/(size(A,2)+size(B,2));
    A = bsxfun(@minus,A,m);
    B = bsxfun(@minus,B,m);
    D = full((-2)*(A'*M*B));
    D = bsxfun(@plus,D,full(sum(B.*(M*B),1)));
    D = bsxfun(@plus,D,full(sum(A.*(M*A),1)'));
end


function [varargout] = wavefilter(wname, type)
%WAVEFILTER Create wavelet decomposition and reconstruction filters.
%   [VARARGOUT] = WAVEFILTER(WNAME, TYPE) returns the decomposition
%   and/or reconstruction filters used in the computation of the
%   forward and inverse FWT (fast wavelet transform).
%
%   EXAMPLES:
%     [ld, hd, lr, hr] = wavefilter('haar') Get the low and highpass
%                                           decomposition (ld, hd)
%                                           and reconstruction
%                                           (lr, hr) filters for
%                                           wavelet 'haar'.
%     [ld, hd] = wavefilter('haar','d')     Get decomposition filters
%                                           ld and hd.
%     [lr, hr] = wavefilter('haar','r')     Get reconstruction
%                                           filters lr and hr.
%
%   INPUTS:
%     WNAME             Wavelet Name
%     ---------------------------------------------------------
%     'haar' or 'db1'   Haar
%     'db4'             4th order Daubechies
%     'sym4'            4th order Symlets
%     'bior6.8'         Cohen-Daubechies-Feauveau biorthogonal
%     'jpeg9.7'         Antonini-Barlaud-Mathieu-Daubechies
%
%     TYPE              Filter Type
%     ---------------------------------------------------------
%     'd'               Decomposition filters
%     'r'               Reconstruction filters
%
%   See also WAVEFAST and WAVEBACK.

%   Copyright 2002-2004 R. C. Gonzalez, R. E. Woods, & S. L. Eddins
%   Digital Image Processing Using MATLAB, Prentice-Hall, 2004
%   $Revision: 1.5 $  $Date: 2003/10/13 01:09:39 $

% Check the input and output arguments.
error(nargchk(1, 2, nargin));

if (nargin == 1 & nargout ~= 4) | (nargin == 2 & nargout ~= 2)
   error('Invalid number of output arguments.');
end

if nargin == 1 & ~ischar(wname)
   error('WNAME must be a string.');
end

if nargin == 2 & ~ischar(type)
   error('TYPE must be a string.');
end

% Create filters for the requested wavelet.
switch lower(wname)
case {'haar', 'db1'}
   ld = [1 1]/sqrt(2);     hd = [-1 1]/sqrt(2);
   lr = ld;                hr = -hd;

case 'db4'
   ld = [-1.059740178499728e-002 3.288301166698295e-002 ...
         3.084138183598697e-002 -1.870348117188811e-001 ...
         -2.798376941698385e-002 6.308807679295904e-001 ...
         7.148465705525415e-001 2.303778133088552e-001];
   t = (0:7);
   hd = ld;    hd(end:-1:1) = cos(pi * t) .* ld;
   lr = ld;    lr(end:-1:1) = ld;
   hr = cos(pi * t) .* ld;

case 'sym4'
   ld = [-7.576571478927333e-002 -2.963552764599851e-002 ...
         4.976186676320155e-001 8.037387518059161e-001 ...
         2.978577956052774e-001 -9.921954357684722e-002 ...
         -1.260396726203783e-002 3.222310060404270e-002];
   t = (0:7);
   hd = ld;    hd(end:-1:1) = cos(pi * t) .* ld;
   lr = ld;    lr(end:-1:1) = ld;
   hr = cos(pi * t) .* ld;

case 'bior6.8'
   ld = [0 1.908831736481291e-003 -1.914286129088767e-003 ...
         -1.699063986760234e-002 1.193456527972926e-002 ...
         4.973290349094079e-002 -7.726317316720414e-002 ...
         -9.405920349573646e-002 4.207962846098268e-001 ...
         8.259229974584023e-001 4.207962846098268e-001 ...
         -9.405920349573646e-002 -7.726317316720414e-002 ...
         4.973290349094079e-002 1.193456527972926e-002 ...
         -1.699063986760234e-002 -1.914286129088767e-003 ...
         1.908831736481291e-003];
   hd = [0 0 0 1.442628250562444e-002 -1.446750489679015e-002 ...
         -7.872200106262882e-002 4.036797903033992e-002 ...
         4.178491091502746e-001 -7.589077294536542e-001 ...
         4.178491091502746e-001 4.036797903033992e-002 ...
         -7.872200106262882e-002 -1.446750489679015e-002 ...
         1.442628250562444e-002 0 0 0 0];
   t = (0:17);
   lr = cos(pi * (t + 1)) .* hd;
   hr = cos(pi * t) .* ld;

case 'jpeg9.7'
   ld = [0 0.02674875741080976 -0.01686411844287495 ...
         -0.07822326652898785 0.2668641184428723 ...
         0.6029490182363579 0.2668641184428723 ...
         -0.07822326652898785 -0.01686411844287495 ...
         0.02674875741080976];
   hd = [0 -0.09127176311424948 0.05754352622849957 ...
         0.5912717631142470 -1.115087052456994 ...
         0.5912717631142470 0.05754352622849957 ...
         -0.09127176311424948 0 0];
   t = (0:9);
   lr = cos(pi * (t + 1)) .* hd;
   hr = cos(pi * t) .* ld;

otherwise
   error('Unrecognizable wavelet name (WNAME).');
end

% Output the requested filters.
if (nargin == 1)
   varargout(1:4) = {ld, hd, lr, hr};
else
   switch lower(type(1))
   case 'd'
      varargout = {ld, hd};
   case 'r'
      varargout = {lr, hr};
   otherwise
      error('Unrecognizable filter TYPE.');
   end
end


function hpdi = hpdi(x, p)
% HPDI - Estimates the Bayesian HPD intervals
%
%   Y = HPDI(X,P) returns a Highest Posterior Density (HPD) interval
%   for each column of X. P must be a scalar. Y is a 2 row matrix
%   where ith column is HPDI for ith column of X.

%   References:
%      [1] Chen, M.-H., Shao, Q.-M., and Ibrahim, J. Q., (2000).
%          Monte Carlo Methods in Bayesian Computation. Springer-Verlag.

% Copyright (C) 2001 Aki Vehtari
%
% This software is distributed under the GNU General Public
% Licence (version 3 or later); please refer to the file
% Licence.txt, included with the software, for details.

if nargin < 2
  error('Not enough arguments')
end

m=size(x,2);
pts=linspace(0.1,99.9-p,20);
pt1=prctile(x,pts);
pt2=prctile(x,p+pts);
cis=abs(pt2-pt1);
[foo,hpdpi]=min(cis);
if m==1
  hpdi=[pt1(hpdpi); pt2(hpdpi)];
else
  hpdpi=sub2ind(size(pt1),hpdpi,1:m);
  hpdi=[pt1(hpdpi); pt2(hpdpi)];
end


function I = integral2(f, S)
%INTEGRAL2    Surface integral of a CHEBFUN3.
%   INTEGRAL2(F, S) returns integral of the CHEBFUN3 object F over the
%   parametric surface S defined as a CHEBFUN2V object.
%
%   I = INTEGRAL2(F) is the same as I = SUM2(F).
%
% See also CHEBFUN3/INTEGRAL, CHEBFUN3/INTEGRAL3, CHEBFUN3/SUM,
% CHEBFUN3/SUM2 and CHEBFUN3/SUM3.

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

% Developer Note: If F = F(x,y,z) is a CHEBFUN3 and
% S = {(u,v)\in DOM, s.t. x = x(u,v), y = y(u,v) and z = z(u,v)}, is a
% parametric surface represented by a CHEBFUN2V object, then
% \int \int_S F(x,y,z) dS = \int \int_DOM F(x(u,v), y(u,v), z(u,v))) ...
%                                                 norm(cross(r_u, r_v)) dA.
% Note that the domain of f should contain the range of S.
% TODO: Check for this in the code.

% Empty check:
if ( isempty(f) )
    I = [];
    return
end

if ( nargin == 1 )
    % Double definite integral:
    I = sum2(f);

   elseif ( nargin == 2 )
       if ( isa(S, 'chebfun2v') )
           % Integral over a parametric surface represented by a CHEBFUN2V.
           % Get the surface:
           S_compon = S.components;
           S1 = S_compon{1};
           S2 = S_compon{2};
           S3 = S_compon{3};

           % Surface integral:
           diffCu = diff(S, 1, 2); % Note the Chebfun2 convention to
           diffCv = diff(S, 1, 1); % use 2 for the 1st variable.
           ds = cross(diffCu, diffCv);
           op = @(u,v) feval(f, feval(S1, u, v), feval(S2, u, v), ...
               feval(S3, u, v));
           I = sum2(chebfun2(op, S1.domain).*ds);

       elseif ( isvector(S) && numel(S) == 2 )
           % Double definite integral over specified dimensions:
           I = sum2(f, S);
       end
else
    error('CHEBFUN:CHEBFUN3:integral2:nargin', ['Incorrect number of '...
        'input arguments.']);
end

end

function ds = cross(F, G)
H = [F(2).*G(3) - F(3).*G(2);
     F(3).*G(1) - F(1).*G(3);
     F(1).*G(2) - F(2).*G(1)];
% Developer note: In principle, here we should use
% ds = sqrt(H(1).^2 + H(2).^2 + H(3).^2);
% which uses chebfun2/sqrt. But, that code calls a singleSingTest
% subroutine which sometimes gives error even in this case where the input
% to sqrt is always nonnegative. We bypass that code by calling chebfun2
% constructor as follows:
ds = chebfun2(@(u,v) sqrt(abs(feval(H(1),u,v).^2 + feval(H(2), u, v).^2 +...
    feval(H(3), u, v).^2)), H(1).domain);
end


function chud_pi_test ( )

%*****************************************************************************80
%
%% CHUD_PI_TEST tests the CHUD_PI function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 February 2012
%
%  Author:
%
%    John Burkardt
%
  clear all

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CHUD_PI_TEST\n' );
  fprintf ( 1, '  CHUD_PI computes the value of pi using the \n' );
  fprintf ( 1, '  Chudnovsky approach.\n' );
%
%  Get pi to 10, 20, 40, 80 decimal digits:
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Request PI to 10, 20, 40, 80 digits.\n' );
  fprintf ( 1, '\n' );
%
%  Trying to print P out neatly using concatenation fails here,
%  although it works in AGM_PI_TEST.
%
%  The procedure for printing out the D-digit representation of
%  a symbolic variable (if you are not willing to simply name
%  the variable and have its value plop out on the screen) is
%  not apparent to me!
%
  d = 10;

  for i = 1 : 4

    p = chud_pi ( d );

    fprintf ( 1, '  Request %6d digits:', d );
    p
    d = d * 2;

  end
%
%  Timings.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  How long does it take to compute PI to D digits?\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '          Digits    Seconds\n' );
  fprintf ( 1, '\n' );
  d = 2;
  for i = 1 : 13
    tic
    p = chud_pi ( d );
    t = toc;
    fprintf ( 1, '  %2d  %10d  %14g\n', i, d, t );
    d = d * 2;
  end

  return
end


function a = cheby_t_inverse ( n )

%*****************************************************************************80
%
%% CHEBY_T_INVERSE returns the inverse of the CHEBY_T matrix.
%
%
%  Example:
%
%    N = 11
%
%      1   .   .  .   .  .  .  .  .  .  .
%      .   1   .  .   .  .  .  .  .  .  .
%      1   .   1  .   .  .  .  .  .  .  .  /   2
%      .   3   .  1   .  .  .  .  .  .  .  /   4
%      3   .   4  .   1  .  .  .  .  .  .  /   8
%      .  10   .  5   .  1  .  .  .  .  .  /  16
%     10   .  15  .   6  .  1  .  .  .  .  /  32
%      .  35   . 21   .  7  .  1  .  .  .  /  64
%     35   .  56  .  28  .  8  .  1  .  .  / 128
%      . 126   . 84   . 36  .  9  .  1  .  / 256
%    126   . 210  . 120  . 45  . 10  .  1  / 512
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 September 20007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  a(1,1) = 1.0;

  if ( n == 1 )
    return
  end

  a(2,2) = 1.0;

  if ( n == 2 )
    return
  end

  for i = 3 : n
    for j = 1 : n
      if ( j == 1 )
        a(i,j) =                      a(i-1,j+1)   / 2.0;
      elseif ( j == 2 )
        a(i,j) = ( 2.0 * a(i-1,j-1) + a(i-1,j+1) ) / 2.0;
      elseif ( j < n )
        a(i,j) = (       a(i-1,j-1) + a(i-1,j+1) ) / 2.0;
      else
        a(i,j) =         a(i-1,j-1)                / 2.0;
      end
    end
  end

  return
end


function value = r4_si ( x )

%*****************************************************************************80
%
%% R4_SI evaluates the sine integral Si of an R4 argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VASLUE, the sine integral Si evaluated at X.
%
  persistent nsi
  persistent sics
  persistent xsml

  if ( isempty ( nsi ) )

    sics = [ ...
      -0.1315646598184841929, ...
      -0.2776578526973601892, ...
       0.0354414054866659180, ...
      -0.0025631631447933978, ...
       0.0001162365390497009, ...
      -0.0000035904327241606, ...
       0.0000000802342123706, ...
      -0.0000000013562997693, ...
       0.0000000000179440722, ...
      -0.0000000000001908387, ...
       0.0000000000000016670, ...
      -0.0000000000000000122 ]';

    nsi = r4_inits ( sics, 12, 0.1 * r4_mach ( 3 ) );
    xsml = sqrt ( eps );

  end

  absx = abs ( x );

  if ( absx < xsml )

    value = x;

  elseif ( absx <= 4.0 )

    value = x * ( 0.75 + r4_csevl ( ( x * x - 8.0 ) * 0.125, sics, nsi ) );

  else

    [ f, g ] = r4_sifg ( absx );
    cosx = cos ( absx );

    if ( x < 0.0 )
      value = - 0.5 * pi + f * cosx + g * sin ( x );
    else
      value = 0.5 * pi - f * cosx - g * sin ( x );
    end

  end

  return
end


function [ vX ] = ProxLogisticLossFunctionGd( vX, vY, vC, paramLambda, numIterations, stopThr )
% ----------------------------------------------------------------------------------------------- %
% [ vX ] = ProxLogisticLossFunction( vX, vY, vC, paramLambda, numIterations, stopThr )
%   Calculates the Prox of the Logistic Cost Function using Gradient Descent.
% Input:
%   - vX            -   Input Vector.
%                       Starting point for the iterative procedure.
%                       Structure: Vector (Column).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
%   - vY            -   Input Vector.
%                       Structure: Vector (Column).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
%   - vC            -   Model Vector.
%                       The model vector in the Logsitic Cost Function.
%                       Structure: Vector (Column).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
%   - numIterations -   Number of Iterations.
%                       Sets the number of iterations for the algorithm to
%                       run.
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range {1, 2, ...}.
%   - stopTol       -   Stopping Condition Tolerance.
%                       Sets the stopping threshold for the L Inf (Maximum
%                       Absolute Value) of the change between 2 iterations
%                       of the algorithm.
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range [0, inf).
% Output:
%   - vX            -   Output Vector.
%                       The Prox for the logistic cost function for the
%                       input vector 'vY'.
%                       Structure: Vector (Column).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
% References
%   1.  https://math.stackexchange.com/a/3571521/33.
%   2.  Elementary Numerical Analysis MATH:3800/CS:3700(22M:072/22C:072)
%       (https://homepage.divms.uiowa.edu/~whan/3800.d/3800.html, Section 3.4).
% Remarks:
%   1.  On some cases it fails to converge (While Newton Method converge to
%       the right solution).
% TODO:
%   1.  C
% Release Notes:
%   -   1.0.000     06/03/2020  Royi Avital
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

FALSE   = 0;
TRUE    = 1;

OFF     = 0;
ON      = 1;

vXPrev = vX;
vG = zeros(size(vX, 1), 1);

hObjFun = @(vX) 0.5 * sum((vX - vY) .^ 2) + (paramLambda * log(1 + exp(-vC.' * vX)));
stepSizeMax = 2;
sSolverOptions = optimset('fminbnd');
sSolverOptions = optimset(sSolverOptions, 'Display', 'off');

for ii = 1: numIterations
    vXPrev(:) = vX;

    valExp  = exp(-vC.' * vX);
    vG(:) = vX - vY - (paramLambda * (valExp / (1 + valExp)) * vC);

    hStepSizeFun    = @(stepSize) hObjFun(vX - (stepSize * vG));
    stepSize        = fminbnd(hStepSizeFun, 0, stepSizeMax, sSolverOptions);

    vX(:) = vX - (stepSize * vG);

    if(max(abs(vX - vXPrev)) < stopThr)
        break;
    end
end

end


function [zgrid,xgrid,ygrid] = gridfit(x,y,z,xnodes,ynodes,varargin)
% gridfit: estimates a surface on a 2d grid, based on scattered data
%          Replicates are allowed. All methods extrapolate to the grid
%          boundaries. Gridfit uses a modified ridge estimator to
%          generate the surface, where the bias is toward smoothness.
%
%          Gridfit is not an interpolant. Its goal is a smooth surface
%          that approximates your data, but allows you to control the
%          amount of smoothing.
%
% usage #1: zgrid = gridfit(x,y,z,xnodes,ynodes);
% usage #2: [zgrid,xgrid,ygrid] = gridfit(x,y,z,xnodes,ynodes);
% usage #3: zgrid = gridfit(x,y,z,xnodes,ynodes,prop,val,prop,val,...);
%
% Arguments: (input)
%  x,y,z - vectors of equal lengths, containing arbitrary scattered data
%          The only constraint on x and y is they cannot ALL fall on a
%          single line in the x-y plane. Replicate points will be treated
%          in a least squares sense.
%
%          ANY points containing a NaN are ignored in the estimation
%
%  xnodes - vector defining the nodes in the grid in the independent
%          variable (x). xnodes need not be equally spaced. xnodes
%          must completely span the data. If they do not, then the
%          'extend' property is applied, adjusting the first and last
%          nodes to be extended as necessary. See below for a complete
%          description of the 'extend' property.
%
%          If xnodes is a scalar integer, then it specifies the number
%          of equally spaced nodes between the min and max of the data.
%
%  ynodes - vector defining the nodes in the grid in the independent
%          variable (y). ynodes need not be equally spaced.
%
%          If ynodes is a scalar integer, then it specifies the number
%          of equally spaced nodes between the min and max of the data.
%
%          Also see the extend property.
%
%  Additional arguments follow in the form of property/value pairs.
%  Valid properties are:
%    'smoothness', 'interp', 'regularizer', 'solver', 'maxiter'
%    'extend', 'tilesize', 'overlap'
%
%  Any UNAMBIGUOUS shortening (even down to a single letter) is
%  valid for property names. All properties have default values,
%  chosen (I hope) to give a reasonable result out of the box.
%
%   'smoothness' - scalar or vector of length 2 - determines the
%          eventual smoothness of the estimated surface. A larger
%          value here means the surface will be smoother. Smoothness
%          must be a non-negative real number.
%
%          If this parameter is a vector of length 2, then it defines
%          the relative smoothing to be associated with the x and y
%          variables. This allows the user to apply a different amount
%          of smoothing in the x dimension compared to the y dimension.
%
%          Note: the problem is normalized in advance so that a
%          smoothness of 1 MAY generate reasonable results. If you
%          find the result is too smooth, then use a smaller value
%          for this parameter. Likewise, bumpy surfaces suggest use
%          of a larger value. (Sometimes, use of an iterative solver
%          with too small a limit on the maximum number of iterations
%          will result in non-convergence.)
%
%          DEFAULT: 1
%
%
%   'interp' - character, denotes the interpolation scheme used
%          to interpolate the data.
%
%          DEFAULT: 'triangle'
%
%          'bilinear' - use bilinear interpolation within the grid
%                     (also known as tensor product linear interpolation)
%
%          'triangle' - split each cell in the grid into a triangle,
%                     then linear interpolation inside each triangle
%
%          'nearest' - nearest neighbor interpolation. This will
%                     rarely be a good choice, but I included it
%                     as an option for completeness.
%
%
%   'regularizer' - character flag, denotes the regularization
%          paradignm to be used. There are currently three options.
%
%          DEFAULT: 'gradient'
%
%          'diffusion' or 'laplacian' - uses a finite difference
%              approximation to the Laplacian operator (i.e, del^2).
%
%              We can think of the surface as a plate, wherein the
%              bending rigidity of the plate is specified by the user
%              as a number relative to the importance of fidelity to
%              the data. A stiffer plate will result in a smoother
%              surface overall, but fit the data less well. I've
%              modeled a simple plate using the Laplacian, del^2. (A
%              projected enhancement is to do a better job with the
%              plate equations.)
%
%              We can also view the regularizer as a diffusion problem,
%              where the relative thermal conductivity is supplied.
%              Here interpolation is seen as a problem of finding the
%              steady temperature profile in an object, given a set of
%              points held at a fixed temperature. Extrapolation will
%              be linear. Both paradigms are appropriate for a Laplacian
%              regularizer.
%
%          'gradient' - attempts to ensure the gradient is as smooth
%              as possible everywhere. Its subtly different from the
%              'diffusion' option, in that here the directional
%              derivatives are biased to be smooth across cell
%              boundaries in the grid.
%
%              The gradient option uncouples the terms in the Laplacian.
%              Think of it as two coupled PDEs instead of one PDE. Why
%              are they different at all? The terms in the Laplacian
%              can balance each other.
%
%          'springs' - uses a spring model connecting nodes to each
%              other, as well as connecting data points to the nodes
%              in the grid. This choice will cause any extrapolation
%              to be as constant as possible.
%
%              Here the smoothing parameter is the relative stiffness
%              of the springs connecting the nodes to each other compared
%              to the stiffness of a spting connecting the lattice to
%              each data point. Since all springs have a rest length
%              (length at which the spring has zero potential energy)
%              of zero, any extrapolation will be minimized.
%
%          Note: The 'springs' regularizer tends to drag the surface
%          towards the mean of all the data, so too large a smoothing
%          parameter may be a problem.
%
%
%   'solver' - character flag - denotes the solver used for the
%          resulting linear system. Different solvers will have
%          different solution times depending upon the specific
%          problem to be solved. Up to a certain size grid, the
%          direct \ solver will often be speedy, until memory
%          swaps causes problems.
%
%          What solver should you use? Problems with a significant
%          amount of extrapolation should avoid lsqr. \ may be
%          best numerically for small smoothnesss parameters and
%          high extents of extrapolation.
%
%          Large numbers of points will slow down the direct
%          \, but when applied to the normal equations, \ can be
%          quite fast. Since the equations generated by these
%          methods will tend to be well conditioned, the normal
%          equations are not a bad choice of method to use. Beware
%          when a small smoothing parameter is used, since this will
%          make the equations less well conditioned.
%
%          DEFAULT: 'normal'
%
%          '\' - uses matlab's backslash operator to solve the sparse
%                     system. 'backslash' is an alternate name.
%
%          'symmlq' - uses matlab's iterative symmlq solver
%
%          'lsqr' - uses matlab's iterative lsqr solver
%
%          'normal' - uses \ to solve the normal equations.
%
%
%   'maxiter' - only applies to iterative solvers - defines the
%          maximum number of iterations for an iterative solver
%
%          DEFAULT: min(10000,length(xnodes)*length(ynodes))
%
%
%   'extend' - character flag - controls whether the first and last
%          nodes in each dimension are allowed to be adjusted to
%          bound the data, and whether the user will be warned if
%          this was deemed necessary to happen.
%
%          DEFAULT: 'warning'
%
%          'warning' - Adjust the first and/or last node in
%                     x or y if the nodes do not FULLY contain
%                     the data. Issue a warning message to this
%                     effect, telling the amount of adjustment
%                     applied.
%
%          'never'  - Issue an error message when the nodes do
%                     not absolutely contain the data.
%
%          'always' - automatically adjust the first and last
%                     nodes in each dimension if necessary.
%                     No warning is given when this option is set.
%
%
%   'tilesize' - grids which are simply too large to solve for
%          in one single estimation step can be built as a set
%          of tiles. For example, a 1000x1000 grid will require
%          the estimation of 1e6 unknowns. This is likely to
%          require more memory (and time) than you have available.
%          But if your data is dense enough, then you can model
%          it locally using smaller tiles of the grid.
%
%          My recommendation for a reasonable tilesize is
%          roughly 100 to 200. Tiles of this size take only
%          a few seconds to solve normally, so the entire grid
%          can be modeled in a finite amount of time. The minimum
%          tilesize can never be less than 3, although even this
%          size tile is so small as to be ridiculous.
%
%          If your data is so sparse than some tiles contain
%          insufficient data to model, then those tiles will
%          be left as NaNs.
%
%          DEFAULT: inf
%
%
%   'overlap' - Tiles in a grid have some overlap, so they
%          can minimize any problems along the edge of a tile.
%          In this overlapped region, the grid is built using a
%          bi-linear combination of the overlapping tiles.
%
%          The overlap is specified as a fraction of the tile
%          size, so an overlap of 0.20 means there will be a 20%
%          overlap of successive tiles. I do allow a zero overlap,
%          but it must be no more than 1/2.
%
%          0 <= overlap <= 0.5
%
%          Overlap is ignored if the tilesize is greater than the
%          number of nodes in both directions.
%
%          DEFAULT: 0.20
%
%
%   'autoscale' - Some data may have widely different scales on
%          the respective x and y axes. If this happens, then
%          the regularization may experience difficulties.
%
%          autoscale = 'on' will cause gridfit to scale the x
%          and y node intervals to a unit length. This should
%          improve the regularization procedure. The scaling is
%          purely internal.
%
%          autoscale = 'off' will disable automatic scaling
%
%          DEFAULT: 'on'
%
%
% Arguments: (output)
%  zgrid   - (nx,ny) array containing the fitted surface
%
%  xgrid, ygrid - as returned by meshgrid(xnodes,ynodes)
%
%
% Speed considerations:
%  Remember that gridfit must solve a LARGE system of linear
%  equations. There will be as many unknowns as the total
%  number of nodes in the final lattice. While these equations
%  may be sparse, solving a system of 10000 equations may take
%  a second or so. Very large problems may benefit from the
%  iterative solvers or from tiling.
%
%
% Example usage:
%
%  x = rand(100,1);
%  y = rand(100,1);
%  z = exp(x+2*y);
%  xnodes = 0:.1:1;
%  ynodes = 0:.1:1;
%
%  g = gridfit(x,y,z,xnodes,ynodes);
%
% Note: this is equivalent to the following call:
%
%  g = gridfit(x,y,z,xnodes,ynodes, ...
%              'smooth',1, ...
%              'interp','triangle', ...
%              'solver','normal', ...
%              'regularizer','gradient', ...
%              'extend','warning', ...
%              'tilesize',inf);
%
%
% Author: John D'Errico
% e-mail address: woodchips@rochester.rr.com
% Release: 2.0
% Release date: 5/23/06

% set defaults
params.smoothness = 1;
params.interp = 'triangle';
params.regularizer = 'gradient';
params.solver = 'backslash';
params.maxiter = [];
params.extend = 'warning';
params.tilesize = inf;
params.overlap = 0.20;
params.mask = [];
params.autoscale = 'on';
params.xscale = 1;
params.yscale = 1;

% was the params struct supplied?
if ~isempty(varargin)
  if isstruct(varargin{1})
    % params is only supplied if its a call from tiled_gridfit
    params = varargin{1};
    if length(varargin)>1
      % check for any overrides
      params = parse_pv_pairs(params,varargin{2:end});
    end
  else
    % check for any overrides of the defaults
    params = parse_pv_pairs(params,varargin);

  end
end

% check the parameters for acceptability
params = check_params(params);

% ensure all of x,y,z,xnodes,ynodes are column vectors,
% also drop any NaN data
x=x(:);
y=y(:);
z=z(:);
k = isnan(x) | isnan(y) | isnan(z);
if any(k)
  x(k)=[];
  y(k)=[];
  z(k)=[];
end
xmin = min(x);
xmax = max(x);
ymin = min(y);
ymax = max(y);

% did they supply a scalar for the nodes?
if length(xnodes)==1
  xnodes = linspace(xmin,xmax,xnodes)';
  xnodes(end) = xmax; % make sure it hits the max
end
if length(ynodes)==1
  ynodes = linspace(ymin,ymax,ynodes)';
  ynodes(end) = ymax; % make sure it hits the max
end

xnodes=xnodes(:);
ynodes=ynodes(:);
dx = diff(xnodes);
dy = diff(ynodes);
nx = length(xnodes);
ny = length(ynodes);
ngrid = nx*ny;

% set the scaling if autoscale was on
if strcmpi(params.autoscale,'on')
  params.xscale = mean(dx);
  params.yscale = mean(dy);
  params.autoscale = 'off';
end

% check to see if any tiling is necessary
if (params.tilesize < max(nx,ny))
  % split it into smaller tiles. compute zgrid and ygrid
  % at the very end if requested
  zgrid = tiled_gridfit(x,y,z,xnodes,ynodes,params);
else
  % its a single tile.

  % mask must be either an empty array, or a boolean
  % aray of the same size as the final grid.
  nmask = size(params.mask);
  if ~isempty(params.mask) && ((nmask(2)~=nx) || (nmask(1)~=ny))
    if ((nmask(2)==ny) || (nmask(1)==nx))
      error 'Mask array is probably transposed from proper orientation.'
    else
      error 'Mask array must be the same size as the final grid.'
    end
  end
  if ~isempty(params.mask)
    params.maskflag = 1;
  else
    params.maskflag = 0;
  end

  % default for maxiter?
  if isempty(params.maxiter)
    params.maxiter = min(10000,nx*ny);
  end

  % check lengths of the data
  n = length(x);
  if (length(y)~=n) || (length(z)~=n)
    error 'Data vectors are incompatible in size.'
  end
  if n<3
    error 'Insufficient data for surface estimation.'
  end

  % verify the nodes are distinct
  if any(diff(xnodes)<=0) || any(diff(ynodes)<=0)
    error 'xnodes and ynodes must be monotone increasing'
  end

  % do we need to tweak the first or last node in x or y?
  if xmin<xnodes(1)
    switch params.extend
      case 'always'
        xnodes(1) = xmin;
      case 'warning'
        warning('GRIDFIT:extend',['xnodes(1) was decreased by: ',num2str(xnodes(1)-xmin),', new node = ',num2str(xmin)])
        xnodes(1) = xmin;
      case 'never'
        error(['Some x (',num2str(xmin),') falls below xnodes(1) by: ',num2str(xnodes(1)-xmin)])
    end
  end
  if xmax>xnodes(end)
    switch params.extend
      case 'always'
        xnodes(end) = xmax;
      case 'warning'
        warning('GRIDFIT:extend',['xnodes(end) was increased by: ',num2str(xmax-xnodes(end)),', new node = ',num2str(xmax)])
        xnodes(end) = xmax;
      case 'never'
        error(['Some x (',num2str(xmax),') falls above xnodes(end) by: ',num2str(xmax-xnodes(end))])
    end
  end
  if ymin<ynodes(1)
    switch params.extend
      case 'always'
        ynodes(1) = ymin;
      case 'warning'
        warning('GRIDFIT:extend',['ynodes(1) was decreased by: ',num2str(ynodes(1)-ymin),', new node = ',num2str(ymin)])
        ynodes(1) = ymin;
      case 'never'
        error(['Some y (',num2str(ymin),') falls below ynodes(1) by: ',num2str(ynodes(1)-ymin)])
    end
  end
  if ymax>ynodes(end)
    switch params.extend
      case 'always'
        ynodes(end) = ymax;
      case 'warning'
        warning('GRIDFIT:extend',['ynodes(end) was increased by: ',num2str(ymax-ynodes(end)),', new node = ',num2str(ymax)])
        ynodes(end) = ymax;
      case 'never'
        error(['Some y (',num2str(ymax),') falls above ynodes(end) by: ',num2str(ymax-ynodes(end))])
    end
  end

  % determine which cell in the array each point lies in
  [junk,indx] = histc(x,xnodes); %#ok
  [junk,indy] = histc(y,ynodes); %#ok
  % any point falling at the last node is taken to be
  % inside the last cell in x or y.
  k=(indx==nx);
  indx(k)=indx(k)-1;
  k=(indy==ny);
  indy(k)=indy(k)-1;
  ind = indy + ny*(indx-1);

  % Do we have a mask to apply?
  if params.maskflag
    % if we do, then we need to ensure that every
    % cell with at least one data point also has at
    % least all of its corners unmasked.
    params.mask(ind) = 1;
    params.mask(ind+1) = 1;
    params.mask(ind+ny) = 1;
    params.mask(ind+ny+1) = 1;
  end

  % interpolation equations for each point
  tx = min(1,max(0,(x - xnodes(indx))./dx(indx)));
  ty = min(1,max(0,(y - ynodes(indy))./dy(indy)));
  % Future enhancement: add cubic interpolant
  switch params.interp
    case 'triangle'
      % linear interpolation inside each triangle
      k = (tx > ty);
      L = ones(n,1);
      L(k) = ny;

      t1 = min(tx,ty);
      t2 = max(tx,ty);
      A = sparse(repmat((1:n)',1,3),[ind,ind+ny+1,ind+L], ...
        [1-t2,t1,t2-t1],n,ngrid);

    case 'nearest'
      % nearest neighbor interpolation in a cell
      k = round(1-ty) + round(1-tx)*ny;
      A = sparse((1:n)',ind+k,ones(n,1),n,ngrid);

    case 'bilinear'
      % bilinear interpolation in a cell
      A = sparse(repmat((1:n)',1,4),[ind,ind+1,ind+ny,ind+ny+1], ...
        [(1-tx).*(1-ty), (1-tx).*ty, tx.*(1-ty), tx.*ty], ...
        n,ngrid);

  end
  rhs = z;

  % do we have relative smoothing parameters?
  if numel(params.smoothness) == 1
    % it was scalar, so treat both dimensions equally
    smoothparam = params.smoothness;
    xyRelativeStiffness = [1;1];
  else
    % It was a vector, so anisotropy reigns.
    % I've already checked that the vector was of length 2
    smoothparam = sqrt(prod(params.smoothness));
    xyRelativeStiffness = params.smoothness(:)./smoothparam;
  end

  % Build regularizer. Add del^4 regularizer one day.
  switch params.regularizer
    case 'springs'
      % zero "rest length" springs
      [i,j] = meshgrid(1:nx,1:(ny-1));
      ind = j(:) + ny*(i(:)-1);
      m = nx*(ny-1);
      stiffness = 1./(dy/params.yscale);
      Areg = sparse(repmat((1:m)',1,2),[ind,ind+1], ...
        xyRelativeStiffness(2)*stiffness(j(:))*[-1 1], ...
        m,ngrid);

      [i,j] = meshgrid(1:(nx-1),1:ny);
      ind = j(:) + ny*(i(:)-1);
      m = (nx-1)*ny;
      stiffness = 1./(dx/params.xscale);
      Areg = [Areg;sparse(repmat((1:m)',1,2),[ind,ind+ny], ...
        xyRelativeStiffness(1)*stiffness(i(:))*[-1 1],m,ngrid)];

      [i,j] = meshgrid(1:(nx-1),1:(ny-1));
      ind = j(:) + ny*(i(:)-1);
      m = (nx-1)*(ny-1);
      stiffness = 1./sqrt((dx(i(:))/params.xscale/xyRelativeStiffness(1)).^2 + ...
        (dy(j(:))/params.yscale/xyRelativeStiffness(2)).^2);

      Areg = [Areg;sparse(repmat((1:m)',1,2),[ind,ind+ny+1], ...
        stiffness*[-1 1],m,ngrid)];

      Areg = [Areg;sparse(repmat((1:m)',1,2),[ind+1,ind+ny], ...
        stiffness*[-1 1],m,ngrid)];

    case {'diffusion' 'laplacian'}
      % thermal diffusion using Laplacian (del^2)
      [i,j] = meshgrid(1:nx,2:(ny-1));
      ind = j(:) + ny*(i(:)-1);
      dy1 = dy(j(:)-1)/params.yscale;
      dy2 = dy(j(:))/params.yscale;

      Areg = sparse(repmat(ind,1,3),[ind-1,ind,ind+1], ...
        xyRelativeStiffness(2)*[-2./(dy1.*(dy1+dy2)), ...
        2./(dy1.*dy2), -2./(dy2.*(dy1+dy2))],ngrid,ngrid);

      [i,j] = meshgrid(2:(nx-1),1:ny);
      ind = j(:) + ny*(i(:)-1);
      dx1 = dx(i(:)-1)/params.xscale;
      dx2 = dx(i(:))/params.xscale;

      Areg = Areg + sparse(repmat(ind,1,3),[ind-ny,ind,ind+ny], ...
        xyRelativeStiffness(1)*[-2./(dx1.*(dx1+dx2)), ...
        2./(dx1.*dx2), -2./(dx2.*(dx1+dx2))],ngrid,ngrid);

    case 'gradient'
      % Subtly different from the Laplacian. A point for future
      % enhancement is to do it better for the triangle interpolation
      % case.
      [i,j] = meshgrid(1:nx,2:(ny-1));
      ind = j(:) + ny*(i(:)-1);
      dy1 = dy(j(:)-1)/params.yscale;
      dy2 = dy(j(:))/params.yscale;

      Areg = sparse(repmat(ind,1,3),[ind-1,ind,ind+1], ...
        xyRelativeStiffness(2)*[-2./(dy1.*(dy1+dy2)), ...
        2./(dy1.*dy2), -2./(dy2.*(dy1+dy2))],ngrid,ngrid);

      [i,j] = meshgrid(2:(nx-1),1:ny);
      ind = j(:) + ny*(i(:)-1);
      dx1 = dx(i(:)-1)/params.xscale;
      dx2 = dx(i(:))/params.xscale;

      Areg = [Areg;sparse(repmat(ind,1,3),[ind-ny,ind,ind+ny], ...
        xyRelativeStiffness(1)*[-2./(dx1.*(dx1+dx2)), ...
        2./(dx1.*dx2), -2./(dx2.*(dx1+dx2))],ngrid,ngrid)];

  end
  nreg = size(Areg,1);

  % Append the regularizer to the interpolation equations,
  % scaling the problem first. Use the 1-norm for speed.
  NA = norm(A,1);
  NR = norm(Areg,1);
  A = [A;Areg*(smoothparam*NA/NR)];
  rhs = [rhs;zeros(nreg,1)];
  % do we have a mask to apply?
  if params.maskflag
    unmasked = find(params.mask);
  end
  % solve the full system, with regularizer attached
  switch params.solver
    case {'\' 'backslash'}
      if params.maskflag
        % there is a mask to use
        zgrid=nan(ny,nx);
        zgrid(unmasked) = A(:,unmasked)\rhs;
      else
        % no mask
        zgrid = reshape(A\rhs,ny,nx);
      end

    case 'normal'
      % The normal equations, solved with \. Can be faster
      % for huge numbers of data points, but reasonably
      % sized grids. The regularizer makes A well conditioned
      % so the normal equations are not a terribly bad thing
      % here.
      if params.maskflag
        % there is a mask to use
        Aunmasked = A(:,unmasked);
        zgrid=nan(ny,nx);
        zgrid(unmasked) = (Aunmasked'*Aunmasked)\(Aunmasked'*rhs);
      else
        zgrid = reshape((A'*A)\(A'*rhs),ny,nx);
      end

    case 'symmlq'
      % iterative solver - symmlq - requires a symmetric matrix,
      % so use it to solve the normal equations. No preconditioner.
      tol = abs(max(z)-min(z))*1.e-13;
      if params.maskflag
        % there is a mask to use
        zgrid=nan(ny,nx);
        [zgrid(unmasked),flag] = symmlq(A(:,unmasked)'*A(:,unmasked), ...
          A(:,unmasked)'*rhs,tol,params.maxiter);
      else
        [zgrid,flag] = symmlq(A'*A,A'*rhs,tol,params.maxiter);
        zgrid = reshape(zgrid,ny,nx);
      end
      % display a warning if convergence problems
      switch flag
        case 0
          % no problems with convergence
        case 1
          % SYMMLQ iterated MAXIT times but did not converge.
          warning('GRIDFIT:solver',['Symmlq performed ',num2str(params.maxiter), ...
            ' iterations but did not converge.'])
        case 3
          % SYMMLQ stagnated, successive iterates were the same
          warning('GRIDFIT:solver','Symmlq stagnated without apparent convergence.')
        otherwise
          warning('GRIDFIT:solver',['One of the scalar quantities calculated in',...
            ' symmlq was too small or too large to continue computing.'])
      end

    case 'lsqr'
      % iterative solver - lsqr. No preconditioner here.
      tol = abs(max(z)-min(z))*1.e-13;
      if params.maskflag
        % there is a mask to use
        zgrid=nan(ny,nx);
        [zgrid(unmasked),flag] = lsqr(A(:,unmasked),rhs,tol,params.maxiter);
      else
        [zgrid,flag] = lsqr(A,rhs,tol,params.maxiter);
        zgrid = reshape(zgrid,ny,nx);
      end

      % display a warning if convergence problems
      switch flag
        case 0
          % no problems with convergence
        case 1
          % lsqr iterated MAXIT times but did not converge.
          warning('GRIDFIT:solver',['Lsqr performed ', ...
            num2str(params.maxiter),' iterations but did not converge.'])
        case 3
          % lsqr stagnated, successive iterates were the same
          warning('GRIDFIT:solver','Lsqr stagnated without apparent convergence.')
        case 4
          warning('GRIDFIT:solver',['One of the scalar quantities calculated in',...
            ' LSQR was too small or too large to continue computing.'])
      end

  end  % switch params.solver

end  % if params.tilesize...

% only generate xgrid and ygrid if requested.
if nargout>1
  [xgrid,ygrid]=meshgrid(xnodes,ynodes);
end

% ============================================
% End of main function - gridfit
% ============================================

% ============================================
% subfunction - parse_pv_pairs
% ============================================
function params=parse_pv_pairs(params,pv_pairs)
% parse_pv_pairs: parses sets of property value pairs, allows defaults
% usage: params=parse_pv_pairs(default_params,pv_pairs)
%
% arguments: (input)
%  default_params - structure, with one field for every potential
%             property/value pair. Each field will contain the default
%             value for that property. If no default is supplied for a
%             given property, then that field must be empty.
%
%  pv_array - cell array of property/value pairs.
%             Case is ignored when comparing properties to the list
%             of field names. Also, any unambiguous shortening of a
%             field/property name is allowed.
%
% arguments: (output)
%  params   - parameter struct that reflects any updated property/value
%             pairs in the pv_array.
%
% Example usage:
% First, set default values for the parameters. Assume we
% have four parameters that we wish to use optionally in
% the function examplefun.
%
%  - 'viscosity', which will have a default value of 1
%  - 'volume', which will default to 1
%  - 'pie' - which will have default value 3.141592653589793
%  - 'description' - a text field, left empty by default
%
% The first argument to examplefun is one which will always be
% supplied.
%
%   function examplefun(dummyarg1,varargin)
%   params.Viscosity = 1;
%   params.Volume = 1;
%   params.Pie = 3.141592653589793
%
%   params.Description = '';
%   params=parse_pv_pairs(params,varargin);
%   params
%
% Use examplefun, overriding the defaults for 'pie', 'viscosity'
% and 'description'. The 'volume' parameter is left at its default.
%
%   examplefun(rand(10),'vis',10,'pie',3,'Description','Hello world')
%
% params =
%     Viscosity: 10
%        Volume: 1
%           Pie: 3
%   Description: 'Hello world'
%
% Note that capitalization was ignored, and the property 'viscosity'
% was truncated as supplied. Also note that the order the pairs were
% supplied was arbitrary.

npv = length(pv_pairs);
n = npv/2;

if n~=floor(n)
  error 'Property/value pairs must come in PAIRS.'
end
if n<=0
  % just return the defaults
  return
end

if ~isstruct(params)
  error 'No structure for defaults was supplied'
end

% there was at least one pv pair. process any supplied
propnames = fieldnames(params);
lpropnames = lower(propnames);
for i=1:n
  p_i = lower(pv_pairs{2*i-1});
  v_i = pv_pairs{2*i};

  ind = strmatch(p_i,lpropnames,'exact');
  if isempty(ind)
    ind = find(strncmp(p_i,lpropnames,length(p_i)));
    if isempty(ind)
      error(['No matching property found for: ',pv_pairs{2*i-1}])
    elseif length(ind)>1
      error(['Ambiguous property name: ',pv_pairs{2*i-1}])
    end
  end
  p_i = propnames{ind};

  % override the corresponding default in params
  params = setfield(params,p_i,v_i); %#ok

end

% ============================================
% subfunction - check_params
% ============================================
function params = check_params(params)

% check the parameters for acceptability
% smoothness == 1 by default
if isempty(params.smoothness)
  params.smoothness = 1;
else
  if (numel(params.smoothness)>2) || any(params.smoothness<=0)
    error 'Smoothness must be scalar (or length 2 vector), real, finite, and positive.'
  end
end

% regularizer  - must be one of 4 options - the second and
% third are actually synonyms.
valid = {'springs', 'diffusion', 'laplacian', 'gradient'};
if isempty(params.regularizer)
  params.regularizer = 'diffusion';
end
ind = find(strncmpi(params.regularizer,valid,length(params.regularizer)));
if (length(ind)==1)
  params.regularizer = valid{ind};
else
  error(['Invalid regularization method: ',params.regularizer])
end

% interp must be one of:
%    'bilinear', 'nearest', or 'triangle'
% but accept any shortening thereof.
valid = {'bilinear', 'nearest', 'triangle'};
if isempty(params.interp)
  params.interp = 'triangle';
end
ind = find(strncmpi(params.interp,valid,length(params.interp)));
if (length(ind)==1)
  params.interp = valid{ind};
else
  error(['Invalid interpolation method: ',params.interp])
end

% solver must be one of:
%    'backslash', '\', 'symmlq', 'lsqr', or 'normal'
% but accept any shortening thereof.
valid = {'backslash', '\', 'symmlq', 'lsqr', 'normal'};
if isempty(params.solver)
  params.solver = '\';
end
ind = find(strncmpi(params.solver,valid,length(params.solver)));
if (length(ind)==1)
  params.solver = valid{ind};
else
  error(['Invalid solver option: ',params.solver])
end

% extend must be one of:
%    'never', 'warning', 'always'
% but accept any shortening thereof.
valid = {'never', 'warning', 'always'};
if isempty(params.extend)
  params.extend = 'warning';
end
ind = find(strncmpi(params.extend,valid,length(params.extend)));
if (length(ind)==1)
  params.extend = valid{ind};
else
  error(['Invalid extend option: ',params.extend])
end

% tilesize == inf by default
if isempty(params.tilesize)
  params.tilesize = inf;
elseif (length(params.tilesize)>1) || (params.tilesize<3)
  error 'Tilesize must be scalar and > 0.'
end

% overlap == 0.20 by default
if isempty(params.overlap)
  params.overlap = 0.20;
elseif (length(params.overlap)>1) || (params.overlap<0) || (params.overlap>0.5)
  error 'Overlap must be scalar and 0 < overlap < 1.'
end

% ============================================
% subfunction - tiled_gridfit
% ============================================
function zgrid=tiled_gridfit(x,y,z,xnodes,ynodes,params)
% tiled_gridfit: a tiled version of gridfit, continuous across tile boundaries
% usage: [zgrid,xgrid,ygrid]=tiled_gridfit(x,y,z,xnodes,ynodes,params)
%
% Tiled_gridfit is used when the total grid is far too large
% to model using a single call to gridfit. While gridfit may take
% only a second or so to build a 100x100 grid, a 2000x2000 grid
% will probably not run at all due to memory problems.
%
% Tiles in the grid with insufficient data (<4 points) will be
% filled with NaNs. Avoid use of too small tiles, especially
% if your data has holes in it that may encompass an entire tile.
%
% A mask may also be applied, in which case tiled_gridfit will
% subdivide the mask into tiles. Note that any boolean mask
% provided is assumed to be the size of the complete grid.
%
% Tiled_gridfit may not be fast on huge grids, but it should run
% as long as you use a reasonable tilesize. 8-)

% Note that we have already verified all parameters in check_params

% Matrix elements in a square tile
tilesize = params.tilesize;
% Size of overlap in terms of matrix elements. Overlaps
% of purely zero cause problems, so force at least two
% elements to overlap.
overlap = max(2,floor(tilesize*params.overlap));

% reset the tilesize for each particular tile to be inf, so
% we will never see a recursive call to tiled_gridfit
Tparams = params;
Tparams.tilesize = inf;

nx = length(xnodes);
ny = length(ynodes);
zgrid = zeros(ny,nx);

% linear ramp for the bilinear interpolation
rampfun = inline('(t-t(1))/(t(end)-t(1))','t');

% loop over each tile in the grid
h = waitbar(0,'Relax and have a cup of JAVA. Its my treat.');
warncount = 0;
xtind = 1:min(nx,tilesize);
while ~isempty(xtind) && (xtind(1)<=nx)

  xinterp = ones(1,length(xtind));
  if (xtind(1) ~= 1)
    xinterp(1:overlap) = rampfun(xnodes(xtind(1:overlap)));
  end
  if (xtind(end) ~= nx)
    xinterp((end-overlap+1):end) = 1-rampfun(xnodes(xtind((end-overlap+1):end)));
  end

  ytind = 1:min(ny,tilesize);
  while ~isempty(ytind) && (ytind(1)<=ny)
    % update the waitbar
    waitbar((xtind(end)-tilesize)/nx + tilesize*ytind(end)/ny/nx)

    yinterp = ones(length(ytind),1);
    if (ytind(1) ~= 1)
      yinterp(1:overlap) = rampfun(ynodes(ytind(1:overlap)));
    end
    if (ytind(end) ~= ny)
      yinterp((end-overlap+1):end) = 1-rampfun(ynodes(ytind((end-overlap+1):end)));
    end

    % was a mask supplied?
    if ~isempty(params.mask)
      submask = params.mask(ytind,xtind);
      Tparams.mask = submask;
    end

    % extract data that lies in this grid tile
    k = (x>=xnodes(xtind(1))) & (x<=xnodes(xtind(end))) & ...
        (y>=ynodes(ytind(1))) & (y<=ynodes(ytind(end)));
    k = find(k);

    if length(k)<4
      if warncount == 0
        warning('GRIDFIT:tiling','A tile was too underpopulated to model. Filled with NaNs.')
      end
      warncount = warncount + 1;

      % fill this part of the grid with NaNs
      zgrid(ytind,xtind) = NaN;

    else
      % build this tile
      zgtile = gridfit(x(k),y(k),z(k),xnodes(xtind),ynodes(ytind),Tparams);

      % bilinear interpolation (using an outer product)
      interp_coef = yinterp*xinterp;

      % accumulate the tile into the complete grid
      zgrid(ytind,xtind) = zgrid(ytind,xtind) + zgtile.*interp_coef;

    end

    % step to the next tile in y
    if ytind(end)<ny
      ytind = ytind + tilesize - overlap;
      % are we within overlap elements of the edge of the grid?
      if (ytind(end)+max(3,overlap))>=ny
        % extend this tile to the edge
        ytind = ytind(1):ny;
      end
    else
      ytind = ny+1;
    end

  end % while loop over y

  % step to the next tile in x
  if xtind(end)<nx
    xtind = xtind + tilesize - overlap;
    % are we within overlap elements of the edge of the grid?
    if (xtind(end)+max(3,overlap))>=nx
      % extend this tile to the edge
      xtind = xtind(1):nx;
    end
  else
    xtind = nx+1;
  end

end % while loop over x

% close down the waitbar
close(h)

if warncount>0
  warning('GRIDFIT:tiling',[num2str(warncount),' tiles were underpopulated & filled with NaNs'])
end


%% Edge Element Discretization of Maxwell Equations
% We test Maxwell solvers in iFEM.

clear all; close all
rowNames ={'h=1/2';'h=1/4';'h=1/8'};
colHeaders = {'H^curl Error','L^2 Error'};

%% The data of the pde
%
% * pde = Maxwelldata1; % zero Neumann boundary condition and curl u = 0
% * pde = Maxwelldata2; % non-homogenous Neumann boundary condition
% * pde = Maxwelldata3; % polynomial data and curl u = 0
% * pde = Maxwelldata4; % zero Dirichlet boundary condition
% * pde = Maxwelldata5; % linear polynomial data
% * pde = planewavedataC; % plane wave with complex coefficients
% * pde = planewavedata1; % plane wave with real coefficients

%% Positive Definite Case
% curl curl E + E = f.

help Maxwelldata2
%%
% Dirichlet boundary condition
[node,elem,HB] = cubemesh([-1,1,-1,1,-1,1],0.25);
pde = Maxwelldata2;
bdFlag = setboundary3(node,elem,'Dirichlet');
option.solver = 'cg';
cubeMaxwell2;
%%
makeHtmlTable([energyErr L2Err],[],rowNames,colHeaders);

%%
% Neumann boundary condition
[node,elem,HB] = cubemesh([-1,1,-1,1,-1,1],1);
pde = Maxwelldata2;
bdFlag = setboundary3(node,elem,'Neumann');
option.solver = 'cg';
cubeMaxwell2;
%%
makeHtmlTable([energyErr L2Err],[],rowNames,colHeaders);

%%
% Optimal first order of convergence is achieved. HX preconditioned CG
% converges around 20 steps.

%% Indefinite with real coefficients
% curl curl E - E = f.

help planewavedata1
%%
% Dirichlet boundary condition
[node,elem,HB] = cubemesh([-1,1,-1,1,-1,1],1);
pde = planewavedata1;
bdFlag = setboundary3(node,elem,'Dirichlet');
option.solver = 'cg';
cubeMaxwell2;
%%
makeHtmlTable([energyErr L2Err],[],rowNames,colHeaders);

%%
% Neumann boundary condition
[node,elem,HB] = cubemesh([-1,1,-1,1,-1,1],1);
pde = planewavedata1;
bdFlag = setboundary3(node,elem,'Neumann');
option.solver = 'cg';
cubeMaxwell2;
%%
makeHtmlTable([energyErr L2Err],[],rowNames,colHeaders);

%%
% Optimal first order of convergence is achieved. HX preconditioned CG
% converges around 40 steps although the system is indefinite.

%% Indefinite: complex coefficients, real solution
% curl curl E - (1-i)E = f.

help planewavedataC
%%
% Dirichlet boundary condition
[node,elem,HB] = cubemesh([-1,1,-1,1,-1,1],1);
pde = planewavedataC;
bdFlag = setboundary3(node,elem,'Dirichlet');
option.solver = 'gmres';
cubeMaxwell2;
%%
makeHtmlTable([energyErr L2Err],[],rowNames,colHeaders);

%%
% Neumann boundary condition
[node,elem,HB] = cubemesh([-1,1,-1,1,-1,1],1);
pde = planewavedataC;
bdFlag = setboundary3(node,elem,'Neumann');
option.solver = 'gmres';
cubeMaxwell2;
%%
makeHtmlTable([energyErr L2Err],[],rowNames,colHeaders);

%%
% Optimal first order of convergence is achieved. HX preconditioned GMRES
% converges around 90 steps although the system is indefinite.
%
% Bug: For Neumann boundary condition, the rate of L2 error is not quite right.

%% Indefinite: real coefficents, complex solution
help planewavedata
%%
% Dirichlet boundary condition
[node,elem,HB] = cubemesh([-1,1,-1,1,-1,1],1);
bdFlag = setboundary3(node,elem,'Dirichlet');
option.solver = 'cg';
planewaveMaxwell2;
%%
makeHtmlTable([energyErr L2Err],[],rowNames,colHeaders);

%%
% Neumann boundary condition
[node,elem,HB] = cubemesh([-1,1,-1,1,-1,1],1);
bdFlag = setboundary3(node,elem,'Neumann');
option.solver = 'cg';
planewaveMaxwell2;
%%
makeHtmlTable([energyErr L2Err],[],rowNames,colHeaders);

%%
% The computation of error can't handle complex functions. So in
% planewaveMaxwell the error between uI and uh is computed. Therefore
% slightly better rate of convergence is observed. The rate of L2 error is
% almost second order.


function f = goldstein_price ( x )

%*****************************************************************************80
%
%% GOLDSTEIN_PRICE evaluates the Goldstein-Price polynomial.
%
%  Discussion:
%
%    The minimizer is
%
%      X* = [ 0.0, -1.0 ]
%      F(X*) = 3.0
%
%    Suggested starting point:
%
%      X init = [ -0.5, 0.25 ] (easy convergence)
%      X init = [ -4.0, 5.00 ] (harder convergence)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Zbigniew Michalewicz,
%    Genetic Algorithms + Data Structures = Evolution Programs,
%    Third Edition,
%    Springer Verlag, 1996,
%    ISBN: 3-540-60676-9,
%    LC: QA76.618.M53.
%
%  Parameters:
%
%    Input, real X(2), the argument of the function.
%
%    Output, real F, the value of the function at X.
%
  if ( length ( x ) ~= 2 )
    error ( 'Error: function expects a two dimensional input\n' );
  end

  a = x(1) + x(2) + 1.0;

  b = 19.0 - 14.0 * x(1) + 3.0 * x(1) * x(1) - 14.0 * x(2) ...
    + 6.0 * x(1) * x(2) + 3.0 * x(2) * x(2);

  c = 2.0 * x(1) - 3.0 * x(2);

  d = 18.0 - 32.0 * x(1) + 12.0 * x(1) * x(1) + 48.0 * x(2) ...
    - 36.0 * x(1) * x(2) + 27.0 * x(2) * x(2);

  f = ( 1.0 + a * a * b ) * ( 30.0 + c * c * d );

  return
end


function  gnss = gnss_m2r (lat, h, gnss)
% gnss_m2r: converts GPS standard deviation from meters to radians.
%
% INPUT
%   gnss, GNSS data structure with fields:
%       lat, 1x1 latitude (radians).
%       h,   1x1 altitude (meters).
%       stdm, 1x3 position error profile (m, m, m).
%
% OUTPUT
%   gnss.std, 1x3 position error profile (rad, rad, m).
%
%   Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
%
%   This file is part of NaveGo, an open-source MATLAB toolbox for
%   simulation of integrated navigation systems.
%
%   NaveGo is free software: you can redistribute it and/or modify
%   it under the terms of the GNU Lesser General Public License (LGPL)
%   version 3 as published by the Free Software Foundation.
%
%   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 Lesser General Public License for more details.
%
%   You should have received a copy of the GNU Lesser General Public
%   License along with this program. If not, see
%   <http://www.gnu.org/licenses/>.
%
% References:
%
%   R. Gonzalez, J. Giribet, and H. Patiño. NaveGo: a
% simulation framework for low-cost integrated navigation systems,
% Journal of Control Engineering and Applied Informatics, vol. 17,
% issue 2, pp. 110-120, 2015. Eq. 20.
%
%  	R. Gonzalez, J. Giribet, and H. Patiño. An approach to
% benchmarking of loosely coupled low-cost navigation systems,
% Mathematical and Computer Modelling of Dynamical Systems, vol. 21,
% issue 3, pp. 272-287, 2015. Eq. 7.
%
% Version: 003
% Date:    2021/03/09
% Author:  Rodrigo Gonzalez <rodralez@frm.utn.edu.ar>
% URL:     https://github.com/rodralez/navego

gnss.std =  zeros(1,3);

[RM, RN] = radius(lat);

gnss.std(1) = gnss.stdm(1) / (RM + h);
gnss.std(2) = gnss.stdm(2) / (RN + h) / cos (lat);
gnss.std(3) = gnss.stdm(3);

end


function [ o, x, w ] = epn_glg_02_xiu ( n, alpha )

%*****************************************************************************80
%
%% EPN_GLG_02_XIU implements the Xiu rule for region EPN_GLG.
%
%  Discussion:
%
%    The rule has order
%
%      O = N + 1.
%
%    The rule has precision P = 2.
%
%    EPN_GLG is the N-dimensional positive space [0,+oo)^N with generalized
%    Laguerre weight function:
%
%      w(alpha;x) = product ( 1 <= i <= n ) x(i)^alpha exp ( - x(i) )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 March 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Dongbin Xiu,
%    Numerical integration formulas of degree two,
%    Applied Numerical Mathematics,
%    Volume 58, 2008, pages 1515-1520.
%
%  Parameters:
%
%    Input, integer N, the spatial dimension.
%
%    Input, real ALPHA, the exponent of X in the weight function.
%    -1.0 < ALPHA.
%
%    Input, integer O, the order.
%
%    Output, real X(N,O), the abscissas.
%
%    Output, real W(O), the weights.
%
  if ( alpha <= -1.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'EPN_GLG_02_XIU - Fatal error!\n' );
    fprintf ( 1, '  ALPHA <= -1.0\n' );
    error ( 'EPN_GLG_02_XIU - Fatal error!' );
  end

  o = n + 1;

  x = zeros ( n, o );
  w = zeros ( o, 1 );

  for j = 1 : o

    i = 0;
    for r = 1 : floor ( n / 2 )
      arg = 2 * r * ( j - 1 ) * pi / ( n + 1 );
      i = i + 1;
      x(i,j) = sqrt ( 2.0 ) * cos ( arg );
      i = i + 1;
      x(i,j) = sqrt ( 2.0 ) * sin ( arg );
    end

    if ( i < n )
      i = i + 1;
      x(i,j) = r8_mop ( j - 1 );
    end

  end

  gamma0 = - 1.0;
  delta0 = alpha + 1.0;
  c1 = - alpha - 1.0;

  x(1:n,1:o) = ( sqrt ( gamma0 * c1 ) * x(1:n,1:o) - delta0 ) / gamma0;

  expon = 0;
  volume_1d = ep1_glg_monomial_integral ( expon, alpha );
  volume = volume_1d ^ n;

  w(1:o) = volume / o;

  return
end


function [pt degen] = line_intersect(p1, p2, p3, p4)
% function pt = line_intersect(p1, p2, p3, p4)
% intersection point of line defined by p1 & p2 and line defined by p3 & p4
% http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline2d/
x1 = p1(1); y1 = p1(2);
x2 = p2(1); y2 = p2(2);
x3 = p3(1); y3 = p3(2);
x4 = p4(1); y4 = p4(2);

if (y4-y3)*(x2-x1)-(x4-x3)*(y2-y1) == 0
% 	warning('line_intersect.m degenerate --dclee');
% 	pt = (p1 + p2 + p3 + p4)/4;
    pt = [];
    degen = 1;
	return;
end

pt = [ ...
	x1 + (x2-x1) * ((x4-x3)*(y1-y3)-(y4-y3)*(x1-x3))/((y4-y3)*(x2-x1)-(x4-x3)*(y2-y1)) ...
	y1 + (y2-y1) * ((x4-x3)*(y1-y3)-(y4-y3)*(x1-x3))/((y4-y3)*(x2-x1)-(x4-x3)*(y2-y1)) ];
degen = 0;


function [ x, seed ] = disk01_sample ( n, seed )

%*****************************************************************************80
%
%% DISK01_SAMPLE uniformly samples the unit disk.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 January 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of points.
%
%    Input/output, integer SEED, a seed for the random
%    number generator.
%
%    Output, real X(2,N), the points.
%
  x = randn ( 2, n );
  norm = ones ( 1, 2 ) * ( x.^2 );
  norm = sqrt ( norm );
  for i = 1 : 2
    x(i,1:n) = x(i,1:n) ./ norm(1:n);
  end

  for j = 1 : n
    r = rand ( 1, 1 );
    x(1:2,j) = sqrt ( r  ) * x(1:2,j);
  end

  return
end


function geometry_test061 ( )

%*****************************************************************************80
%
%% TEST061 tests PLANE_NORMAL_BASIS_3D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
  test_num = 5;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST061\n' );
  fprintf ( 1, '  PLANE_NORMAL_BASIS_3D, given a plane in\n' );
  fprintf ( 1, '    point, normal form (P,N), finds two unit\n' );
  fprintf ( 1, '    vectors Q and R that "lie" in the plane\n' );
  fprintf ( 1, '    and are mutually orthogonal.\n' );

  for test = 1 : test_num

    [ pp(1:3,1), seed ] = r8vec_uniform_01 ( 3, seed );

    [ normal(1:3,1), seed ] = r8vec_uniform_01 ( 3, seed );
    t = norm ( normal );
    normal = normal / t;

    [ pq, pr ] = plane_normal_basis_3d ( pp, normal );

    if ( test == 1 )
      fprintf ( 1, '\n' );
      fprintf ( 1, '  Data for test 1:\n' );
      fprintf ( 1, '\n' );
      r8vec_print ( 3, pp, '  Point PP:' );
      r8vec_print ( 3, normal, '  Normal vector N:' );
      r8vec_print ( 3, pq, '  Vector PQ:' );
      r8vec_print ( 3, pr, '  Vector PR:' );
    end

    b(1,1) = normal(1:3,1)' * normal(1:3,1);
    b(1,2) = normal(1:3,1)' * pq(1:3,1);
    b(1,3) = normal(1:3,1)' * pr(1:3,1);

    b(2,1) = pq(1:3,1)' * normal(1:3,1);
    b(2,2) = pq(1:3,1)' * pq(1:3,1);
    b(2,3) = pq(1:3,1)' * pr(1:3,1);

    b(3,1) = pr(1:3,1)' * normal(1:3,1);
    b(3,2) = pr(1:3,1)' * pq(1:3,1);
    b(3,3) = pr(1:3,1)' * pr(1:3,1);

    r8mat_print ( 3, 3, b, '  Dot product matrix:' );

  end

  return
end


function mbasis = basis_matrix_overhauser_uni_r ( )

%*****************************************************************************80
%
%% BASIS_MATRIX_OVERHAUSER_UNI_R sets up the right uniform Overhauser spline basis matrix.
%
%  Discussion:
%
%    This basis matrix assumes that the data points P(N-2), P(N-1),
%    and P(N) are uniformly spaced in T, and that P(N-1) corresponds to
%    T = 0, and P(N) to T = 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real MBASIS(3,3), the basis matrix.
%
  mbasis(1,1) =   2.0;
  mbasis(1,2) = - 4.0;
  mbasis(1,3) =   2.0;

  mbasis(2,1) = - 3.0;
  mbasis(2,2) =   4.0;
  mbasis(2,3) = - 1.0;

  mbasis(3,1) =   1.0;
  mbasis(3,2) =   0.0;
  mbasis(3,3) =   0.0;

  return
end


function [f] = psin(n,z)
%Psin   Arbitrary order Polygamma function valid in the entire complex plane.
%
%                         d^(n+1)
%        polygamma(n,z) = --------log(Gamma(z))
%                         dz^(n+1)
%
%usage: [f] = Psin(n,z)
%
%        if n is 0 or absent then f will be the Digamma function.
%        if n=1,2,3,4,5 etc then f will be
%        the tri-, tetra-, penta-, hexa-, hepta- etc gamma functon
%        Real(n) must be zero or positive.
%
%tested under versions 6.0 and 5.3.1
%
%        Z may be complex and of any size.
%
%        This program uses the partial fraction expansion of the
%        derivative of the Log of an excellent Lanczos series approximation
%        for the Gamma function. Accurate to about 12 digits.
%
%example: psin(101, -45.6-i*29.4)
%         is near 12.5 + 9*i
%
%example: psin(10, -11.5-i*0.577007813568142)
%         is near a root of the decagamma function
%
%example: x=[1:0.005:1.250]'; [x gamma(x) log(gamma(x)) psin(0,x) psin(1,x)]
%         recreates Table 6.1 page 267 from A&S
%
%example: x=[1:0.01:2.00]'; [x psin(2,x) psin(3,x)]
%         recreates Table 6.2 page 271 from A&S
%
%example: x=1; y=[0:0.1:10]'; f=psin(0,x+i*y); [y real(f) imag(f)]
%         recreates Table 6.8 page 288 from A&S
%
%example: x=2; y=[0:0.1:10]'; f=psin(0,x+i*y); [y real(f) imag(f)]
%         recreates the last part of Table 6.8 page 293 from A&S
%
%References: C. Lanczos, SIAM JNA  1, 1964. pp. 86-96
%            Y. Luke, "The Special ... approximations", 1969 pp. 29-31
%            Y. Luke, "Algorithms ... functions", 1977
%            J. Spouge,  SIAM JNA 31, 1994. pp. 931
%            W. Press,  "Numerical Recipes"
%            S. Chang, "Computation of special functions", 1996
%
%
%see also:   GAMMA GAMMALN GAMMAINC
%see also:   mhelp psi
%see also:   mhelp GAMMA

%Paul Godfrey
%pgodfrey@conexant.com
%July 22, 2004
%see gamma for calculation details...

%this routine still works even if n is complex with real(n)>=0
%don't know what the resulting function is called though

% we have bit of a problem for real(z)<0
% we could use the reflection formula, eq #6.4.7 in A&S
% but arbitrary order derivs of cot are cumberson to compute
% (it generates a polynomial in powers of cot)
% so instead we will use a modification of eq #6.4.6
% and call this program recursively to shift z by 500 until real(z)>=0

if nargin==1
   z=n;
   n=0;
end

if n==0
   f=psi(z);
   return
end

if real(n)<0
   error('Invalid Polygamma order')
end

sizeofz=size(z);

isneg=find(real(z)< 0);
isok =find(real(z)>=0);

negmethod=1;
if ~isempty(isneg)
   if negmethod==0
      zneg=z(isneg);
      gneg=psin(n,zneg+1); % recurse if to far to the left...
      hneg=-(-1).^n.*gamma(n+1).*zneg.^-(n+1);
      fneg=gneg+hneg;
   else
      zneg=z(isneg);
      %shift by, say, 500, to speed things up.
      m=500;
      gneg=psin(n,zneg+m);
      hneg=0;
      for k=m-1:-1:0
          hneg=hneg+(zneg+k).^-(n+1);
      end
      hneg=-(-1).^n.*gamma(n+1).*hneg;
      fneg=gneg+hneg;
   end
end
if ~isempty(isok)
   z=z(isok);
end

% the zeros of the Lanczos PFE series when g=607/128 are:

r=[ -4.1614709798720630-.14578107125196249*i;
    -4.1614709798720630+.14578107125196249*i;
    -4.3851935502539474-.19149326909941256*i;
    -4.3851935502539474+.19149326909941256*i;
    -4.0914355423005926;
    -5.0205261882982271;
    -5.9957952053472399;
    -7.0024851819328395;
    -7.9981186370233868;
    -9.0013449037361806;
    -9.9992157162305535;
   -11.0003314815563886;
   -11.9999115102434217;
   -13.0000110489923175587];

% the poles of the Lanczos PFE series are:
%p=[ 0 -1 -2 -3 -4 -5 -6 -7 -8 -9 -10 -11 -12 -13];

e=exp(1);
g=607/128; % best results when 4<=g<=5
h=1/2;

%compute tricky PFE expansion of the deriv of the log
%of the Lanczos series. The series Lanczos' coeffs manifest
%themselves in the zero locations. The residues are all +/- 1
%compare this to A&S page 259, eq# 6.3.16 and page 260, eq# 6.4.10

s=0;
for k=length(r)-1:-1:0
    s=s+(1./((z-r(k+1)).^(n+1))-1./((z+k).^(n+1)));
end
%what happens if n is not a positive integer?
s=(-1).^n.*gamma(n+1).*s;

zgh=z+(g-h);
if n==0
%  s=log(zgh)+(-g./zgh + s);
%  use existing more accurate digamma function if n=0
%  should never reach this code since we trapped it above
   f=psi(z);
   return
else
% do derivs of front end stuff
   s=(-1)^(n+1)*(gamma(n).*zgh.^(-n) + g*gamma(n+1).*zgh.^-(n+1))+s;
end

if ~isempty(isneg)
   f(isneg)=fneg;
end
if ~isempty(isok)
   f(isok)=s;
end

f=reshape(f,sizeofz);

return

%a demo of this program is

warning off
x=[-5:1/64:5]';

figure(1)
axis([min(x) max(x) -20 20])
grid on
hold on

y=[];
for n=0:6
    y(:,n+1)=psin(n,x);
end

plot(repmat(x,1,size(y,2)),y)

figure(2)
x=-10:1/16:10;
y=-5:1/16:5;
[X,Y]=meshgrid(x,y);
z=complex(X,Y);
f=psin(4,z);
g=log10(abs(f));
mesh(x,y,g)
rotate3d on

z=-76+54*i;
[z psin(98, z)]

disp('A zero of Psin1')
z = -0.412134547951937-i*0.597811942320597;
[z psin(1,z)]

warning on
return

% Include this complex psi function
% in case user doesn't have one

function [f] = psi(z)
%Psi     Psi (or Digamma) function valid in the entire complex plane.
%
%                 d
%        Psi(z) = --log(Gamma(z))
%                 dz
%
%usage: [f] = psi(z)
%
%tested under versions 6.0 and 5.3.1
%
%        Z may be complex and of any size.
%
%        This program uses the analytical derivative of the
%        Log of an excellent Lanczos series approximation
%        for the Gamma function.
%
%References: C. Lanczos, SIAM JNA  1, 1964. pp. 86-96
%            Y. Luke, "The Special ... approximations", 1969 pp. 29-31
%            Y. Luke, "Algorithms ... functions", 1977
%            J. Spouge,  SIAM JNA 31, 1994. pp. 931
%            W. Press,  "Numerical Recipes"
%            S. Chang, "Computation of special functions", 1996
%
%
%see also:   GAMMA GAMMALN GAMMAINC
%see also:   mhelp psi
%see also:   mhelp GAMMA

%Paul Godfrey
%pgodfrey@intersil.com
%July 13, 2001
%see gamma for calculation details...

siz = size(z);
z=z(:);
zz=z;

f = 0.*z; % reserve space in advance

%reflection point
p=find(real(z)<0.5);
if ~isempty(p)
   z(p)=1-z(p);
end

%Lanczos approximation for the complex plane

g=607/128; % best results when 4<=g<=5

c = [  0.99999999999999709182;
      57.156235665862923517;
     -59.597960355475491248;
      14.136097974741747174;
      -0.49191381609762019978;
        .33994649984811888699e-4;
        .46523628927048575665e-4;
       -.98374475304879564677e-4;
        .15808870322491248884e-3;
       -.21026444172410488319e-3;
        .21743961811521264320e-3;
       -.16431810653676389022e-3;
        .84418223983852743293e-4;
       -.26190838401581408670e-4;
        .36899182659531622704e-5];

n=0;
d=0;
for k=size(c,1):-1:2
    dz=1./(z+k-2);
    dd=c(k).*dz;
    d=d+dd;
    n=n-dd.*dz;
end
d=d+c(1);
gg=z+g-0.5;
%log is accurate to about 13 digits...

f = log(gg) + (n./d - g./gg) ;

if ~isempty(p)
   f(p) = f(p)-pi*cot(pi*zz(p));
end

p=find(round(zz)==zz & real(zz)<=0 & imag(zz)==0);
if ~isempty(p)
   f(p) = Inf;
end

f=reshape(f,siz);

return

%A demo of this routine is:
clc
clear all
close all
x=-4:1/16:4.5;
y=-4:1/16:4;
[X,Y]=meshgrid(x,y);
z=X+i*Y;
f=psi(z);
p=find(abs(f)>10);
f(p)=10;

mesh(x,y,abs(f),phase(f));
view([45 10]);
rotate3d;

figure(2);
ezplot psi;
grid on;

One=psi(2)-psi(1)
EulerGamma=-psi(1)

return