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


%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                           NACA airfoil generator
%                   coded by Manuel Diaz, NTU, 2015.04.27
%
% Refs:
% [1] Geometry for Aerodynamicists, Appendix A. Accessible online at:
%     http://www.dept.aoe.vt.edu/~mason/Mason_f/CAtxtAppA.pdf
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

close all;

%-------------------------------------------------------------------------%
% Define all the parameters
%-------------------------------------------------------------------------%

Na = 50;
M = 30;
A = 10; %Clustering points near leading edge, inf = no clustering
B = 2; %Clustering points near the surface, 1 = no clustering
t = .12;
c = 1;
L = 1;
N = floor(2*c/(2*c+L)*Na);
x1 = c*ones(1,N+1);
y1 = zeros(1,N+1);
x2 = x1;
y2 = y1;
y2(1) = c;
s1 = y1;
s2 = s1;
N2 = 250;

%-------------------------------------------------------------------------%
% Solve Algebraic Method
%-------------------------------------------------------------------------%
for i = 2:N2+1
    x1(i) = c*(1-(i-1)/N2);
    y1(i) = max([t/.2*(.2969*x1(i)^.5-.126*x1(i)-.3516*x1(i)^2+.2843*x1(i)^3-.1015*x1(i)^4) 0]);
    s1(i) = s1(i-1)+sqrt((x1(i)-x1(i-1))^2+(y1(i)-y1(i-1))^2);
    x2(i) = c*(1-2*(i-1)/N2);
    if x2(i) >= 0;
        y2(i) = c;
    else
        y2(i) = sqrt(c^2-(x2(i))^2);
    end
    s2(i) = s2(i-1)+sqrt((x2(i)-x2(i-1))^2+(y2(i)-y2(i-1))^2);
end
S1 = s1(end)*(1-exp(-(0:N)/A))/(1-exp(-(N)/A));
S1(end)/s1(end)
S2 = (0:N)*s2(end)/N;
for i = 1:N+1
    X1(i) = interp1(s1,x1,S1(i));
    Y1(i) = interp1(s1,y1,S1(i));
    X2(i) = interp1(s2,x2,S2(i));
    Y2(i) = interp1(s2,y2,S2(i));
end
r = exp(((1:(M+1))-M+1)/((M+1)/B));
r = r-r(1);
r = r/max(r);
for i = 1:N+1
    for j = 1:M+1
        x(i,j) = X1(i) + r(j)*(X2(i)-X1(i));
        y(i,j) = Y1(i) + r(j)*(Y2(i)-Y1(i));
    end
