function [imo,mask] = rbfwarp2d( im, ps, pd, varargin )
% Radial base function/Thin-plate spline 2D image warping.
% [imo,mask] = rbfwarp2d( im, ps, pd, method)
%   input:
%       im: image 2d matrix
%       ps: 2d source landmark [n*2]
%       pd: 2d destin landmark [n*2]
%       method:
%         'gau',r  - for Gaussian function   ko = exp(-|pi-pj|/r.^2);
%         'thin'   - for Thin plate function ko = (|pi-pj|^2) * log(|pi-pj|^2)
%   output:
%       imo  : output matrix
%       mask : mask for output matrix, 0/1 means out/in the border
%
%   Bookstein, F. L.
%   "Principal Warps: Thin Plate Splines and the Decomposition of Deformations."
%   IEEE Trans. Pattern Anal. Mach. Intell. 11, 567-585, 1989.
%
%   Code by WangLin
%   2015-11-5
%   wanglin193@hotmail.com

num_required_parameters = 3;
if nargin < num_required_parameters
    help rbfwarp2d.m
    return;
end

% initialize default parameters
[imh,imw,imc] = size(im);
r = 0.1*imw;
imo = zeros(imh,imw,imc);
% mask = zeros(imh,imw); % commented by Abdo

% parse parameters
if nargin > num_required_parameters
    iVarargin = 1;
    while iVarargin <= nargin - num_required_parameters
        switch lower(varargin{iVarargin})
            case 'thin'
                method = 't';
            case 'gau'
                method = 'g';
                r = varargin{iVarargin+1};
                iVarargin = iVarargin + 1;
        end
        iVarargin = iVarargin + 1;
    end
end

%% Training w with L
nump = size(pd,1);
num_center = size(ps,1);
K=zeros(nump,num_center);

for i=1:num_center
    %Inverse warping from destination!
    dx = ones(nump,1)*ps(i,:)-pd;
    K(:,i) = sum(dx.^2,2);
end

if( strcmpi(method,'g') )
    K = rbf(K,r);
elseif( strcmpi(method,'t') )
    K = ThinPlate(K);
end

% P = [1,xp,yp] where (xp,yp) are n landmark points (nx2)
P = [ones(num_center,1),pd];
% L = [ K  P;
%       P' 0 ]
L = [K,P;P',zeros(3,3)];
% Y = [x,y;
%      0,0]; (n+3)x2
Y = [ps;zeros(3,2)];
%w = inv(L)*Y;
w = L\Y;

%% Using w
[x,y] = meshgrid(1:imw,1:imh);
pt = [x(:), y(:)];

nump = size(pt,1);
Kp = zeros(nump,num_center);
for i=1:num_center
    dx = ones(nump,1)*ps(i,:)-pt;
    Kp(:,i) = sum(dx.^2,2);
end
if( strcmpi(method,'g') )
    Kp = rbf(Kp,r);
elseif( strcmpi(method,'t') )
    Kp = ThinPlate(Kp);
end

L = [Kp,ones(nump,1),pt];
ptall = L*w;

%reshape to 2d image
xd = reshape( ptall(:,1),imh,imw );
yd = reshape( ptall(:,2),imh,imw );

for i = 1:imc
    %tmpx = gpuArray(single(im(:,:,i)));
    imt= interp2( single(im(:,:,i)),xd,yd,'linear',0); %% 'linear' 'cubic' 'spline',    extrapval
%     imo(:,:,i) = uint8(imt);
    imo(:,:,i) = (imt);
end

mask = ~isnan(imt);
end

function ko = rbf(d,r)
    ko = exp(-d/r.^2);
end

function ko = ThinPlate(ri)
% k=(r^2) * log(r^2)
    r1i = ri;
    r1i((ri==0))=realmin; % Avoid log(0)=inf
    ko = (ri).*log(r1i);
end


function call_price=american_call_bjerkesun_stensland(S, K, r, b, sigma, T)

%--------------------------------------------------------------------------
%
% DESCRIPTION:
%
% Approximation of American call due to Bjerksund and Stensland (1993)
%
%
% Reference:
%
% Petter Bjerksund and Gunnar Stensland,
% "Closed form approximations of american options",
% Scandinavian Journal of Management, 20(5):761-764, 1993.
%
%--------------------------------------------------------------------------
%
% INPUTS:
%
%  S:       spot price
%  K:       exercice price
%  r:       interest rate
%  b:       dividend yield
%  sigma:   volatility
%  T:       time to maturity
%
% This function uses phi1.m
%
%--------------------------------------------------------------------------
%
% OUTPUT:
%
% call_price: price of a call option
%
%--------------------------------------------------------------------------
%
% Author:  Paolo Z., February 2012
%
%--------------------------------------------------------------------------

sigma_sqr=sigma^2;
B0=max(K,(r/(r-b)*K));
beta = (0.5 - b/sigma_sqr) + sqrt( ((b/sigma_sqr-0.5)^2) + 2.0 * r/sigma_sqr);
Binf = beta/(beta-1.0)*K;
hT= - (b*T + 2.0*sigma*sqrt(T))*((K*K)/(Binf-B0));
XT = B0+(Binf-B0)*(1.0-exp(hT));
alpha = (XT-K)*(XT^-beta);
C=alpha*(S^beta) ...
    -alpha*phi1(S,T,beta,XT,XT,r,b,sigma)...
    +phi1(S,T,1,XT,XT,r,b,sigma)...
    -phi1(S,T,1,K,XT,r,b,sigma)...
    -K*phi1(S,T,0,XT,XT,r,b,sigma)...
    +K*phi1(S,T,0,K,XT,r,b,sigma);

c=european_call_contpay(S,K,r,b,sigma,T);

call_price = max(c,C);


function pt = steinerPoint(varargin)
%STEINERPOINT Compute steiner point (weighted centroid) of a polygon
%
%   PT = steinerPoint(POINTS);
%   PT = steinerPoint(PTX, PTY);
%   Computes steiner point of a polygon defined by POINTS. POINTS is a
%   [N*2] array of double.
%
%   The steiner point is computed the same way as the polygon centroid,
%   except that a weight depending on the angle is given to each vertex.
%
%   See also:
%   polygons2d, polygonArea, polygonCentroid, drawPolygon
%
%   ---------
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 11/11/2004.
%

if nargin==1
    var = varargin{1};
    px = var(:,1);
    py = var(:,2);
elseif nargin==2
    px = varargin{1};
    py = varargin{2};
end

% Algorithme P. Bourke
sx = 0;
sy = 0;
N = length(px);
for i=1:N-1
    sx = sx + (px(i)+px(i+1))*(px(i)*py(i+1) - px(i+1)*py(i));
    sy = sy + (py(i)+py(i+1))*(px(i)*py(i+1) - px(i+1)*py(i));
end
sx = sx + (px(N)+px(1))*(px(N)*py(1) - px(1)*py(N));
sy = sy + (py(N)+py(1))*(px(N)*py(1) - px(1)*py(N));

pt = [sx sy]/6/polygonArea(px, py);


function b = isPointInEllipse(point, ellipse, varargin)
%ISPOINTINELLIPSE Check if a point is located inside a given ellipse.
%
%   B = isPointInEllipse(POINT, ELLIPSE)
%   Returns true if point is located inside the given ellipse.
%
%   B = isPointInEllipse(POINT, ELLIPSE, TOL)
%   Specifies the tolerance value
%
%   Example:
%   isPointInEllipse([1 0], [0 0 2 1 0])
%   ans =
%       1
%   isPointInEllipse([0 0], [0 0 2 1 0])
%   ans =
%       1
%   isPointInEllipse([1 1], [0 0 2 1 0])
%   ans =
%       0
%   isPointInEllipse([1 1], [0 0 2 1 30])
%   ans =
%       1
%
%   See also
%     ellipses2d, isPointInCircle
%

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2011-03-11
% Copyright 2011-2022 INRA - TPV URPOI - BIA IMASTE

% extract computation tolerance
tol = 1e-14;
if ~isempty(varargin)
    tol = varargin{1};
end

% compute ellipse to unit circle transform
rot = createRotation(-deg2rad(ellipse(5)));
sca = createScaling(1./ellipse(3:4));
trans = sca * rot;

% transform points to unit circle basis
pTrans = bsxfun(@minus, point, ellipse(:,1:2));
pTrans = transformPoint(pTrans, trans);

% test if distance to origin smaller than 1
b = sqrt(sum(power(pTrans, 2), 2)) - 1 <= tol;


function c = correlation_rational_quadratic ( n, rho, rho0 )

%*****************************************************************************80
%
%% CORRELATION_RATIONAL_QUADRATIC evaluates the rational quadratic correlation function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Petter Abrahamsen,
%    A Review of Gaussian Random Fields and Correlation Functions,
%    Norwegian Computing Center, 1997.
%
%  Parameters:
%
%    Input, integer N, the number of arguments.
%
%    Input, real RHO(N,1), the arguments.
%
%    Input, real RHO0, the correlation length.
%
%    Output, real C(N,1), the correlations.
%
  rho = rho ( : );

  rhohat = rho / rho0;

  c = 1.0 ./ ( 1.0 + rhohat.^2 );

  return
end


function [ zmat, seed ] = c8mat_uniform_01 ( m, n, seed )

%*****************************************************************************80
%
%% C8MAT_UNIFORM_01 returns a unit pseudorandom C8MAT.
%
%  Discussion:
%
%    The angles should be uniformly distributed between 0 and 2 * PI,
%    the square roots of the radius uniformly distributed between 0 and 1.
%
%    This results in a uniform distribution of values in the unit circle.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns in the matrix.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, double complex ZMAT(M,N), the pseudorandom complex matrix.
%
%    Output, integer SEED, a seed for the random number generator.
%
  for i2 = 1 : n
    for i1 = 1 : m

      k = floor ( seed / 127773 );

      seed = 16807 * ( seed - k * 127773 ) - k * 2836;

      if ( seed < 0 )
        seed = seed + 2147483647;
      end

      r = sqrt ( seed * 4.656612875E-10 );

      k = floor ( seed / 127773 );

      seed = 16807 * ( seed - k * 127773 ) - k * 2836;

      if ( seed < 0 )
        seed = seed + 2147483647;
      end

      theta = 2.0 * pi * seed * 4.656612875E-10;

      zmat(i1,i2) = r * ( cos ( theta ) + i * sin ( theta ) );

    end

  end

  return
end


function int_exactness_gen_hermite ( quad_filename, degree_max, alpha, option )

%*****************************************************************************80
%
%% MAIN is the main program for INT_EXACTNESS_GEN_HERMITE.
%
%  Discussion:
%
%    This program investigates a generalized Gauss-Hermite quadrature rule
%    by using it to integrate monomials over (-oo,+oo), and comparing the
%    approximate result to the known exact value.
%
%    The user specifies:
%    * the "root" name of the R, W and X files that specify the rule;
%    * DEGREE_MAX, the maximum monomial degree to be checked;
%    * ALPHA, the power of X in the weighting function;
%    * OPTION, whether the rule is for |x|^alpha*exp(-x*x)*f(x) or f(x).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 August 2009
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'INT_EXACTNESS_GEN_HERMITE\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Investigate the polynomial exactness of a generalized Gauss-Hermite\n' );
  fprintf ( 1, '  quadrature rule by integrating exponentially weighted\n' );
  fprintf ( 1, '  monomials up to a given degree over the (-oo,+oo) interval.\n' );
%
%  Get the quadrature file root name:
%
  if ( 1 <= nargin )

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, 'INT_EXACTNESS_GEN_HERMITE:\n' );

    quad_filename = input ( '  Enter the "root" name of the quadrature files.' );

  end
%
%  Create the names of:
%    the quadrature X file;
%    the quadrature W file;
%    the quadrature R file;
%
  quad_x_filename = strcat ( quad_filename, '_x.txt' );
  quad_w_filename = strcat ( quad_filename, '_w.txt' );
  quad_r_filename = strcat ( quad_filename, '_r.txt' );
%
%  The second command line argument is the maximum degree.
%
  if ( 2 <= nargin )

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, 'INT_EXACTNESS_GEN_HERMITE:\n' );

    degree_max = input ( '  Please enter the maximum degree to check.' );

  end
%
%  The third command line argument is ALPHA.
%
  if ( 3 <= nargin )

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, 'INT_EXACTNESS_GEN_HERMITE:\n' );
    fprintf ( 1, '  ALPHA is the power of |X| in the weighting function.\n' );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  ALPHA is a real number greater than -1.0.\n' );
    fprintf ( 1, '\n' );

    alpha = input ( '  Please enter ALPHA.' );

  end
%
%  The fourth command line argument is OPTION.
%  0 for the standard rule for integrating |x|^alpha*exp(-x*x)*f(x),
%  1 for a rule for integrating f(x).
%
  if ( 4 <= nargin )

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, 'INT_EXACTNESS_GEN_HERMITE:\n' );
    fprintf ( 1, '  OPTION chooses the standard or modified rule:\n' );
    fprintf ( 1, '  0: standard rule for integrating |x|^alpha*exp(-x*x)*f(x);\n' );
    fprintf ( 1, '  1: modified rule for integrating                     f(x).\n' );
    fprintf ( 1, '\n' );
    option = input ( '  Please enter OPTION' );

  end
