function y = jed_to_year_hebrew ( jed )

%*****************************************************************************80
%
%% JED_TO_YEAR_HEBREW: the year in the Hebrew calendar when a JED occurred.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Edward Richards,
%    Algorithm H,
%    Mapping Time, The Calendar and Its History,
%    Oxford, 1999, page 331.
%
%  Parameters:
%
%    Input, real JED, the Julian Ephemeris Date.
%
%    Output, integer Y, the year in the Hebrew calendar that
%    included the JED.  If the input JED is less than the epoch of the Hebrew
%    calendar, then Y is always returned as -1.
%
  jed_epoch = epoch_to_jed_hebrew ( );

  if ( jed < jed_epoch )
    y = -1;
    return
  end
%
%  Using integer arithmetic in this computation may cause overflow.
%
%  Compute the number of months elapsed up to the date.
%
  m = 1 + floor ( ( 25920.0 * ( jed - jed_epoch + 2.5 ) ) / 765433.0 );
%
%  Estimate the number of years represented by these months.
%
  y = 19 * floor ( m / 235 ) + floor ( ( 19 * ( i4_modp ( m, 235 ) - 2 ) ) / 235 ) + 1;
  y = max ( y, 1 );
%
%  Determine the JED of the first day of that year.
%
  jed2 = new_year_to_jed_hebrew ( y );
%
%  We might have been off by 1 year.
%
  if ( jed < jed2 )
    y = y - 1;
  end

  return
end


% Builds and solves a simple least-squares problem using cvx

echo on

n = 100;
A = randn(2*n,n);
b = randn(2*n,1);

cvx_begin
   variable x1(n)
   minimize( norm( A*x1-b ) )
cvx_end
v1 = cvx_optval;

cvx_begin
   variable x2(n)
   minimize( sum_square( A*x2-b ) )
cvx_end
v2 = cvx_optval;

% The differences between the unsquared and squared versions should be small
norm( x1 - x2 ) / norm( x1 ) %#ok
abs( v1 - sqrt(v2) ) / v1 %#ok

echo off


%% AMG TEST II: Different boundary conditions
%
% We consider the linear finite element discretization of Poisson equation
% on the unstructured mesh with Dirichlet or Neumann boundary conditions.

%%
load lakemesh
%% Dirichlet problem
[N,itStep,time,err] = amgtest(node,elem,1);
%%
colHeaders = {'Unknowns','Iterations','Time(sec)','Error'};
makeHtmlTable([N itStep time err],[],[],colHeaders,[],6);
%%
r = showrate(N.*log(N),time,3);
xlabel('NlogN'); ylabel('Time');
title(['Complexity is (NlogN)^{' num2str(r) '}'] );

%% Neumann problem
[N,itStep,time,err] = amgtest(node,elem,0);
%%
colHeaders = {'Unknowns','Iterations','Time(sec)','Error'};
makeHtmlTable([N itStep time err],[],[],colHeaders,[],6);
%%
r = showrate(N.*log(N),time,3);
xlabel('NlogN'); ylabel('Time');
title(['Complexity is (NlogN)^{' num2str(r) '}'] );


% ASTROTIK by Francesco Santilli
% R2BP (Restricted Two Bodies Problem)
% M2E inverts the keplerian equation for an elliptical orbit and finds
% eccentric anomaly with a numeric iterative method.
%
% Usage: E = M2E(M,e)
%
% where: M(k) = mean anomaly [rad]
%        e = eccentricity [-] (0<e<1)
%        E(k) = eccentric anomaly [rad]

function E = M2E(M,e)

    if ~(nargin == 2)
        error('Wrong number of input arguments.')
    end

    K = check(M,1);
    check(e,0)

    if (e<=0 || e>=1)
        error('e must be in the range (0,1)')
    end

    E = zeros(K,1);
    for k = 1:K
        F = @(E) E-e*sin(E)-M(k);
        E1 = M(k)-e-eps;
        E2 = M(k)+e+eps;
        E(k) = regulafalsi(F,E1,E2);
    end

end


function [ num_int, pint ] = plane_normal_triangle_int_3d ( pp, normal, t )

%*****************************************************************************80
%
%% PLANE_NORMAL_TRIANGLE_INT_3D: intersection ( normal plane, triangle ) in 3D.
%
%  Discussion:
%
%    The normal form of a plane in 3D is:
%
%      PP is a point on the plane,
%      N is a normal vector to the plane.
%
%    There may be 0, 1, 2 or 3 points of intersection returned.
%
%    If two intersection points are returned, then the entire line
%    between them comprises points of intersection.
%
%    If three intersection points are returned, then all points of
%    the triangle intersect the plane.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real PP(3), a point on the plane.
%
%    Input, real NORMAL(3), a normal vector to the plane.
%
%    Input, real T(3,3), the vertices of the triangle.
%
%    Output, integer NUM_INT, the number of intersection points returned.
%
%    Output, real PINT(3,3), the coordinates of the
%    intersection points.
%
  dim_num = 3;

  num_int = 0;
  pint = [];
%
%  Compute the signed distances between the vertices and the plane.
%
  d = - normal(1:dim_num) * pp(1:dim_num)';

  dist1 = normal(1:dim_num) * t(1:dim_num,1) + d;
  dist2 = normal(1:dim_num) * t(1:dim_num,2) + d;
  dist3 = normal(1:dim_num) * t(1:dim_num,3) + d;
%
%  Consider any zero distances.
%
  if ( dist1 == 0.0 )

    num_int = num_int + 1;
    pint(1:dim_num,num_int) = t(1:dim_num,1);

  end

  if ( dist2 == 0.0 )

    num_int = num_int + 1;
    pint(1:dim_num,num_int) = t(1:dim_num,2);

  end

  if ( dist3 == 0.0 )

    num_int = num_int + 1;
    pint(1:dim_num,num_int) = t(1:dim_num,3);

  end
%
%  If 2 or 3 of the nodes intersect, we're already done.
%
  if ( 2 <= num_int )
    return
  end
%
%  If one node intersects, then we're done unless the other two
%  are of opposite signs.
%
  if ( num_int == 1 )

    if ( dist1 == 0.0 )

      [ num_int, pint ] = plane_imp_triangle_int_add_3d ( t(1:dim_num,2), ...
        t(1:dim_num,3), dist2, dist3, num_int, pint );

    elseif ( dist2 == 0.0 )

      [ num_int, pint ] = plane_imp_triangle_int_add_3d ( t(1:dim_num,1), ...
        t(1:dim_num,3), dist1, dist3, num_int, pint );

    elseif ( dist3 == 0.0 )

      [ num_int, pint ] = plane_imp_triangle_int_add_3d ( t(1:dim_num,1), ...
        t(1:dim_num,2), dist1, dist2, num_int, pint );

    end

    return

  end
%
%  All nodal distances are nonzero, and there is at least one
%  positive and one negative.
%
  if ( dist1 * dist2 < 0.0 & dist1 * dist3 < 0.0 )

    [ num_int, pint ] = plane_imp_triangle_int_add_3d ( t(1:dim_num,1), ...
      t(1:dim_num,2), dist1, dist2, num_int, pint );

    [ num_int, pint ] = plane_imp_triangle_int_add_3d ( t(1:dim_num,1), ...
      t(1:dim_num,3), dist1, dist3, num_int, pint );

  elseif ( dist2 * dist1 < 0.0 & dist2 * dist3 < 0.0 )

    [ num_int, pint ] = plane_imp_triangle_int_add_3d ( t(1:dim_num,2), ...
      t(1:dim_num,1), dist2, dist1, num_int, pint );

    [ num_int, pint ] = plane_imp_triangle_int_add_3d ( t(1:dim_num,2), ...
      t(1:dim_num,3), dist2, dist3, num_int, pint );

  elseif ( dist3 * dist1 < 0.0 & dist3 * dist2 < 0.0 )

    [ num_int, pint ] = plane_imp_triangle_int_add_3d ( t(1:dim_num,3), ...
      t(1:dim_num,1), dist3, dist1, num_int, pint );

    [ num_int, pint ] = plane_imp_triangle_int_add_3d ( t(1:dim_num,3), ...
      t(1:dim_num,2), dist3, dist2, num_int, pint );

  end

  return
end


function a = sweet3 (  )

%*****************************************************************************80
%
%% SWEET3 returns the SWEET3 matrix.
%
%  Example:
%
%      8    4    1    6    2    3
%      4    8    4    1    6    2
%    -34    4    8    4    1    6
%      5  -34    4    8    4    1
%      3    5  -34    4    8    4
%      1    3    5  -34    4    8
%
%  Properties:
%
%    A is Toeplitz: constant along diagonals.
%
%    A is generally not symmetric: A' /= A.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Per Hansen, Tony Chan,
%    FORTRAN Subroutines for General Toeplitz Systems,
%    ACM Transactions on Mathematical Software,
%    Volume 18, Number 3, September 1992, pages 256-273.
%
%    Douglas Sweet,
%    The use of pivoting to improve the numerical performance of
%    Toeplitz solvers,
%    In "Advanced Algorithms and Architectures for Signal Processing",
%    Edited by J M Speiser,
%    Proceedings SPIE 696, 1986, pages 8-18.
%
%  Parameters:
%
%    Output, real A(6,6), the matrix.
%
  n = 6;

  v = [ 1.0, 3.0, 5.0, -34.0, 4.0, 8.0, 4.0, ...
    1.0, 6.0, 2.0, 3.0 ];

  a = zeros ( n, n );

  for i = 1 : n
    for j = 1 : n

      a(i,j) = v ( j - i + 6 );

    end
  end

  return
end


function rank = rgf_rank ( m, f )

%*****************************************************************************80
%
%% RGF_RANK ranks a restricted growth function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 January 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Donald Kreher, Douglas Simpson,
%    Combinatorial Algorithms,
%    CRC Press, 1998,
%    ISBN: 0-8493-3988-X,
%    LC: QA164.K73.
%
%  Parameters:
%
%    Input, integer M, the domain of the RGF is the integers
%    from 1 to M.  M must be positive.
%
%    Input, integer F(M), the restricted growth function.
%
%    Output, integer RANK, the rank of the restricted growth
%    function.
%