end
N = Na-N;
x = [((L+c):-L/N:(c+L/N))'*ones(1,M+1); x];
y = [(c*r'*ones(1,N))'; y];
x = [x; x(end-1:-1:1,:)];
y = [y ;-y(end-1:-1:1,:)];
[n,m] = size(x);

%-------------------------------------------------------------------------%
% Plot Algebraic Method
%-------------------------------------------------------------------------%
mesh(x,y,zeros(n,m))
view(0,90)
colormap([0 0 0])
axis('equal','tight');
xlabel('Length [unitless]','FontSize',14);
ylabel('Length [unitless]','FontSize',14);
title('Algebraic Method ','FontSize',18);

%-------------------------------------------------------------------------%
% Predetermined Constants
%-------------------------------------------------------------------------%
de = 1/M;
de2 = de^2;
dn = 1/Na;
dn2 = dn^2;
dedn = 2/(M*Na);
P = 100; %number of iterations for the elliptic solver.

%-------------------------------------------------------------------------%
% Solve Elliptic Method
%-------------------------------------------------------------------------%

for k = 1:P
    for i = 2:2*Na-1
        for j = 2:M-1
            xn = (x(i+1,j)-x(i-1,j))/(2*dn);
            yn = (y(i+1,j)-y(i-1,j))/(2*dn);
            xe = (x(i,j+1)-x(i,j-1))/(2*de);
            ye = (y(i,j+1)-y(i,j-1))/(2*de);
            a(i,j) = xn^2+yn^2;
            b(i,j) = xe*xn+ye*yn;
            c(i,j) = xe^2+ye^2;
        end
    end
    for i = 2:2*Na-1
        for j = 2:M-1
            x(i,j) = (a(i,j)/de2*(x(i+1,j)+x(i-1,j))+c(i,j)/dn2*(x(i,j+1)+x(i,j-1))...
                -b(i,j)/dedn*(x(i+1,j+1)-x(i+1,j-1)+x(i-1,j-1)-x(i-1,j+1)))...
                /(2*(a(i,j)/de2+c(i,j)/dn2));
            y(i,j) = (a(i,j)/de2*(y(i+1,j)+y(i-1,j))+c(i,j)/dn2*(y(i,j+1)+y(i,j-1))...
                -b(i,j)/dedn*(y(i+1,j+1)-y(i+1,j-1)+y(i-1,j-1)-y(i-1,j+1)))...
                /(2*(a(i,j)/de2+c(i,j)/dn2));
        end
    end
end
xn = ones(Na,M)/0;
yn = xn;
xe = xn;
ye = xn;
for i = 2:Na-1
    for j = 2:M-1
        xn(i,j) = (x(i+1,j)-x(i-1,j))/(2*dn);
        yn(i,j) = (y(i+1,j)-y(i-1,j))/(2*dn);
        xe(i,j) = (x(i,j+1)-x(i,j-1))/(2*de);
        ye(i,j) = (y(i,j+1)-y(i,j-1))/(2*de);
        J(i,j) = yn(i,j)*xe(i,j)-xn(i,j)*ye(i,j); % Jacobian
    end
end
figure(2)
mesh(x,y,zeros(n,m))
xlabel('Length [unitless]','FontSize',14);
ylabel('Length [unitless]','FontSize',14);
title('Elliptic Method ','FontSize',18);
axis('equal','tight');

%-------------------------------------------------------------------------%
% Plot Elliptic Method
%-------------------------------------------------------------------------%

view(0,90)
colormap([0 0 0])

%-------------------------------------------------------------------------%
% Plot Jacobian
%-------------------------------------------------------------------------%

figure(3)
x=2:30; y=2:50; [X,Y]=meshgrid(x,y);
surf(X,Y,J);
view(-45,45)
title('Jacobian ','FontSize',18);


function r=geodistance(ci,cf,m)

%GEODISTANCE: Calculates the distance in meters between two points on earth surface.
%
% Usage:  r = geodistance( coordinates1 , coordinates2 , method ) ;
%
%	  Where coordinates1 = [longitude1,latitude1] defines the
%	  initial position and coordinates2 = [longitude2,latitude2]
%	  defines the final position.
%	  Coordinates values should be specified in decimal degrees.
%	  Method can be an integer between 1 and 23, default is m = 6.
%         Methods 1 and 2 are based on spherical trigonometry and a
%         spheroidal model for the earth, respectively.
%	  Methods 3 to 24 use Vincenty's formulae, based on ellipsoid
%         parameters.
%         Here it follows the correspondence between m and the type of
%         ellipsoid:
%
%         m =  3 -> ANS ,        m =  4 -> GRS80,    m = 5 -> WGS72,
%         m =  6 -> WGS84,       m =  7 -> NSWC-9Z2,
%         m =  8 -> Clarke 1866, m =  9 -> Clarke 1880,
%         m = 10 -> Airy 1830,
%         m = 11 -> Bessel 1841 (Ethiopia,Indonesia,Japan,Korea),
%         m = 12 -> Bessel 1841 (Namibia),
%         m = 13 -> Sabah and Sarawak (Everest,Brunei,E.Malaysia),
%         m = 14 -> India 1830, m = 15 -> India 1956,
%         m = 16 -> W. Malaysia and Singapore 1948,
%         m = 17 -> W. Malaysia 1969,
%         m = 18 -> Helmert 1906, m = 19 -> Helmert 1960,
%         m = 20 -> Hayford International 1924,
%         m = 21 -> Hough 1960, m = 22 -> Krassovsky 1940,
%         m = 23 -> Modified Fischer 1960,
%         m = 24 -> South American 1969.
%
%	  Important notes:
%
%	 1)South latitudes are negative.
%	 2)East longitudes are positive.
%	 3)Great circle distance is the shortest distance between two points
%          on a sphere. This coincides with the circumference of a circle which
%          passes through both points and the centre of the sphere.
%	 4)Geodesic distance is the shortest distance between two points on a spheroid.
%	 5)Normal section distance is formed by a plane on a spheroid containing a
%          point at one end of the line and the normal of the point at the other end.
%          For all practical purposes, the difference between a normal section and a
%          geodesic distance is insignificant.
%	 6)The method m=2 assumes a spheroidal model for the earth with an average
%          radius of 6364.963 km. It has been derived for use within Australia.
%          The formula is estimated to have an accuracy of about 200 metres over 50 km,
%          but may deteriorate with longer distances.
%          However, it is not symmetric when the points are exchanged.
%
%  Examples: A = [150 -30]; B = [150 -31]; L = [151 -80];
%            [geodistance(A,B,1) geodistance(A,B,2) geodistance(A,B,3)]
%            [geodistance(A,L,1) geodistance(A,L,2) geodistance(A,L,3)]
%            geodistance([0 0],[2 3])
%            geodistance([2 3],[0 0])
%            geodistance([0 0],[2 3],1)
%            geodistance([2 3],[0 0],1)
%            geodistance([0 0],[2 3],2)
%            geodistance([2 3],[0 0],2)
%            for m = 1:24
%            r(m) = geodistance([150 -30],[151 -80],m);
%            end
%            plot([1:m],r), box on, grid on
%
%***************************************************************************************
% Second version: 07/11/2007
% Third  version: 03/08/2010
%
% Contact: orodrig@ualg.pt
%
% Any suggestions to improve the performance of this
% code will be greatly appreciated.
%
% Reference: Geodetic Calculations Methods
%            Geoscience Australia
%            (http://www.ga.gov.au/geodesy/calcs/)
%
%***************************************************************************************
% Changed a little bit by Mao,
% So, it works in vector computaiton now.
%
% Contact: argansos@hotmail.com
% Ganquan Mao, 15/09/2011
%
%***************************************************************************************

if size(ci,2)~=2 || size(cf,2)~=2
    error('ci cf must be a n*2 matrix!')
end

if size(ci,1)~=size(cf,1)
    error('ci cf must have same size!')
end

r=[];%for convergence

if nargin==2
    m=6;
end

lambda1=ci(:,1)*pi/180;
   phi1=ci(:,2)*pi/180;

lambda2=cf(:,1)*pi/180;
   phi2=cf(:,2)*pi/180;

L=lambda2-lambda1;

if m==1%great circle distance, based on spherical trigonometry
    r=180*1.852*60*acos(sin(phi1).*sin(phi2)...
      +cos(phi1).*cos(phi2).*cos(lambda2-lambda1))./pi;
    r=1000*abs(r);

elseif m==2%spheroidal model for the earth
    term1=111.08956*(ci(:,2)-cf(:,2)+0.000001);
    term2=cos(phi1+((phi2-phi1)/2));
    term3=(cf(:,1)-ci(:,1)+0.000001)/(cf(:,2)-ci(:,2)+0.000001);
    r=1000*abs(term1./cos(atan(term2.*term3)));

else%apply Vincenty's formulae (as long as the points are not coincident)
    alla=[0,0,6378160,6378137,6378135,6378137,6378145,6378206.4,...
          6378249.145,6377563.396,6377397.155,6377483.865,6377298.556,...
          6377276.345,6377301.243,6377304.063,6377295.664,6378200,...
          6378270,6378388,6378270,6378245,6378155,6378160];

    allf=[0,0,1/298.25,1/298.257222101,1/298.26,1/298.257223563,...
          1/298.25,1/294.9786982,1/293.465,1/299.3249646,1/299.1528128,...
          1/299.1528128,1/300.8017,1/300.8017,1/300.8017,1/300.8017,...
          1/300.8017,1/298.3,1/297,1/297,1/297,1/298.3,1/298.3,1/298.25];

    a=alla(m);
    f=allf(m);

    b=a*(1-f);

    axa=a^2;
    bxb=b^2;

    U1=atan((1-f)*tan(phi1));
    U2=atan((1-f)*tan(phi2));

    lambda=L;
    lambda_old=sqrt(-1);%there is no way a complex number is going to coincide with a real number!

    ntrials=0;%just in case...

    while any(abs(lambda-lambda_old)>1e-9)

        ntrials=ntrials+1;

        lambda_old=lambda;
        sin_sigma=sqrt((cos(U2).*sin(lambda)).^2+(cos(U1).*sin(U2)...
                  -sin(U1).*cos(U2).*cos(lambda)).^2);
        cos_sigma=sin(U1).*sin(U2)+cos(U1).*cos(U2).*cos(lambda);
        sigma=atan2(sin_sigma,cos_sigma);
        sin_alpha=cos(U1).*cos(U2).*sin(lambda)./sin_sigma;
        cos2_alpha=1-sin_alpha.^2;
        cos_2sigmam=cos_sigma-2*sin(U1).*sin(U2)./cos2_alpha;

        C=(f/16)*cos2_alpha.*(4+f*(4-3*cos2_alpha));

        lambda=L+f*(1-C).*sin_alpha.*(sigma+C.*sin_sigma.*(cos_2sigmam...
               +C.*cos_sigma.*(-1+2*(cos_2sigmam).^2)));

       % stop the function if convergence is not achieved
        if ntrials>1000
            disp('Convergence failure...')
            return
        end

    end

    % convergence achieved? get the distance
    uxu=cos2_alpha*(axa-bxb)/bxb;
    A=1+(uxu/16384).*(4096+uxu.*(-768+uxu.*(320-175*uxu)));
    B=(uxu/1024).*(256+uxu.*(-128+uxu.*(74-47*uxu)));
    delta_sigma=B.*sin_sigma.*(cos_2sigmam+(B/4).*(cos_sigma.*(-1+...
                2*cos_2sigmam.^2)-(B/6).*cos_2sigmam.*(-3+...
                4*sin_sigma.^2).*(-3+4*cos_2sigmam.^2)));
    r=b*A.*(sigma-delta_sigma);

end

r(isnan(r))=0;

end


function circle_segment_test08 ( )

%*****************************************************************************80
%
%% CIRCLE_SEGMENT_TEST08 tests CIRCLE_SEGMENT_CONTAINS_POINT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 May 2013
%
%  Author:
%
%    John Burkardt
%
  n = 1000;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CIRCLE_SEGMENT_TEST08\n' );
  fprintf ( 1, '  CIRCLE_SEGMENT_CONTAINS_POINT reports whether\n' );
  fprintf ( 1, '  a circle segment contains a point.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Pick a circle segment at random.\n' );
  fprintf ( 1, '  Compute %d sample points in the surrounding box.\n', n );
  fprintf ( 1, '  Compare the area of the segment to the percentage of points\n' );
  fprintf ( 1, '  contained in the circle segment.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '       N          Omega1          Omega2           Area         Estimate\n' );
  fprintf ( 1, '\n' );

  r = 1.0;
  c = [ 0.0; 0.0 ];

  for test = 1 : 5

    [ u1, seed ] = r8_uniform_01 ( seed );
    [ u2, seed ] = r8_uniform_01 ( seed );
    omega1 = 2.0 * pi * u1;
    omega2 = 2.0 * pi * u2;

    if ( omega2 < omega1 )
      omega2 = omega2 + 2.0 * pi;
    end

    [ xy, seed ] = r8mat_uniform_01 ( 2, n, seed );
    xy = 2.0 * xy - 1.0;

    inout = zeros ( n, 1 );
    for j = 1 : n
      inout(j) = circle_segment_contains_point ( r, c, omega1, omega2, xy(1:2,j) );
    end

    theta = circle_segment_angle_from_chord_angles ( omega1, omega2 );
    area = circle_segment_area_from_angle ( r, theta );
    area_est = 4.0 * sum ( inout(1:n) ) / n;

    fprintf ( 1, '  %6d  %14g  %14g  %14g  %14g\n', n, omega1, omega2, area, area_est );

  end

  return
end


function aList=genAllBinCombinations(n,t,algorithm)
%%GENALLBINARYCOMBINATIONS Generate all combinations of t items chosen from
%              a set of n items, presenting the results as binary strings.
%
%INPUTS: n The number of items from which t items are chosen .
%        t The number of items chosen, t<=n.
% algorithm An optional parameter affecting which algorithm is used (and
%          thus the ordering of the codes). Possible values are:
%          0 (The default if omitted or an empty matrix is passed). Use
%            Algorithm C in Chapter 7.2.1.3 of [1]. The combinations are
%            given in the order of Chase's sequence.
%          1 Use Algorithm 2 in [2]. The combinations are given as a gray
%            code. Consecutive combinations differ in only 2 bits.
%
%OUTPUTS: aList An nXnumCombos list of all of the binary strings of n bits
%               choosing t of them to be 1.
%
%There is a total of numCombos=binomial(n,t) combinations.
%
%EXAMPLE:
% aList0=genAllBinCombinations(4,2,0)
% aList1=genAllBinCombinations(4,2,1)
%One will find that
% aList0=[0, 1, 0, 0, 1, 1;
%         0, 0, 1, 1, 0, 1;
%         1, 0, 0, 1, 1, 0;
%         1, 1, 1, 0, 0, 0];
% aList1=[1, 0, 1, 0, 0, 1;
%         1, 1, 0, 0, 1, 0;
%         0, 1, 1, 1, 0, 0;
%         0, 0, 0, 1, 1, 1];
%
%REFERENCES:
%[1] D. E. Knuth, The Art of Computer Programming. Vol. 4A: Combinatorial
%    Algorithms, Part I, Boston: Addison-Wesley, 2011.
%[2] J. R. Bitner, G. Ehrlich, and E. M. Reingold, "Efficient generation of
%    the binary reflected gray code and its applications," Communications
%    of the ACM, vol. 19, no. 9, pp. 517-521, Sep. 1976.
%
%October 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<3||isempty(algorithm))
    algorithm=0;
end

switch(algorithm)
    case 0
        aList=genAllBinCombinationsKnuth(n,t);
    case 1
        aList=genAllBinCombinationsGray(n,t);
    otherwise
        error('Unknown algorithm specified.')
end

end

function aList=genAllBinCombinationsKnuth(n,t)
%%GENALLBINARYCOMBINATIONS Generate all combinations of t items chosen from
%              a set of n items, presenting the results as binary strings.
%
%INPUTS: n The number of items from which t items are chosen.
%        t The number of items chosen, t<=n.
%
%OUTPUTS: aList An nXnumCombos list of all of the binary strings of n bits
%               choosing t of them to be 1.
%
%There is a total of numCombos=binomial(n,t) combinations. Algorithm C in
%Chapter 7.2.1.3 of [1] is used to generate the strings in the order of
%Chase's sequence.
%
%EXAMPLE:
% aList=genAllBinCombinations(4,2)
%One will find that
% aList=[0,1,0,0,1,1;
%        0,0,1,1,0,1;
%        1,0,0,1,1,0;
%        1,1,1,0,0,0];
%
%REFERENCES:
%[1] D. E. Knuth, The Art of Computer Programming. Vol. 4A: Combinatorial
%    Algorithms, Part I, Boston: Addison-Wesley, 2011.
%
%October 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.

if(t==n)
    aList=ones(n,1);
    return
elseif(t==0)
    aList=zeros(n,1);
    return
end

s=n-t;

numCombos=binomial(n,t);
aList=zeros(n,numCombos);

%Step C1. a and w are indexed from 0 in [1].
a=[zeros(s,1);ones(t,1)];%
w=ones(n+1,1);
r=s;

for curCombo=1:numCombos
    %Step C1
    aList(:,curCombo)=a;

    %Step C3
    j=r;
    while(w(j+1)==0)
        w(j+1)=1;
        j=j+1;
        if(j==n)
            return;
        end
    end

    w(j+1)=0;
    if(mod(j,2)==1)%j is odd
        if(a(j+1)~=0)
            %Step C4
            a(j-1+1)=1;
            a(j+1)=0;
            if(r==j&&j>1)
                r=j-1;
            elseif(r==j-1)
                r=j;
            end
        else
            %Step C7
            if(a(j-1+1)~=0)
                %Step C6
                a(j+1)=1;
                a(j-1+1)=0;
                if(r==j&&j>1)
                    r=j-1;
                elseif(r==j-1)
                    r=j;
                end
            else
               a(j+1)=1;
               a(j-2+1)=0;
               if(r==j-2)
                   r=j;
               elseif(r==j-1)
                   r=j-2;
               end
            end
        end
    else%j is even
        if(a(j+1)~=0)
            %Step C5
            if(a(j-2+1)~=0)
                %Step C4
                a(j-1+1)=1;
                a(j+1)=0;
                if(r==j&&j>1)
                    r=j-1;
                elseif(r==j-1)
                    r=j;
                end
            else
                a(j-2+1)=1;
                a(j+1)=0;
                if(r==j)
                    r=max(j-2,1);
                elseif(r==j-2)
                    r=j-1;
                end
            end
        else
            %Step C6
            a(j+1)=1;
            a(j-1+1)=0;
            if(r==j&&j>1)
                r=j-1;
            elseif(r==j-1)
                r=j;
            end
        end
    end

end

end

function theCodes=genAllBinCombinationsGray(n,k)
%%GENALLBINCOMBINATIONSGRAY Generate all combinations of k items chosen
%              from a set of n items, presenting the results as binary
%              strings. Subsequent combinations differ by 2 bits.
%
%INPUTS: n The number of items from which k items are chosen.
%        t The number of items chosen, t<=n.
%
%OUTPUTS: aList An nXnumCombos list of all of the binary strings of n bits
%               choosing k of them to be 1.
%
%There is a total of numCombos=binomial(n,t) combinations. Algorithm 2 in
%[1] is used to generate all combinations are a gray code. Subsequent
%combinations differ in only 2 bits.
%
%REFERENCES:
%[1] J. R. Bitner, G. Ehrlich, and E. M. Reingold, "Efficient generation of
%    the binary reflected gray code and its applications," Communications
%    of the ACM, vol. 19, no. 9, pp. 517-521, Sep. 1976.
%
%July 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.

if(k==0)
    theCodes=zeros(n,1);
    return;
end

numCodes=binomial(n,k);
theCodes=zeros(n,numCodes);

g=[ones(k,1);zeros(n-k+2,1)];
tau=2:(n+2);
t=k;
tau(1)=k+1;
i=0;

curGrayCode=1;
while(i<n+1)
    theCodes(:,curGrayCode)=g(1:n);
    curGrayCode=curGrayCode+1;

    i=tau(1);
    tau(1)=tau(i);
    tau(i)=i+1;

    if(g(i)==1)
        if(t~=0)
            g(t)=1-g(t);
        else
            g(i-1)=1-g(i-1);
        end
        t=t+1;
    else
        if(t~=1)
            g(t-1)=1-g(t-1);
        else
            g(i-1)=1-g(i-1);
        end
        t=t-1;
    end

    g(i)=1-g(i);

    if(t==i-1||t==0)
        t=t+1;
    else
        t=t-g(i-1);
        tau(i-1)=tau(1);

        if(t==0)
            tau(1)=i-1;
        else
            tau(1)=t+1;
        end
    end
end
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 [ area, radin, side ] = polygon_outrad_data ( n, radout )

%*****************************************************************************80
%
%% POLYGON_OUTRAD_DATA determines polygonal data from its outer radius.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 September 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of sides of the polygon.
%    N must be at least 3.
%
%    Input, real RADOUT, the outer radius of the polygon, that is,
%    the radius of the smallest circle that can be described
%    around the polygon.
%
%    Output, real AREA, the area of the regular polygon.
%
%    Output, real RADIN, the inner radius of the polygon, that is,
%    the radius of the largest circle that can be inscribed
%    within the polygon.
%
%    Output, real SIDE, the length of one side of the polygon.
%
  if ( n < 3 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'POLYGON_OUTRAD_DATA - Fatal error!\n' );
    fprintf ( 1, '  Input value of N must be at least 3.\n' );
    fprintf ( 1, '  but your input value was N = %d\n', n );
    error ( 'POLYGON_OUTRAD_DATA - Fatal error!' );
  end

  angle = pi / n;
  area = 0.5 * n * radout * radout * sin ( 2.0 * angle );
  side = 2.0 * radout * sin ( angle );
  radin = 0.5 * side / tan ( angle );

  return
end


function [vert,face] = MakeZcylinder(pos, radius,len)

a = 10;    % number of side faces
theta = (0:a-1)/a * 2*pi;

x  = radius*cos(theta);
y  = radius*sin(theta);
z1 = len/2 * ones(1,a);
z2 = -z1;

vert    = [x x 0 0;
           y y 0 0;
           z1 z2 len/2 -len/2];
for n = 1:3
   vert(n,:) = vert(n,:) + pos(n);
end

% make index data specifying polygons
face_side = [1:a; a+1:2*a; a+2:2*a a+1; 2:a 1];
face_up   = [1:a; 2:a 1];
face_up(3:4,:) = 2*a+1;  % index of up center
face_down = [a+2:2*a a+1; a+1:2*a];
face_down(3:4,:) = 2*a+2; % index of down center
face = [face_side face_up face_down];


function [xi,w]=transformSimplexTriPts(xi,w,v)
%%TRANSFORMSIMPLEXTRIPTSTOTRI Given cubature points and weights for
%   integration over a the standard simplex triangle with vertices (1,0),
%   (0,1), and (0,0), transform the points and weights for integration over
%   the triangle with vertices given in v.
%
%INPUTS: xi The 2XnumCubPoints set of cubature points for the standard 2D
%           simplex triangle.
%         w The numCubPointsX1 associated cubature weights for the standard
%           2D simplex triangle.
%         v The 2X3 set of vertices of the new triangle.
%
%OUTPUTS: xi The 2XnumCubPoints set of cubature points for the specified
%            triangle.
%          w The 2XnumCubPoints set of transformed weights.
%
%With cubature formulae as in [1], the weights must sum to the area of the
%triangle. Thus, the first step here is to scale the weights by 2*the
%triangle area (the 2 is because the area of the basic simplex triangle is
%1/6). An affine transformation, as described in Chapter 5 of [2], is then
%performed to move the cubature weights into the new triangle.
%
%EXAMPLE:
%In this example, we find the integral of x^3*y^2 using fifth-order simplex
%points. The exact solution is 128/5 and one can see that the relative
%error between the solution found and the exact solution is within what one
%would expect due to finite precision limits.
% vertices=[0,2,4;
%           0,2,0];
% [xi,w]=fifthOrderSimplexCubPoints(2);
% [xi,w]=transformSimplexTriPts(xi,w,vertices);
% alpha=[3;2];
% cubSol=findMomentFromSamp(alpha,xi,w)
% exactSol=128/5
%
%REFERENCES:
%[1] A.H. Stroud, Approximate Calculation of Multiple Integrals. Cliffs,
%    NJ: Prentice-Hall, Inc., 1971.
%[2] K. Anjyo and H. Ochiai, Mathematical Basics of Motion and Deformation
%    in Computer Graphics, 2nd ed. Morgan and Claypool Publishers, 2017.
%
%October 2022 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%Adjust w for the triangle area.
T=triangleArea(v(:,1),v(:,2),v(:,3),true);
w=w*(2*T);

%Transforms the points into the new triangle.
A=[v(:,1)-v(:,3),v(:,2)-v(:,3)];
xi=A*xi+v(:,3);

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 [theta,axis] = dquat2rot(dq)

% DQUAT2ROT  extracts the rotation axis and angle of a rotation dual
%            quaternion
%
%   [THETA,AXIS] = DQUAT2ROT(DQ) returns the rotation angle, THETA [deg],
%       and the rotation axis, AXIS, of a rotation dual quaternion DQ.
%       - DQ is a rotation dual quaternion. It is a 8-vector or an 8*N
%       array (each column is a rotation dual quaternion) where N is the
%       number of rotation dual quaternions.
%       - THETA is the rotation angle [deg]. It is comprised between 0 and
%       180deg. THETA is a scalar or an 1*N array (element i is the
%       rotation angle of dual quaternion i).
%       - AXIS is the unitary (norm equal to 1) rotation axis. It is a
%       3-vector or an 3*N array (column i is the rotation axis of
%       dual quaternion i).
%
% See also DQUAT2TRANS, DQUAT2SCREW, ROT2DQUAT

sdq = size(dq);
if sdq == [1 8], dq = dq'; sdq = size(dq); end
n = sdq(2);

% wrong format
if sdq(1) ~= 8
    error('DualQuaternion:dquat2rot:wrongsize',...
        '%d rows in the DQ array. It should be 8.',sdq(1));
end

% check that it is a rotation dual quaternion
tol = 1e-5;
[maxval,imax] = max(max(abs(dq(5:8,:))));
if maxval > tol
    warning('DualQuaternion:dquat2rot:wrongFormat',...
        'At least one dual quaternion is not a rotation dual quaternion (tol = %.1e).\n Indices of max values: %d \n Max value = %.2e',...
        tol,imax,maxval);
end
normDQ = sqrt(sum(dq(1:4,:).^2));
[maxval2,imax2] = max(abs(normDQ-1));
if maxval2 > tol
       warning('DualQuaternion:dquat2rot:wrongFormatForRotation',...
        'At least one dual quaternion is not a rotation dual quaternion (is it a rotation dual quaternion?) (tol = %.1e).\n Indices of max values: %d \n Max value = %.2e',...
        tol,imax2,maxval2);
end

indpos = find(dq(1,:) > 1);
dq(1,indpos) = ones(1,length(indpos));
indneg = find(dq(1,:) < -1);
dq(1,indneg) = ones(1,length(indneg));

% Extraction of the rotation parameters
theta = 2*acosd(dq(1,:)); % acosd: [-1,1] --> [0,180]
theta = mod(theta,360);
indthetaOK = find(theta > 0);
axis = [ones(1,n) ; zeros(2,n)];
if length(indthetaOK > 0)
    repsin = repmat(sind(theta(indthetaOK)/2),3,1);
    axis(:,indthetaOK) = dq(2:4,indthetaOK)./repsin;
end


function area = trimeshSurfaceArea(v, e, f)
%TRIMESHSURFACEAREA Surface area of a triangular mesh.
%
%   S = trimeshSurfaceArea(V, F)
%   S = trimeshSurfaceArea(V, E, F)
%   Computes the surface area of the mesh specified by vertex array V and
%   face array F. Vertex array is a NV-by-3 array of coordinates.
%   Face array is a NF-by-3, containing vertex indices of each face.
%
%   Example
%     % Compute area of an octahedron (equal to 2*sqrt(3)*a*a, with
%     % a = sqrt(2) in this case)
%     [v f] = createOctahedron;
%     trimeshSurfaceArea(v, f)
%     ans =
%         6.9282
%
%     % triangulate a compute area of a unit cube
%     [v f] = createCube;
%     f2 = triangulateFaces(f);
%     trimeshSurfaceArea(v, f2)
%     ans =
%         6
%
%   See also
%   meshes3d, meshSurfaceArea, trimeshMeanBreadth, triangulateFaces

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2011-08-26,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2011 INRA - Cepia Software Platform.

% check input number
if nargin == 2
    f = e;
end

% compute two direction vectors, using first vertex of each face as origin
v1 = v(f(:, 2), :) - v(f(:, 1), :);
v2 = v(f(:, 3), :) - v(f(:, 1), :);

% area of each triangle is half the cross product norm
vn = vectorNorm3d(crossProduct3d(v1, v2));

% sum up and normalize
area = sum(vn) / 2;


function cube_plots ( )

%*****************************************************************************80
%
%% CUBE_PLOTS plots the surface and the R(Theta) function for a cube.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 May 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CUBE_PLOTS:\n' );
  fprintf ( 1, '  For a 3D cube defined by a 0/1 characteristic function,\n' );
  fprintf ( 1, '  plot the surface, and R(Theta),\n' );
  fprintf ( 1, '  using a centered point, and then an offcentered point.\n' );
%
%  1): Plot the surface, using a centered base point.
%
  n1 = 31;
  n2 = 61;
  theta1 = linspace ( 0.0,       pi, n1 );
  theta2 = linspace ( 0.0, 2.0 * pi, n2 );

  [ t1, t2 ] = meshgrid ( theta2, theta1 );
  r = zeros ( n1, n2 );

  x0 = [ 0.0, 0.0, 0.0 ];
  for i = 1 : n1
    for j = 1 : n2
      r(i,j) = bisect_characteristic ( x0, [ t1(i,j); t2(i,j) ], @cube_characteristic );
    end
  end

  x1 = r .* cos ( t1 );
  x2 = r .* sin ( t1 ) .* cos ( t2 );
  x3 = r .* sin ( t1 ) .* sin ( t2 );

  figure ( 1 )
  mesh ( x3, x2, x1, 'FaceColor', 'interp' );
  axis equal
  grid on
  xlabel ( '<---X--->', 'FontSize', 16 );
  ylabel ( '<---Y--->', 'FontSize', 16 );
  zlabel ( '<---Z--->', 'FontSize', 16 );
  title ( 'Cube transition surface', 'FontSize', 24 )
  hold off
  filename =  'cube_centered_surface.png';
  print ( '-dpng', filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Created plotfile "%s".\n', filename );
%
%  2): Plot R as a function of Theta for a centered base point.
%
  figure ( 2 )
  mesh ( t1, t2, r, 'FaceColor', 'Interp' );
  axis ( [0, 2.0 * pi, 0.0, pi, 0.0, 1.5 ] );
  xlabel ( '<---Theta2--->', 'FontSize', 16 )
  ylabel ( '<---Theta1--->', 'FontSize', 16 );
  zlabel ( '<---R(Theta1,Theta2)--->', 'FontSize', 16 )
  title ( 'R(*,*) from base point to surface', 'FontSize', 24 )
  grid on
  filename = 'cube_centered_plot.png';
  print ( '-dpng', filename );
  fprintf ( 1, '  Created plotfile "%s".\n', filename );