%
%  Summarize the input.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'INT_EXACTNESS_GEN_HERMITE: User input:\n' );
  fprintf ( 1, '  Quadrature rule X file = "%s".\n', quad_x_filename );
  fprintf ( 1, '  Quadrature rule W file = "%s".\n', quad_w_filename );
  fprintf ( 1, '  Quadrature rule R file = "%s".\n', quad_r_filename );
  fprintf ( 1, '  Maximum degree to check = %d\n', degree_max );
  fprintf ( 1, '  Weighting function exponent ALPHA = %f\n', alpha );
  if ( option == 0 )
    fprintf ( 1, '  OPTION = 0, integrate |x|^alpha*exp(-x*x)*f(x).\n' );
  else
    fprintf ( 1, '  OPTION = 1, integrate                     f(x).\n' );
  end
%
%  Read the X file.
%
  [ dim_num, order ] = r8mat_header_read ( quad_x_filename );

  if ( dim_num ~= 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'INT_EXACTNESS_GEN_HERMITE - Fatal error!\n' );
    fprintf ( 1, '  The spatial dimension should be 1.\n');
    fprintf ( 1, '  The spatial dimension in the X file is %d\n', dim_num );
    error ( 'INT_EXACTNESS_GEN_HERMITE - Fatal error!' );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Spatial dimension = %d\n', dim_num );
  fprintf ( 1, '  Number of points  = %d\n', order );

  x = r8mat_data_read ( quad_x_filename, dim_num, order );
%
%  Read the W file.
%
  [ dim_num2, point_num ] = r8mat_header_read ( quad_w_filename );

  if ( dim_num2 ~= 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'INT_EXACTNESS_GEN_HERMITE - Fatal error!\n' );
    fprintf ( 1, '  The quadrature weight file should have exactly\n');
    fprintf ( 1, '  one value on each line.\n' );
    error ( 'INT_EXACTNESS_GEN_HERMITE - Fatal error!' );
  end

  if ( point_num ~= order )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'INT_EXACTNESS_GEN_HERMITE - Fatal error!\n' );
    fprintf ( 1, '  The quadrature weight file should have exactly\n' );
    fprintf ( 1, '  the same number of lines as the abscissa file.\n' );
    error ( 'INT_EXACTNESS_GEN_HERMITE - Fatal error!' );
  end

  w = r8mat_data_read ( quad_w_filename, 1, order );
%
%  Read the R file.
%
  [ dim_num2, point_num ] = r8mat_header_read ( quad_r_filename );

  if ( dim_num2 ~= dim_num )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'INT_EXACTNESS_GEN_HERMITE - Fatal error!\n' );
    fprintf ( 1, '  The quadrature region file should have the same\n' );
    fprintf ( 1, '  number of values on each line as the abscissa file\n' );
    fprintf ( 1, '  does.\n' );
    error ( 'INT_EXACTNESS_GEN_HERMITE - Fatal error!' );
  end

  if ( point_num ~= 2 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'INT_EXACTNESS_GEN_HERMITE - Fatal error!\n' );
    fprintf ( 1, '  The quadrature region file should have two lines.\n' );
    error ( 'INT_EXACTNESS_GEN_HERMITE - Fatal error!' );
  end

  r = r8mat_data_read ( quad_r_filename, dim_num, 2 );
%
%  Print the input quadrature rule.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The quadrature rule to be tested is\n' );
  fprintf ( 1, '  a generalized Gauss-Hermite rule\n' );
  fprintf ( 1, '  ORDER = %d\n', order );
  fprintf ( 1, '  ALPHA = %f\n', alpha );
  fprintf ( 1, '\n' );
  if ( option == 0 )
    fprintf ( 1, '  OPTION = 0, standard rule:\n' );
    fprintf ( 1, '    Integral ( -oo < x < +oo ) |x|^alpha exp(-x*x) f(x) dx\n' );
    fprintf ( 1, '    is to be approximated by\n' );
    fprintf ( 1, '    sum ( 1 <= I <= ORDER ) w(i) * f(x(i)).\n' );
  else
    fprintf ( 1, '  OPTION = 1, modified rule:\n' );
    fprintf ( 1, '    Integral ( -oo < x < +oo ) f(x) dx\n' );
    fprintf ( 1, '    is to be approximated by\n' );
    fprintf ( 1, '    sum ( 1 <= I <= ORDER ) w(i) * f(x(i)).\n' );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Weights W:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : order
    fprintf ( 1, '  w(%d) = %24.16f\n', i, w(i) );
  end
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Abscissas X:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : order
    fprintf ( 1, '  x(%d) = %24.16f\n', i, x(i) );
  end
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Region R:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : 2
    fprintf ( 1, '  r(%d) = %e\n', i, r(i) );
  end
%
%  Explore the monomials.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  A generalized Gauss-Hermite rule would be able to exactly\n' );
  fprintf ( 1, '  integrate monomials up to and including \n' );
  fprintf ( 1, '  degree = %d\n', 2 * order - 1 );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      Error    Degree\n' );
  fprintf ( 1, '\n' );

  for degree = 0 : degree_max

    quad_error = monomial_quadrature_gen_hermite ( degree, alpha, order, option, w, x );

    fprintf ( 1, '  %24.16f   %2d\n', quad_error, degree );

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

  return
end
function column_num = file_column_count ( input_file_name )

%*****************************************************************************80
%
%% FILE_COLUMN_COUNT counts the columns in the first line of a file.
%
%  Discussion:
%
%    The file is assumed to be a simple text file.
%
%    Most lines of the file are presumed to consist of COLUMN_NUM words,
%    separated by spaces.  There may also be some blank lines, and some
%    comment lines, which have a "#" in column 1.
%
%    The routine tries to find the first non-comment non-blank line and
%    counts the number of words in that line.
%
%    If all lines are blanks or comments, it goes back and tries to analyze
%    a comment line.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILE_NAME, the name of the file.
%
%    Output, integer COLUMN_NUM, the number of columns in the file.
%
  FALSE = 0;
  TRUE = 1;
%
%  Open the file.
%
  input_unit = fopen ( input_file_name );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_COLUMN_COUNT - Error!\n' );
    fprintf ( 1, '  Could not open the file "%s".\n', input_file_name );
    error ( 'FILE_COLUMN_COUNT - Error!' );
  end
%
%  Read one line, but skip blank lines and comment lines.
%  Use FGETL so we drop the newline character!
%
  got_one = FALSE;

  while ( 1 )

    line = fgetl ( input_unit );

    if ( line == -1 )
      break;
    end

    if ( s_len_trim ( line ) == 0 )

    elseif ( line(1) == '#' )

    else
      got_one = TRUE;
      break;
    end

  end

  fclose ( input_unit );

  if ( got_one == FALSE )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_COLUMN_COUNT - Warning!\n' );
    fprintf ( 1, '  The file does not seem to contain any data.\n' );
    column_num = -1;
    return;
  end

  column_num = s_word_count ( line );

  return
end
function row_num = file_row_count ( input_file_name )

%*****************************************************************************80
%
%% FILE_ROW_COUNT counts the number of row records in a file.
%
%  Discussion:
%
%    Each input line is a "RECORD".
%
%    The records are divided into three groups:
%
%    * BLANK LINES (nothing but blanks)
%    * COMMENT LINES (begin with a '#')
%    * DATA RECORDS (anything else)
%
%    The value returned by the function is the number of data records.
%
%    By the way, if the MATLAB routine FGETS is used, instead of
%    FGETL, then the variable LINE will include line termination
%    characters, which means that a blank line would not actually
%    have zero characters.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 December 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILE_NAME, the name of the input file.
%
%    Output, integer ROW_NUM, the number of rows found.
%
  input_unit = fopen ( input_file_name );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_ROW_COUNT - Error!\n' );
    fprintf ( 1, '  Could not open the file "%s".\n', input_file_name );
    error ( 'FILE_ROW_COUNT - Error!' );
  end

  blank_num = 0;
  comment_num = 0;
  row_num = 0;

  record_num = 0;

  while ( 1 )

    line = fgetl ( input_unit );

    if ( line == -1 )
      break;
    end

    record_num = record_num + 1;
    record_length = s_len_trim ( line );

    if ( record_length <= 0 )
      blank_num = blank_num + 1;
    elseif ( line(1) == '#' )
      comment_num = comment_num + 1;
    else
      row_num = row_num + 1;
    end

  end

  fclose ( input_unit );

  return
end
function value = gen_hermite_integral ( expon, alpha )

%*****************************************************************************80
%
%% GEN_HERMITE_INTEGRAL evaluates a monomial generalized Hermite integral.
%
%  Discussion:
%
%    H(n,alpha) = Integral ( -oo < x < +oo ) x^n |x|^alpha exp(-x^2) dx
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 February 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, int EXPON, the exponent of the monomial.
%
%    Input, real ALPHA, the exponent of |X| in the integral.
%    -1.0 < ALPHA.
%
%    Output, real VALUE, the value of the integral.
%
  if ( mod ( expon, 2 ) == 1 )

    value = 0.0;

  else

    a = alpha + expon;

    if ( a <= -1.0 )

      value = - r8_huge ( );

    else

      value = r8_gamma ( ( a + 1.0 ) / 2.0 );

    end

  end

  return
end
function quad_error = monomial_quadrature_gen_hermite ( expon, alpha, order, ...
  option, w, x )

%*****************************************************************************80
%
%% MONOMIAL_QUADRATURE_GEN_HERMITE applies a quadrature rule to a monomial.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 February 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer EXPON, the exponent.
%
%    Input, real ALPHA, the exponent of X in the weight factor.
%
%    Input, intege ORDER, the number of points in the rule.
%
%    Input, integer OPTION, indicates standard or modified rule.
%    0, standard generalized Gauss-Hermite rule for
%       integrand |x|^alpha*exp(-x*x)*f(x).
%    1, modified generalized Gauss-Laguerre rule for
%       integrand                     f(x).
%
%    Input, real W(ORDER), the quadrature weights.
%
%    Input, real X(ORDER), the quadrature points.
%
%    Output, real QUAD_ERROR, the quadrature error.
%

%
%  Get the exact value of the integral of the monomial.
%
   exact = gen_hermite_integral ( expon, alpha );
%
%  Evaluate the unweighted monomial at the quadrature points.
%
  if ( option == 0 )
    value(1:order) = x(1:order).^expon;
  else
    value(1:order) = ( abs ( x(1:order) ) ).^alpha ...
    .* exp ( - x(1:order).^2 ) .* x(1:order).^expon;
  end
%
%  Compute the weighted sum.
%
  quad = w(1:order) * value(1:order)';
%
%  Error:
%
  if ( exact == 0.0 )
    quad_error = abs ( quad );
  else
    quad_error = abs ( ( quad - exact ) / exact );
  end

  return
end
function value = r8_gamma ( x )

%*****************************************************************************80
%
%% R8_GAMMA evaluates Gamma(X) for a real argument.
%
%  Discussion:
%
%    This routine calculates the gamma function for a real argument X.
%
%    Computation is based on an algorithm outlined in reference 1.
%    The program uses rational functions that approximate the gamma
%    function to at least 20 significant decimal digits.  Coefficients
%    for the approximation over the interval (1,2) are unpublished.
%    Those for the approximation for 12 <= X are from reference 2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 January 2008
%
%  Author:
%
%    Original FORTRAN77 version by William Cody, Laura Stoltz.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    William Cody,
%    An Overview of Software Development for Special Functions,
%    in Numerical Analysis Dundee, 1975,
%    edited by GA Watson,
%    Lecture Notes in Mathematics 506,
%    Springer, 1976.
%
%    John Hart, Ward Cheney, Charles Lawson, Hans Maehly,
%    Charles Mesztenyi, John Rice, Henry Thatcher,
%    Christoph Witzgall,
%    Computer Approximations,
%    Wiley, 1968,
%    LC: QA297.C64.
%
%  Parameters:
%
%    Input, real X, the argument of the function.
%
%    Output, real VALUE, the value of the function.
%

%
%  Coefficients for minimax approximation over (12, INF).
%
  c = [ ...
   -1.910444077728E-03, ...
    8.4171387781295E-04, ...
   -5.952379913043012E-04, ...
    7.93650793500350248E-04, ...
   -2.777777777777681622553E-03, ...
    8.333333333333333331554247E-02, ...
    5.7083835261E-03 ];
%
%  Mathematical constants
%
  one = 1.0;
  half = 0.5;
  twelve = 12.0;
  two = 2.0;
  zero = 0.0;
  sqrtpi = 0.9189385332046727417803297;
%
%  Machine dependent parameters
%
  xbig = 171.624E+00;
  xminin = 2.23E-308;
  eps = 2.22E-16;
  xinf = 1.79E+308;