%
%  Check.
%
  ierror = rgf_check ( m, f );

  if ( ierror ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'RGF_RANK - Fatal error!\n' );
    fprintf ( 1, '  The input array is illegal!\n' );
    fprintf ( 1, '  IERROR = %d\n', ierror );
    error ( 'RGF_RANK - Fatal error!' );
  end
%
%  Get the generalized restricted growth function table.
%
  offset = 1;
  d = rgf_g_table ( m );

  rank = 0;
  j = 1;
  for i = 2 : m
    rank = rank + ( f(i) - 1 ) * d(m-i+offset,j+offset);
    j = max ( j, f(i) );
  end

  return
end


function funhan = piecewise(varargin)
%PIECEWISE Piecewise-defined functions.
%
%  F = PIECEWISE(COND1,DEFN1,...CONDn,DEFNn,DEFAULT) returns a callable
%  function F that applies different definitions according to supplied
%  conditions. For a given X, F(X) will test COND1 and apply DEFN1 if true,
%  etc. If all conditions fail, then DEFAULT is applied. For any particular
%  input, the first condition to match is the only one applied.
%
%  Each condition should be either a:
%    * function handle evaluating to logical values, or
%    * vector [a b] representing membership in the half-open interval [a,b).
%  Each definition should be either a:
%    * function handle, or
%    * scalar value.
%  The DEFAULT definition is optional; if omitted, it will be set to NaN.
%
%  All function definitions can accept multiple input variables, but they
%  all must accept the same number as in the call to F. They also should
%  all be vectorized, returning results the same size and shape as their
%  inputs. Complex inputs will work if the definitions are set up
%  accordingly; in that case, intervals will be tested using only Re parts.
%
%  The special syntax F() displays all the conditions and definitions for F.
%
%  Examples:
%     f = piecewise([-1 1],@(x) 1+cos(pi*x),0);   % a "cosine bell"
%     ezplot(f,[-2 2])
%     g = piecewise(@(x) sin(x)<0.5,@sin,@(x) 1-sin(x));
%     ezplot(g,[-2*pi 2*pi])
%     h = piecewise(@(x,y) (x<0)|(y<0),@(x,y) sin(x-y));  % defined on L
%     ezsurf(h,[-1 1])
%     chi = piecewise(@(x,y,z) x.^2+y.^2+z.^2<1,1,0); % characteristic func
%     [ triplequad(chi,-1,1,-1,1,-1,1), 4/3*pi ]
%         ans =
%                4.1888    4.1888
%
%  See also FUNCTION_HANDLE.

% Copyright (c) 2007 by Toby Driscoll.
% Version 1, 6 Aug 2007.

% If an even number of inputs was given, no default value exists.
if rem(nargin,2)==0
  default = NaN;
else
  default = varargin{end};
end

% Number of condition/definition pairs.
numdefs = floor(nargin/2);
condn = varargin(1:2:2*numdefs);
defn = varargin(2:2:2*numdefs);

funhan = @piecewisefun;   % this is the return value

  % This is the defintion of the returned function.
  function f = piecewisefun(varargin)
  % Special syntax to display the function.
  if nargin==0
    fprintf('\nPiecewise function with conditions/definitions:\n')
    for k = 1:numdefs
      fprintf('  If %s : %s\n',funcornum2str(condn{k}),funcornum2str(defn{k}))
    end
    fprintf('  Otherwise : %s\n\n',funcornum2str(default))
    return
  end

  % Normal call
  f = zeros(size(varargin{1}));
  mask = false(size(f));
  for k = 1:numdefs
    % Determine the values satistfying condition k.
    if isa(condn{k},'function_handle')
      newpts = logical( condn{k}(varargin{:}) );
    else
      newpts = (varargin{1}>=condn{k}(1)) & (varargin{1}<condn{k}(2));
    end
    newpts = newpts & (~mask);
    % Evaluate definition k
    if isa(defn{k},'function_handle')
      % Tricky part: Extract subset of points from each variable.
      sref.type = '()';
      sref.subs = {newpts};
      extract = @(x) subsref(x,sref);
      x = cellfun(extract,varargin,'uniformoutput',false);
      f(newpts) = defn{k}(x{:});
    else
      f(newpts) = defn{k};  % scalar expansion
    end
    mask = mask | newpts;
  end

  % Default case
  if isa(default,'function_handle')
    sref.subs = {~mask};
    extract = @(x) subsref(x,sref);
    x = cellfun(extract,varargin,'uniformoutput',false);
    f(~mask) = default(x{:});
  else
    f(~mask) = default;  % scalar expansion
  end

  end

  % This subfunction converts a function or scalar to its native char form,
  % and a 2-vector into an interval notation.
  function s = funcornum2str(f)
    if isa(f,'function_handle')
      s = char(f);
    elseif numel(f)==1
      s = num2str(f);
    else
      s = [ 'in [' num2str(f(1)) ',' num2str(f(2))  ')' ];
    end
  end

end


function a = antihadamard ( n )

%*****************************************************************************80
%
%% ANTIHADAMARD returns the ANTIHADAMARD matrix.
%
%  Discussion:
%
%    An Anti-Hadamard matrix is one whose elements are all 0 or 1,
%    and for which the Frobenius norm of the inverse is as large as
%    possible.  This routine returns a matrix for which the Frobenius norm
%    of the inverse is large, though not necessarily maximal.
%
%  Formula:
%
%    if ( I = J )
%      A(I,J) = 1
%    else if ( I < J and mod ( I+J, 2 ) = 1 )
%      A(I,J) = 1
%    else
%      A(I,J) = 0
%
%  Example:
%
%    N = 5
%
%    1 1 0 1 0
%    0 1 1 0 1
%    0 0 1 1 0
%    0 0 0 1 1
%    0 0 0 0 1
%
%  Properties:
%
%    A is generally not symmetric: A' /= A.
%
%    A is Toeplitz: constant along diagonals.
%
%    A is upper triangular.
%
%    A is integral: int ( A ) = A.
%
%    A is a zero-one matrix.
%
%    The inverse of A is integral.
%
%    det ( A ) = 1.
%
%    A is unimodular.
%
%    LAMBDA(1:N) = 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 September 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Ronald Graham, Neal Sloane,
%    Anti-Hadamard Matrices,
%    Linear Algebra and Applications,
%    Volume 62, 1984, pages 113-137.
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  for i = 1 : n
    for j = 1 : n

      if ( j < i )
        a(i,j) = 0.0;
      elseif ( i == j )
        a(i,j) = 1.0;
      elseif ( mod ( i+j, 2 ) == 1 )
        a(i,j) = 1.0;
      else
        a(i,j) = 0.0;
      end

    end
  end

  return
end


function convex_hull ( prefix )

%*****************************************************************************80
%
%% CONVEX_HULL: an algorithm for the convex hull of a set of 2D points.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 February 2011
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CONVEX_HULL:\n' );
  fprintf ( 1, '  MATLAB version.\n' );

  if ( nargin < 1 )
    prefix = 'random points'
    n = 25;
    xy = rand ( n, 2 );
  else
    node_filename = strcat ( prefix, '_nodes.txt' );
    xy = load ( node_filename );
  end
%
%  Plot the initial data.
%
  figure ( 1 )
  clf
  plot ( xy(:,1), xy(:,2), 'b.', 'MarkerSize', 20 );
  grid on
  axis tight
  axis equal
  xlabel ( '<--- X --->' );
  ylabel ( '<--- Y --->' );
  title ( prefix );
%
%  Plot the convex hull as computed by MATLAB.
%
  figure ( 2 )
  clf
  k = convhull ( xy(:,1), xy(:,2) );
  plot ( xy(:,1), xy(:,2), 'b.', 'MarkerSize', 20 );
  hold on
  plot( xy(k,1), xy(k,2), 'r-', 'LineWidth', 2 );
  grid on
  axis tight
  axis equal
  xlabel ( '<--- X --->' );
  ylabel ( '<--- Y --->' );
  title ( sprintf ( '%s by MATLAB convhull', prefix ) );
  hold off
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CONVEX_HULL:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  fprintf ( 1, '\n' );
  timestamp ( );

  return
end
function timestamp ( )

%*****************************************************************************80
%
%% TIMESTAMP prints the current YMDHMS date as a timestamp.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  t = now;
  c = datevec ( t );
  s = datestr ( c, 0 );
  fprintf ( 1, '%s\n', s );

  return
end


function rational_quadratic_test ( )

%*****************************************************************************80
%
%% RATIONAL_QUADRATIC_TEST examines the rational_quadratic correlation.
%
%  Discussion:
%
%    This code is based substantially on a document by Toby Driscoll.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Toby Driscoll,
%    Mercer's theorem and the Karhunen-Loeve expansion,
%    Oxford University Mathematical Institute,
%    http://www2.maths.ox.ac.uk/chebfun/examples/stats/pdf/MercerKarhunenLoeve.pdf
%
  rmpath ( '/usr/local/matlab/toolbox/datafeed/datafeed' )
  addpath ( '../chebfun' )

  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'RATIONAL_QUADRATIC_TEST:\n' );
  fprintf ( 1, '  Demonstrate Mercer''s theorem and the KL expansion\n' );
  fprintf ( 1, '  for the rational_quadratic kernel.\n' );
%
%  Set the interval.
%
  a = 0.0;
  b = +10.0;
  fprintf ( 1, '  Using interval [%g,%g]\n', a, b );
  s_num = 21;
  s_vec = linspace ( a, b, s_num );
%
%  FRED is a function from the CHEBFUN library.
%
%  It constructs a "chebop" representing the Fredholm integral operator
%  with kernel K for functions in domain [A,B].
%
  F = fred ( @rational_quadratic_correlation, domain ( [ a, b ] ) );