%
%  3): Plot the surface, using an offcentered base point.
%
  n1 = 31;
  n2 = 61;
  theta1 = linspace ( 0.0,       pi, n1 );
  theta2 = linspace ( 0.0, 2.0 * pi, n2 );

  [ t1, t2 ] = meshgrid ( theta2, theta1 );
  r = zeros ( n1, n2 );

  x0 = [ +0.7, -0.2, -0.5 ];
  for i = 1 : n1
    for j = 1 : n2
      r(i,j) = bisect_characteristic ( x0, [ t1(i,j); t2(i,j) ], @cube_characteristic );
    end
  end

  x1 = r .* cos ( t1 );
  x2 = r .* sin ( t1 ) .* cos ( t2 );
  x3 = r .* sin ( t1 ) .* sin ( t2 );

  figure ( 3 )
  c = zeros ( n1, n2 );
  colormap ( 'gray' )
  mesh ( x3, x2, x1, c );
  axis equal
  grid on
  xlabel ( '<---X--->', 'FontSize', 16 );
  ylabel ( '<---Y--->', 'FontSize', 16 );
  zlabel ( '<---Z--->', 'FontSize', 16 );
  title ( 'Cube transition surface', 'FontSize', 24 )
  hold off
  filename =  'cube_offcentered_surface.png';
  print ( '-dpng', filename );
  fprintf ( 1, '  Created plotfile "%s".\n', filename );