%
%  Numerator and denominator coefficients for rational minimax
%  approximation over (1,2).
%
  p = [ ...
   -1.71618513886549492533811E+00, ...
    2.47656508055759199108314E+01, ...
   -3.79804256470945635097577E+02, ...
    6.29331155312818442661052E+02, ...
    8.66966202790413211295064E+02, ...
   -3.14512729688483675254357E+04, ...
   -3.61444134186911729807069E+04, ...
    6.64561438202405440627855E+04 ];

  q = [ ...
   -3.08402300119738975254353E+01, ...
    3.15350626979604161529144E+02, ...
   -1.01515636749021914166146E+03, ...
   -3.10777167157231109440444E+03, ...
    2.25381184209801510330112E+04, ...
    4.75584627752788110767815E+03, ...
   -1.34659959864969306392456E+05, ...
   -1.15132259675553483497211E+05 ];

  parity = 0;
  fact = one;
  n = 0;
  y = x;
%
%  Argument is negative.
%
  if ( y <= zero )

    y = - x;
    y1 = floor ( y );
    res = y - y1;

    if ( res ~= zero )

      if ( y1 ~= floor ( y1 * half ) * two )
        parity = 1;
      end

      fact = - pi / sin ( pi * res );
      y = y + one;

    else

      res = xinf;
      value = res;
      return

    end

  end
%
%  Argument is positive.
%
  if ( y < eps )
%
%  Argument < EPS.
%
    if ( xminin <= y )
      res = one / y;
    else
      res = xinf;
      value = res;
      return
    end

  elseif ( y < twelve )

    y1 = y;
%
%  0.0 < argument < 1.0.
%
    if ( y < one )

      z = y;
      y = y + one;
%
%  1.0 < argument < 12.0.
%  Reduce argument if necessary.
%
    else

      n = floor ( y ) - 1;
      y = y - n;
      z = y - one;

    end
%
%  Evaluate approximation for 1.0 < argument < 2.0.
%
    xnum = zero;
    xden = one;
    for i = 1 : 8
      xnum = ( xnum + p(i) ) * z;
      xden = xden * z + q(i);
    end

    res = xnum / xden + one;
%
%  Adjust result for case  0.0 < argument < 1.0.
%
    if ( y1 < y )

      res = res / y1;
%
%  Adjust result for case 2.0 < argument < 12.0.
%
    elseif ( y < y1 )

      for i = 1 : n
        res = res * y;
        y = y + one;
      end

    end

  else
%
%  Evaluate for 12.0 <= argument.
%
    if ( y <= xbig )

      ysq = y * y;
      sum = c(7);
      for i = 1 : 6
        sum = sum / ysq + c(i);
      end
      sum = sum / y - y + sqrtpi;
      sum = sum + ( y - half ) * log ( y );
      res = exp ( sum );

    else

      res = xinf;
      value = res;
      return

    end

  end
%
%  Final adjustments and return.
%
  if ( parity )
    res = - res;
  end

  if ( fact ~= one )
    res = fact / res;
  end

  value = res;

  return
end
function table = r8mat_data_read ( input_filename, m, n )

%*****************************************************************************80
%
%% R8MAT_DATA_READ reads data from an R8MAT file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 January 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Input, integer M, N, the number of rows and columns of data.
%
%    Output, real TABLE(M,N), the point coordinates.
%
  table = zeros ( m, n );
%
%  Build up the format string for reading M real numbers.
%
  string = ' ';

  for i = 0 : m
    string = strcat ( string, ' %f' );
  end

  input_unit = fopen ( input_filename );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_DATA_READ - Error!\n' );
    fprintf ( 1, '  Could not open the file.\n' );
    error ( 'R8MAT_DATA_READ - Error!' );
  end

  i = 0;

  while ( i < n )

    line = fgets ( input_unit );

    if ( line == -1 )
      break;
    end

    if ( line(1) == '#' )

    elseif ( s_len_trim ( line ) == 0 )

    else

      [ x, count ] = sscanf ( line, string );

      if ( count == m )
        i = i + 1;
        table(1:m,i) = x(1:m);
      end

    end

  end

  fclose ( input_unit );

  return
end
function [ m, n ] = r8mat_header_read ( input_filename )

%*****************************************************************************80
%
%% R8MAT_HEADER_READ reads the header from an R8MAT file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Output, integer M, the spatial dimension.
%
%    Output, integer N, the number of points.
%
  m = file_column_count ( input_filename );

  if ( m <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_HEADER_READ - Fatal error!\n' );
    fprintf ( 1, '  There was some kind of I/O problem while trying\n' );
    fprintf ( 1, '  to count the number of data columns in\n' );
    fprintf ( 1, '  the file %s.\n', input_filename );
  end

  n = file_row_count ( input_filename );

  if ( n <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_HEADER_READ - Fatal error!\n' );
    fprintf ( 1, '  There was some kind of I/O problem while trying\n' );
    fprintf ( 1, '  to count the number of data rows in\n' );
    fprintf ( 1, '  the file %s\n', input_filename );
  end

  return
end
function len = s_len_trim ( s )

%*****************************************************************************80
%
%% S_LEN_TRIM returns the length of a character string to the last nonblank.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 June 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string S, the string to be measured.
%
%    Output, integer LEN, the length of the string up to the last nonblank.
%
  len = length ( s );

  while ( 0 < len )
    if ( s(len) ~= ' ' )
      return
    end
    len = len - 1;
  end

  return
end
function word_num = s_word_count ( s )

%*****************************************************************************80
%
%% S_WORD_COUNT counts the number of "words" in a string.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string S, the string to be examined.
%
%    Output, integer WORD_NUM, the number of "words" in the string.
%    Words are presumed to be separated by one or more blanks.
%
  FALSE = 0;
  TRUE = 1;

  word_num = 0;
  s_length = length ( s );

  if ( s_length <= 0 )
    return;
  end

  blank = TRUE;

  for i = 1 : s_length

    if ( s(i) == ' ' )
      blank = TRUE;
    elseif ( blank == TRUE )
      word_num = word_num + 1;
      blank = FALSE;
    end

  end

  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 laplacian_test06 ( )

%*****************************************************************************80
%
%% LAPLACIAN_TEST06 tests L1DD_LU and similar routines.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 November 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'LAPLACIAN_TEST06\n' );
  fprintf ( 1, '  Compute LU factors for the Laplacian:\n' );
  fprintf ( 1, '  L1DD_LU for Dirichlet/Dirichlet BC;\n' );
  fprintf ( 1, '  L1DN_LU for Dirichlet/Neumann BC;\n' );
  fprintf ( 1, '  L1ND_LU for Neumann/Dirichlet BC;\n' );
  fprintf ( 1, '  L1NN_LU for Neumann/Neumann BC;\n' );
  fprintf ( 1, '  L1PP_LU for Periodic BC;\n' );

  n = 5;

  for test = 1 : 2

    if ( test == 1 )
      h = 1.0;
    else
      h = 1.0 / ( n + 1 );
    end

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Using spacing H = %g\n', h );

    a = l1dd ( n, h );
    [ l, u ] = l1dd_lu ( n, h );
    r8mat_print ( n, n, l, '  L1DD L factor:' );
    r8mat_print ( n, n, u, '  L1DD U factor:' );
    err = lu_error ( n, a, l, u );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  L1DD LU error = %g\n', err );

    a = l1dn ( n, h );
    [ l, u ] = l1dn_lu ( n, h );
    r8mat_print ( n, n, l, '  L1DN L factor:' );
    r8mat_print ( n, n, u, '  L1DN U factor:' );
    err = lu_error ( n, a, l, u );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  L1DN LU error = %g\n', err );

    a = l1nd ( n, h );
    [ l, u ] = l1nd_lu ( n, h );
    r8mat_print ( n, n, l, '  L1ND L factor:' );
    r8mat_print ( n, n, u, '  L1ND U factor:' );
    err = lu_error ( n, a, l, u );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  L1ND LU error = %g\n', err );

    a = l1nn ( n, h );
    [ l, u ] = l1nn_lu ( n, h );
    r8mat_print ( n, n, l, '  L1NN L factor:' );
    r8mat_print ( n, n, u, '  L1NN U factor:' );
    err = lu_error ( n, a, l, u );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  L1NN LU error = %g\n', err );

    a = l1pp ( n, h );
    [ l, u ] = l1pp_lu ( n, h );
    r8mat_print ( n, n, l, '  L1PP L factor:' );
    r8mat_print ( n, n, u, '  L1PP U factor:' );
    err = lu_error ( n, a, l, u );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  L1PP LU error = %g\n', err );

  end

  return
end


%% DEMOGRADIENT  Short demonstration of gradients
%

%% Some sample applications of the gradient toolbox
% Gradients implement automatic differentiation in forward mode, which is
% conveniently to implement using the Matlab operator concept.
%

%% Initialization of gradients
% In order to use automatic differentiation, the independent variables need
% to be identified and values have to be assigned. This is performed by
% the function "gradientinit", for example

format compact short _
u = gradientinit([ -3.1 ; 4e-3 ])

%%
% The total size of the input is the number of independent variables,
% in the example 2, hence u represents a column vector of length 2 and
% defines two independent variables u(1) and u(2) with gradients [1 0]
% and [0 1], respectively.

%% Operations between gradients
% If at least one operand is of type gradient, operations are executed as
% gradient operations.
% For example,

x = gradientinit(3.5);
y = exp(3*x-sqrt(x))

%%
% For f(x):=exp(3*x-sqrt(x)), the result y contains in y.x the function value f(3.5)
% and in y.dx the derivative f'(3.5):
%

y.x, y.dx

%% Complex arguments
% When evaluating the expression for another argument, use the same
% statement as before with new values.

x = gradientinit(-3.5+.2i);
y = exp(3*x-sqrt(x))

%% Access to the gradient
% The principle works for functions in several unknowns the same way. Define, for
% example, the following function from R^3->R^3 :

f = @(x)( [ -2*x(1)*x(2)+4*x(3)^2 ; sin(x(2))/sqrt(pi-x(1)) ; atan(x(2)-x(3)) ] )
f([1.5;-1;0.7])

%%
% then the function value and gradient at [1.5;-1;0.7] is computed by

y = f(gradientinit([1.5;-1;0.7]))

%%
% where y.x contains the function value and y.dx the gradient, which is
% in this case the Jacobian. The gradient with respect the third unknown
% x(3) can be accessed by

y.dx(3,:)

%%
% However, it is recommended to use

y(3).dx

%%
% that is not to access the components of the gradient (Jacobian) but the
% gradient of the component. The advantage is visible when redefining the input function
% as a row vector:

f = @(x)( [ -2*x(1)*x(2)+4*x(3)^2  sin(x(2))/sqrt(pi-x(1))  atan(x(2)-x(3)) ] )
f([1.5;-1;0.7])

%%
% Then the "Jacobian" is a three-dimensional array
% because the gradient is always stored in the "next" dimension:

y = f(gradientinit([1.5;-1;0.7]))

%%
% It is problematic to access the components of y.dx, while accessing the
% gradient of the component works as expected:

y(3).dx

%% An example in one unknown: The Gamma function
% According to Stirling's formula it is for u -> inf,
%
%                         1      1       139        571
%   Gamma(u) ~ C * ( 1 + --- + ----- - ------- - --------- + ... )
%                        12u       2         3           4
%                              288u    51840u    2488320u
%
% with
%
%        -u  u-0.5
%   C = e   u      sqrt(2*pi) .
%
% The following function evaluates Stirling's formula. It is also
% suited for vector input.
%
%   function y = g(u)
%       C = exp(-u) .* ( u.^(u-0.5) ) * sqrt(2.0*pi) ;
%       v = (((( -571.0/2488320.0 ./ u - 139.0/51840.0 ) ./ u ...
%            + 1.0/288.0) ./ u ) + 1.0/12.0 ) ./ u + 1.0;
%       y = C .* v;
%
% A corresponding inline function is

format long e
g = @(u) ( ( exp(-u) .* ( u.^(u-0.5) ) * sqrt(2.0*pi) ) .* ...
           ( (((( -571.0/2488320.0 ./ u - 139.0/51840.0 ) ./ u ...
             + 1.0/288.0) ./ u ) + 1.0/12.0 ) ./ u + 1.0 ) )
u = [ 3.5 61 5 ]
g(u)

%% The inverse Gamma function
% Next we calculate the inverse Gamma function. For example, compute u such
% that g(u) = 100. Consider the following simple Newton procedure with starting
% value u=5.

u = gradientinit(5);
uold = u;
k = 0;
while abs(u.x-uold.x) > 1e-12*abs(u.x) | k < 1
  uold = u;
  k = k+1;
  y = g(u) - 100;
  u = u - y.x/y.dx;
end
k
u.x
g(u.x)

%%
% Due to the approximation error in Stirling''s formula, about six figures are correct.

%% The inverse Gamma function with complex arguments
% The same is possible for complex arguments. We use the same Gamma function
% and the same Newton procedure except that some u is searched with
% g(u) = 100 + 100i. We use the same starting value u=5.
%

 u = gradientinit(5);
 uold = u;
 k = 0;
 while abs(u.x-uold.x) > 1e-12*abs(u.x) | k < 1
  uold = u;
  k = k+1;
  y = g(u) - 100 - 100i;
  u = u - y.x/y.dx;
 end
 k
 u.x
 g(u.x)

%%
% Due to approximation error in Stirling''s formula, about six figures are correct.

%% Automatic differentiation with several unknowns
% Automatic differentiation with several unknowns works the same way.
% Consider the following example by Broyden:
%
%                               .5*sin(x1*x2) - x2/(4*pi) - x1/2  =  0
% (1-1/(4*pi))*(exp(2*x1)-exp(1)) + exp(1)*x2/pi - 2*exp(1)*x1 )  =  0
%
% with initial approximation [ .6 ; 3 ] and one solution [ .5 ; pi ].
% The following inline function evaluates Broyden's function.