%
%  EIGS has been extended to be able to compute the eigenvalues Lambda
%  and eigenfunctions Psi of the Fredholm integral operator represented by F.
%
%  The "LM" switch requests that we return the eigenvalues of largest magnitude.
%
%  Each Psi is a CHEBFUN, that is, it takes a real number argument.
%
  eigen_num = 20;
  [ Psi, Lambda ] = eigs ( F, eigen_num, 'lm' );

  eigen_found = length ( Lambda );
  fprintf ( 1, '  Requested %d eigenmodes, computed %d\n', eigen_num, eigen_found );
  eigen_num = min ( eigen_num, eigen_found );
%
%  Print the eigenvalues.
%
  lambda_vec = diag ( Lambda );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   I       Lambda(I)\n' );
  fprintf ( 1, '\n' );
  for i = 1 : eigen_num
    fprintf ( 1, '  %2d  %10g\n', i, lambda_vec(i) );
  end
%
%  Plot the eigenvalues.
%
  figure ( 1 )
  clf
  plot ( lambda_vec, 'Linewidth', 2 );
  hold on
  plot ( lambda_vec, 'b.', 'Markersize', 20 );
  title ( 'rational_quadratic: Mercer eigenvalues' );
  xlabel ( '<--- N --->')
  grid on
  print -dpng 'rational_quadratic_figure1.png'
%
%  Plot selected eigenfunctions.
%
  figure ( 2 )
  subplot ( 4, 1, 1 )
  plot ( Psi(:,1), 'Linewidth', 2 );
  title ( 'rational_quadratic: Mercer eigenfunction PSI(1)' )
  grid on
  subplot ( 4, 1, 2 )
  plot ( Psi(:,2), 'Linewidth', 2 );
  title ( 'rational_quadratic: Mercer eigenfunction PSI(2)' )
  grid on
  subplot ( 4, 1, 3 )
  plot ( Psi(:,5), 'Linewidth', 2 );
  title ( 'rational_quadratic: Mercer eigenfunction PSI(5)' )
  grid on
  subplot ( 4, 1, 4 )
  plot ( Psi(:,10), 'Linewidth', 2 );
  title ( 'rational_quadratic: Mercer eigenfunction PSI(10)' )
  grid on
  print -dpng 'rational_quadratic_figure2.png'
%
%  Orthonormality check.
%
  ptp = Psi' * Psi;
  error_frobenius = r8mat_is_identity ( eigen_num, ptp );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Frobenius norm of I - Psi'' * Psi = %g\n', error_frobenius );
%
%  K(S,S) should be exactly 1.
%  Because we are using a truncated representation of K, our estimate of K(S,S)
%  will be smaller than 1.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Truncated estimate of K(s,s) = 1 for S in the interval.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '        S        K(s,s) estimate\n' );
  fprintf ( 1, '\n' );
  for i = 1 : 21
    s = s_vec(i);
    ptlp = Psi(s,:) * Lambda * Psi(s,:)';
    fprintf ( 1, '  %10g  %14g\n', s, ptlp );
  end
%
%  Look at eigenvalue decay.
%
  x = diff ( log ( ( 1:eigen_num ) ) );
  y = diff ( log ( lambda_vec ) )';
  c = y ./ x;
  figure ( 3 );
  clf
  plot ( c, 'Linewidth', 2 );
  grid on
  hold on
  plot ( c, 'b.', 'Markersize', 20 );
  xlabel ( '<-- N -->' )
  title ( 'rational_quadratic: Eigenvalue decay rate' );
  print -dpng 'rational_quadratic_figure3.png'
%
%  Look at eigenvalue sum.
%
%  The trace of K(s,s) over [a,b] should be the integral of 1 over [a,b],
%  that is, b - a.
%
%  We compare the trace to the partial sums of the lambda's to see how much
%  of the variance of the process we have captured.
%
  trace_K = b - a;
  lambda_cum = cumsum ( lambda_vec ) / trace_K;
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Index    Cumulative Eigenvalue sum\n' );
  fprintf ( 1, '\n' );
  for i = 1 : eigen_num
    fprintf ( 1, '  %5d  %10g\n', i, lambda_cum(i) );
  end
%
%  We decide to use the first 10 eigenfunctions.
%
  eigen_use = 10;
%
%  Find 400 realizations of the process by selecting, for each realization,
%  10 random parameters Z in the truncated KL expansion.
%
  Z = randn ( eigen_use, 400 );
  X = Psi(:,1:eigen_use) * ( sqrt ( Lambda(1:eigen_use,1:eigen_use) ) * Z );
%
%  Plot 40 of the realizations;
%  Plot their mean, computed from all 400.
%
  figure ( 4 )
  clf
  plot ( X(:,1:40) )
  mu = sum ( X, 2 ) / 400;
  hold on;
  plot ( mu, 'k', 'linewidth', 3 );
  title ( 'rational_quadratic: 40 Random Realizations X(t,omega), and their Mean.' )
  print -dpng 'rational_quadratic_figure4.png'