%
%  4): Plot R as a function of Theta for an offcentered base point.
%
  figure ( 4 )
  mesh ( t1, t2, r, 'FaceColor', 'Interp' );
  xlabel ( '<---Theta2--->', 'FontSize', 16 )
  ylabel ( '<---Theta1--->', 'FontSize', 16 );
  zlabel ( '<---R(Theta1,Theta2)--->', 'FontSize', 16 )
  title ( 'R(*,*) from base point to surface', 'FontSize', 24 )
  grid on
  filename = 'cube_offcentered_plot.png';
  print ( '-dpng', filename );
  fprintf ( 1, '  Created plotfile "%s".\n', filename );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Note variation in R for the offcenter case:\n' );
  fprintf ( 1, '  Min R = %g\n', min ( min ( r ) ) );
  fprintf ( 1, '  Max R = %g\n', max ( max ( r ) ) );
  return
end


function [model, llh] = hmmEm(x, init)
% EM algorithm to fit the parameters of HMM model (a.k.a Baum-Welch algorithm)
% Input:
%   x: 1 x n integer vector which is the sequence of observations
%   init: model or k
% Output:s
%   model: trained model structure
%   llh: loglikelihood
% Written by Mo Chen (sth4nth@gmail.com).
n = size(x,2);
X = sparse(x,1:n,1);
d = size(X,1);
if isstruct(init)   % init with a model
    A = init.A;
    E = init.E;
    s = init.s;