f = @(x) ( [ .5*sin(x(1)*x(2)) - x(2)/(4*pi) - x(1)/2 ; ...
             (1-1/(4*pi))*(exp(2*x(1))-exp(1)) + exp(1)*x(2)/pi - 2*exp(1)*x(1) ] )

%% Solution of a nonlinear system
% The nonlinear system defined by Broyden's function is solved by Newton's procedure as follows:

 x = gradientinit([ .6 ; 3 ]);
 for i=1:5
  y = f(x);
  x = x - y.dx\y.x;
 end
 x

%%
% For simplicity, we omitted the stopping criterion (see above).
% Here, y.dx is the Jacobian, y.x the function value at x.x, and -y.dx\y.x
% is the correction obtained by the (approximate) solution of a linear system.
%

%% Verified solution of the nonlinear system
% For verified solution of the nonlinear system, we need a correct definition
% of the function. The main point is to make sure that a function evaluation with
% interval argument computes an inclusion of the function value. So first the
% transcendental number pi has to be replaced by an interval containing pi, for example

cPi = midrad(3.141592653589793,1e-15)

%%
% Second, Broyden's function contains exp(1), which would be computed in pure
% floating-point without extra care. This can be cured using exp(intval(1)).
%
% However, a new problem arises.
% When replacing "pi" in the function by "cPi" and 1 by intval(1), the function is
% _always_ evaluated in interval arithmetic; a pure floating point iteration
% is no longer possible.
%
% To solve this problem, we have to know the type of
% the incoming unknown "x". If "x" is double, replace "cPi" and intval(1) by its midpoint,
% if "x" is an interval, use "cPi" and intval(1) as is. This is done as follows.
%
%  function  y = f(x)
%    y = x;
%    c1 = typeadj( 1 , typeof(x) );
%    cpi = typeadj( midrad(3.14159265358979323,1e-16) , typeof(x) );
%    y(1) = .5*sin(x(1)*x(2)) - x(2)/(4*cpi) - x(1)/2;
%    y(2) = (1-1/(4*cpi))*(exp(2*x(1))-exp(c1)) + exp(c1)*x(2)/cpi - 2*exp(c1)*x(1);
%
% This code is implemented in the function test.m .

%% Real function evaluation
% Consider the following two function evaluations. First, f(x) is evaluated
% for real argument:

x = [ .6 ; 3 ];
test(x)

%% Interval function evaluation
% Second, f(x) is evaluated with interval argument:

x = [intval('.6') ; 3 ]
y = test(x)

%%
% The mathematical statement is the following. First, x is an interval vector
% such that x(1) is an inclusion of 0.6 and x(2)=3. Second, cPi is an interval
% containg the transcendental number pi. Third, y is an interval vector
% containing the exact value of Broyden's function evaluated at [ .6 ; 3 ].

%% Interval gradient function evaluation
% Finally, we may define the interval x to be of type gradient:

x = gradientinit([intval('.6') ; 3 ])
Y = test(x)

%%
% The mathematical statement is that Y is an interval vector such that Y.x
% contains the exact value of Broyden's function evaluated at [ .6 ; 3 ], and
% Y.dx is an interval matrix containing the Jacobian of Broyden's function
% evaluated at [ .6 ; 3 ].

%% Verified solution of the nonlinear system with Broyden's function
% The nonlinear system with Broyden's function and the given starting value
% [ .6 ; 3 ] can be solved with verification by

Y = verifynlss(@test,[ .6 ; 3 ])

%%
% The first parameter gives the name of the function such that test(x)
% evaluates the function at "x". The result vector Y is verified to contain
% a real vector X such that f(X)=0. This solution X of the nonlinear system
% is proved to be unique within Y. This statement is mathematically true,
% it is taken care of all procedural, approximation and rounding errors.
%
% It follows that an inclusion is not possible is roots are very close together:
% Since uniqueness of the root is proved, an inclusion is only possible if roots
% can be separated. An escape of that is described in DEMOSLOPE.

%% Verified solution of a nonlinear system with sparse gradients
% Up to now we considered only toy examples to explain how the nonlinear
% system solver works. For a larger example consider the following example
% proposed by Abbot and Brent, which is implemented in the function test.
%

%%
%  function y = test(x);
%  % Abbot/Brent     3 y" y + y'^2 = 0;    y(0)=0; y(1)=20;
%  % approximation   10*ones(n,1)
%  % solution        20*x^.75
%    y = x;
%    n = length(x); v=2:n-1;
%    y(1) = 3*x(1)*(x(2)-2*x(1)) + x(2)*x(2)/4;
%    y(v) = 3*x(v).*(x(v+1)-2*x(v)+x(v-1)) + (x(v+1)-x(v-1)).^2/4;
%    y(n) = 3*x(n).*(20-2*x(n)+x(n-1)) + (20-x(n-1)).^2/4;
%
% An inclusion of the solution for 1000 unknowns is computed by
%

format short
sparsegradient(50)
n = 1000;
tic
x = verifynlss(@test,10*ones(n,1));
toc
max(relerr(x))

%% Verified solution of a nonlinear system with full gradients
% Here we specified that gradients with 50 unknowns and more are stored
% in sparse mode. This is the defauls when calling "sparsegradient".
%
% Forcing gradients to use full storage results in a significantly increase
% of computing time.

sparsegradient(inf)
n = 1000;
tic
x = verifynlss(@test,10*ones(n,1));
toc
max(relerr(x))

%% Verified solution of a nonlinear system with 5000 uknowns
% Note that the inclusion is of high accuracy. The results for a larger
% nonlinear system with 5000 unknowns is as follows.

sparsegradient(0)
n = 5000;
tic
x = verifynlss(@test,10*ones(n,1));
toc
max(relerr(x))

%% Non-differentiable functions
% The given function need not be differentiable everywhere. Consider, for example,

f = inline('abs(x)')
f(gradientinit(infsup(-.1,2)))

%% Verified solution of non-differentiable functions
% The inclusion of a root is searched for near the given approximation. Consider

f = vectorize(inline('x*sinh(x)-3*exp(abs(x)-.5)+x*cos(x+1)+2*cosh(x)'))
close
x=linspace(-1,2.3);
plot(x,f(x),x,0*x)

%%
% It seems there are three roots. Note that the function contains abs(x). Indeed,

format long
verifynlss(f,-.5)
verifynlss(f,.5)
verifynlss(f,2)

%%
% there are three roots, and the inlcusions are of high accuracy. One can
% also calculate an inclusion of multiple roots. Since the problem is ill-posed,
% it has t be regularized. Consider

X = verifynlss2(@(x)(sin(x)-1),1.5)

%%
% It is proved that there exists some parameter e in X(2) such that the function
% g(x):=f(x)-e has a truely double root in X(1). Note the accuracy of the inclusions.
% It looks like the inclusion of e is a true zero. However, this is due to the "_"-format:
% add +/-1 to the last visible digit produces a valid inclusion:

format long e infsup
X

%%
% The function "verifynlss2" is applicable to multivariate functions as well.
%
% For more details, see "help verifynlss" or "help verifynlss2" or
%
%  S.M. Rump: Verification methods: Rigorous results using floating-point arithmetic.
%    Acta Numerica, 19:287-449, 2010.
%
% to be downloaded from "www.ti3.tuhh.de/rump" and the literature cited over there.

%% Enjoy INTLAB
% INTLAB was designed and written by S.M. Rump, head of the Institute for Reliable Computing,
% Hamburg University of Technology. Suggestions are always welcome to rump (at) tuhh.de


function [bound_coeff,vertex_coeff] =  eq_diam_coeff(dim,N)
%EQ_DIAM_COEFF Coefficients of diameter bound and vertex diameter of EQ partition
%
%Syntax
% [bound_coeff,vertex_coeff] = eq_diam_coeff(dim,N);
%
%Description
% [BOUND_COEFF,VERTEX_COEFF] = EQ_DIAM_COEFF(dim,N) does the following:
% 1) uses the recursive zonal equal area sphere partitioning algorithm to
%    partition the unit sphere S^dim into N regions,
% 2) finds the maximum of the per-region diameter bound over all the regions
%    of the partition,
% 3) sets BOUND_COEFF to be the diameter bound coefficient, defined as the
%    solution to
%
%    max_diam_bound == BOUND_COEFF N^(-1/dim),
%
% 4) optionally finds the maximum vertex diameter over all the regions of the
%    partition, and
% 5) optionally sets VERTEX_COEFF to be the vertex diameter coefficient,
%    defined as the solution to
%
%    max_vertex_diam == VERTEX_COEFF N^(-1/dim).
%
% The argument dim must be a positive integer.
% The argument N must be a positive integer or an array of positive integers.
% The result BOUND_COEFF and the optional result VERTEX_COEFF will be arrays of
% the same size as N.
%
%Examples
% > bound_coeff=eq_diam_coeff(2,10)
%  bound_coeff =
%      5.2915
%
% > [bound_coeff,vertex_coeff]=eq_diam_coeff(3,1:6)
%  bound_coeff =
%      2.0000    2.5198    2.8845    3.1748    3.4200    3.6342
%
%  vertex_coeff =
%      2.0000    2.5198    2.8845    3.1748    3.4200    3.6342
%
%See also
% EQ_DIAM_BOUND, EQ_VERTEX_DIAM, EQ_REGIONS, EQ_VERTEX_DIAM_COEFF

% Copyright 2004-2005 Paul Leopardi for the University of NSW.
% $Revision 1.10 $ $Date 2005-06-01 $
% Documentation files renamed
% $Revision 1.00 $ $Date 2005-02-12 $
%
% For licensing, see COPYING.
% For references, see AUTHORS.
% For revision history, see CHANGELOG.

%
% Check number of arguments
%
error(nargchk(2,2,nargin));
error(nargoutchk(0,2,nargout));

if nargout < 2
    bound_coeff = eq_diam_bound(dim,N) .* N.^(1/dim);
else
    %
    % Flatten N into a row vector.
    %
    shape = size(N);
    n_partitions = prod(shape);
    N = reshape(N,1,n_partitions);

    bound_coeff =  zeros(size(N));
    vertex_coeff = zeros(size(N));
    for partition_n = 1:n_partitions
        n = N(partition_n);
        regions = eq_regions(dim,n);
        scale = n^(1/dim);
        bound_coeff(partition_n) =  max_diam_bound_of_regions(regions)  * scale;
        vertex_coeff(partition_n) = max_vertex_diam_of_regions(regions) * scale;
    end
    %
    % Reshape output to same array size as original N.
    %
    bound_coeff =  reshape(bound_coeff,shape);
    vertex_coeff = reshape(vertex_coeff,shape);
end
%
%end function


 % Copyright 2001, Brown University, Providence, Rhode Island.
 %
 % All Rights Reserved
 %
 % Permission to use this software for noncommercial research and
 % educational purposes is hereby granted without fee.
 % Redistribution, sale, or incorporation of this software into a
 % commercial product is prohibited.
 %
 % BROWN UNIVERSITY DISCLAIMS ANY AND ALL WARRANTIES WITH REGARD TO
 % THIS SOFTWARE,INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY
 % AND FITNESS FOR ANY PARTICULAR PURPOSE.  IN NO EVENT SHALL BROWN
 % UNIVERSITY BE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL
 % DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE,
 % DATA OR PROFITS.

% -----------------------------------------------------------------
%   jacobi() - jacobi polynomials
%
%   Get a vector 'poly' of values of the n_th order Jacobi polynomial
%   P^(alpha,beta)_n(z) alpha > -1, beta > -1 at the np points in z
%   -----------------------------------------------------------------

function jf = JACOBI1D(z, n, alpha, beta)

  dims = size(z);
  jf   = zeros(dims);
  one = 1.0;
  two = 2.0;

  if(n == 0)
   jf(:) = one;
  elseif (n == 1)
   jf(:) = 0.5*(alpha - beta + (alpha + beta + two)*z);
  else
   two = 2.0;
   apb = alpha + beta;

   poly   = zeros(dims);
   polyn2 = ones(dims);
   polyn1 = 0.5*(alpha - beta + (alpha + beta + two)*z);

   for k = 2:n
    a1 =  two*k*(k + apb)*(two*k + apb - two);
    a2 = (two*k + apb - one)*(alpha*alpha - beta*beta);
    a3 = (two*k + apb - two)*(two*k + apb - one)*(two*k + apb);
    a4 =  two*(k + alpha - one)*(k + beta - one)*(two*k + apb);

    a2 = a2/a1;
    a3 = a3/a1;
    a4 = a4/a1;

    poly   = (a2 + a3*z).*polyn1 - a4*polyn2;
    polyn2 = polyn1;
    polyn1 = poly;
   end
   jf = poly;
  end