%
%  Estimate the covariance from the data.
%
  figure ( 5 )
  clf
  [ S, T ] = meshgrid ( s_vec, s_vec );
  C = cov ( X(s_vec,:)' );
  mesh ( S, T, C );
  hold on;
  D = rational_quadratic_correlation ( S, T );
  plot3 ( S, T, D, 'k.', 'markersize', 10 )
  title ( 'rational_quadratic: Covariance K(S,T) (dots), and Estimate from Realizations (mesh)' )
  print -dpng 'rational_quadratic_figure5.png'
%
%  Using just 10 functions in the expansion,
%  reduce the correlation length,
%  and examine the sum of the lambda's.
%
  if ( 0 )
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Use a fixed number of eigenfunctions = %d\n', eigen_use );
  fprintf ( 1, '  but vary the correlation length RHOBAR.\n' );
  fprintf ( 1, '  (We used RHOBAR = 1 above.)\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The sum of the eigenvalues, divided by (B-A),\n' );
  fprintf ( 1, '  discloses the relative amount of the total variation\n' );
  fprintf ( 1, '  that is captured by the truncated expansion.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      RHOBAR      VARSUM\n' );
  fprintf ( 1, '\n' );
  rhobar = 4.0;
  for i = 1 : 10
    K = @ ( s, t ) exp ( - abs ( s - t ) / rhobar );
    F = fred ( K, domain ( [ a, b ] ) );
    lambda_vec = eigs ( F, 20, 'lm' );
    varsum = sum ( lambda_vec(1:eigen_use) ) / ( b - a );
    fprintf ( 1, '  %10g  %10g\n', rhobar, varsum );
    rhobar = rhobar / 2.0;
  end

  end
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'RATIONAL_QUADRATIC_TEST:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  rmpath ( '../chebfun' )

  return
end


function J = lotkaObjectiveFCN(u,x,Ts,N,xref,u0,p,Q,R,Ru)
%% Cost function of nonlinear MPC for Lotka-Volterra system
%
% Inputs:
%   u:      optimization variable, from time k to time k+N-1
%   x:      current state at time k
%   Ts:     controller sample time
%   N:      prediction horizon
%   xref:   state references, constant from time k+1 to k+N
%   u0:     previous controller output at time k-1
%
% Output:
%   J:      objective function cost
%

%% Nonlinear MPC design parameters

%% Cost Calculation
% Set initial plant states, controller output and cost
xk = x;
uk = u(1);
J = 0;
% Loop through each prediction step
for ct=1:N
    % Obtain plant state at next prediction step
    xk1 = rk4u(@sparseGalerkinControl_Discrete,xk,uk,Ts,1,[],p);

    % Accumulate state tracking cost from x(k+1) to x(k+N)
    J = J + (xk1-xref)'*Q*(xk1-xref);
    % Accumulate MV rate of change cost from u(k) to u(k+N-1)
    if ct==1
        J = J + (uk-u0)'*R*(uk-u0) + uk'*Ru*uk;
    else
        J = J + (uk-u(ct-1))'*R*(uk-u(ct-1)) + uk'*Ru*uk;
    end
    % Update xk and uk for the next prediction step
    xk = xk1;
    if ct<N
        uk = u(ct+1);
    end
end


%JTRAJ Compute a joint space trajectory
%
% [Q,QD,QDD] = JTRAJ(Q0, QF, M) is a joint space trajectory Q (MxN) where the joint
% coordinates vary from Q0 (1xN) to QF (1xN).  A quintic (5th order) polynomial is used
% with default zero boundary conditions for velocity and acceleration.
% Time is assumed to vary from 0 to 1 in M steps.  Joint velocity and
% acceleration can be optionally returned as QD (MxN) and QDD (MxN) respectively.
% The trajectory Q, QD and QDD are MxN matrices, with one row per time step,
% and one column per joint.
%
% [Q,QD,QDD] = JTRAJ(Q0, QF, M, QD0, QDF) as above but also specifies
% initial QD0 (1xN) and final QDF (1xN) joint velocity for the trajectory.
%
% [Q,QD,QDD] = JTRAJ(Q0, QF, T) as above but the number of steps in the
% trajectory is defined by the length of the time vector T (Mx1).
%
% [Q,QD,QDD] = JTRAJ(Q0, QF, T, QD0, QDF) as above but specifies initial and
% final joint velocity for the trajectory and a time vector.
%
% Notes::
% - When a time vector is provided the velocity and acceleration outputs
%   are scaled assumign that the time vector starts at zero and increases
%   linearly.
%
% See also QPLOT, CTRAJ, SerialLink.jtraj.

% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

function [qt,qdt,qddt] = jtraj(q0, q1, tv, qd0, qd1)
    if length(tv) > 1
        tscal = max(tv);
        t = tv(:)/tscal;
    else
        tscal = 1;
        t = (0:(tv-1))'/(tv-1); % normalized time from 0 -> 1
    end

    q0 = q0(:);
    q1 = q1(:);

    if nargin == 3
        qd0 = zeros(size(q0));
        qd1 = qd0;
    elseif nargin == 5
        qd0 = qd0(:);
        qd1 = qd1(:);
    else
        error('incorrect number of arguments')
    end

    % compute the polynomial coefficients
    A = 6*(q1 - q0) - 3*(qd1+qd0)*tscal;
    B = -15*(q1 - q0) + (8*qd0 + 7*qd1)*tscal;
    C = 10*(q1 - q0) - (6*qd0 + 4*qd1)*tscal;
    E = qd0*tscal; % as the t vector has been normalized
    F = q0;

    tt = [t.^5 t.^4 t.^3 t.^2 t ones(size(t))];
    c = [A B C zeros(size(A)) E F]';

    qt = tt*c;

    % compute optional velocity
    if nargout >= 2
        c = [ zeros(size(A)) 5*A 4*B 3*C  zeros(size(A)) E ]';
        qdt = tt*c/tscal;
    end

    % compute optional acceleration
    if nargout == 3
        c = [ zeros(size(A))  zeros(size(A)) 20*A 12*B 6*C  zeros(size(A))]';
        qddt = tt*c/tscal^2;
    end


function DEM_demo_convolution
% DEM demo for linear deconvolution:  This demo considers the deconvolution
% of the responses of a single-input-multiple output input-state-output
% model (DCM) to disclose the input or causes.  It focuses on estimating the
% causes and hidden states: The notes provide a comparative evaluation with
% extended Kalman filtering (see script after return).

% get a simple convolution model
%==========================================================================
M       = spm_DEM_M('convolution model');
M(1).V  = exp(8);                             % error precision
M(1).W  = exp(8);                             % error precision

% and generate data
%==========================================================================
N       = 32;                                 % length of data sequence
U       = exp(-([1:N] - 12).^2/(2.^2));       % this is the Gaussian cause;
DEM     = spm_DEM_generate(M,U,{},{[] 16},{16});

% invert model
%==========================================================================
DEM     = spm_DEM(DEM);

% overlay true values
%--------------------------------------------------------------------------
spm_DEM_qU(DEM.qU,DEM.pU)

return

% explore dimensions (n)
%==========================================================================
for i = 1:8
    DEM.M(1).E.n = i;
    DEM.M(1).E.d = 3;

    D{i}   = spm_DEM(DEM);
    F(i)   = D{i}.F;
    Sx(i)  = sum(sum((D{i}.qU.x{1} - DEM.pU.x{1}).^2));
    Sv(i)  = sum(sum((D{i}.qU.v{2} - DEM.pU.v{2}).^2));
end

% plot
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 1');

subplot(2,1,1)
bar(Sv)
xlabel('n - 1 (d = 2)')
ylabel('sum squared error (causal states)')
title('embedding dimension (n)','FontSize',16)
axis square

d     = [1 8];
for i = 1:length(d)
    subplot(2,length(d),i + length(d))
    hold on
    plot([1:N],D{d(i)}.qU.v{2})
    plot([1:N],DEM.pU.v{2},'linewidth',2,'color',[1 1 1]/2)
    hold off
    axis square
    xlabel('time')
    title(sprintf('n = %i',d(i)),'FontSize',16)
    if i == 1, a = axis; else, axis(a); end
end

% and d
%--------------------------------------------------------------------------
clear D F Sx Sv
for i = 1:6
    DEM.M(1).E.n = 15;
    DEM.M(1).E.d = i - 1;

    D{i}   = spm_DEM(DEM);
    F(i)   = D{i}.F;
    Sx(i)  = sum(sum((D{i}.qU.x{1} - DEM.pU.x{1}).^2));
    Sv(i)  = sum(sum((D{i}.qU.v{2} - DEM.pU.v{2}).^2));

end

% plot
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 2'); clf

subplot(2,2,1)
bar(Sx)
xlabel('d (n = 15)')
ylabel('sum squared error (hidden states)')
title('embedding dimension (d)','FontSize',16)
axis square
set(gca,'XTickLabel',[0:7])

subplot(2,2,2)
bar(F - min(F) + 32)
xlabel('d (n = 15)')
ylabel('log-evidence')
title('log-evidence','FontSize',16)
axis square
set(gca,'XTickLabel',[0:7])

d     = [1 4];
for i = 1:length(d)
    subplot(2,length(d),i + length(d))
    hold on
    plot([1:N],D{d(i)}.qU.x{1})
    plot([1:N],DEM.pU.x{1},'linewidth',2,'color',[1 1 1]/2)
    hold off
    axis square
    xlabel('time')
    title(sprintf('d = %i',d(i) - 1),'FontSize',16)
    if i == 1, a = axis; else, axis(a); end
end

% Comparison with EKF
%==========================================================================
clear SSE
for i = 1:8

    % i.i.d.
    %----------------------------------------------------------------------
    DEM      = spm_DEM_generate(M,U,{},{[] 16});
    DEM      = spm_DEM(DEM);

    % serial correlations
    %----------------------------------------------------------------------
    D0          = DEM;
    D0.M(1).E.s = 0;
    D0          = spm_DEM(D0);

    % extended kalman filter - i.i.d.
    %----------------------------------------------------------------------
    e_x      = spm_ekf(DEM.M,DEM.Y);
    d_x      = DEM.qU.x{1};
    t_x      = DEM.pU.x{1};
    i_x      = D0.qU.x{1};

    SSE(i,1) = sum(sum((e_x - t_x).^2));
    SSE(i,2) = sum(sum((i_x - t_x).^2));
    SSE(i,3) = sum(sum((d_x - t_x).^2));

end

spm_figure('GetWin','Figure 3');

subplot(2,1,1)
plot(1:N,i_x,'g',1:N,d_x,'r',1:N,e_x,'b',1:N,t_x,'k')
legend('DEM(0)',' ','DEM(0.5)',' ','EKF',' ','true',' ')
xlabel('time')
title({'hidden states','comparison with EKF'},'FontSize',16)
axis square

subplot(2,1,2)
plot(SSE','k:'), hold on
plot(SSE','k.','Markersize',16), hold off
set(gca,'Xtick',[1 2 3],'XLim',[0 4])
set(gca,'Xticklabel',{'EKF','DEM(0)','DEM(0.5)'})
title('sum of squared error (hidden states)','FontSize',16)
axis square

% Show equivalence when causes are not structured
%==========================================================================
M(1).E.s  = 0;
M(1).pE.h = eye(2);
M(1).pC   = [];

M(2).v    = [0;0];
M(2).V    = eye(2);
U         = [U; 0*U];

DEM       = spm_DEM_generate(M,U,{},{[] 16});
DEM       = spm_DEM(DEM);
e_x       = spm_ekf(DEM.M,DEM.Y);
d_x       = DEM.qU.x{1};

spm_figure('GetWin','Figure 4');

subplot(2,1,1)
plot(1:N,d_x,'r',1:N,e_x,'b')
legend('DEM(0)',' ','EKF',' ')
xlabel('time')
title({'hidden states';'no correlations'},'FontSize',16)
axis square

% Show equivalence when causes are removed
%--------------------------------------------------------------------------
DEM.M(1).pE.h = sparse(2,2);
DEM.M(1).W    = speye(2);

DEM           = spm_DEM(DEM);
e_x           = spm_ekf(DEM.M,DEM.Y);
d_x           = DEM.qU.x{1};

spm_figure('GetWin','Figure 4');

subplot(2,1,2)
plot(1:N,d_x,'r',1:N,e_x,'b')
legend('DEM(0)',' ','EKF',' ')
xlabel('time')
title({'hidden states';'no causes'},'FontSize',16)
axis square


function [v,beta,s] = house (x)
%HOUSE find a Householder reflection.
% real or complex case.
% Example:
%   [v,beta,s] = house (x)
% See also: cs_demo

%   Copyright 2006-2007, Timothy A. Davis.
%   http://www.cise.ufl.edu/research/sparse

v = x ;
s = norm (x) ;
if (s == 0)
    beta = 0 ;
    v (1) = 1 ;
else
    if (x (1) ~= 0)
        s = sign (x (1)) * s ;
    end
    v (1) = v (1) + s ;
    beta = 1 / real (conj (s) * v (1)) ;
end
s = - s ;


function [x, istop, itn, normr, normAr, normA, condA, normx]...
   = lsmr(A, b, lambda, atol, btol, conlim, itnlim, localSize, show)

% LSMR   Iterative solver for least-squares problems.
%   X = LSMR(A,B) solves the system of linear equations A*X=B. If the system
%   is inconsistent, it solves the least-squares problem min ||b - Ax||_2.
%   A is a rectangular matrix of dimension m-by-n, where all cases are
%   allowed: m=n, m>n, or m<n. B is a vector of length m.
%   The matrix A may be dense or sparse (usually sparse).
%
%   X = LSMR(AFUN,B) takes a function handle AFUN instead of the matrix A.
%   AFUN(X,1) takes a vector X and returns A*X. AFUN(X,2) returns A'*X.
%   AFUN can be used in all the following syntaxes.
%
%   X = LSMR(A,B,LAMBDA) solves the regularized least-squares problem
%      min ||(B) - (   A    )X||
%          ||(0)   (LAMBDA*I) ||_2
%   where LAMBDA is a scalar.  If LAMBDA is [] or 0, the system is solved
%   without regularization.
%
%   X = LSMR(A,B,LAMBDA,ATOL,BTOL) continues iterations until a certain
%   backward error estimate is smaller than some quantity depending on
%   ATOL and BTOL.  Let RES = B - A*X be the residual vector for the
%   current approximate solution X.  If A*X = B seems to be consistent,
%   LSMR terminates when NORM(RES) <= ATOL*NORM(A)*NORM(X) + BTOL*NORM(B).
%   Otherwise, LSMR terminates when NORM(A'*RES) <= ATOL*NORM(A)*NORM(RES).
%   If both tolerances are 1.0e-6 (say), the final NORM(RES) should be
%   accurate to about 6 digits. (The final X will usually have fewer
%   correct digits, depending on cond(A) and the size of LAMBDA.)
%   If ATOL or BTOL is [], a default value of 1.0e-6 will be used.
%   Ideally, they should be estimates of the relative error in the
%   entries of A and B respectively.  For example, if the entries of A
%   have 7 correct digits, set ATOL = 1e-7. This prevents the algorithm
%   from doing unnecessary work beyond the uncertainty of the input data.
%
%   X = LSMR(A,B,LAMBDA,ATOL,BTOL,CONLIM) terminates if an estimate
%   of cond(A) exceeds CONLIM. For compatible systems Ax = b,
%   conlim could be as large as 1.0e+12 (say).  For least-squares problems,
%   conlim should be less than 1.0e+8. If CONLIM is [], the default value
%   is CONLIM = 1e+8. Maximum precision can be obtained by setting
%   ATOL = BTOL = CONLIM = 0, but the number of iterations may then be
%   excessive.
%
%   X = LSMR(A,B,LAMBDA,ATOL,BTOL,CONLIM,ITNLIM) terminates if the
%   number of iterations reaches ITNLIM.  The default is ITNLIM = min(m,n).
%   For ill-conditioned systems, a larger value of ITNLIM may be needed.
%
%   X = LSMR(A,B,LAMBDA,ATOL,BTOL,CONLIM,ITNLIM,LOCALSIZE) runs LSMR
%   with reorthogonalization on the last LOCALSIZE v_k's (v-vectors
%   generated by the Golub-Kahan bidiagonalization). LOCALSIZE = 0 or []
%   runs LSMR without reorthogonalization. LOCALSIZE = Inf specifies
%   full reorthogonalization of the v_k's.  Reorthogonalizing only u_k or
%   both u_k and v_k are not an option here, because reorthogonalizing all
%   v_k's makes the u_k's close to orthogonal. Details are given in the
%   submitted SIAM paper.
%
%   X = LSMR(A,B,LAMBDA,ATOL,BTOL,CONLIM,ITNLIM,LOCALSIZE,SHOW) prints an
%   iteration log if SHOW=true. The default value is SHOW=false.
%
%   [X,ISTOP] = LSMR(A,B,...) gives the reason for termination.
%      ISTOP  = 0 means X=0 is a solution.
%             = 1 means X is an approximate solution to A*X = B,
%                 according to ATOL and BTOL.
%             = 2 means X approximately solves the least-squares problem
%                 according to ATOL.
%             = 3 means COND(A) seems to be greater than CONLIM.
%             = 4 is the same as 1 with ATOL = BTOL = EPS.
%             = 5 is the same as 2 with ATOL = EPS.
%             = 6 is the same as 3 with CONLIM = 1/EPS.
%             = 7 means ITN reached ITNLIM before the other stopping
%                 conditions were satisfied.
%
%   [X,ISTOP,ITN] = LSMR(A,B,...) gives ITN = the number of LSMR iterations.
%
%   [X,ISTOP,ITN,NORMR] = LSMR(A,B,...) gives an estimate of the residual
%   norm: NORMR = norm(B-A*X).
%
%   [X,ISTOP,ITN,NORMR,NORMAR] = LSMR(A,B,...) gives an estimate of the
%   residual for the normal equation: NORMAR = NORM(A'*(B-A*X)).
%
%   [X,ISTOP,ITN,NORMR,NORMAR,NORMA] = LSMR(A,B,...) gives an estimate of
%   the Frobenius norm of A.
%
%   [X,ISTOP,ITN,NORMR,NORMAR,NORMA,CONDA] = LSMR(A,B,...) gives an estimate
%   of the condition number of A.
%
%   [X,ISTOP,ITN,NORMR,NORMAR,NORMA,CONDA,NORMX] = LSMR(A,B,...) gives an
%   estimate of NORM(X).
%
%   LSMR uses an iterative method requiring matrix-vector products A*v
%   and A'*u.  For further information, see
%      D. C.-L. Fong and M. A. Saunders,
%      LSMR: An iterative algorithm for sparse least-squares problems,
%      SIAM J. Sci. Comput., submitted 1 June 2010.
%      See http://www.stanford.edu/~clfong/lsmr.html.
%
% 08 Dec 2009: First release version of LSMR.
% 09 Apr 2010: Updated documentation and default parameters.
% 14 Apr 2010: Updated documentation.
% 03 Jun 2010: LSMR with local and/or full reorthogonalization of v_k.
% 10 Mar 2011: Bug fix in reorthgonalization. (suggested by David Gleich)

% David Chin-lung Fong            clfong@stanford.edu
% Institute for Computational and Mathematical Engineering
% Stanford University
%
% Michael Saunders                saunders@stanford.edu
% Systems Optimization Laboratory
% Dept of MS&E, Stanford University.
%-----------------------------------------------------------------------

  % Initialize.

  if isa(A,'numeric')
    explicitA = true;
  elseif isa(A,'function_handle')
    explicitA = false;
  else
    error('SOL:lsmr:Atype','%s','A must be numeric or a function handle');
  end

  msg = ['The exact solution is  x = 0                              '
         'Ax - b is small enough, given atol, btol                  '
         'The least-squares solution is good enough, given atol     '
         'The estimate of cond(Abar) has exceeded conlim            '
         'Ax - b is small enough for this machine                   '
         'The least-squares solution is good enough for this machine'
         'Cond(Abar) seems to be too large for this machine         '
         'The iteration limit has been reached                      '];

  hdg1 = '   itn      x(1)       norm r    norm A''r';
  hdg2 = ' compatible   LS      norm A   cond A';
  pfreq  = 20;   % print frequency (for repeating the heading)
  pcount = 0;    % print counter

  % Determine dimensions m and n, and
  % form the first vectors u and v.
  % These satisfy  beta*u = b,  alpha*v = A'u.

  u    = b;
  beta = norm(u);
  if beta > 0
    u  = u/beta;
  end

  if explicitA
    v = A'*u;
    [m n] = size(A);
  else
    v = A(u,2);
    m = size(b,1);
    n = size(v,1);
  end

  minDim = min([m n]);

  % Set default parameters.

  if nargin < 3 || isempty(lambda)   , lambda    = 0;          end
  if nargin < 4 || isempty(atol)     , atol      = 1e-6;       end
  if nargin < 5 || isempty(btol)     , btol      = 1e-6;       end
  if nargin < 6 || isempty(conlim)   , conlim    = 1e+8;       end
  if nargin < 7 || isempty(itnlim)   , itnlim    = minDim;     end
  if nargin < 8 || isempty(localSize), localSize = 0;          end
  if nargin < 9 || isempty(show)     , show      = false;      end

  if show
    fprintf('\n\nLSMR            Least-squares solution of  Ax = b')
    fprintf('\nVersion 1.11                          09 Jun 2010')
    fprintf('\nThe matrix A has %8g rows  and %8g cols', m,n)
    fprintf('\nlambda = %16.10e', lambda )
    fprintf('\natol   = %8.2e               conlim = %8.2e', atol,conlim)
    fprintf('\nbtol   = %8.2e               itnlim = %8g'  , btol,itnlim)
  end

  alpha = norm(v);
  if alpha > 0
    v = (1/alpha)*v;
  end

  % Initialization for local reorthogonalization.

  localOrtho = false;
  if localSize > 0
    localPointer    = 0;
    localOrtho      = true;
    localVQueueFull = false;

    % Preallocate storage for the relevant number of latest v_k's.

    localV = zeros(n, min([localSize minDim]));
  end

  % Initialize variables for 1st iteration.

  itn      = 0;
  zetabar  = alpha*beta;
  alphabar = alpha;
  rho      = 1;
  rhobar   = 1;
  cbar     = 1;
  sbar     = 0;

  h    = v;
  hbar = zeros(n,1);
  x    = zeros(n,1);

  % Initialize variables for estimation of ||r||.

  betadd      = beta;
  betad       = 0;
  rhodold     = 1;
  tautildeold = 0;
  thetatilde  = 0;
  zeta        = 0;
  d           = 0;

  % Initialize variables for estimation of ||A|| and cond(A).

  normA2  = alpha^2;
  maxrbar = 0;
  minrbar = 1e+100;

  % Items for use in stopping rules.
  normb  = beta;
  istop  = 0;
  ctol   = 0;         if conlim > 0, ctol = 1/conlim; end;
  normr  = beta;

  % Exit if b=0 or A'b = 0.

  normAr = alpha * beta;
  if normAr == 0, disp(msg(1,:)); return, end

  % Heading for iteration log.

  if show
    test1 = 1;
    test2 = alpha/beta;
    fprintf('\n\n%s%s'      , hdg1 , hdg2   )
    fprintf('\n%6g %12.5e'  , itn  , x(1)   )
    fprintf(' %10.3e %10.3e', normr, normAr )
    fprintf('  %8.1e %8.1e' , test1, test2  )
  end

  %------------------------------------------------------------------
  %     Main iteration loop.
  %------------------------------------------------------------------
  while itn < itnlim
    itn = itn + 1;

    % Perform the next step of the bidiagonalization to obtain the
    % next beta, u, alpha, v.  These satisfy the relations
    %      beta*u  =  A*v  - alpha*u,
    %      alpha*v  =  A'*u - beta*v.

    if explicitA
      u = A*v    - alpha*u;
    else
      u = A(v,1) - alpha*u;
    end
    beta = norm(u);

    if beta > 0
      u = (1/beta)*u;
      if localOrtho
        localVEnqueue(v);    % Store old v for local reorthogonalization of new v.
      end
      if explicitA
        v = A'*u   - beta*v;
      else
        v = A(u,2) - beta*v;
      end

      if localOrtho
        v = localVOrtho(v);  % Local-reorthogonalization of new v.
      end
      alpha  = norm(v);
      if alpha > 0,  v = (1/alpha)*v; end
    end

    % At this point, beta = beta_{k+1}, alpha = alpha_{k+1}.

    % Construct rotation Qhat_{k,2k+1}.

    alphahat = norm([alphabar lambda]);
    chat     = alphabar/alphahat;
    shat     = lambda/alphahat;

    % Use a plane rotation (Q_i) to turn B_i to R_i.

    rhoold   = rho;
    rho      = norm([alphahat beta]);
    c        = alphahat/rho;
    s        = beta/rho;
    thetanew = s*alpha;
    alphabar = c*alpha;

    % Use a plane rotation (Qbar_i) to turn R_i^T to R_i^bar.

    rhobarold = rhobar;
    zetaold   = zeta;
    thetabar  = sbar*rho;
    rhotemp   = cbar*rho;
    rhobar    = norm([cbar*rho thetanew]);
    cbar      = cbar*rho/rhobar;
    sbar      = thetanew/rhobar;
    zeta      =   cbar*zetabar;
    zetabar   = - sbar*zetabar;

    % Update h, h_hat, x.

    hbar      = h - (thetabar*rho/(rhoold*rhobarold))*hbar;
    x         = x + (zeta/(rho*rhobar))*hbar;
    h         = v - (thetanew/rho)*h;

    % Estimate of ||r||.

    % Apply rotation Qhat_{k,2k+1}.
    betaacute =   chat* betadd;
    betacheck = - shat* betadd;

    % Apply rotation Q_{k,k+1}.
    betahat   =   c*betaacute;
    betadd    = - s*betaacute;

    % Apply rotation Qtilde_{k-1}.
    % betad = betad_{k-1} here.

    thetatildeold = thetatilde;
    rhotildeold   = norm([rhodold thetabar]);
    ctildeold     = rhodold/rhotildeold;
    stildeold     = thetabar/rhotildeold;
    thetatilde    = stildeold* rhobar;
    rhodold       =   ctildeold* rhobar;
    betad         = - stildeold*betad + ctildeold*betahat;

    % betad   = betad_k here.
    % rhodold = rhod_k  here.

    tautildeold   = (zetaold - thetatildeold*tautildeold)/rhotildeold;
    taud          = (zeta - thetatilde*tautildeold)/rhodold;
    d             = d + betacheck^2;
    normr         = sqrt(d + (betad - taud)^2 + betadd^2);

    % Estimate ||A||.
    normA2        = normA2 + beta^2;
    normA         = sqrt(normA2);
    normA2        = normA2 + alpha^2;

    % Estimate cond(A).
    maxrbar       = max(maxrbar,rhobarold);
    if itn>1
      minrbar     = min(minrbar,rhobarold);
    end
    condA         = max(maxrbar,rhotemp)/min(minrbar,rhotemp);

    % Test for convergence.

    % Compute norms for convergence testing.
    normAr  = abs(zetabar);
    normx   = norm(x);

    % Now use these norms to estimate certain other quantities,
    % some of which will be small near a solution.

    test1   = normr /normb;
    test2   = normAr/(normA*normr);
    test3   =      1/condA;
    t1      =  test1/(1 + normA*normx/normb);
    rtol    = btol + atol*normA*normx/normb;

    % The following tests guard against extremely small values of
    % atol, btol or ctol.  (The user may have set any or all of
    % the parameters atol, btol, conlim  to 0.)
    % The effect is equivalent to the normAl tests using
    % atol = eps,  btol = eps,  conlim = 1/eps.

    if itn >= itnlim,   istop = 7; end
    if 1 + test3  <= 1, istop = 6; end
    if 1 + test2  <= 1, istop = 5; end
    if 1 + t1     <= 1, istop = 4; end

    % Allow for tolerances set by the user.

    if  test3 <= ctol,  istop = 3; end
    if  test2 <= atol,  istop = 2; end
    if  test1 <= rtol,  istop = 1; end

    % See if it is time to print something.

    if show
      prnt = 0;
      if n     <= 40       , prnt = 1; end
      if itn   <= 10       , prnt = 1; end
      if itn   >= itnlim-10, prnt = 1; end
      if rem(itn,10) == 0  , prnt = 1; end
      if test3 <= 1.1*ctol , prnt = 1; end
      if test2 <= 1.1*atol , prnt = 1; end
      if test1 <= 1.1*rtol , prnt = 1; end
      if istop ~=  0       , prnt = 1; end

      if prnt
	if pcount >= pfreq
	  pcount = 0;
          fprintf('\n\n%s%s'    , hdg1 , hdg2  )
	end
	pcount = pcount + 1;
        fprintf('\n%6g %12.5e'  , itn  , x(1)  )
        fprintf(' %10.3e %10.3e', normr, normAr)
        fprintf('  %8.1e %8.1e' , test1, test2 )
        fprintf(' %8.1e %8.1e'  , normA, condA )
      end
    end

    if istop > 0, break, end
  end % iteration loop

  % Print the stopping condition.

  if show
    fprintf('\n\nLSMR finished')
    fprintf('\n%s', msg(istop+1,:))
    fprintf('\nistop =%8g    normr =%8.1e'     , istop, normr )
    fprintf('    normA =%8.1e    normAr =%8.1e', normA, normAr)
    fprintf('\nitn   =%8g    condA =%8.1e'     , itn  , condA )
    fprintf('    normx =%8.1e\n', normx)
  end

% end function lsmr

%---------------------------------------------------------------------
% Nested functions.
%---------------------------------------------------------------------

  function localVEnqueue(v)

  % Store v into the circular buffer localV.

    if localPointer < localSize
      localPointer = localPointer + 1;
    else
      localPointer = 1;
      localVQueueFull = true;
    end
    localV(:,localPointer) = v;

  end % nested function localVEnqueue

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

  function vOutput = localVOrtho(v)

  % Perform local reorthogonalization of V.

    vOutput = v;
    if localVQueueFull
      localOrthoLimit = localSize;
    else
      localOrthoLimit = localPointer;
    end
    for localOrthoCount = 1:localOrthoLimit
      vtemp   = localV(:, localOrthoCount);
      vOutput = vOutput - (vOutput'*vtemp)*vtemp;
    end

  end % nested function localVOrtho

end % function lsmr


function X = positive_definite_intrinsic_mean(A)
% Computes an intrinsic mean of a collection of positive definite matrices.
%
% function X = positive_definite_intrinsic_mean(A)
%
% Input:  A 3D matrix A of size nxnxm such that each slice A(:, :, k) is a
%         positive definite matrix of size nxn.
%
% Output: A positive definite matrix X of size nxn which is an intrinsic mean
%         of the m matrices in A, that is, X minimizes the sum of squared
%         Riemannian distances to the matrices in A:
%            f(X) = sum_k=1^m .5*dist^2(X, A(:, :, k))
%         The distance is defined by the natural metric on the set of
%         positive definite matrices: see sympositivedefinitefactory.
%
% This simple example is not the best way to compute intrinsic means. Its
% purpose it to serve as base code to explore other algorithms. In
% particular, in the presence of large noise, this algorithm seems not to
% be able to reach points with a very small gradient norm. This may be
% caused by insufficient accuracy in the gradient computation.
%
% See also: sympositivedefinitefactory

% This file is part of Manopt and is copyrighted. See the license file.
%
% Main author: Nicolas Boumal, Sept. 3, 2013
% Contributors:
%
% Change log:
%     Sep. 15, 2022 (NB):
%         Changed name from positive_definite_karcher_mean, clarified
%         some comments.

    % Generate some random data to test the function if none is given.
    if ~exist('A', 'var') || isempty(A)
        n = 5;
        m = 50;
        A = zeros(n, n, m);
        ref = diag(max(.1, 1+.1*randn(n, 1)));
        for i = 1 : m
            noise = 0.01*randn(n);
            noise = (noise + noise')/2;
            [V, D] = eig(ref + noise);
            A(:, :, i) = V*diag(max(.01, diag(D)))*V';
        end
    end

    % Retrieve the size of the problem:
    % There are m matrices of size nxn to average.
    n = size(A, 1);
    m = size(A, 3);
    assert(n == size(A, 2), ...
           ['The slices of A must be square, i.e., the ' ...
	        'first and second dimensions of A must be equal.']);

    % Our search space is the set of positive definite matrices of size n.
    % Notice that this is the only place we specify on which manifold we
    % wish to compute Karcher means. Replacing this factory for another
    % geometry will yield code to compute Karcher means on that other
    % manifold, provided that manifold is equipped with a dist function and
    % a logarithmic map log.
    M = sympositivedefinitefactory(n);

    % Define a problem structure, specifying the manifold M, the cost
    % function and its gradient.
    problem.M = M;
    problem.cost = @cost;
    problem.grad = @grad;

    % Explicitly pick an approximate Hessian for the trust-region method.
    % (This is only to show an example of how it can be done; the solver
    % below, rlbfgs, does not use the approximate Hessian; trustregions
    % would, but it would figure it out automatically with default
    % stepsize if the line below is omitted.)
    problem.approxhess = approxhessianFD(problem, struct('stepsize', 1e-4));

    % The functions below make many redundant computations. This
    % performance hit can be alleviated by using the caching system. We go
    % for a simple implementation here, as a tutorial example.

    % Cost function
    function f = cost(X)
        f = 0;
        for k = 1 : m
            f = f + M.dist(X, A(:, :, k))^2;
        end
        f = f/(2*m);
    end

    % Riemannian gradient of the cost function
    function g = grad(X)
        g = M.zerovec(X);
        for k = 1 : m
            % Update g in a linear combination of the form
            % g = g - [something]/m.
            g = M.lincomb(X, 1, g, -1/m, M.log(X, A(:, :, k)));
        end
    end

    % Execute some checks on the derivatives for early debugging.
    % These things can be commented out of course.
    % The slopes should agree on part of the plot at least. In this case,
    % it is sometimes necessary to inspect the plot visually to make the
    % call, but it is indeed correct.
    % checkgradient(problem);
    % pause;

    % Execute this if you want to force using a proper parallel vector
    % transport. This is not necessary. If you omit this, the default
    % vector transport is the identity map, which is (of course) cheaper
    % and seems to perform well in practice.
    % M.transp = M.paralleltransp;

    % Issue a call to a solver. Default options are selected.
    % Our initial guess is the first data point. Most solvers work well
    % with this problem. Limited-memory BFGS is one good example:
    X = rlbfgs(problem, A(:, :, 1));

end


% Fig. 6.30   Feedback Control of Dynamic Systems, 5e
%             Franklin, Powell, Emami
%

clear all;
close all;

num=[1 1];
den=conv([1 0],[0.1 -1]);
w=logspace(-1,2,100);
[m,p]=bode(num,den,w);
subplot(2,1,1),loglog(w,m,'-',w,ones(size(w)),'-');
grid;
ylabel('Magnitude');
title('Fig. 6.30 Bode Plot for G=(s+1)/[s(s/10 -1)] (a) magnitude');
subplot(2,1,2)
if p>0, p=p-360; end      % Matlab quadrant control varies in different versions
semilogx(w,p,'-',w,-180*ones(size(w)),'-');
axis([.1 100 -270 -90])
grid;
xlabel('\omega (rad/sec)');
ylabel('Phase (deg)');
title('(b) phase');


% This program obtains the analytical step response of a parallel RLC
% circuit. In addition, graphs of the voltage across the parallel
% combination and branch currents are obtained.
% H. Saadat, Copyright 1999

function PresponseIL
warning('off','MATLAB:dispatcher:InexactMatch')
Rhandle=findobj('Tag','Rtext');
R=eval(get(Rhandle,'String'));
KOhandle=findobj('Tag','KOhm');
KO=get(KOhandle,'Value');
if KO==1
   R=1000*R;
else, end
Lhandle=findobj('Tag','Ltext');
L=eval(get(Lhandle,'String'));
mHhandle=findobj('Tag','mHenry');
ml=get(mHhandle,'Value');
if ml==1
   L=L/1000;
else, end
Chandle=findobj('Tag','Ctext');
C=eval(get(Chandle,'String'));
micFhandle=findobj('Tag','micFarad');
micF=get(micFhandle,'Value');
if micF==1
   C=C/1000000;
else, end
Rhandle=findobj('Tag','IStext');
Is=eval(get(Rhandle,'String'));
Rhandle=findobj('Tag','I0text');
I0=eval(get(Rhandle,'String'));
Rhandle=findobj('Tag','V0text');
V0=eval(get(Rhandle,'String'));
% Step Response of a parallel RLC circuit
if L==inf | C==inf
   plot(0, 0)
   text(-0.8, 0.80, 'Parameters must lie in the following ranges:', 'color', [0 0 0.6275])
   text(-0.8, 0.65, '0 < R \leq inf,   0 < L < inf,   &    0 < C < inf', 'color', [0 0 0.6275])
   text(-0.8, 0.5, 'Enter the correct value and press Solve', 'color', [0 0 0.6275])
   axis([-1  1  -1  1]);return,  else
end
if R==0 | L==0 | C == 0
plot(0, 0)
title('Short-circuit across the parallel circuit', 'color', [ 1 0 0])
text(-0.8, 0.80, 'Parameters must lie in the following ranges:', 'color', [0 0 0.6275])
text(-0.8, 0.65, '0 < R \leq inf,   0 < L < inf,   &    0 < C < inf', 'color', [0 0 0.6275])
text(-0.8, 0.5, 'Enter the correct value and press Solve', 'color', [0 0 0.6275])
axis([-1  1  -1  1]);return,  else
alpha =1/(2*R*C);
end
w02 = 1/(L*C); w0 = sqrt(w02);
if alpha~= 0
   err=(alpha-w0)/alpha;
   if abs(err) <1e-4
   w0=alpha;
   else, end
else, end
dv = Is/C-I0/C-V0/(R*C);
di = V0/L;
if alpha > w0
 % Overdamped response
    s(1) = -alpha + sqrt(alpha^2 - w02);
    s(2) = -alpha - sqrt(alpha^2 - w02);
    A1 = (dv-s(2)*V0)/(s(1)-s(2));
    A2 = (dv-s(1)*V0)/(s(2)-s(1));
    A1i =(di-s(2)*(I0-Is))/(s(1)-s(2));
    A2i =(di-s(1)*(I0-Is))/(s(2)-s(1));
    tf = 6*max(abs(1/s(1)), abs(1/s(2)));
    t=0:tf/100:tf;
    v = A1*exp(s(1)*t)+A2*exp(s(2)*t);
    iL=Is + A1i*exp(s(1)*t)+A2i*exp(s(2)*t);
    iR = v/R;
    iC= Is -iR-iL;
    it=iR+iL+iC;
    plot(t,iL,'erasemode','none','color',[0.9 0 0.8]), grid
    if Is==0
    title(['I_L(t) = (', num2str(A1i), ') e^{(', num2str(s(1)), 't)} + (', num2str(A2i), ') e^{(', num2str(s(2)), 't)}'],'color',[0.9 0 0.8])
    elseif Is~=0
    title(['i_L(t) = ',num2str(Is), ' + (', num2str(A1i), ') e^{(', num2str(s(1)), 't)} + (', num2str(A2i), ') e^{(', num2str(s(2)), 't)}'],'color',[0.9 0 0.8])
    end
    xlabel(['\alpha = ',num2str(alpha), ',  \omega_0 = ',  num2str(w0), '  (O.D.)          t, sec'])
    ylabel('i_L(t), Amps')
    elseif alpha < w0
     if alpha~=0
      % Underdamped response
      tf = 6/alpha;
      t=0:tf/200:tf;
      wd= sqrt(w02 - alpha^2);
      s(1) = -alpha +j*wd;
      s(2) = -alpha -j*wd;
      B1 = V0; B2 = (dv+alpha*B1)/wd;
      B1i = I0 - Is; B2i = (di+alpha*B1i)/wd;
      v = exp(-alpha*t).*(B1*cos(wd*t)+B2*sin(wd*t));
      iL = Is + exp(-alpha*t).*(B1i*cos(wd*t)+B2i*sin(wd*t));
      iR = v/R;
      iC= Is -iR-iL;
      it=iR+iL+iC;
      plot(t,iL,'erasemode','none','color',[.9 0 0.8]), grid
         if Is == 0
         title(['I_L(t) = e^{(-', num2str(alpha), 't)}[(', num2str(B1i), ')cos',num2str(wd),'t + (' , num2str(B2i), ')sin',num2str(wd),'t]'],'color',[0.9 0 0.8])
         elseif Is~= 0
         title(['i_L(t) = ',num2str(Is), ' + e^{(-', num2str(alpha), 't)}[(', num2str(B1i), ')cos',num2str(wd),'t + (' , num2str(B2i), ')sin',num2str(wd),'t]'],'color',[0.9 0 0.8])
         end
      xlabel(['\alpha = ',num2str(alpha), ',  \omega_0 = ',  num2str(w0), '  (U.D.)          t, sec'])
      ylabel('i_L(t), Amps')
      else
      % Undamped Response
      tf = 8*pi/w0;
      t=0:tf/200:tf;
      wd= w0;
      B1 = V0; B2 = (dv)/wd;
      B1i = I0 - Is; B2i = (di)/wd;
      iL = Is + B1i*cos(wd*t)+B2i*sin(wd*t);
      v = B1*cos(wd*t)+B2*sin(wd*t);
      iR = v/R;
      iC= Is -iR-iL;
      it=iR+iL+iC;
      plot(t,iL,'erasemode','none','color',[0.9 0 0.80]), grid
      	if Is == 0
      	title(['I_L(t) = (', num2str(B1i), ')cos',num2str(wd),'t + (' , num2str(B2i), ')sin',num2str(wd),'t'],'color',[0.9 0 0.8])
      	elseif Is ~= 0
      	title(['i_L(t) = ',num2str(Is), ' + (', num2str(B1i), ')cos',num2str(wd),'t + (' , num2str(B2i), ')sin',num2str(wd),'t'],'color',[0.9 0 0.8])
      	end
      xlabel(['\alpha = ',num2str(alpha), ',  \omega_0 = ',  num2str(w0), '  (Undamped)          t, sec'])
      ylabel('i_L(t), Amps')
      end
   elseif alpha ==w0
    % Critically damped response
    s(1)=-alpha;  s(2) = s(1);
    D2 = V0;
    D1 = dv+alpha*D2;
    D2i = I0 - Is; D1i = di+alpha*D2i;
    tf = 8/alpha;
    t=0:tf/100:tf;
    iL=Is + exp(-alpha*t).*(D1i*t+D2i);
    v = exp(-alpha*t).*(D1*t+D2);
    iR = v/R;
    iC= Is -iR-iL;
    it=iR+iL+iC;
    plot(t,iL,'erasemode','none','color',[0.9 0 0.8]), grid
    	if Is == 0
          title(['i_L(t) = e^{(-', num2str(alpha), 't)}[(', num2str(D1i), ')t + (' , num2str(D2i), ')]'],'color',[0.9 0 0.8])
          elseif Is ~= 0
          title(['i_L(t) = ',num2str(Is), ' + e^{(-', num2str(alpha), 't)}[(', num2str(D1i), ')t + (' , num2str(D2i), ')]'],'color',[0.9 0 0.8])
      end
    xlabel(['\alpha = ',num2str(alpha), ',  \omega_0 = ',  num2str(w0), '  (C.D.)          t, sec'])
    ylabel('i_L(t), Amps')
    else, end


function [out, idxtrain,eigvector, eigvalue] = kpca(spectraldata, No_Train, dimension, kernel, parameter)

%
%  Kernel principal component analysis, KPCA
%
% [out, eigvector, eigvalue] = kpca(spectraldata, No_Train, dimension, kernel, parameter)
%
% Input
%
% spectraldata       - input hyperspectral data with 3-D, ncols by nrows by nbands
% No_Train           - randomly selected samples for training (<= 5000),
%                       depending on the memory of your PC
% dimension          - the dimension of extracted kernelized PCs (with largest eigen values)
% kernel             - kernel function
% parameter          - parameters for kernel fuction
%                        - kernel = 'linear';% linear kernel
%                        - kernel = 'Gaussian'; parameter=1; %Gausian kernel
%                        - kernel = 'poly'; parameter=[1,3];% third order polynomial
%
%
% Output
%
% out                - the extracted kernelized PCs
% eigvector          - the eigenvectors divided by square root of corresponding eigenvalues
% eigvalue           - the first dimension largest eigenvalues

if nargin <  3, error('not enough input'); end
if nargin <  4
    if strncmp(kernel,'Gaussian',1)
        parameter=1;
    elseif strncmp(kernel,'poly',1)
        parameter=[1, 3];
    end
end

[nrows,ncols,nbands] = size(spectraldata);
X0 = reshape(spectraldata,nrows*ncols,nbands);
clear spectraldata

%% sub-sample for training, select No_Train samples randomly
rand('state',4711007);% initialization of rand
if No_Train>nrows*ncols, No_Train = nrows*ncols; end
idxtrain = randsample(nrows*ncols, No_Train);
X = double(X0(idxtrain,:));
ntrain = size(X,1);

Xtest = X0;
ntest = size(Xtest,1);
clear X0;

%% kernelized training data and centering the kernel matrix
[K scale sums] = kernelize_training(kernel, X, parameter);
meanK = mean(K(:));
meanrowsK = mean(K);
K = centering(K);

%% select the first dimension eigvectors
dimout = ntrain;
if dimout>dimension, dimout=dimension; end
[eigvector,eigvalue,flagk] = eigs(K, dimout, 'LM');

if flagk~=0, warning('*** Convergence problems in eigs ***'); end
eigvalue = diag(abs(eigvalue))';
eigvector = sqrt(ntrain-1)*eigvector*diag(1./sqrt(eigvalue));
clear K

out = NaN(ntest,dimout);
%% kernelized the test samples, center kernel and calculate kernel PCs
for rr=1:nrows
     idx = (rr-1)*ncols+1;
     idx = idx:(idx+ncols-1);
     Xk = kernelize_test(kernel, X, Xtest(idx,:), scale, sums);
     Xk = Xk - repmat(meanrowsK,ncols,1)' - repmat(mean(Xk),ntrain,1) + meanK;
     out(idx,:) = Xk'*eigvector;
end

out = reshape(out,nrows,ncols,dimout);


function [d, pt1, pt2] = distanceLines3d(line1, line2)
%DISTANCELINES3D Minimal distance between two 3D lines.
%
%   D = distanceLines3d(LINE1, LINE2);
%   Returns the distance between line LINE1 and the line LINE2, given as:
%   LINE1 : [x0 y0 z0 dx dy dz] (or M-by-6 array)
%   LINE2 : [x0 y0 z0 dx dy dz] (or N-by-6 array)
%   D     : (positive) array M-by-N
%
%   [D, PT1, PT2] = distanceLines3d(LINE1, LINE2);
%   Also returns the points located on LINE1 and LINE2 corresponding to the
%   shortest distance.
%   One should get the following:
%   distancePoints3d(PT1, PT2) - D == 0
%
%
%   Example
%     line1 = [2 3 4 0 1 0];
%     line2 = [8 8 8 0 0 1];
%     distanceLines3d(line1, line2)
%     ans =
%         6.0000
%
%   See also:
%   lines3d, distancePoints3d
%
%   ---------
%   authors: Brandon Baker, oqilipo, David Legland
%   created January 19, 2011
%

% number of points of each array
n1 = size(line1, 1);
n2 = size(line2, 1);

if nargout <= 1
    % express line coordinate as n1-by-n2 arrays
    v1x = repmat(line1(:,4), [1 n2]);
    v1y = repmat(line1(:,5), [1 n2]);
    v1z = repmat(line1(:,6), [1 n2]);
    p1x = repmat(line1(:,1), [1 n2]);
    p1y = repmat(line1(:,2), [1 n2]);
    p1z = repmat(line1(:,3), [1 n2]);

    v2x = repmat(line2(:,4)', [n1 1]);
    v2y = repmat(line2(:,5)', [n1 1]);
    v2z = repmat(line2(:,6)', [n1 1]);
    p2x = repmat(line2(:,1)', [n1 1]);
    p2y = repmat(line2(:,2)', [n1 1]);
    p2z = repmat(line2(:,3)', [n1 1]);

    % calculates distance for each set of lines
    vcross = cross([v1x(:) v1y(:) v1z(:)], [v2x(:) v2y(:) v2z(:)]);
    num = ([p1x(:) p1y(:) p1z(:)] - [p2x(:) p2y(:) p2z(:)]) .* vcross;
    t1 = sum(num,2);
    d = abs(t1) ./ (vectorNorm3d(vcross) + eps);

    % returns result as n1-by-n2 array
    d = reshape(d, n1, n2);

else
    % check input dimension, as we need to be able to match each pair of
    % lines
    if n1 ~= n2
        error('geom3d:distanceLines3d:IllegalInputArgument', ...
            'when output points are requested, number of lines should be the same');
    end

    p1 = line1(:, 1:3);
    p2 = line2(:, 1:3);
    dp = p2 - p1;
    v1 = line1(:, 4:6);
    v2 = line2(:, 4:6);

    % compute distance
    vcross = cross(v1, v2, 2);
    num = dp .* vcross;
    t1 = sum(num, 2);
    d = abs(t1) ./ (vectorNorm3d(vcross) + eps);

    % precomputations
    a = dot(v1, v1, 2);
    b = dot(v1, v2, 2);
    e = dot(v2, v2, 2);
    den = a.*e - b.*b; % 0, if lines are parallel

    % vector between origin of both lines
    r = line1(:,1:3) - line2(:,1:3);

    % solve linear system
    c = dot(v1, r, 2);
    f = dot(v2, r, 2);
    s = (b .* f - c .* e) ./ den;
    t = (a .* f - c .* b) ./ den;

    % convert to coordinates of points on lines
    pt1 = line1(:,1:3) + v1 .* s;
    pt2 = line2(:,1:3) + v2 .* t;
end


function [zgrid,xgrid,ygrid] = gridfit(x,y,z,xnodes,ynodes,varargin)
% gridfit: estimates a surface on a 2d grid, based on scattered data
%          Replicates are allowed. All methods extrapolate to the grid
%          boundaries. Gridfit uses a modified ridge estimator to
%          generate the surface, where the bias is toward smoothness.
%
%          Gridfit is not an interpolant. Its goal is a smooth surface
%          that approximates your data, but allows you to control the
%          amount of smoothing.
%
% usage #1: zgrid = gridfit(x,y,z,xnodes,ynodes);
% usage #2: [zgrid,xgrid,ygrid] = gridfit(x,y,z,xnodes,ynodes);
% usage #3: zgrid = gridfit(x,y,z,xnodes,ynodes,prop,val,prop,val,...);
%
% Arguments: (input)
%  x,y,z - vectors of equal lengths, containing arbitrary scattered data
%          The only constraint on x and y is they cannot ALL fall on a
%          single line in the x-y plane. Replicate points will be treated
%          in a least squares sense.
%
%          ANY points containing a NaN are ignored in the estimation
%
%  xnodes - vector defining the nodes in the grid in the independent
%          variable (x). xnodes need not be equally spaced. xnodes
%          must completely span the data. If they do not, then the
%          'extend' property is applied, adjusting the first and last
%          nodes to be extended as necessary. See below for a complete
%          description of the 'extend' property.
%
%          If xnodes is a scalar integer, then it specifies the number
%          of equally spaced nodes between the min and max of the data.
%
%  ynodes - vector defining the nodes in the grid in the independent
%          variable (y). ynodes need not be equally spaced.
%
%          If ynodes is a scalar integer, then it specifies the number
%          of equally spaced nodes between the min and max of the data.
%
%          Also see the extend property.
%
%  Additional arguments follow in the form of property/value pairs.
%  Valid properties are:
%    'smoothness', 'interp', 'regularizer', 'solver', 'maxiter'
%    'extend', 'tilesize', 'overlap'
%
%  Any UNAMBIGUOUS shortening (even down to a single letter) is
%  valid for property names. All properties have default values,
%  chosen (I hope) to give a reasonable result out of the box.
%
%   'smoothness' - scalar - 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.
%
%          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: I don't terribly like the 'springs' strategy.
%          It tends to drag the surface towards the mean of all
%          the data. Its been left in only because the paradigm
%          interests me.
%
%
%   '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 = 'normal';
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(['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(['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(['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(['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;

  % 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], ...
        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], ...
        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).^2 + ...
        (dy(j(:))/params.yscale).^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], ...
        [-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], ...
        [-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], ...
        [-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], ...
        [-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*(params.smoothness*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
        % permute for minimum fill in for R (in the QR)
        p = colamd(A(:,unmasked));
        zgrid=nan(ny,nx);
        zgrid(unmasked(p)) = A(:,unmasked(p))\rhs;
      else
        % permute for minimum fill in for R (in the QR)
        p = colamd(A);
        zgrid=zeros(ny,nx);
        zgrid(p) = A(:,p)\rhs;
      end

    case 'normal'
      % The normal equations, solved with \. Can be fast
      % for huge numbers of data points.
      if params.maskflag
        % there is a mask to use
        % Permute for minimum fill-in for \ (in chol)
        APA = A(:,unmasked)'*A(:,unmasked);
        p = symamd(APA);
        zgrid=nan(ny,nx);
        zgrid(unmasked(p)) = APA(p,p)\(A(:,unmasked(p))'*rhs);
      else
        % Permute for minimum fill-in for \ (in chol)
        APA = A'*A;
        p = symamd(APA);
        zgrid=zeros(ny,nx);
        zgrid(p) = APA(p,p)\(A(:,p)'*rhs);
      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(['Symmlq performed ',num2str(params.maxiter), ...
            ' iterations but did not converge.'])
        case 3
          % SYMMLQ stagnated, successive iterates were the same
          warning 'Symmlq stagnated without apparent convergence.'
        otherwise
          warning(['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(['Lsqr performed ',num2str(params.maxiter), ...
            ' iterations but did not converge.'])
        case 3
          % lsqr stagnated, successive iterates were the same
          warning 'Lsqr stagnated without apparent convergence.'
        case 4
          warning(['One of the scalar quantities calculated in',...
            ' LSQR was too small or too large to continue computing.'])
      end

  end

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 (length(params.smoothness)>1) || (params.smoothness<=0)
    error 'Smoothness must be scalar, 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 = mrvWaitbar(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 mrvWaitbar
    mrvWaitbar((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 '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 mrvWaitbar
close(h)

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