elseif numel(init) == 1  % random init with latent k
    k = init;
    s = normalize(rand(k,1),1);
    A = normalize(rand(k,k),2);
    E = normalize(rand(k,d),2);
end
tol = 1e-4;
maxIter = 1000;
llh = -inf(1,maxIter);
for iter = 2:maxIter
    M = E*X;
%     E-step
    [gamma,alpha,beta,c] = hmmSmoother(M,A,s);
    llh(iter) = mean(log(c));
    if abs(llh(iter)-llh(iter-1)) < tol*abs(llh(iter-1)); break; end   % check likelihood for convergence
%     M-step
    s = gamma(:,1);                                                                             % 13.18
    A = normalize(A.*(alpha(:,1:n-1)*(beta(:,2:n).*M(:,2:n)./c(2:n))'),2);      % 13.19 13.43 13.65
    E = (gamma*X')./sum(gamma,2);                            % 13.23
end
model.s = s;
model.A = A;
model.E = E;
llh = llh(2:iter);

function [gamma, alpha, beta, c] = hmmSmoother(M, A, s)
[K,T] = size(M);
At = A';
c = zeros(1,T);
alpha = zeros(K,T);
[alpha(:,1),c(1)] = normalize(s.*M(:,1),1);
for t = 2:T
    [alpha(:,t),c(t)] = normalize((At*alpha(:,t-1)).*M(:,t),1);  % 13.59
end
beta = ones(K,T);
for t = T-1:-1:1
    beta(:,t) = A*(beta(:,t+1).*M(:,t+1))/c(t+1);   % 13.62
end
gamma = alpha.*beta;                  % 13.64


function fx = p43_fun ( n, x )

%*****************************************************************************80
%
%% P43_FUN evaluates the integrand for problem 43.
%
%  Discussion:
%
%    The problem has a parameter ALPHA that can be set by calling
%    P43_PARAM_SET.
%
%    The suggested parameter range is 0.1 <= ALPHA <= 2.0.
%
%    The integrand has an algebraic endpoint singularity at X = 1
%    times a singular factor.
%
%  Interval:
%
%    0 <= x <= 1
%
%  Integrand:
%
%    ( ln ( 1 / x ) )^( alpha - 1 )
%
%  Exact Integral:
%
%    Gamma(alpha)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 November 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Robert Piessens, Elise de Doncker-Kapenga,
%    Christian Ueberhuber, David Kahaner,
%    QUADPACK: A Subroutine Package for Automatic Integration,
%    Springer, 1983, page 84.
%
%  Parameters:
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real X(N), the evaluation points.
%
%    Output, real FX(N), the integrand values.
%
  alpha = p43_param_get ( );

  fx(1:n) = ( log ( 1.0 ./ x(1:n) ) ).^( alpha - 1.0 );

  i = find ( x == 0.0 | x == 1.0 );
  fx(i) = 0.0;

  return
end


function timestamps = edgeDetect(signal, threshold, type)

% edgeDetect
%
% Detects rising or falling edges of a signal whenever the signal crosses
% the threshold.
%
%
% Use timestamps = edgeDetect(signal, threshold, type)
%
% Variables fname and threshold are optional.
%
%   signal:       Name of the file to be opened. If the fname is omitted
%                 the user will be prompted to select a file.
%
%   threshold:    The value used for threshold crossings. This function
%                 will detect when the signal crosses this value.
%                 DEFAULT: It will automatically choose the threshold at
%                 80% of the maximum value in the signal.
%
%
%   type:         Contains the points in the signal where the threshold
%                 crossing occurs.
%                 DEFAULT: It will find the rising edges.
%
%   Example 1:
%   timestamps = edgeDetect(signal, 1000);
%
%   In the example above, the points where signal crosses value 1000
%   (rising edge) is detected and returned in variable timestamps.
%
%   timestamps = edgeDetect(signal, 1000, 'falling');
%
%   In the example above, the points where signal crosses value 1000
%   (falling edge) is detected and returned in variable timestamps.
%
%   Kian Torab
%   kian@blackrockmicro.com
%   Blackrock Microsystems
%   Version 1.1.0.0
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Version History
%
% 1.0.0.0:
%   - Initial release.
%
% 1.1.0.0:
%   - Added automatic edge detection.
%   - Updated help.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Validations

% Validating input arguments.
if nargin < 1 || nargin > 3
    disp('Invalid number of input arguments. Use ''help edgeDetect'' for more information.');
    return;
end

if ~exist('threshold', 'var')
    threshold = 0.8 * max(signal);
    disp(['Threshold was not provided. It wss automatically calculated and set at ' num2str(threshold) '.']);
end

% Validating variable 'type'
if ~exist('type', 'var')
    type = 'rising';
end

% Validating type and determining the threshold crossing points
if strcmpi(type, 'rising')
    timestamps = signal>threshold;
elseif strcmpi(type, 'falling')
    timestamps = signal<threshold;
else
    disp('Type does not exist. Please type ''help edgeDetect'' to see all available types.');
    timestamps = 0;
    return;
end

% Finding all the points where the signal crosses the threshold
timestamps = diff(timestamps);
timestamps = find(timestamps==1);


function A=vech2Mat(theVec,isSymmetric,offDiagScalFact)
%%VECH2MAT Given the main diagonal and lower-triangular elements of a
%          matrix stacked column-wise, obtain the original
%          symmetric or lower-triangular matrix. This function is
%          essentially the inverse of the vech function when applied to a
%          symmetric or lower-triangular matrix with the proper
%          parameterization.
%
%INPUTS: theVect An nX1 or 1Xn vector holding the diagonal and
%                lower-triangular components of a symmetric matrix stacked
%                column-wise.
%    isSymmetric An optional boolean parameter specifying whether the
%                matrix being recreated is symmetric. If false, a lower-
%                triangular matrix is formed. The default if this parameter
%                is omitted or an empty matrix is passed is true.
%   offDiagScalFact An optional scalar factor by which the off-diagonal
%                elements are scaled. If this parameter is omitted or an
%                empty matrix is passed, then the default value of 1 is
%                used.
%
%OUTPUTS: A The matrix impled by theVec and isSymmetric. This is a dXd
%           matrix, where n=d*(d+1)/2.
%
%The opposite of this function is vech.
%
%December 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<3||isempty(offDiagScalFact))
    offDiagScalFact=1;
end

if(nargin<2||isempty(isSymmetric))
   isSymmetric=true;
end

n=length(theVec);
theVec=theVec(:);

d=(1/2)*(-1+sqrt(1+8*n));
A=zeros(d,d);

curStart=1;
if(isSymmetric)
    for curCol=1:d
        %The diagonal element
        A(curCol,curCol)=theVec(curStart);
        span=(curStart+1):(curStart+d-curCol);

        %The lower-triangular part
        A((curCol+1):end,curCol)=theVec(span)*offDiagScalFact;
        %The upper triangular part.
        A(curCol,(curCol+1):end)=theVec(span)'*offDiagScalFact;
        curStart=curStart+d-curCol+1;
    end
else
    for curCol=1:d
        %The diagonal element
        A(curCol,curCol)=theVec(curStart);
        span=(curStart+1):(curStart+d-curCol);

        %The lower-triangular part
        A((curCol+1):end,curCol)=theVec(span)*offDiagScalFact;

        %The upper-triangular part is just zero.
        curStart=curStart+d-curCol+1;
    end
end
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 varargout = createCube()
%CREATECUBE Create a 3D mesh representing the unit cube
%
%   [V, E, F] = createCube
%   Create a unit cube, as a polyhedra representation.
%   c has the form [V E F], where V is a 8-by-3 array with vertices
%   coordinates, E is a 12-by-2 array containing indices of neighbour
%   vertices, and F is a 6-by-4 array containing vertices array of each
%   face.
%
%   [V, F] = createCube;
%   Returns only the vertices and the face vertex indices.
%
%   MESH = createCube;
%   Returns the data as a mesh structure, with fields 'vertices', 'edges'
%   and 'faces'.
%
%   Example
%   [n, e, f] = createCube;
%   drawMesh(n, f);
%
%   See also
%   meshes3d, drawMesh
%   createOctahedron, createTetrahedron, createDodecahedron
%   createIcosahedron, createCubeOctahedron
%

% ---------
% author : David Legland
% e-mail: david.legland@inra.fr
% INRA - TPV URPOI - BIA IMASTE
% created the 10/02/2005.

%   HISTORY
%   04/01/2007: remove unused variables

x0 = 0; dx= 1;
y0 = 0; dy= 1;
z0 = 0; dz= 1;

nodes = [...
    x0 y0 z0; ...
    x0+dx y0 z0; ...
    x0 y0+dy z0; ...
    x0+dx y0+dy z0; ...
    x0 y0 z0+dz; ...
    x0+dx y0 z0+dz; ...
    x0 y0+dy z0+dz; ...
    x0+dx y0+dy z0+dz];

edges = [1 2;1 3;1 5;2 4;2 6;3 4;3 7;4 8;5 6;5 7;6 8;7 8];

% faces are oriented such that normals point outwards
faces = [1 3 4 2;5 6 8 7;2 4 8 6;1 5 7 3;1 2 6 5;3 7 8 4];

% format output
varargout = formatMeshOutput(nargout, nodes, edges, faces);