function [thrust, torque] = computePropOpPoint(RPM, rho, d_prop, C_t, C_q)
% [thrust, torque] = computePropOpPoint(RPM, rho, d_prop, C_t, C_q)
%
% Calculates thrust and torque for a series of propeller RPM inputs.
%   Note: Other parameters are considered constant
%   See: https://web.mit.edu/16.unified/www/FALL/thermodynamics/notes/node86.html
%
%
% INPUTS:
%   RPM = [1 x n] (RPM) propeller RPM
%   rho = [scalar] (kg/m^3) air density
%   d_prop = [scalar] (m) propeller diameter
%   C_t = [scalar] () propeller thrust coefficient.
%   C_q = [scalar] () propeller torque coefficient.
%
% OUTPUTS:
%   thrust = [scalar] (N) propeller thrust
%   torque = [scalar] (Nm) propeller torque
%
% Written by Conrad McGreal 2020-1-25

revs = RPM/60 ; % convert to revolution per second

% Calculate thrust
thrust = C_t*rho*revs.^2*d_prop^4 ;

% Calculate torque
torque = C_q*rho*revs.^2*d_prop^5 ;


function [sl, sh] = lowpass(s, lambda, npad)

% lowpass -- Lowpass filter image and return low and high frequency
%            components, consisting of the lowpass filtered image and
%            its difference with the input image. The lowpass filter
%            is equivalent to Tikhonov regularization with lambda as
%            the regularization parameter and a discrete gradient as
%            the operator in the regularization term.
%
% Usage:
%       [sl, sh] = lowpass(s, lambda, npad)
%
% Input:
%       s         Input image or 3d array of images
%       lambda    Regularization parameter controlling lowpass filtering
%       npad      Number of samples to pad at image boundaries
%
% Output:
%       sl        Lowpass component
%       sh        Highpass component
%
%
% Author: Brendt Wohlberg <brendt@lanl.gov>  Modified: 2015-04-09
%
% This file is part of the SPORCO library. Details of the copyright
% and user license can be found in the 'License' file distributed with
% the library.

if nargin < 3,
  npad = 16;
end

grv = [-1 1];
gcv = [-1 1]';
Gr = fft2(grv, size(s,1)+2*npad, size(s,2)+2*npad);
Gc = fft2(gcv, size(s,1)+2*npad, size(s,2)+2*npad);
A = 1 + lambda*conj(Gr).*Gr + lambda*conj(Gc).*Gc;
sp = padarray(s, [npad npad], 'symmetric', 'both');
slp = ifft2(bsxfun(@rdivide, fft2(sp), A), 'symmetric');
sl = slp((npad+1):(size(slp,1)-npad), (npad+1):(size(slp,2)-npad), :);
sh = s - sl;

return


function [area] = triarea(pp,tt)
%TRIAREA calc. triangle areas for a 2-simplex triangulation
%embedded in the two-dimensional plane.
%   [AREA] = TRIAREA(VERT,TRIA) returns the signed triangle
%   areas, where AREA is a T-by-1 vector, VERT is a V-by-2
%   array of XY coordinates, and TRIA is a T-by-3 array of
%   vertex indexing, where each row defines a triangle, such
%   that VERT(TRIA(II,1),:), VERT(TRIA(II,2),:) and VERT(
%   TRIA(II,3),:) are the coordinates of the II-TH triangle.
%
%   See also TRISCR2, TRIANG2, TRIBAL2

%   Darren Engwirda : 2017 --
%   Email           : de2363@columbia.edu
%   Last updated    : 17/01/2017

%---------------------------------------------- basic checks
    if (~isnumeric(pp) || ~isnumeric(tt) )
        error('triarea:incorrectInputClass' , ...
            'Incorrect input class.') ;
    end

%---------------------------------------------- basic checks
    if (ndims(pp) ~= +2 || ndims(tt) ~= +2 )
        error('triarea:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end
    if (size(pp,2)~= +2 || size(tt,2) < +3 )
        error('triarea:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end

    nnod = size(pp,1) ;

%---------------------------------------------- basic checks
    if (min(min(tt(:,1:3))) < +1 || ...
            max(max(tt(:,1:3))) > nnod )
        error('triarea:invalidInputs', ...
            'Invalid TRIA input array.') ;
    end

%--------------------------------------- compute signed area
    ev12 = pp(tt(:,2),:)-pp(tt(:,1),:) ;
    ev13 = pp(tt(:,3),:)-pp(tt(:,1),:) ;

    switch (size(pp,2))
        case +2

        area = ev12(:,1).*ev13(:,2) ...
             - ev12(:,2).*ev13(:,1) ;
        area = 0.5 * area;

        case +3

        avec = cross(ev12,ev13);
        area = sqrt(sum(avec.^2,2)) ;
        area = 0.5 * area;

        otherwise
        error('Unsupported dimension.') ;
    end

end


function rho_w = WeightedPearsonCorr(XI,XJ, w )
%WeightedPearsonCorr Pearson Correlation with Weights for use with
%outlier ensembles
%   Detailed explanation goes here

% Ensure that w is a column vector summing to 1
if size(w, 1) == 1
    w = w';
elseif size(w, 2) == 1
else
    error('w must be a vector');
end
w = w ./ sum(w);

m=size(XJ,1); % number of samples of p
p=size(XI,2); % dimension of samples

assert(p == size(XJ,2)); % equal dimensions
assert(size(XI,1) == 1); % pdist requires XI to be a single sample

mean_XI = dot(w, XI);
var_XI = dot(w, (XI - mean_XI).^2);

mean_XJ = sum(XJ * w, 2);
var_XJ = sum(((XJ - repmat(mean_XJ, 1, size(XJ, 2))).^2) * w, 2);

cov_XIJ = sum( (repmat((XI-mean_XI), size(XJ, 1), 1) .* (XJ - repmat(mean_XJ, 1, size(XJ, 2)))) * w, 2);

rho_w = cov_XIJ ./ sqrt(var_XJ) /  sqrt(var_XI);

% rho_w=zeros(m,1); % initialize output array
% for i=1:m
%     mean_XJ = dot(w, X
%     var_XJ
%     covariance = cov(XI, XJ(i,:)); covariance = covariance(1,2);
%     d(i,1) = 1 - covariance/stdXI/std(XJ(i,:));
% end

end


function fx = p14_fun ( n, x )

%*****************************************************************************80
%
%% P14_FUN evaluates the integrand for problem 14.
%
%  Discussion:
%
%    S&S gives "exact" value as     1.0634618101...
%    Mathematica returns            1.0634618101722400407...
%    S&S gives Laguerre(16) as      1.0634713425...
%    S&S gives EXP_TRANSFORM(16) as 1.0634618101...
%
%    The FORTRAN version of this routine, compiled with G95, was getting
%    a floating point exception when evaluating the integrand
%    and using a Laguerre rule of order 64.  So I have had to truncate
%    the evaluation of the exponential.
%
%  Integral:
%
%    Integral ( 0 <= x < +oo ) sin ( exp ( - x ) + exp ( - 4 x ) ) dx
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 July 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Arthur Stroud, Don Secrest,
%    Gaussian Quadrature Formulas,
%    Prentice Hall, 1966,
%    LC: QA299.4G3S7.
%
%  Parameters:
%
%    Input, integer N, the number of points.
%
%    Input, real X(N), the evaluation points.
%
%    Output, real FX(N), the function values.
%
  fx(1:n) = sin ( exp ( -x(1:n) ) + exp ( -4.0 * x(1:n) ) );

  return
end


function [ m, d ] = easter_stewart ( y )

%*****************************************************************************80
%
%% EASTER_STEWART computes the month and day of Easter for a Gregorian year.
%
%  Example:
%
%    Y = 2001
%
%    A = 6
%    B = 20
%    C = 1
%    DD = 5
%    E = 0
%    G = 6
%    H = 18
%    MM = 0
%    J = 0
%    K = 1
%    L = 6
%    M = 4
%    D = 15
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 June 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Thomas O'Beirne,
%    Puzzles and Paradoxes,
%    Oxford University Press, 1965.
%
%    Ian Stewart,
%    Easter is a Quasicrystal,
%    Scientific American,
%    March 2001, pages 80-83.
%
%  Parameters:
%
%    Input, integer Y, the year.
%
%    Output, integer M, D, the month and day of Easter.
%
  a = mod ( y, 19 );
  b = floor ( y / 100 );
  c = mod ( y, 100 );
  dd = floor ( b / 4 );
  e = mod ( b, 4 );
  g = floor ( ( 8 * b + 13 ) / 25 );
  h = mod ( 19 * a + b - dd - g + 15, 30 );
  mm = floor ( ( a + 11 * h ) / 319 );
  j = floor ( c / 4 );
  k = mod ( c, 4 );
  l = mod ( 2 * e + 2 * j - k - h + mm + 32, 7 );

  m = floor ( ( h - mm + l + 90 ) / 25 );
  d = mod ( h - mm + l + m + 19 , 32 );

  return
end


function [ ns, xyz ] = sphere_cubed_points_face ( n, i1, j1, k1, i2, ...
  j2, k2, ns, xyz )

%*****************************************************************************80
%
%% SPHERE_CUBED_POINTS_FACE: points on one face of a cubed sphere grid.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 September 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of sections into which each face of
%    the cube is to be divided.
%
%    Input, integer I1, J1, K1, I2, J2, K2, the logical indices, between 0
%    and N, of two corners of the face grid.  It is guaranteed that I1 <= I2,
%    J1 <= J2, and K1 <= K2.
%
%    Input, integer NS, the number of points.
%
%    Input, real XYZ(3,NS), distinct points on the unit sphere
%    generated by a cubed sphere grid.
%
%    Output, integer NS, the number of points.
%
%    Output, real XYZ(3,NS), distinct points on the unit sphere
%    generated by a cubed sphere grid.
%
  for i = i1 : i2

    if ( i1 < i2 )
      xc = tan ( ( 2 * i - n ) * 0.25 * pi / n );
    elseif ( i1 == 0 )
      xc = -1.0;
    elseif ( i1 == n )
      xc = +1.0;
    else
      xc = 0.0;
    end

    for j = j1 : j2

      if ( j1 < j2 )
        yc = tan ( ( 2 * j - n ) * 0.25 * pi / n );
      elseif ( j1 == 0 )
        yc = -1.0;
      elseif ( j1 == n )
        yc = +1.0;
      else
        yc = 0.0;
      end

      for k = k1 : k2

        if ( k1 < k2 )
          zc = tan ( ( 2 * k - n ) * 0.25 * pi / n );
        elseif ( k1 == 0 )
          zc = -1.0;
        elseif ( k1 == n )
          zc = +1.0;
        else
          zc = 0.0;
        end

        xyzn = sqrt ( xc^2 + yc^2 + zc^2 );

        ns = ns + 1;
        xyz(1,ns) = xc / xyzn;
        xyz(2,ns) = yc / xyzn;
        xyz(3,ns) = zc / xyzn;

      end
    end
  end

  return
end


function Yq = QuantLloyd (Nlev, FPDF, QSym)
%  Iterate to find the output levels for a minimum mean square error
%  quantizer.
%
% This subroutine searches for a set of quantizer output levels which are
% the conditional means (centroids) of the quantizer decision regions. The
% decision boundaries are assumed to lie mid-way between output levels.
%
% The procedure is based on a Lloyd-Max iteration.
% (1) Given the start of an interval and an output level, find the end of
%     the interval, such that the output level is the centroid of the
%     probability density function in that interval.
% (2) Use the output level and upper interval edge to find the output
%     level in the next interval (the interval edge must lie midway
%     between output levels).
% (3) With the start and output level of the next interval
%     determined, repeat step (1) for this interval.
% The iteration continues by modifying the initial output level based
% on whether the centroid property of the last interval was satisfied.
%    S. P. Lloyd, "Least Squares Quantization in PCM", IEEE Trans. Inform.
%      Theory, vol. 28, no. 2, pp. 129-137, March 1982.
%    J. Max, "Quantizing for Minimum Distortion", IEEE Trans. Inform.
%      Theory, vol. 6, no. l, pp. 7-12, March 1960.
%
% Nlev - Number of quantizer output levels. For symmetric quantizers
%   this is the number of levels above the mean.
% FPDF - Cell array of function pointers {Farea, Fmean, Fvar}
% QSym - Symmetry flag (optional, default 0)
%   0 - Quantizer not constrained to be symmetric
%   1 - Quantizer is symmetric with an odd number of levels. The middle
%       level is fixed at the mean. This routine finds the Nlev output
%       levels above the mean.
%   2 - Quantizer is symmetric with an even number of levels. The middle
%       decision level is at the mean. This routine finds the Nlev output
%       levels above the mean.
%
% Yq - Nlev output levels in ascending order

% There are 3 symmetry cases to consider for symmetry
% QSym == 0: No symmetry. The first interval is defined by a lower
%   boundary XL = -Inf and an output level (centroid) Yc. The centroid
%   position is iterated.
% QSym == 1: Symmetrical quantizer with fixed output level at the mean.
%   If we place another output level Yc, the decision level XL lies
%   midway between the mean and Yc. When we iterate Yc, the decision level
%   is also be readjusted.
% QSym == 2: Symmetric quantizer with a decision boundary XL at the mean.
%   The output level Yc is iterated.

if (nargin < 3)
  QSym = 0;
end

Fmean = FPDF{2};
Fvar = FPDF{3};

% Parameters
MaxIter = 100;
TolR = 1e-5;

% Set the optimization parameters
Xmean = feval(Fmean, -Inf, Inf);
sd = feval(Fvar, -Inf, Inf) - Xmean^2;

% Convergence criterion
Tol = TolR * sd;

if (QSym == 0)                  % No symmetry
  XL = -Inf;
  Yc = Xmean - 4 * sd;
  Xstep = sd;
elseif (QSym == 1)              % Symmetric, odd number of coefficients
  Xstep = 2 * sd / Nlev;
  Yc = Xmean + Xstep;
  XL = 0.5 * (Xmean + Yc);
elseif (QSym == 2)              % Symmetric, even number of coefficients
  Xstep = 2 * sd / Nlev;
  XL = Xmean;
  Yc = Xmean + Xstep;
end

% Initialization
Ytrial = Yc;
Ybase = Yc;
Xstep = 0.5 * Xstep;
FL = false;         % True if an upper bound has been found
FU = false;         % True if a lower bound has been found

for (Iter = 1:MaxIter)

% Find a set of output levels satisfying the necessary conditions
% for a minimum mean square error quantizer
% XU is the largest quantizer decision level
%  Yc = Ytrial;
  if (QSym == 1)
    XL = 0.5 * (Xmean + Ytrial);
  end

  [Yq, XU] = QuantLevel(Nlev, FPDF, XL, Ytrial);

  if (isnan(XU) || Yq(end) == XU)
    FU = true;
  else
    FL = true;
    Ybase = Ytrial;
  end

% Check for convergence, adjust the step size
  if (FL && FU)
    if (Xstep < Tol)
      break
    end
    Xstep = 0.5 * Xstep;
    Ytrial = Ybase + Xstep;
  elseif (FL)     % Need to extend the search upward
    Xstep = 2 * Xstep;
    Ytrial = Ybase + Xstep;
  else            % Need to back up the initial value
    Ybase = Ytrial;
    Xstep = 2 * Xstep;
    if (~isinf(XL))
      Xstep = min(Xstep, 0.5*(Ybase - XL)); % Don't step below XL
    end
    Ytrial = Ybase - Xstep;
  end

end

if (Iter >= MaxIter)
  error('QuantLloyd: Failed to converge');
end
if (~FU || ~FL)
  error('QuantLloyd: Feasible solution not found');
end
fprintf('QuantLloyd: Converged, %d iterations\n', Iter);

return

% ----- -----
function [Yq, XU] = QuantLevel(Nlev, FPDF, XL, Yc)
% Find a set of quantizer output levels, given the lower boundary and
% centroid of the first interval.
%
% Given the lower boundary (decision level) for an initial interval and the
% output level (centroid) of that interval, this routine first finds the
% upper decision boundary for that interval. These values are telescoped to
% give the lower boundary and output level for the second interval. This
% process continues for each interval.
%
% The last decision level is returned by this routine. If this value is
% finite, the last interval does not fully encompass the tail of the
% probability density function. If the last decision level is NaN, the last
% output level is not the centroid of the last region extenting to infinity.
%
% Nlev - Number of quantizer output levels. For symmetric quantizers this
%   is the number of levels above the mean.
% FPDF - Cell array of function handles {Farea, Fmean, Fvar}
% XL - Lower boundary of the first interval
% Yc - Centroid of the first interval
%
% Yq - Nlev quantizer output levels in ascending order. Trailing values may
%   be NaN if it is not possible to have intervals with these output levels
%   as the centroids of the corresponding intervals. The quantizer decision
%   levels lie midway between output levels.
% XU - Largest decision level (greater than or equal to Yq(Nlev)) or
%   NaN.

% If XU is finite, then the levels should be adjusted upward (increase
% Yc). If XU is NaN, the last output level is not the centroid of the last
% interval, and the levels should be adjusted downward. Another case occurs
% if an entire interval is zero probability. Then the upper decision level
% falls on the output level for that interval. Test for XU == Yq(Nlev).

Yq = zeros(1, Nlev);
for (i = 1:Nlev)

% Given the lower decision level and the output level for an interval,
% find the upper decision level
  if (isnan(XL))
    XU = XL;
    Yc = XL;
  else
    XU = QuantInterval(FPDF, XL, Yc);
  end

% Given the interval limits just found, telescope to find the
% output level to be used in the next iteration
  Yq(i) = Yc;
  XL = XU;
  Yc = XU + (XU - Yc);

end

return

% ----- ------
function Xb = QuantInterval (FPDF, Xa, Yc)
% Find the upper edge of an interval which has a given centroid.
%
% This routine solves for upper limit of the integral
%    Xb
%   Int (x-Yc) p(x) dx = 0.
%    Xa
%
% The routine is designed to allow for Yc to be less than Xa, in which case
% Xb <= Yc, or for Yc to be greater than Xa, in which case Xb >= Yc.
%
% FPDF - Cell array of function handles {Farea, Fmean, Fvar}
% Xa - Lower boundary of the interval
% Yc - Centroid of the interval
%
% Xb - Returned value representing the upper boundary of the interval. This
%   value is set to NaN if there is no solution for Xb on the opposite side
%   of Yc from Xa.

% Parameters
XstepR = 1.2;    % Initial relative step size
TolF = 1e-5;     % Function amplitude relative tolerance
TolX = 1e-5;     % Position relative tolerance
MaxIter = 100;
EpsdF = 0.01;    % Choose between bisection or linear interpolation
EpsdX = 0.05;    % For linear interpolation, constrain the relative
                 % step size to EpsdX <= dX <= 1-EpsdX.

% Searching for a solution of the equation F(Xb) = 0. Consider the
% case that Yc > Xa. The function F(Xb) is zero at Xb = Xa. It becomes
% negative with increasing Xb (since p(x) is positive). It takes on its
% most negative value at Xb = Yc. It then decreases as Xb increases. It
% is the second zero crossing we seek.

% The search procedure has as its stopping criteria:
%  a) abs(F(Xb) < TolF*abs(F(YC)).
%  b) The interval of uncertainty is less than TolX*abs(Xb)
%  c) The probability from the present trial point to Inf or -Inf (as
%     appropriate) is zero. In this case Xb is set to NaN.
%  d) The maximum number of iterations is exceeded.
%  e) F(Yc)=0. This indicates that the probability density function is
%     zero in the interval (Xa,Yc).

if (Yc == Xa)
  Xb = Yc;
  return
end

Farea = FPDF{1};
Fmean = FPDF{2};
Fvar = FPDF{3};

% Evaluate the function at Yc to check if the probability
% is zero, and to determine the stopping criterion Feps
Fm = feval(Fmean, Xa, Yc) - Yc*feval(Farea, Xa, Yc);  % Should be negative
if (Fm >= 0)
  if (Fm > 0)
    error('QuantInterval: Error, function value at centroid positive');
  end
  Xb = Yc;          % Fm == 0;
  return
end

% Set up the boundaries of the search and the step size
FL = Fm;            % Value at lower boundary (initially negative)
XL = Yc;            % Lower boundary of search
if (~isinf(Xa))

  Xb = Yc + XstepR * (Yc - Xa);  % Initial trial upper boundary

else

% Find the standard deviation of the distribution
  Xmean = feval(Fmean, -Inf, Inf);
  sd = sqrt(feval(Fvar, -Inf, Inf) - Xmean^2);
  Xb = Yc + sign(Yc-Xa) * sd;   % Initial test upper boundary
end

FR = -1;            % Same sign as FL to indicate no zero found
XR = Xb;
Feps = TolF * abs(Fm); % Tolerance on integral value
Xstep = 0.5 * (Xb - Yc);

% Search loop
for (Iter = 1:MaxIter)

  Fp = Fm;          % Previous Fm
  Fm = feval(Fmean, Xa, Xb) - Yc*feval(Farea, Xa, Xb);

% Update the end-points of the search
if (Fm >= 0)
  FR = Fm;
  XR = Xb;
else
  FL = Fm;
  XL = Xb;
end

% ----- ------
  if (FR >= 0)
% Straddling a root

% Check for convergence in the function value
% Check for convergence of the position
    if (abs(Fm) <= Feps) || ...
       (~isinf(XL) && ~isinf(XR) && ...
        abs(XR-XL) <= TolX*max(abs(XR), abs(XL)))
      return
    end

% The root location is sought by cautious linear interpolation; however
% if successive function values are nearly equal, bisection is used.
    if (abs(Fp-Fm) > EpsdF * abs(FL-FR))
      dX = FL / (FL-FR);    % Estimated zero crossing position
      dX = max(min(dX, 1-EpsdX), EpsdX); % Avoid region close to XL or XR
    else
      dX = 0.5;             % Bisection
    end
    Xb = XL + dX*(XR-XL);

% ------ ------
  else
% Not straddling a root

% Right boundary undefined
% Check for successive identical returned values
    if (Yc > Xa)
      if (Fm == Fp && feval(Farea, Xb, Inf) <= 0)
        Xb = NaN;
        return
      end
    else
      if (Fm == Fp && feval(Farea, -Inf, Xb) <= 0)
        Xb = NaN;
        return
      end
    end

% Increase the step size
    Xb = Xb + Xstep;
    Xstep = 2 * Xstep;

  end

end

error('QuantInterval: Failed to converge');


%% Gammatone-like spectrograms
% Gammatone filters are a popular linear approximation to the
% filtering performed by the ear.  This routine provides a simple
% wrapper for generating time-frequency surfaces based on a
% gammatone analysis, which can be used as a replacement for a
% conventional spectrogram.  It also provides a fast approximation
% to this surface based on weighting the output of a conventional
% FFT.

%% Introduction
% It is very natural to visualize sound as a time-varying
% distribution of energy in frequency - not least because this is
% one way of describing the information our brains get from our
% ears via the auditory nerve.  The spectrogram is the traditional
% time-frequency visualization, but it actually has some important
% differences from how sound is analyzed by the ear, most
% significantly that the ear's frequency subbands get wider for
% higher frequencies, whereas the spectrogram has a constant
% bandwidth across all frequency channels.
%
% There have been many signal-processing approximations proposed
% for the frequency analysis performed by the ear; one of the most
% popular is the Gammatone filterbank originally proposed by
% Roy Patterson and colleagues in 1992.  Gammatone filters were
% conceived as a simple fit to experimental observations of
% the mammalian cochlea, and have a repeated pole structure leading
% to an impulse response that is the product of a Gamma envelope
% g(t) = t^n e^{-t} and a sinusoid (tone).
%
% One reason for the popularity of this approach is the
% availability of an implementation by Malcolm Slaney, as
% described in:
%
% Malcolm Slaney (1998) "Auditory Toolbox Version 2",
% Technical Report #1998-010, Interval Research Corporation, 1998.
% http://cobweb.ecn.purdue.edu/~malcolm/interval/1998-010/
%
% Malcolm's toolbox includes routines to design a Gammatone
% filterbank and to process a signal by every filter in a bank,
% but in order to convert this into a time-frequency visualization
% it is necessary to sum up the energy within regular time bins.
% While this is not complicated, the function here provides a
% convenient wrapper to achieve this final step, for applications
% that are content to work with time-frequency magnitude
% distributions instead of going down to the waveform levels.  In
% this mode of operation, the routine uses Malcolm's MakeERBFilters
% and ERBFilterBank routines.
%
% This is, however, quite a computationally expensive approach, so
% we also provide an alternative algorithm that gives very similar
% results.  In this mode, the Gammatone-based spectrogram is
% constructed by first calculating a conventional, fixed-bandwidth
% spectrogram, then combining the fine frequency resolution of the
% FFT-based spectra into the coarser, smoother Gammatone responses
% via a weighting function.  This calculates the time-frequency
% distribution some 30-40x faster than the full approach.

%% Routines
% The code consists of a main routine, <gammatonegram.m gammatonegram>,
% which takes a waveform and other parameters and returns a
% spectrogram-like time-frequency matrix, and a helper function
% <fft2gammatonemx.m fft2gammatonemx>, which constructs the
% weighting matrix to convert FFT output spectra into gammatone
% approximations.

%% Example usage
% First, we calculate a Gammatone-based spectrogram-like image of
% a speech waveform using the fast approximation.  Then we do the
% same thing using the full filtering approach, for comparison.

% Load a waveform, calculate its gammatone spectrogram, then display:
[d,sr] = wavread('sa2.wav');
tic; D = gammatonegram(d,sr); toc
%Elapsed time is 0.140742 seconds.
subplot(211)
imagesc(20*log10(D)); axis xy
caxis([-90 -30])
colorbar
title('Gammatonegram - fast method')

% Now repeat with flag to use actual subband filters.
% Since it's the last argument, we have to include all the other
% arguments.  These are the default values for: summation window
% (0.025 sec), hop between successive windows (0.010 sec),
% number of gammatone channels (64), lowest frequency (50 Hz),
% and highest frequency (sr/2).  The last argument as zero
% means not to use the FFT approach.
tic; D2 = gammatonegram(d,sr,0.025,0.010,64,50,sr/2,0); toc
%Elapsed time is 3.165083 seconds.
subplot(212)
imagesc(20*log10(D2)); axis xy
caxis([-90 -30])
colorbar
title('Gammatonegram - accurate method')
% Actual gammatone filters appear somewhat narrower.  The fast
% version assumes coherence of addition of amplitude from
% different channels, whereas the actual subband energies will
% depend on how the energy in different frequencies combines.
% Also notice the visible time smearing in the low frequency
% channels that does not occur in the fast version.

%% Validation
% We can check the frequency responses of the filterbank
% simulated with the fast method against the actual filters
% from Malcolm's toolbox.  They match very closely, but of
% course this still doesn't mean the two approaches will give
% identical results - because the fast method ignores the phase
% of each frequency channel when summing up.

% Check the frequency responses to see that they match:
% Put an impulse through the Slaney ERB filters, then take the
% frequency response of each impulse response.
fcfs = flipud(MakeERBFilters(16000,64,50));
gtir = ERBFilterBank([1, zeros(1,1000)],fcfs);
H = zeros(64,512);
for i = 1:64; H(i,:) = abs(freqz(gtir(i,:),1,512)); end
% The weighting matrix for the FFT is the frequency response
% of each output filter
gtm = fft2gammatonemx(1024,16000,64,1,50,8000,512);
% Plot every 5th channel from both.  Offset by 3 dB just so we can
% see both
fs = [0:511]/512*8000;
figure
plot(fs,20*log10(H(5:5:64,:))','b',fs, -3 + 20*log10(gtm(5:5:64,:))','r')
axis([0 8000 -150 0])
grid
% Line up pretty well, apart from wiggles below -100 dB
% (from truncating the impulse response at 1000 samples?)

%% Download
% You can download all the code and data for these examples here:
% <gammatone.tgz gammatone.tgz>.

%% Referencing
% If you use this work in a publication, I would be grateful
% if you referenced this page as follows:
%
%  D. P. W. Ellis (2009).  "Gammatone-like spectrograms", web resource, http://www.ee.columbia.edu/~dpwe/resources/matlab/gammatonegram/ .

%% Acknowledgment
% This project was supported in part by the NSF under
% grant IIS-0535168. Any opinions, findings and conclusions
% or recommendations expressed in this material are those of the
% authors and do not necessarily reflect the views of the Sponsors.

% Last updated: $Date: 2009/02/22 01:46:42 $
% Dan Ellis <dpwe@ee.columbia.edu>


function [coef]=ref_dwiltii_1(f,g,a,M)
%COMP_DWILT  Compute Discrete Wilson transform.
%
%   Do not call this function directly, use DWILT instead.

%   Author : Peter L. Søndergaard.

L=size(g,1);
N=L/a;
W=size(f,2);

c=ref_gdgt(f,g,a,2*M,.5,0,0);

coef2=reshape(c,2*M,N,W); % ----- Type II ------
%coef2=dgt(f,g,a,2*M);

coef=zeros(2*M,N/2,W);

if 0
  % --- Loop version ---
  for n=0:N/2-1

    % ---- m is zero ---------
    coef(1,n+1,:)=coef2(1,2*n+1,:);

    for m=1:2:M-1
      % --- m is odd ----------
      coef(m+1,n+1,:)=   i/sqrt(2)*(coef2(m+1,2*n+1,:)+coef2(2*M-m+1,2*n+1,:));
      coef(M+m+1,n+1,:)= 1/sqrt(2)*(coef2(m+1,2*n+2,:)-coef2(2*M-m+1,2*n+2,:));
    end;
    for m=2:2:M-1
      % --- m is even ---------
      coef(m+1,n+1,:)=   1/sqrt(2)*(coef2(m+1,2*n+1,:)-coef2(2*M-m+1,2*n+1,:));
      coef(M+m+1,n+1,:)= i/sqrt(2)*(coef2(m+1,2*n+2,:)+coef2(2*M-m+1,2*n+2,:));
    end;

    % --- m is nyquest ------
    if mod(M,2)==0
      coef(M+1,n+1,:) = i*coef2(M+1,2*n+2,:);
    else
      coef(M+1,n+1,:) = i*coef2(M+1,2*n+1,:);
    end;

  end;

else
  % --- Vector version---

  % ---- m is zero ---------
  coef(1,:,:)=coef2(1,1:2:N,:);

  % --- m is odd ----------
  coef(2:2:M,:,:)    = i/sqrt(2)*(coef2(2:2:M,1:2:N,:)+coef2(2*M:-2:M+2,1:2:N,:));
  coef(M+2:2:2*M,:,:)= 1/sqrt(2)*(coef2(2:2:M,2:2:N,:)-coef2(2*M:-2:M+2,2:2:N,:));

  % --- m is even ---------
  coef(3:2:M,:,:)=     1/sqrt(2)*(coef2(3:2:M,1:2:N,:)-coef2(2*M-1:-2:M+2,1:2:N,:));
  coef(M+3:2:2*M,:,:)= i/sqrt(2)*(coef2(3:2:M,2:2:N,:)+coef2(2*M-1:-2:M+2,2:2:N,:));

  % --- m is nyquest ------
  if mod(M,2)==0
    coef(M+1,:,:) = i*coef2(M+1,2:2:N,:);
  else
    coef(M+1,:,:) = i*coef2(M+1,1:2:N,:);
  end;

end;

coef=reshape(coef,M*N,W);


function b = det(a)
% function b=det(a)
%
% DESCRIPTION
%   Determinant of a matrix polynomial
%
% INPUTS
%   A: polynomial
%
% OUTPUTS
%   B: polynomial, the determinant of A
%
% SYNTAX
%   B = det(A);

% 6/14/2002: PJS  Initial Coding

sza=size(a);
if sza(1)~=sza(2)
    error('Matrix must be square');
end

if isempty(a)
    b = polynomial(1);
elseif sza(1)==1
    b = a;
else
    L.type = '()';
    b = polynomial(0);
    for i1 = 1:sza(1);
        % Recursive cofactor expansion for det
        % XXX Faster algos for computing det exist
        L.subs = {[2:sza(1)] [1:i1-1, i1+1:sza(1)]};
        M = subsref(a,L);
        cofactor = (-1)^(1+i1)*det(M);

        L.subs = {1, i1};
        a1_i1 = subsref(a,L);
        b = b + a1_i1*cofactor;
    end
end


function euler = rotMat2euler(R)
    % from paper: "Adaptive Filter for a Miniature MEMS Based Attitude and
    % Heading Reference System" by Wang et al, IEEE.

    phi = atan2(R(3,2,:), R(3,3,:) );
    theta = -atan(R(3,1,:) ./ sqrt(1-R(3,1,:).^2) );
    psi = atan2(R(2,1,:), R(1,1,:) );

    euler = [phi(1,:)' theta(1,:)' psi(1,:)'];
end


function rad = deg2rad(deg)
%
% Function Name:
%
%   deg2rad - Convert degrees to radians.
%
% Calling Sequence:
%
%   rad = deg2rad(deg);
%
% Parameters:
%
%   deg		: Angle in degrees.
%
%   rad		: Angle in radians.
%
% Description:
%
%   Convenient utility function for converting degrees to radians, which are
%   often the required angular units for functions in the NURBS toolbox.
%
% Examples:
%
%   // Convert 35 degrees to radians
%   rad = deg2rad(35);

%  D.M. Spink
%  Copyright (c) 2000.

rad = pi*deg/180.0;


function [hd, ind1, ind2] = hausdorffDistance(pts1, pts2)
%HAUSDORFFDISTANCE  Hausdorff distance between two point sets
%
%   HD = hausdorffDistance(PTS1, PTS2)
%   Computes the Hausdorff distance between the two point sets PTS1 and
%   PTS2. The Hausdorf distance can be used to compare two shapes.
%
%   The distance between a point x and a set Y is given by:
%     d(x, Y) = inf { d(x,y) | y in Y }
%   The distance between two non empty sets X and Y is given by:
%     d(X, Y) = sup { d(x,Y) | x in X }
%   The Hausdorff distance between sets X and Y distance is defined as the
%   maximum of d(X,Y) and d(Y,X):
%     HD(X,Y) = max { d(X,Y), d(Y,X) }
%
%
%   Example
%   % Compute Hausdorff distance between an ellipse and a rectangle
%     % first define two shapes
%     rect = resamplePolygon(orientedBoxToPolygon([20 30 80 40 30]), 60);
%     poly = ellipseToPolygon([20 30 40 20 30], 500);
%     % display the shapes
%     figure; hold on
%     drawPolygon(poly, 'b');
%     drawPolygon(rect, 'g');
%     axis equal;
%     % compute hausdorff distance
%     [hd ind1 ind2] = hausdorffDistance(poly, rect);
%     p1h = poly(ind1, :);
%     p2h = rect(ind2, :);
%     drawPoint([p1h;p2h], 'mo');
%     drawEdge([p1h p2h], 'm')
%
%   See also
%   minDistancePoints
%
%   References
%   http://en.wikipedia.org/wiki/Hausdorff_distance
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2012-05-04,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2012 INRA - Cepia Software Platform.

% distance from pts1 to pts2
[dists1, ind12] = minDistancePoints(pts1, pts2);
[max1, ind11] = max(dists1);

% distance from pts2 to pts1
[dists2, ind22] = minDistancePoints(pts2, pts1);
[max2, ind21] = max(dists2);

% keep the max of the two distances
hd = max(max1, max2);

% keep the rigt indices
if max1 > max2
    ind1 = ind11;
    ind2 = ind12(ind11);
else
    ind1 = ind22(ind21);
    ind2 = ind21;
end


clear all; close all; clc

% A=randn(10,10,10);
% model=parafac(A,3);
% [A1,A2,A3]=fac2let(model)

x=-5:0.1:5; y=-6:0.1:6; t=0:0.1:10*pi;
[X,Y,T]=meshgrid(x,y,t);
A=exp(-(X.^2+0.5*Y.^2)).*(cos(2*T))+ ...
    (sech(X).*tanh(X).*exp(-0.2*Y.^2)).*sin(T);

for j=1:length(t)
  pcolor(x,y,A(:,:,j)), shading interp, caxis([-1 1]), drawnow
end

figure(1)
for j=1:8
  subplot(2,4,j)
  pcolor(x,y,A(:,:,8*j-3)), colormap(hot), shading interp, caxis([-1 1]), axis off
end

figure(2)
model=parafac(A,2);
[A1,A2,A3]=fac2let(model);
subplot(3,1,1), plot(y,A1,'Linewidth',[2])
subplot(3,1,2), plot(x,A2,'Linewidth',[2])
subplot(3,1,3), plot(t,A3,'Linewidth',[2])

subplot(3,1,1), set(gca,'Xtick',[-6 0 6],'Fontsize',[15])
subplot(3,1,2), set(gca,'Xtick',[-5 0 5],'Fontsize',[15])
subplot(3,1,3), set(gca,'Xlim',[0 10*pi],'Xtick',[0 5*pi 10*pi],'Xticklabels',{'0','5\pi','10\pi'},'Fontsize',[15])


function determ = circulant2_determinant ( n )

%*****************************************************************************80
%
%% CIRCULANT2_DETERMINANT returns the determinant of the CIRCULANT2 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 November 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Output, real DETERM, the determinant.
%
  w = c8vec_unity ( n );

  lambda = zeros ( n, 1 );
  lambda(1:n) = n;
  for i = n-1 : -1 : 1
    lambda(1:n) = lambda(1:n) .* w(1:n) + i;
  end
%
%  First eigenvalue is "special".
%
  determ = real ( lambda(1) );
%
%  Eigenvalues 2, 3     through ( N + 1 ) / 2 are paired with complex conjugates.
%
  for i = 2 : floor ( ( n + 1 ) / 2 )
    determ = determ * ( abs ( lambda(i) ) )^2;
  end
%
%  If N is even, there is another unpaired eigenvalue.
%
  if ( mod ( n, 2 ) == 0 )
    determ = determ * real ( lambda((n/2)+1) );
  end

  return
end


function y = wprctile(x, p, w)
% WPRCTILE Percentiles of a weighted sample.
%
%    Description
%      Y = PRCTILE(X, P, W) returns percentiles of the values in X. P
%       is a scalar or a vector of percent values. W is a vector of
%       unnormalized weights for samples. Length of W has to be same
%       as length of X. X need to be a a vector. Y is the same size as
%       P, and Y(i) contains the P(i)-th percentile.
%
%      Example
%       y = prctile(x,50,w); % the median of x given sample weights w
%
%    See also wmean, prctile

% BUGS: Accepts only vector valued X

% Copyright (c) 2000-2010 Aki Vehtari

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

x=sort(x);
p=p./100;
y=p;
ww=cumsum(w);ww=ww./ww(end);
for j=1:length(p)
  wi=min(find(ww>=p(j)));
  if wi==1
    y(j)=x(1);
  else
    w1=ww(wi-1);x1=x(wi-1);
    y(j)=x1+(x(wi)-x1).*(p(j)-w1)./(ww(wi)-w1);
  end
end


function d=sqdist(a,b)
% SQDIST - computes squared Euclidean distance matrix
%          computes a rectangular matrix of pairwise distances
% between points in A (given in columns) and points in B

% NB: very fast implementation taken from Roland Bunschoten

aa = sum(a.*a,1); bb = sum(b.*b,1); ab = a'*b;
d = abs(repmat(aa',[1 size(bb,2)]) + repmat(bb,[size(aa,2) 1]) - 2*ab);


function q=rotMat2Quat(R,handed)
%%ROTMAT2QUAT Get a unit quaternion corresponding to a particular rotation
%             matrix. The quaternion can be chosen to support standard
%             right-handed quaternion multiplication rules, or non-standard
%             left-handed rules that some authors choose to use.
%
%INPUTS:  M A 3X3 orthonormal real rotation matrix.
%    handed The handedness of the quaternion. If omitted, it is assumed
%           that the quaternion is right-handed (the standard). Possible
%           values are:
%           'right' The default if omitted. The quaternion multiplication
%                   is assumed right-handed (standard).
%           'left'  The quaternion multiplication is assumed left-handed.
%                   This is used in someplaces, including the reference
%                   from Shuster, below.
%
%OUTPUTS: q A 4X1 unit quaternion corresponding to the rotation matrix. The
%           quaternion is ordered [cos(theta/2);sin(theta/2)u'] where u is
%           a unit vector for the axis of rotation and theta is the
%           counterclockwise (right-handed) or clockwise (left-handed)
%           rotation angle about that unit vector.
%
%The formulae for converting a rotation matrix are from [1], where a minor
%change has been performed to support both right and left-handed
%quaternions. Both q and -q represent the same rotations. Here, the
%solution is chosen so that when considering a left-handed rotation, the
%sign of the largest magnitude element is positive. When considering a
%right handed rotation, the sign of the largest element may or may not be
%positive.
%
%A quaternion of form q(1)+i*q(2)+j*q(3)+k*q(4) that obeys right-handed
%multiplication rules supports the following rules for multiplication of i,
%j, and k, where an item in a row is multiplied by an item in the column to
%get the result:
%  i,  j, k
%i -1, k,-j
%j -k,-1, i
%k  j,-i,-1
%On the other hand, left-handed multiplication rules flip the signs of the
%off-diagonal terms:
%  i,  j, k
%i -1,-k, j
%j  k,-1,-i
%k -j, i,-1
%
%REFERENCES:
%[1] W. F. Phillips, C. E. Hailey, and G. A. Gebert, "Review of attitude
%    representations used for aircraft kinematics," Journal of Aircraft,
%    vol. 38, no. 4, pp. 718-737, Jul. - Aug. 2001.
%
%August 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<2||isempty(handed))
    handed='right';
end

R11=R(1,1);
R22=R(2,2);
R33=R(3,3);
R12=R(1,2);
R21=R(2,1);
R13=R(1,3);
R31=R(3,1);
R23=R(2,3);
R32=R(3,2);

%The square of the quaternion...
q2=(1/4)*[1+R11+R22+R33;
          1+R11-R22-R33;
          1-R11+R22-R33;
          1-R11-R22+R33];

q2Max=max(q2);

if(q2Max==q2(1))
    q0=0.5*sqrt(1+R11+R22+R33);
    q1=(1/(4*q0))*(R23-R32);
    q2=(1/(4*q0))*(R31-R13);
    q3=(1/(4*q0))*(R12-R21);
elseif(q2Max==q2(2))
    q1=0.5*sqrt(1+R11-R22-R33);
    q0=(1/(4*q1))*(R23-R32);
    q2=(1/(4*q1))*(R12+R21);
    q3=(1/(4*q1))*(R31+R13);
elseif(q2Max==q2(3))
    q2=0.5*sqrt(1-R11+R22-R33);
    q0=(1/(4*q2))*(R31-R13);
    q1=(1/(4*q2))*(R12+R21);
    q3=(1/(4*q2))*(R23+R32);
else
    q3=0.5*sqrt(1-R11-R22+R33);
    q0=(1/(4*q3))*(R12-R21);
    q1=(1/(4*q3))*(R31+R13);
    q2=(1/(4*q3))*(R23+R32);
end

%The above implementation provides a left-handed quaternion, so the sign of
%the vector part needs to be flipped if the standard right-handed system is
%used.
switch(handed)
    case 'right'
        q=[q0;-q1;-q2;-q3];
    case 'left'
        q=[q0;q1;q2;q3];
    otherwise
        error('Invalid handedness provided.')
end
end

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


function h = qqPlot(varargin)
%QQPLOT Quantile-quantile plot with patch option
%
%   IOSR.STATISTICS.QQPLOT(Y) displays a quantile-quantile plot of the
%   sample quantiles of Y versus theoretical quantiles from a normal
%   distribution. If the distribution of Y is normal, the plot will be
%   close to linear.
%
%   IOSR.STATISTICS.QQPLOT(X,Y) displays a quantile-quantile plot of two
%   samples. If the samples come from the same distribution, the plot will
%   be linear.
%
%   The inputs X and Y should be numeric and have an equal number of
%   elements; every element is treated as a member of the sample.
%
%   The plot displays the sample data with the plot symbol 'x'.
%   Superimposed on the plot is a dashed straight line connecting the first
%   and third quartiles.
%
%   IOSR.STATISTICS.QQPLOT(...,MODE) allows the appearance of the plot to
%   be configured. With MODE='line' (default), the plot appears as
%   described above. With MODE='patch', the data are plotted as a patch
%   object, with the area bound by the x-distribution and the linear fit
%   shaded grey. With mode='both' the two appearances are combined.
%
%   IOSR.STATISTICS.QQPLOT(...,MODE,METHOD) and
%   IOSR.STATISTICS.QQPLOT(...,[],METHOD) allows the method for calculating
%   the quartiles, used for the fit line, to be specified. The default is
%   'R-8'. Type 'help iosr.statistics.quantile' for more information. The
%   latter form of the function call uses the default mode.
%
%   H = IOSR.STATISTICS.QQPLOT(...) returns a two- or three-element vector
%   of handles to the plotted object. The nature of the handles depends
%   upon the mode. In all cases, the first handle is to the sample data,
%   the second handle is to the fit line. With MODE='patch' or MODE='both',
%   there is third handle to the patch object.
%
%   Example
%
%      % Display Q-Q plots for the rand and randn functions
%      figure
%      subplot(2,1,1)
%      iosr.statistics.qqPlot(rand(20),'patch')
%      subplot(2,1,2)
%      h = iosr.statistics.qqPlot(randn(20),'patch');
%      set(h(3),'FaceColor','r') % change fill color
%
%   See also IOSR.STATISTICS.QUANTILE, IOSR.STATISTICS.BOXPLOT.

%   Copyright 2016 University of Surrey.

    %% determine X and Y

    IXn = cellfun(@(x) isnumeric(x) & ~isempty(x),varargin);

    switch sum(IXn)
        case 0
            error('iosr:qqPlot:noData','No input data specified')
        case 1
            % compare to normal distrbution
            Y = get_input_sample(varargin,IXn);
            p = (.5:length(Y))/length(Y);
            X = sqrt(2)*erfinv(2*p - 1);
            x_label = 'Standard normal quantiles';
            y_label = 'Sample quantiles';
        case 2
            % compare to input data distribution
            Y = get_input_sample(varargin,find(IXn,1,'last'));
            X = get_input_sample(varargin,find(IXn,1,'first'));
            assert(isequal(size(X),size(Y)), 'iosr:quantile:invalidInput', 'Input data must be the same size')
            x_label = 'X quantiles';
            y_label = 'Y quantiles';
        otherwise
            error('iosr:qqPlot:unkonwnInput','Unknown input specified')
    end

    %% determine mode and method

    % find inputs
    IXc = cellfun(@(x) ischar(x) | isempty(x),varargin);
    switch sum(IXc)
        case 0
            mode = [];
            method = [];
        case 1
            mode = varargin{IXc};
            method = [];
        case 2
            mode = varargin{find(IXc,1,'first')};
            method = varargin{find(IXc,1,'last')};
        otherwise
            error('iosr:qqPlot:unknownString','Unknown string specified')
    end

    % defaults
    if isempty(mode)
        mode = 'line';
    end
    if isempty(method)
        method = 'R-8';
    end

    %% calculate fit to first and third quartile

    % quartiles
    q1x = iosr.statistics.quantile(X,.25,[],method);
    q3x = iosr.statistics.quantile(X,.75,[],method);
    q1y = iosr.statistics.quantile(Y,.25,[],method);
    q3y = iosr.statistics.quantile(Y,.75,[],method);

    % slope
    slope = (q3y-q1y)./(q3x-q1x);
    centerx = (q1x+q3x)/2;
    centery = (q1y+q3y)/2;

    % fit
    maxx = max(X);
    minx = min(X);
    maxy = centery + slope.*(maxx - centerx);
    miny = centery - slope.*(centerx - minx);

    % lines
    X_fit = linspace(minx,maxx,length(X));
    Y_fit = linspace(miny,maxy,length(X));

    %% plot data

    hold on

    if strcmpi(mode,'patch') || strcmpi(mode,'both')
        Hp = patch([X fliplr(X_fit)],[Y fliplr(Y_fit)],[0.5 0.5 0.5]);
        set(Hp,'edgecolor','none')
    else
        Hp = NaN;
    end

    switch lower(mode)
        case 'line'
            linestyle = 'xk';
        case 'patch'
            linestyle = '-k';
        case 'both'
            linestyle = 'xk';
        otherwise
            error('iosr:qqPlot:unknownMode','Unknown mode specified')
    end

    H = plot(X,Y,linestyle,X_fit,Y_fit,'--k');

    hold off

    % axis labels
    xlabel(x_label)
    ylabel(y_label)

    box on; axis tight
    set(gca,'layer','top')

    % return handle
    if nargout>0
        h = H;
        if isobject(Hp) || ishandle(Hp)
            h = [h; Hp];
        end
    end

end

function Z = get_input_sample(z,IX)
%GET_INPUT_SAMPLE get sample, order, and convert to vector
    Z = z{IX};
    Z = sort(Z(:))';
end


function [tm,D2]=momftfr(tfr,tmin,tmax,time);
%MOMFTFR Frequency moments of a time-frequency representation.
%	[TM,D2]=MOMFTFR(TFR,TMIN,TMAX,TIME) computes the frequeny
%	moments of a time-frequency representation.
%
%	TFR    : time-frequency representation ([Nrow,Ncol]size(TFR)).
%	TMIN   : smallest column element of TFR taken into account
%	                            (default : 1)
%	TMAX   : highest column element of TFR taken into account
%	                            (default : Ncol)
%	TIME   : true time instants (default : 1:Ncol)
%	TM     : averaged time          (first order moment)
%	D2     : squared time duration  (second order moment)
%
%	Example :
%	 sig=fmlin(128,0.1,0.4);
%	 [tfr,t,f]=tfrwv(sig); [tm,D2]=momftfr(tfr);
%	 subplot(211); plot(f,tm); subplot(212); plot(f,D2);
%
%	See also MOMTTFR, MARGTFR.

%	F. Auger, August 1995.
%	Copyright (c) 1996 by CNRS (France).
%
%  This program is free software; you can redistribute it and/or modify
%  it under the terms of the GNU General Public License as published by
%  the Free Software Foundation; either version 2 of the License, or
%  (at your option) any later version.
%
%  This program is distributed in the hope that it will be useful,
%  but WITHOUT ANY WARRANTY; without even the implied warranty of
%  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%  GNU General Public License for more details.
%
%  You should have received a copy of the GNU General Public License
%  along with this program; if not, write to the Free Software
%  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

[tfrrow,tfrcol]=size(tfr);
if (nargin==1),
 tmin=1; tmax=tfrcol; time=tmin:tmax;
elseif (nargin==2),
 tmax=tfrcol; time=tmin:tmax;
elseif (nargin==3),
 time=tmin:tmax;
end;

if (tmin>tmax)|(tmin<=0)|(tmax>tfrcol),
 error('1<=TMIN<=TMAX<=Ncol');
end;

E  = sum(tfr(:,tmin:tmax).');
tm = (time    * tfr(:,tmin:tmax).' ./E).';
D2 = (time.^2 * tfr(:,tmin:tmax).' ./E).' - tm.^2;


function d = distToEpipolarLine(F, p1, p2)
%% Given a fundamental matrix F, compute the matched point pair to their
% corresponding epipolarlines' distances, then sum the two distances.
% Extracted from Matlab built-in function estimateFundamentalMatrix.m

% Very important: F must map a point p1 to an epipolar line in p2 image space, not the other way around.
% More details: http://stackoverflow.com/questions/26582960/sampson-error-for-five-point-essential-matrix-estimation

%% License
% ACADEMIC OR NON-PROFIT ORGANIZATION NONCOMMERCIAL RESEARCH USE ONLY
% Copyright (c) 2018 Bingyao Huang
% All rights reserved.

% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:

% The above copyright notice and this permission notice shall be included in all
% copies or substantial portions of the Software.

% If you publish results obtained using this software, please cite our paper.

% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
% FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
% AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
% LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
% OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
% SOFTWARE.

% convert to homogenious
p1 = [p1, ones(size(p1,1),1)]';
p2 = [p2, ones(size(p2,1),1)]';

% euclidean distance
d = sum(p2.*(F*p1), 1) .^ 2;

% sampson distance
epl1 = F * p1;
epl2 = F' * p2;
d = d ./ (epl1(1,:).^2 + epl1(2,:).^2 + epl2(1,:).^2 + epl2(2,:).^2);
end