function [rho,theta,R]=findTrigMomentFromSamp(n,x,w)
%%FINDTRIGMOMENTFROMSAMPLE Given a set of samples of a circular
%          distribution, determine a particular trigonometric sample
%          moment.
%
%INPUTS: n The order of the moment desired. This is >=1.
%        x The 1XN or NX1 vector of possibly weighted samples.
%        w The NX1 weights associated with the samples. If this parameter
%          is omitted or an empty matrix is passed, then the samples are
%          uniformly weighted.
%
%OUTPUTS: rho The (complex) mean resultant value for the nth moment. Note
%             that rho=R*exp(1j*theta).
%       theta The (real) trigonometric mean angle in radians for the nth
%             moment. This is between -pi and pi.
%           R The (real) mean resultant length for the nth moment.
%
%An expression for the complex trigonometric moments is given in Equation
%14 of [1]. This is just a weighted version of the definition in Chapter
%2.4 of [2].
%
%REFERENCES:
%[1] G. Kurz, I. Gilitschenski, R. Y. Siegwart, and U. D. Hanebeck,
%    "Methods for deterministic approximation of circular densities,"
%    Journal of Advances in Information Fusion, vol. 11, no. 2, pp.
%    138-156, Dec. 2016.
%[2] K. V. Mardia and P. E. Jupp, Directional Statistics. Chichester: John
%    Wiley and Sons, 2000.
%
%April 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

N=length(x);
if(nargin<3||isempty(w))
    w=ones(N,1);
end

%Normalize.
w=w/sum(w);

rho=sum(exp(1i*n*x(:)).*w(:));
if(nargout>1)
    theta=angle(rho);
    R=abs(rho);
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 exactness_test02 ( )

%*****************************************************************************80
%
%% EXACTNESS_TEST02 tests Fejer Type 2 rules for Legendre integrals.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 May 2014
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'EXACTNESS_TEST02\n' );
  fprintf ( 1, '  Test Fejer Type 2 rules on Legendre integrals.\n' );
  fprintf ( 1, '  Density function rho(x) = 1.\n' );
  fprintf ( 1, '  Region: -1 <= x <= +1.\n' );
  fprintf ( 1, '  Exactness: N   for N odd,\n' );
  fprintf ( 1, '             N-1 for N even.\n' );

  for n = 1 : 5

    [ x, w ] = fejer2_set ( n );
    if ( mod ( n, 2 ) == 1 )
      p_max = n + 1;
    else
      p_max = n;
    end
    legendre_exactness ( n, x, w, p_max );

  end

  return
end


function f = fxy5 ( n, x, y )

%*****************************************************************************80
%
%% FXY5 is the fourth 2D example.
%
%  Discussion:
%
%    This is example 3.1 in the reference.
%
%    It is known as the discontinuous medium wave function.
%
%    Here, we are computing the second component of the solution, U(X,Y).
%
%    It should be plotted on (x,y) in [-1,0]x[0,0.1].
%
%    The second variable y actually represents time.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 September 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Rick Archibald, Anne Gelb, Jungho Yoon,
%    Determining the locations and discontinuities in the derivatives
%    of functions,
%    Applied Numerical Mathematics,
%    Volume 58, 2008, pages 577-592.
%
%  Parameters:
%
%    Input, integer N, the number of points.
%
%    Input, real X(N), Y(N), the arguments.
%
%    Output, real F(N,1), the function values.
%

%
%  Destroy all row vectors!
%
  x = x ( : );
  y = y ( : );

  cl = 0.87879;
  cr = 1.0;
  omega = 12.0;
  rhol = 0.55556;
  rhor = 1.0;

  f = zeros ( n, 1 );

  i = find ( x(1:n) <= -0.5 );
  j = find ( -0.5 < x(1:n) );

  f(i) = sin ( pi * omega * ( y(i) - ( x(i) + 0.5 ) / cl ) ) ...
    + ( rhol * cl - rhor * cr ) / ( rhol * cl + rhor * cr ) ...
    / ( rhol * cl ) ...
    * sin ( pi * omega * ( y(i) + ( x(i) + 0.5 ) / cl ) );

  f(j) = 2.0 / ( rhol * cl + rhor * cr ) ...
    * sin ( pi * omega * ( y(j) - ( x(j) + 0.5 ) / cl ) );

  return
end


function [ l, u ] = l1nn_lu ( n, h )

%*****************************************************************************80
%
%% L1NN_LU computes the LU factors of the 1D NN Laplacian.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 November 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of points.
%    N must be at least 3.
%
%    Input, real H, the spacing between points.
%
%    Output, real L(N,N), U(N,n), the LU factors.
%
  if ( n < 3 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'L1NN_LU - Fatal error!\n' );
    fprintf ( 1, '  N < 3.\n' );
    error ( 'L1NN_LU - Fatal error!' );
  end

  l = zeros ( n, n );

  for i = 1 : n
    l(i,i) = 1.0;
  end

  for i = 2 : n
    l(i,i-1) = - 1.0;
  end

  u = zeros ( n, n );

  for i = 1 : n - 1
    u(i,i) = 1.0;
  end
  u(n,n) = 0.0;

  for i = 1 : n - 1
    u(i,i+1) = - 1.0;
  end

  u(1:n,1:n) = u(1:n,1:n) / h / h;

  return
end


function [X maxdot] = packing_on_the_sphere(d, n, epsilon, X0)
% Return a set of points spread out on the sphere.
%
% function [X maxdot] = packing_on_the_sphere(d, n, epsilon, X0)
%
% Using optimization on the oblique manifold, that is, the product of
% spheres, this function returns a set of n points with unit norm in R^d in
% the form of a matrix X of size nxd, such that the points are spread out
% on the sphere. Ideally, we would minimize the maximum inner product
% between any two points X(i, :) and X(j, :), i~=j, but that is a nonsmooth
% cost function. Instead, we replace the max function by a classical
% log-sum-exp approximation and (attempt to) solve:
%
% min_{X in OB(d, n)} log( .5*sum_{i~=j} exp( xi'*xj/epsilon ) ),
%
% with xi = X(:, i) and epsilon is some "diffusion constant". As epsilon
% goes to zero, the cost function is a sharper approximation of the max
% function (under some assumptions), but the cost function becomes stiffer
% and hence harder to optimize.
%
% The second output, maxdot, is the maximum inner product between any two
% points in the returned X. This number is the one we truly are trying to
% minimize.
%
% Notice that this cost function is invariant under rotation of X:
% f(X) = f(XQ) for all orthogonal Q in O(d).
% This calls for optimization over the set of symmetric positive
% semidefinite matrices of size n and rank d with unit diagonal, which can
% be thought of as the quotient of the oblique manifold OB(d, n) by O(d):
% See elliptopefactory.
%
% This is known as the Thomson or, more specifically, the Tammes problem:
% http://en.wikipedia.org/wiki/Tammes_problem
% An interesting page by Neil Sloane collecting best known packings is
% available here http://neilsloane.com/packings/

% This file is part of Manopt and is copyrighted. See the license file.
%
% Main author: Nicolas Boumal, July 2, 2013
% Contributors:
%
% Change log:
%   Aug. 14, 2013 (NB) : Code now compatible to experiment with both the
%                        obliquefactory and the elliptopefactory.
%
%   Jan.  7, 2014 (NB) : Added reference to Neil Sloane's page and the
%                        maxdot output.
%
%   June 24, 2014 (NB) : Now shifting exponentials to alleviate numerical
%                        trouble when epsilon is too small.
%

    if ~exist('d', 'var') || isempty(d)
        % Dimension of the embedding space: R^d
        d = 3;
    end
    if ~exist('n', 'var') || isempty(n)
        % Number n of points to place of the sphere in R^d.
        % For example, n=12 yields an icosahedron:
        % https://en.wikipedia.org/wiki/Icosahedron
        % Notice though that platonic solids are not always optimal.
        % Try for example n = 8: you don't get a cube.
        n = 24;
    end
    if ~exist('epsilon', 'var') || isempty(epsilon)
        % This value should be as close to 0 as affordable.
        % If it is too close to zero, optimization first becomes much
        % slower, than simply doesn't work anymore becomes of floating
        % point overflow errors (NaN's and Inf's start to appear).
        % If it is too large, then log-sum-exp is a poor approximation of
        % the max function, and the spread will be less uniform.
        % An okay value seems to be 0.01 or 0.001 for example. Note that a
        % better strategy than using a small epsilon straightaway is to
        % reduce epsilon bit by bit and to warm-start subsequent
        % optimization in that way. Trustregions will be more appropriate
        % for these fine tunings.
        epsilon = 0.0015;
    end

    % Pick your manifold (the elliptope factory quotients out the global
    % rotation invariance of the problem, which is more natural but
    % conceptually a bit more complicated --- for usage with the toolbox it
    % is the same though: just uncomment the appropriate line).
    manifold = obliquefactory(d, n, true);
    % manifold = elliptopefactory(n, d);

    % Generate a random initial guess if none was given.
    if ~exist('X0', 'var') || isempty(X0)
        X0 = manifold.rand();
    end

    % Define the cost function with caching system used: the store
    % structure we receive as input is tied to the input point X. Everytime
    % this cost function is called at this point X, we will receive the
    % same store structure back. We may modify the store structure inside
    % the function and return it: the changes will be remembered for next
    % time.
    function [f store] = cost(X, store)
        if ~isfield(store, 'ready')
            XXt = X*X';
            % Shift the exponentials by the maximum value to reduce
            % numerical trouble due to possible overflows.
            s = max(max(triu(XXt, 1)));
            expXXt = exp((XXt-s)/epsilon);
            % Zero out the diagonal
            expXXt(1:(n+1):end) = 0;
            u = sum(sum(triu(expXXt, 1)));
            store.XXt = XXt;
            store.s = s;
            store.expXXt = expXXt;
            store.u = u;
            store.ready = true;
        end
        u = store.u;
        s = store.s;
        f = s + epsilon*log(u);
    end

    % Define the gradient of the cost. When the gradient is called at a
    % point X for which the cost was already called, the store structure we
    % receive remember everything that the cost function stored in it, so
    % we can reuse previously computed elements.
    function [g store] = grad(X, store)
        if ~isfield(store, 'ready')
            [~, store] = cost(X, store);
        end
        % Compute the Euclidean gradient
        eg = store.expXXt*X / store.u;
        % Convert to the Riemannian gradient (by projection)
        g = manifold.egrad2rgrad(X, eg);
    end

    % Setup the problem structure with its manifold M and cost+grad
    % functions.
    problem.M = manifold;
    problem.cost = @cost;
    problem.grad = @grad;

    % For debugging, it's always nice to check the gradient a few times.
    % checkgradient(problem);
    % pause;

    % Call a solver on our problem with a few options defined. We did not
    % specify the Hessian but it is still okay to call trustregion: Manopt
    % will approximate the Hessian with finite differences of the gradient.
    opts.tolgradnorm = 1e-8;
    opts.maxtime = 1200;
    opts.maxiter = 1e5;
    % X = trustregions(problem, X0, opts);
    X = conjugategradient(problem, X0, opts);

    % Evaluate the maximum inner product between any two points of X.
    XXt = X*X';
    dots = XXt(find(triu(ones(n), 1))); %#ok<FNDSB>
    maxdot = max(dots);

    % Similarly, even though we did not specify the Hessian, we may still
    % estimate its spectrum at the solution. It should reflect the
    % invariance of the cost function under a global rotatioon of the
    % sphere, which is an invariance under the group O(d) of dimension
    % d(d-1)/2 : this translates into d(d-1)/2 zero eigenvalues in the
    % spectrum of the Hessian.
    % The approximate Hessian is not a linear operator, and is it a
    % fortiori not symmetric. The result of this computation is thus not
    % reliable. It does display the zero eigenvalues as expected though.
    if manifold.dim() < 300
        evs = real(hessianspectrum(problem, X));
        figure;
        stem(1:length(evs), sort(evs), '.');
        title(['Eigenvalues of the approximate Hessian of the cost ' ...
               'function at the solution']);
    end

    % Show how the inner products X(:, i)'*X(:, j) are distributed.
    figure;
    hist(real(acos(dots)), 20);
    title('Histogram of the geodesic distances');

    % This is the quantity we actually want to minimize.
    fprintf('Maximum inner product between two points: %g\n', maxdot);

    % Give some visualization if the dimension allows
    if d == 2
        % For the circle, the optimal solution consists in spreading the
        % points with angles uniformly sampled in (0, 2pi). This
        % corresponds to the following value for the max inner product:
        fprintf('Optimal value for the max inner product: %g\n', cos(2*pi/n));
        figure;
        t = linspace(-pi, pi, 201);
        plot(cos(t), sin(t), '-', 'LineWidth', 3, 'Color', [152,186,220]/255);
        daspect([1 1 1]);
        box off;
        axis off;
        hold on;
        plot(X(:, 1), X(:, 2), 'r.', 'MarkerSize', 25);
        hold off;
    end
    if d == 3
        figure;
        % Plot the sphere
        [sphere_x sphere_y sphere_z] = sphere(50);
        handle = surf(sphere_x, sphere_y, sphere_z);
        set(handle, 'FaceColor', [152,186,220]/255);
        set(handle, 'FaceAlpha', .5);
        set(handle, 'EdgeColor', [152,186,220]/255);
        set(handle, 'EdgeAlpha', .5);
        daspect([1 1 1]);
        box off;
        axis off;
        hold on;
        % Add the chosen points
        Y = 1.02*X';
        plot3(Y(1, :), Y(2, :), Y(3, :), 'r.', 'MarkerSize', 25);
        % And connect the points which are at minimal distance,
        % within some tolerance.
        min_distance = real(acos(maxdot));
        connected = real(acos(XXt)) <= 1.20*min_distance;
        [Ic Jc] = find(triu(connected, 1));
        for k = 1 : length(Ic)
            i = Ic(k); j = Jc(k);
            plot3(Y(1, [i j]), Y(2, [i j]), Y(3, [i j]), 'k-');
        end
        hold off;
    end

end


function [ x, w ] = cce ( l )

%*****************************************************************************80
%
%% CCE computes a Clenshaw Curtis Exponential quadrature rule based on level.
%
%  Discussion:
%
%    Our convention is that the abscissas are numbered from left to right.
%
%    The rule is defined on [0,1].
%
%    The integral to approximate:
%
%      Integral ( 0 <= X <= 1 ) F(X) dX
%
%    The quadrature rule:
%
%      Sum ( 1 <= I <= N ) W(I) * F ( X(I) )
%
%    The input value of L selects the size of the rule as follows:
%    L = 1, N = 1;
%    1 < L, N = 2^(L-1)+1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 February 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer L, the level of the rule.
%    1 <= L.
%
%    Output, real X(N,1), the abscissas.
%
%    Output, real W(N,1), the weights.
%
  if ( l < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'CCE - Fatal error!\n' );
    fprintf ( 1, '  Illegal value of L = %d\n', l );
    error ( 'CCE - Fatal error!' );
  end
%
%  Find the value of N according to the level.
%
  n = cce_order ( l );
%
%  Compute the points and weights.
%
  [ x, w ] = cc ( n );

  return
end


%IMMOMENTS PRTools routine for computing central moments of object images
%
%	  M = IMMOMENTS(A,TYPE,MOMENTS)
%
% INPUT
%   A        Dataset with object images dataset
%   TYPE     Desired type of moments
%   MOMENTS  Desired moments
%
% OUTPUT
%   M        Dataset with moments replacing images
%
% DESCRIPTION
% Computes for the image A a (1*N) vector M moments as defined by TYPE
% and MOMENTS. The following types are supported:
%
% TYPE = 'none'     Standard moments as specified in the Nx2 array MOMENTS.
%                   Moments are computed with respect to the image center.
%                   This is the default for TYPE.
%                   Default MOMENTS = [1 0; 0 1];
% TYPE = 'central'  Central moments as specified in the Nx2 array MOMENTS.
%                   Moments are computed with respect to the image mean
%                   Default MOMENTS = [2 0; 1 1; 0 2], which computes
%                   the variance in the x-direction (horizontal), the
%                   covariance between x and y and the variance in the
%                   y-direction (vertical).
% TYPE = 'scaled'   Scale-invariant moments as specified in the Nx2 array
%                   MOMENTS. Default MOMENTS = [2 0; 1 1; 0 2].
%                   After: M. Sonka et al.,
%                   Image processing, analysis and machine vision.
% TYPE = 'hu'       Calculates 7 moments of Hu, invariant to translation,
%                   rotation and scale.
%                   After: M. Sonka et al.,
%                   Image processing, analysis and machine vision.
% TYPE = 'zer'      Calculates the Zernike moments up to the order as
%                   specified in the scalar MOMENTS (1 <= MOMENTS <= 12).
%                   MOMENTS = 12 generates in total 47 moments.
%                   After: A. Khotanzad and Y.H. Hong, Invariant image
%                   recognition by Zernike moments, IEEE-PAMI, vol. 12,
%                   no. 5, 1990, 489-497.
%
% See DATASETS

% Copyright: D. de Ridder, R.P.W. Duin, r.p.w.duin@37steps.com
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

function b = immoments(a,type,mom)

	if nargin < 3, mom = []; end
	if nargin < 2 | isempty(type), type = 'none'; end
	isobjim(a);
	[m,k] = size(a);
	im = data2im(a);
	for i = 1:m
		imi = im(:,:,i);
		switch type
		case {'none'}
			if isempty(mom)
				mom = [1 0; 0 1];
			end
			mout = moments(imi,mom(:,1),mom(:,2),0,0);
		case {'central'}
			if isempty(mom)
				mom = [2 0; 1 1; 0 2];
			end
			mout = moments(imi,mom(:,1),mom(:,2),1,0);
		case {'scaled'}
			if isempty(mom)
				mom = [2 0; 1 1; 0 2];
			end
			mout = moments(imi,mom(:,1)',mom(:,2)',1,1);
		case {'hu' 'Hu'}
			mout = hu_moments(imi);
		case {'zer' 'zernike' 'Zernike'}
			if isempty(mom)
				mom = 12;
			end
			mout = zernike_moments(imi,mom);
		otherwise
			error('Moments should be of type none, central, scaled, hu or zer')
		end
		if i==1
			b = zeros(m,length(mout));
		end
		b(i,:) = mout;
	end
	b = setdata(a,b);

% M = MOMENTS (IM, P, Q, CENTRAL, SCALED)
%
% Calculates moments of order (P+Q) (can be arrays of indentical length)
% on image IM. If CENTRAL is set to 1 (default: 0), returns translation-
% invariant moments; if SCALED is set to 1 (default: 0), returns scale-
% invariant moments.
%
% After: M. Sonka et al., Image processing, analysis and machine vision.

function m = moments (im,p,q,central,scaled)

	if (nargin < 5), scaled = 0; 	end;
	if (nargin < 4), central = 0; end;
  if (nargin < 3)
  	error ('Insufficient number of parameters.');
  end;

	if (length(p) ~= length(q))
  	error ('Arrays P and Q should have equal length.');
  end;

  if (scaled & ~central)
  	error ('Scale-invariant moments should always be central.');
  end;

  % xx, yy are grids with co-ordinates
  [xs,ys] = size(im);
  [xx,yy] = meshgrid(-(ys-1)/2:1:(ys-1)/2,-(xs-1)/2:1:(xs-1)/2);

	if (central)

  	% Calculate zeroth and first order moments
	  m00 = sum(sum(im));
	  m10 = sum(sum(im.*xx));
	  m01 = sum(sum(im.*yy));

    % This gives the center of gravity
    xc  = m10/m00;
    yc  = m01/m00;

    % Subtract this from the grids to center the object
    xx  = xx - xc;
    yy  = yy - yc;

  end;

  % Calculate moment(s) (p,q).
  for i = 1:length(p)
		m(i) = sum(sum((xx.^p(i)).*(yy.^q(i)).*im));
  end;

  if (scaled)

  	c = 1 + (p+q)/2;

    % m00 should be known, as scaled moments are always central
    m = m ./ (m00.^c);

	end;

return;

% M = HU_MOMENTS (IM)
%
% Calculates 7 moments of Hu on image IM, invariant to translation,
% rotation and scale.
%
% After: M. Sonka et al., Image processing, analysis and machine vision.

function m = hu_moments (im)

	p = [ 1 0 2 1 2 0 3 ];
	q = [ 1 2 0 2 1 3 0 ];

  n = moments(im,p,q,1,1);

  m(1) = n(2) + n(3);
  m(2) = (n(3) - n(2))^2   + 4*n(1)^2;
  m(3) = (n(7) - 3*n(4))^2 + (3*n(5) - n(6))^2;
  m(4) = (n(7) +   n(4))^2 + (  n(5) + n(6))^2;
  m(5) = (  n(7) - 3*n(4)) * (n(7) + n(4)) * ...
           (  (n(7) + n(4))^2 - 3*(n(5) + n(6))^2) + ...
         (3*n(5) -   n(6)) * (n(5) + n(6)) * ...
           (3*(n(7) + n(4))^2 -   (n(5) + n(6))^2);
  m(6) = (n(3) - n(2)) * ((n(7) + n(4))^2 - (n(5) + n(6))^2) + ...
          4*n(1) * (n(7)+n(4)) * (n(5)+n(6));
  m(7) = (3*n(5) -   n(6)) * (n(7) + n(4)) * ...
           (  (n(7) + n(4))^2 - 3*(n(5) + n(6))^2) - ...
         (  n(7) - 3*n(4)) * (n(5) + n(6)) * ...
           (3*(n(7) + n(4))^2 -   (n(5) + n(6))^2);

return;

% M = ZERNIKE_MOMENTS (IM, ORDER)
%
% Calculates Zernike moments up to and including ORDER (<= 12) on image IM.
% Default: ORDER = 12.

function m = zernike_moments (im, order)

  if (nargin < 2),             order = 12;                      end;
  if (order < 1 | order > 12), error ('order should be 1..12'); end;

  % xx, yy are grids with co-ordinates

  [xs,ys] = size(im);
  [xx,yy] = meshgrid(-(ys-1)/2:1:(ys-1)/2,-(xs-1)/2:1:(xs-1)/2);

  % Calculate center of mass and distance of any pixel to it

  m  = moments (im,[0 1 0],[0 0 1],0,0);
  xc = m(2)/m(1); yc = m(3)/m(1);
  xx = xx - xc; yy = yy - yc;

  len     = sqrt(xx.^2+yy.^2);
  max_len = max(max(len));

  % Map pixels to unit circle; prevent divide by zero.

  rho        = len/max_len;
  rho_tmp    = rho; rho_tmp(find(rho==0)) = 1;
  theta      = acos((xx/max_len)./rho_tmp);

  % Flip angle for pixels above center of mass

  yneg            = length(find(yy(:,1)<0));
  theta(:,1:yneg) = 2*pi - theta(:,1:yneg);

  % Calculate coefficients

  c = zeros(order,order);
  s = zeros(order,order);

  i = 1;
  for n = 2:order
    for l = n:-2:0
      r    = polynomial (n,l,rho);
      c    = sum(sum(r.*cos(l*theta)))*((n+1)/(pi*max_len^2));
      s    = sum(sum(r.*sin(l*theta)))*((n+1)/(pi*max_len^2));
      m(i) = sqrt(c^2+s^2);
      i    = i + 1;
    end;
  end;

return

function p = polynomial (n,l,rho)

  switch (n)
    case 2, switch (l)
        case 0, p = 2*(rho.^2)-1;
        case 2, p =   (rho.^2);
      end;
    case 3, switch (l)
        case 1, p = 3*(rho.^3)-2*rho;
        case 3, p =   (rho.^3);
      end;
    case 4, switch (l)
        case 0, p = 6*(rho.^4)-6*(rho.^2)+1;
        case 2, p = 4*(rho.^4)-3*(rho.^2);
        case 4, p =   (rho.^4);
      end;
    case 5, switch (l)
        case 1, p = 10*(rho.^5)-12*(rho.^3)+3*rho;
        case 3, p =  5*(rho.^5)- 4*(rho.^3);
        case 5, p =    (rho.^5);
      end;
    case 6, switch (l)
        case 0, p = 20*(rho.^6)-30*(rho.^4)+12*(rho.^2)-1;
        case 2, p = 15*(rho.^6)-20*(rho.^4)+ 6*(rho.^2);
        case 4, p =  6*(rho.^6)- 5*(rho.^4);
        case 6, p =    (rho.^6);
      end;
    case 7, switch (l)
        case 1, p = 35*(rho.^7)-60*(rho.^5)+30*(rho.^3)-4*rho;
        case 3, p = 21*(rho.^7)-30*(rho.^5)+10*(rho.^3);
        case 5, p =  7*(rho.^7)- 6*(rho.^5);
        case 7, p =    (rho.^7);
      end;
    case 8, switch (l)
        case 0, p = 70*(rho.^8)-140*(rho.^6)+90*(rho.^4)-20*(rho.^2)+1;
        case 2, p = 56*(rho.^8)-105*(rho.^6)+60*(rho.^4)-10*(rho.^2);
        case 4, p = 28*(rho.^8)- 42*(rho.^6)+15*(rho.^4);
        case 6, p =  8*(rho.^8)-  7*(rho.^6);
        case 8, p =    (rho.^8);
      end;
    case 9, switch (l)
        case 1, p = 126*(rho.^9)-280*(rho.^7)+210*(rho.^5)-60*(rho.^3)+5*rho;
        case 3, p =  84*(rho.^9)-168*(rho.^7)+105*(rho.^5)-20*(rho.^3);
        case 5, p =  36*(rho.^9)- 56*(rho.^7)+ 21*(rho.^5);
        case 7, p =   9*(rho.^9)-  8*(rho.^7);
        case 9, p =     (rho.^9);
      end;
    case 10, switch (l)
        case  0, p = 252*(rho.^10)-630*(rho.^8)+560*(rho.^6)-210*(rho.^4)+30*(rho.^2)-1;
        case  2, p = 210*(rho.^10)-504*(rho.^8)+420*(rho.^6)-140*(rho.^4)+15*(rho.^2);
        case  4, p = 129*(rho.^10)-252*(rho.^8)+168*(rho.^6)- 35*(rho.^4);
        case  6, p =  45*(rho.^10)- 72*(rho.^8)+ 28*(rho.^6);
        case  8, p =  10*(rho.^10)-  9*(rho.^8);
        case 10, p =     (rho.^10);
      end;
    case 11, switch (l)
        case  1, p = 462*(rho.^11)-1260*(rho.^9)+1260*(rho.^7)-560*(rho.^5)+105*(rho.^3)-6*rho;
        case  3, p = 330*(rho.^11)- 840*(rho.^9)+ 756*(rho.^7)-280*(rho.^5)+ 35*(rho.^3);
        case  5, p = 165*(rho.^11)- 360*(rho.^9)+ 252*(rho.^7)- 56*(rho.^5);
        case  7, p =  55*(rho.^11)-  90*(rho.^9)+  36*(rho.^7);
        case  9, p =  11*(rho.^11)-  10*(rho.^9);
        case 11, p =     (rho.^11);
      end;
    case 12, switch (l)
        case  0, p = 924*(rho.^12)-2772*(rho.^10)+3150*(rho.^8)-1680*(rho.^6)+420*(rho.^4)-42*(rho.^2)+1;
        case  2, p = 792*(rho.^12)-2310*(rho.^10)+2520*(rho.^8)-1260*(rho.^6)+280*(rho.^4)-21*(rho.^2);
        case  4, p = 495*(rho.^12)-1320*(rho.^10)+1260*(rho.^8)- 504*(rho.^6)+ 70*(rho.^4);
        case  6, p = 220*(rho.^12)- 495*(rho.^10)+ 360*(rho.^8)-  84*(rho.^6);
        case  8, p =  66*(rho.^12)- 110*(rho.^10)+  45*(rho.^8);
        case 10, p =  12*(rho.^12)-  11*(rho.^10);
        case 12, p =     (rho.^12);
      end;
  end;

return


function geometry_test033 ( )

%*****************************************************************************80
%
%% TEST033 tests LINE_EXP_PERP_2D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 July 2009
%
%  Author:
%
%    John Burkardt
%
  dim_num = 2;
  ntest = 3;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST033\n' );
  fprintf ( 1, '  LINE_EXP_PERP_2D is given an explicit line (P1,P2),\n' );
  fprintf ( 1, '  and another point P3.  It then finds a point\n' );
  fprintf ( 1, '  P4 on (P1,P2) so that (P1,P2) is perpendicular\n' );
  fprintf ( 1, '  to (P3,P4).\n' );

  p1(1:dim_num) = [ 1.0, 3.0 ];
  p2(1:dim_num) = [ 4.0, 0.0 ];

  p3test(1:dim_num,1:ntest) = [ ...
    0.0,  0.0; ...
    5.0, -1.0; ...
    5.0,  3.0 ]';

  r8vec_print ( dim_num, p1, '  Point P1:' );
  r8vec_print ( dim_num, p2, '  Point P2:' );

  for j = 1 : ntest

    p3(1:dim_num) = p3test(1:dim_num,j);

    r8vec_print ( dim_num, p3, '  Point P3:' );

    [ p4, flag ] = line_exp_perp_2d ( p1, p2, p3 );

    r8vec_print ( dim_num, p4, '  Point P4:' );

  end

  return
end


function [uHessMat,uTriMat,Q,Z]=HessenbergTriangRed(A,B)
%%HESSENBERGTRIANGRED Perform a Hessenberg triangular reduction of the nXn
%               matrices A and B. This finds Q and Z such that Q'*A*Z= an
%               upper Hessenberg matrix and Q'*B*Z is an upper triangular
%               matrix.
%
%INPUTS: A, B Two real nXn matrices.
%
%OUTPUTS: uHessMat The upper Hessenberg matrix obtained from Q'*A*Z.
%          uTriMat The upper triangular matrix obtained with Q'*B*Z.
%
%This function implements algorithm 7.7.1 in [1].
%
%REFERENCES:
%[1] G. H. Golub and C. F. Van Loan, Matrix Computations, 4th ed.
%    Baltimore: Johns Hopkins University Press, 2013.
%
%September 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

n=size(A,1);

if(nargout>2)
    Z=eye(n,n);
    %B=Q*R. Replace B with Q'*B and A with Q'*A
    [Q,R]=qr(B);
    B=R;%Q'*B=R.
    A=Q'*A;

    for j=1:(n-2)
        for i=n:-1:(j+2)
            %Zeros A(i,j)
            [c,s]=GivensCS(A(i-1,j),A(i,j));
            rotMat=[c, s;
                   -s, c];
            A((i-1):i,j:n)=rotMat'*A((i-1):i,j:n);
            B((i-1):i,(i-1):n)=rotMat'*B((i-1):i,(i-1):n);

            Q(:,(i-1):i) = Q(:,(i-1):i)*rotMat;

            %Zeros B(i,i-1).
            [c,s]=GivensCS(-B(i,i),B(i,i-1));
            rotMat=[c, s;
                   -s, c];
            B(1:i,(i-1):i) = B(1:i,(i-1):i)*rotMat;
            A(1:n,(i-1):i) = A(1:n,(i-1):i)*rotMat;

            Z(:,(i-1):i) = Z(:,(i-1):i)*rotMat;
        end
    end
    uHessMat=A;
    uTriMat=B;
else
    %If Q and Z are not desired, don't compute them.

    %B=Q*R. Replace B with Q'*B and A with Q'*A
    [Q,R]=qr(B);
    B=R;%Q'*B=R.
    A=Q'*A;
    for j=1:(n-2)
        for i=n:-1:(j+2)
            %Zeros A(i,j)
            [c,s]=GivensCS(A(i-1,j),A(i,j));
            rotMat=[c, s;
                   -s, c];
            A((i-1):i,j:n)=rotMat'*A((i-1):i,j:n);
            B((i-1):i,(i-1):n)=rotMat'*B((i-1):i,(i-1):n);

            %Zeros B(i,i-1).
            [c,s]=GivensCS(-B(i,i),B(i,i-1));
            rotMat=[c, s;
                   -s, c];
            B(1:i,(i-1):i) = B(1:i,(i-1):i)*rotMat;
            A(1:n,(i-1):i) = A(1:n,(i-1):i)*rotMat;
        end
    end
    uHessMat=A;
    uTriMat=B;
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 [f] = betad(z)
%BETAD Dirichlet Beta function
%
%usage: f = betad(z)
%
%tested on version 5.3.1
%
%      This program calculates the Dirichlet Beta function
%      for the elements of Z using the Dirichlet deta function.
%      Z may be complex and any size.
%
%      Note: this is NOT the beta function defined by
%            Gamma(x)*Gamma(y)/Gamma(x+y)
%
%      Has zeros for z=(-odd integers),
%      and infinite number of zeros for z=1/2+i*y
%
%
%see also: Zeta, Deta, Eta, Lambda, Bern, Euler

%Paul Godfrey
%pgodfrey@conexant.com
%3-24-01

f=deta(z,2);

return


function euler = qua2euler(qin)
% qua2euler: transforms quaternion to Euler angles.
%
% INPUT
%   qin: 4x1 quaternion.
%
% OUTPUT
%   euler: 3x1 Euler angles [roll pitch yaw] (rad, rad, rad).
%
%   Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
%
%   This file is part of NaveGo, an open-source MATLAB toolbox for
%   simulation of integrated navigation systems.
%
%   NaveGo is free software: you can redistribute it and/or modify
%   it under the terms of the GNU Lesser General Public License (LGPL)
%   version 3 as published by the Free Software Foundation.000000000
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU Lesser General Public License for more details.
%
%   You should have received a copy of the GNU Lesser General Public
%   License along with this program. If not, see
%   <http://www.gnu.org/licenses/>.
%
% References:
%
%		Dr. Paolo Zoccarato's comments at
% https://github.com/rodralez/NaveGo/pull/9
%
%		Crassidis, J.L. and Junkins, J.L. (2011). Optimal Esti-
% mation of Dynamic Systems, 2nd Ed. Chapman and Hall/CRC, USA.
% Eq. 7.39, p. 458.
%
% Version: 005
% Date:    2017/12/05
% Author:  Rodrigo Gonzalez <rodralez@frm.utn.edu.ar>
% URL:     https://github.com/rodralez/navego

% Quaternion format from Crassidis' book.

DCMbn = qua2dcm(qin);

phi   = atan2( DCMbn(3,2), DCMbn(3,3) );    % roll
theta = asin (-DCMbn(3,1) );                % pitch
psi   = atan2( DCMbn(2,1), DCMbn(1,1) );    % yaw

euler = [phi theta psi];

end


function pass = test_nonlinSys1_C2(pref)
% Test 2x2 system (sin/cos). This is nonlinearification of the test
%       test_linearSystem1
%
% Asgeir Birkisson, April 2014.

if ( nargin == 0 )
    pref = cheboppref;
end

tol = 1e-10;

% Smooth domain:
d = [-pi pi];
x = chebfun('x',d);
f = [ 0*x ; 0*x ];

%% Colloc2
pref.discretization = @chebcolloc2;

A = chebop(@(x,u,v) [u - diff(v,2) + u.^2; diff(u) + sin(v)],d);
A.lbc = @(u,v) u-1;
A.rbc = @(u,v) [v-1/2; diff(v)];

u12 = mldivide(A, f, pref);
u1 = u12{1}; u2 = u12{2};

% Want to check BCs as well.
bcFunLeft = A.lbc(u1,u2);
bcFunRight = chebfun(A.rbc(u1,u2));

pass(1) = norm( chebfun(A(x, u1, u2))) < tol;
pass(2) = norm(bcFunLeft(d(1))) < tol && norm(bcFunRight(d(end))) < tol;

end


function disk_integrals_test01 ( )

%*****************************************************************************80
%
%% DISK_INTEGRALS_TEST01 uses DISK01_SAMPLE to estimate various integrals.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 January 2014
%
%  Author:
%
%    John Burkardt
%
  m = 2;
  n = 4192;
  test_num = 20;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST01\n' );
  fprintf ( 1, '  Estimate monomial integrals using Monte Carlo\n' );
  fprintf ( 1, '  over the interior of the unit disk in 2D.\n' );
%
%  Get sample points.
%
  seed = 123456789;
  [ x, seed ] = disk01_sample ( n, seed );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of sample points used is %d\n', n );
%
%  Randomly choose X,Y exponents between 0 and 8.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  If any exponent is odd, the integral is zero.\n' );
  fprintf ( 1, '  We will restrict this test to randomly chosen even exponents.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Ex  Ey     MC-Estimate           Exact      Error\n' );
  fprintf ( 1, '\n' );

  for test = 1 : test_num

    [ e, seed ] = i4vec_uniform_ab ( m, 0, 4, seed );

    e(1:m) = e(1:m) * 2;

    value = monomial_value ( m, n, e, x );

    result = disk01_area ( ) * sum ( value(1:n) ) / n;
    exact = disk01_monomial_integral ( e );
    error = abs ( result - exact );

    fprintf ( 1, '  %2d  %2d  %14.6g  %14.6g  %10.2e\n', ...
      e(1:m), result, exact, error );

  end

  return
end


clear;
clc;
close all;
format short

eul_312 = [30, 40, 50]; % [pitch(绕X轴) roll(绕Y轴)  yaw(绕Z轴)]

fprintf("按312顺序(先转Z-然后X-最后Y)旋转，其中X,Y,Z旋转角度为%.3f° %.3f° %.3f° 得到对应的坐标变换矩阵：\n", eul_312(1), eul_312(2), eul_312(3));

eul_312rad = deg2rad(eul_312);
[Cb2n_312, ~] = ch_eul2m(eul_312rad);
Cb2n_312

fprintf("将Cb2n_312转回欧拉角:\n");
[eul_312, ~]  = ch_m2eul(Cb2n_312);
eul_312 = rad2deg(eul_312);

fprintf("坐标变换矩阵转欧拉角:%.3f°(Pitch) %.3f°(Roll) %.3f°(Yaw)\n", eul_312(1), eul_312(2), eul_312(3));

eul_321 = [30, 40, 50]; % [roll(绕X轴) pitch(绕Y轴)  yaw(绕Z轴)]

fprintf("\n按321顺序(先转Z-然后X-最后Y)旋转，其中X,Y,Z旋转角度为%.3f° %.3f° %.3f° 得到对应的坐标变换矩阵：\n", eul_321(1), eul_321(2), eul_321(3));

eul_321rad = deg2rad(eul_321);
[~, Cb2n_321] = ch_eul2m(eul_321rad);
Cb2n_321

fprintf("将Cb2n_312转回欧拉角:\n");
[~, eul_321]  = ch_m2eul(Cb2n_321);
eul_321 = rad2deg(eul_321);

fprintf("坐标变换矩阵转欧拉角:%.3f°(Roll) %.3f°(Pitch) %.3f°(Yaw)\n", eul_321(1), eul_321(2), eul_321(3));


function p = sigmoid(log_odds)
% SIGMOID: Inverse of the logit function.
%          This is a one-to-one mapping from log odds to probability.
%          i.e. it maps the real line to the interval (0,1).
%
%   p = sigmoid(log_odds)

assert(nargin==1)

p = 1 ./ (1 + exp(-log_odds));


function [qp]=zn3tr(input)
% [qp]=zn3tr(input)
% Ziegler-Nichols PID controller for processes of 3rd order.
% This function computes parameters of the controller (q0, q1, q2, p1, p2).
% Controller is based on trapezoidal method of discretization.
% Transfer function of the controller is as follows:
%
%            q0 + q1*z^-1 + q2*z^-2     q0 + q1*z^-1 + q2*z^-2
% G(z^-1) = ------------------------ = ------------------------
%                  1 - z^-1              1 + p1*z^-1 + p2*z^-2
%
% where p1=-1 and p2=0.
%
% Transfer function of the controlled system is:
%
%               b1*z^-1 + b2*z^-2 + b3*z^-3
% Gs(z^-1) = ---------------------------------
%             1 + a1*z^-1 + a2*z^-2 + a3*z^-3
%
% Input: input ... input parameters
%                  input(1) ... a1
%                  input(2) ... b1
%                  input(3) ... a2
%                  input(4) ... b2
%                  input(5) ... a3
%                  input(6) ... b3
%                  input(7) ... sample time T0
% Output: qp ... controller parameters
%                qp(1) ... q0
%                qp(2) ... q1
%                qp(3) ... q2
%                qp(4) ... p1 (-1)
%                qp(5) ... p2 (0)

a1 = input(1);
b1 = input(2);
a2 = input(3);
b2 = input(4);
a3 = input(5);
b3 = input(6);
T0 = input(7);

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

Kp = 0.6*Kpu;
Ti = Tu/2;
Td = Tu/8;

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

qp=[q0; q1; q2; p1; p2];


%ANGDIFF Difference of two angles
%
% D = ANGDIFF(TH1, TH2) returns the difference between angles TH1 and TH2 on
% the circle.  The result is in the interval [-pi pi).  If TH1 is a column
% vector, and TH2 a scalar then return a column vector where TH2 is modulo
% subtracted from the corresponding elements of TH1.
%
% D = ANGDIFF(TH) returns the equivalent angle to TH in the interval [-pi pi).
%

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

function d = angdiff(th1, th2)

    if nargin < 2
% THIS IS A BAD IDEA, WHERE IS IT USED?
%         if length(th1) > 1
%             d = th1(1) - th1(2);
%         else
%             d = th1;
%         end
        d = th1;
    else
        d = th1 - th2;
    end

    d = mod(d+pi, 2*pi) - pi;

% Simplistic version of the code, easy to see what it does, but slow...
%
% for very negative angles keep adding 2pi
%     while true
%         k = find(d < -pi);
%         if isempty(k)
%             break;
%         end
%         d(k) = d(k) + 2*pi;
%     end
%
%     % for very positive angles keep subtracting 2pi
%     while true
%         k = find(d > pi);
%         if isempty(k)
%             break;
%         end
%         d(k) = d(k) - 2*pi;
%     end


function prob_test039 ( )

%*****************************************************************************80
%
%% TEST039 tests COSINE_MEAN, COSINE_SAMPLE, COSINE_VARIANCE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 April 2009
%
%  Author:
%
%    John Burkardt
%
  nsample = 1000;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST039\n' );
  fprintf ( 1, '  For the Cosine PDF:\n' );
  fprintf ( 1, '  COSINE_MEAN computes the mean;\n' );
  fprintf ( 1, '  COSINE_SAMPLE samples;\n' );
  fprintf ( 1, '  COSINE_VARIANCE computes the variance.\n' );

  a = 2.0;
  b = 1.0;

  check = cosine_check ( a, b );

  if ( ~check );
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TEST039 - Fatal error!\n' );
    fprintf ( 1, '  The parameters are not legal.\n' );
    return
  end

  mean = cosine_mean ( a, b );
  variance = cosine_variance ( a, b );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  PDF parameter A = %14f\n', a );
  fprintf ( 1, '  PDF parameter B = %14f\n', b );
  fprintf ( 1, '  PDF mean =        %14f\n', mean );
  fprintf ( 1, '  PDF variance =    %14f\n', variance );

  for i = 1 : nsample
    [ x(i), seed ] = cosine_sample ( a, b, seed );
  end

  mean = r8vec_mean ( nsample, x );
  variance = r8vec_variance ( nsample, x );
  xmax = max ( x(1:nsample) );
  xmin = min ( x(1:nsample) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Sample size =     %6d\n', nsample );
  fprintf ( 1, '  Sample mean =     %14f\n', mean );
  fprintf ( 1, '  Sample variance = %14f\n', variance );
  fprintf ( 1, '  Sample maximum =  %14f\n', xmax );
  fprintf ( 1, '  Sample minimum =  %14f\n', xmin );

  return
end


%
% y = nnormn(x,dim,p)
%
% NNORMN normalizes an array x by its p-vector norms along dimension <dim>.
%
% dim: dimension along which to calculate norm.  Default first nonsingleton
%   p: norm-type.  Default is 2.
%
% Equivalence: normc(x) == nnormn(x,1,2), normr(x) == nnormn(x,2,2)
%
% See also NORMC, NORMR, NNORM

% Created by Bill Winter December 2005
% Based on normc and normr
function x = nnormn(x,dim,p)
siz = size(x);
if nargin < 2, dim = find(siz > 1,1); end
if nargin < 3, p = 2; end
switch p
    case inf,   a = max(x,[],dim);              % max
    case -inf,  a = min(x,[],dim);              % min
    case 1,     a = sum(abs(x),dim);            % manhattan
    case 2,     a = sqrt(sum(abs(x).^2,dim));   % euclidean
    otherwise,  a = sum(abs(x).^p,dim).^(1/p);  % p-norm
end
a(a == 0) = 1;
N(1:length(siz)) = {':'};
N{dim} = ones(1,siz(dim));
x = x./a(N{:});


function [X, W, mu_X] = prewhiten(X)
%PREWHITEN Performs prewhitening of a dataset X
%
%   [X, W, mu_X] = prewhiten(X)
%
% Performs prewhitening of the dataset X. Prewhitening concentrates the main
% variance in the data in a relatively small number of dimensions, and
% removes all first-order structure from the data. In other words, after
% the prewhitening, the covariance matrix of the data is the identity
% matrix. The function returns the subtracted data mean in mu_X, and the
% applied linear mapping in W.
%
%

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

    welcome;

    % Compute and apply the ZCA mapping
    mu_X = mean(X, 1);
    X = bsxfun(@minus, X, mu_X);
    mappedX = X / sqrtm(cov(X));
    if nargout > 1
        W = X \ mappedX;
    end


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

%%begin

% A large number of Toolbox functions can operate on symbolic rather than
% numeric quantities.
%
% Consider the simple case of a rotation matrix

syms a
rotx(a)

% Or a more complex example for Euler angles

syms a b c
eul2r(a, b, c)

% Now let's consider a robot link with symbolic parameters

syms q A D alpha

L = Link('d', D, 'a', A, 'alpha', a, 'revolute');

% the link transform matrix for a joint angle q is then

L.A(q)

% Consider now a simple two-link robot, we load the model

mdl_twolink

% which has created a robot model in the workspace

twolink

% Now this is a numeric robot model, and we need to create a symbolic model

twolink_sym = twolink.sym()

% which appears very similar, however all the constants are now symbolic rather
% than numeric.

% Next define the two joint angles as symbolic variables

syms q1 q2

% and then the forward kinematics is

twolink_sym.fkine([q1, q2])


function X=RiccatiSolveC(A,B,Q,R,S,E)
%%RICCATISOLVEC Solve the general continuous-time Riccati equation.
%
%INPUTS: A An nXn matrix.
%        B An nXm matrix, where m<=n.
%        Q An nXn matrix such that Q=Q' and all eigenvalues are non-
%          negative.
%        R An optional mXm matrix such that R=R' and all eigenvalues are
%          non-negative. If omitted, eye(m,m) is used.
%        S An optional nXm matrix. If omitted, zeros(n,n) is used.
%        E An optional nXn matrix. If omitted, eye(n) is used.
%
%OUTPUTS: X The nXn nonnegative definite solution to the discrete-time
%           algebraic Ricatti equation.
%
%This function finds the nonnegative definite solution to the
%continuous-time Ricatti equation having the form
%A'*X*E+E'*X*A-(E'*X*B+S)*inv(R)*(B'*X*E+S')+Q=0
%or, if R, S, and E are omitted, the equation under consideration becomes
%A'*X+X*A-X*B*B'*X+Q=0
%The algorithm of  [1] is used. Note that this function does not work with
%problems of the form -X*B*B'*X+Q=0 due to numerical issues.
%
%The continuous-time Ricatti equation arises when solving for the  steady-
%state covariance of a continuous-time linear Kalman filter, as described
%in Chapter 9.2.3 of [2].
%
%REFERENCES:
%[1] W. F. Arnold III and A. J. Laub, "Generalized eigenproblem algorithms
%    and software for algebraic Riccati equations," Proceedings of the
%    IEEE, vol. 72, no. 12, pp. 1746-1754, Dec. 1984.
%[2] Y. Bar-Shalom, X. R. Li, and T. Kirubarajan, Estimation with
%    Applications to Tracking and Navigation. New York: John Wiley and
%    Sons, Inc, 2001.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

n=size(A,1);
m=size(B,2);

if(nargin<4)
    R=eye(m,m);
end

if(nargin<5)
    S=zeros(n,m);
end

if(nargin<6)
   E=eye(n);
end

L=[E,            zeros(n,n),zeros(n,m);
   zeros(n,n),   E',        zeros(n,m);
   zeros(m,n),   zeros(m,n),zeros(m,m)];
M=[A,   zeros(n,n),  B;
  -Q,  -A',         -S;
   S',  B',          R];

[MHat,LHat,V,U]=qz(M,L,'real');
[~,~,~,U]=ordqz(MHat,LHat,V,U,'lhp');

W=[E,           zeros(n,n),zeros(n,m);
   zeros(n,n),  eye(n),zeros(n,m)]*U;

W11=W(1:n,1:n);
W21=W((n+1):(2*n),1:n);

X=W21/W11;
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.


close all
clear all
clc
diary('results1.txt')
n=0;            %initialize iteration counter
e = 1e-6;       %epsilon

%% problem 1a
f = @(x) 2*x(1) - 3*x(2);				%f(x)
G = @(x) [2; -3];						%Gradient of f(x)
H = @(x) [0, 0; 0, 0];					%Hessian of f(x)
g = @(x) x(1)^2 + x(2)^2 - 25;			%g(x)
Dg = @(x) [2*x(1); 2*x(2)];				%gradient of g(x)
Hg = {@(x) [2, 0; 0, 2]};				% Hessian of g(x)
x0 = [-2.8; 4.2];        				% set starting value
xt = [-10/sqrt(13); 15/sqrt(13)]; 		%true value
disp('Problem 1a');
[x, itr] = newton_constrained(x0, f, g, G, H, Dg, Hg, e, xt, 1);

%% Problem 1b
f = @(x) x(1)^2 + 2*x(1)*x(2) + x(2)^2;			%f(x)
G = @(x) [2*x(1) + 2*x(2); 2*x(2) + 2*x(1)];    %Gradient of f(x)
H = @(x) [2, 2; 2, 2];                          %Hessian of f(x)
g = @(x) 3*x(1)^2 + x(2)^2 - 9;                 %g(x)
Dg = @(x) [6*x(1); 2*x(2)];                     %gradient of g(x)
Hg = {@(x) [6, 0; 0, 2]};               		% Hessian of g(x)
x0 = [-1.6; 1.6];                           % set starting value
xt = [-1.5; 1.5];                               %true value
disp('Problem 1b');
[x, itr] = newton_constrained(x0, f, g, G, H, Dg, Hg, e, xt, 1);

%% Problem 1c
f = @(x) 3*x(1)^3 + 2*x(2)^3 + x(3)^3 + x(4)^3;                                           %f(x)
G = @(x) [9*x(1)^2; 6*x(2)^2; 3*x(3)^2; 3*x(4)^2];                                        %Gradient of f(x)
H = @(x) [18*x(1), 0, 0, 0; 0, 12*x(2)^2, 0, 0; 0, 0, 6*x(3), 0; 0, 0, 0, 6*x(4)];        %Hessian of f(x)
g = @(x) [x(1)^2 + x(2)^2 + x(3)^2 + x(4)^2 - 4; x(1) + x(2) + 2*x(3) + 3*x(4) - 1];      %g(x)
Dg = @(x) [2*x(1), 1; 2*x(2), 1; 2*x(3), 2; 2*x(4), 3];                                   %gradient of g(x)
Hg = {@(x) eye(4)*2; @(x) eye(4)*0};                                                      % Hessian of g(x)
x0 =[-1.84; 0.21; 0.42; 0.59];        %set starting value                                 % set starting value
xt = [-1.84931; 0.20806; 0.41612; 0.603003];                                              %true value
disp('Problem 2a');
[x, itr] = newton_constrained(x0, f, g, G, H, Dg, Hg, e, xt, 1);
diary off


function ray = bisector(varargin)
%BISECTOR Return the bisector of two lines, or 3 points
%
%   RAY = bisector(LINE1, LINE2);
%   create the bisector of the two lines, given as [x0 y0 dx dy].
%
%   RAY = bisector(P1, P2, P3);
%   create the bisector of lines (P2 P1) and (P2 P3).
%
%   The result has the form [x0 y0 dx dy], with [x0 y0] being the origin
%   point ans [dx dy] being the direction vector, normalized to have unit
%   norm.
%
%   See also:
%   lines2d, rays2d
%
%   ---------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% created the 31/10/2003.
% Copyright 2010 INRA - Cepia Software Platform.

%   HISTORY
%   2005-07-07 add bisector of 3 points
%   2010-11-05 ode cleanup

if length(varargin)==2
    % two lines
    line1 = varargin{1};
    line2 = varargin{2};

    point = intersectLines(line1, line2);

elseif length(varargin)==3
    % three points
    p1 = varargin{1};
    p2 = varargin{2};
    p3 = varargin{3};

    line1 = createLine(p2, p1);
    line2 = createLine(p2, p3);
    point = p2;

elseif length(varargin)==1
    % three points, given in one array
    var = varargin{1};
    p1 = var(1, :);
    p2 = var(2, :);
    p3 = var(3, :);

    line1 = createLine(p2, p1);
    line2 = createLine(p2, p3);
    point = p2;
end

% compute line angles
a1 = lineAngle(line1);
a2 = lineAngle(line2);

% compute bisector angle (angle of first line + half angle between lines)
angle = mod(a1 + mod(a2-a1+2*pi, 2*pi)/2, pi*2);

% create the resulting ray
ray = [point cos(angle) sin(angle)];


%this is in separate m file example_exp.m
% function y = example_exp(x,p);
% y = exp(p(1)*(x-p(2))+p(3);

x = 1:0.1:5;
y = example_exp(x, [1 5 1]); %creates exp function
y_exp = y + 0.05*randn(size(x)); %simulates noisy data

[pbest,perror,nchi]=nonlinft('example_exp' ,x,y_exp,ones(size(x)),[2 3 0.5],[1 1 1]); %fits the data wit exponential...
figure
plot(x,y,'xb',x,y_exp,'or',x,example_exp(x,pbest),'g'); %original data without noise - blue, noisy data - red, fit - green...


% DERIVEST demo script

% This script file is designed to be used in cell mode
% from the matlab editor, or best of all, use the publish
% to HTML feature from the matlab editor. Older versions
% of matlab can copy and paste entire blocks of code into
% the Matlab command window.

% DERIVEST is property/value is driven for its arguments.
% Properties can be shortened to the

%% derivative of exp(x), at x == 0
[deriv,err] = derivest(@(x) exp(x),0)

%% DERIVEST can also use an inline function
[deriv,err] = derivest(inline('exp(x)'),0)

%% Higher order derivatives (second derivative)
% Truth: 0
[deriv,err] = derivest(@(x) sin(x),pi,'deriv',2)

%% Higher order derivatives (third derivative)
% Truth: 1
[deriv,err] = derivest(@(x) cos(x),pi/2,'der',3)

%% Higher order derivatives (up to the fourth derivative)
% Truth: sqrt(2)/2 = 0.707106781186548
[deriv,err] = derivest(@(x) sin(x),pi/4,'d',4)

%% Evaluate the indicated (default = first) derivative at multiple points
[deriv,err] = derivest(@(x) sin(x),linspace(0,2*pi,13))

%% Specify the step size (default stepsize = 0.1)
deriv = derivest(@(x) polyval(1:5,x),1,'deriv',4,'FixedStep',1)

%% Provide other parameters via an anonymous function
% At a minimizer of a function, its derivative should be
% essentially zero. So, first, find a local minima of a
% first kind bessel function of order nu.
nu = 0;
fun = @(t) besselj(nu,t);
fplot(fun,[0,10])
x0 = fminbnd(fun,0,10,optimset('TolX',1.e-15))
hold on
plot(x0,fun(x0),'ro')
hold off

deriv = derivest(fun,x0,'d',1)

%% The second derivative should be positive at a minimizer.
deriv = derivest(fun,x0,'d',2)

%% Compute the numerical gradient vector of a 2-d function
% Note: the gradient at this point should be [4 6]
fun = @(x,y) x.^2 + y.^2;
xy = [2 3];
gradvec = [derivest(@(x) fun(x,xy(2)),xy(1),'d',1), ...
           derivest(@(y) fun(xy(1),y),xy(2),'d',1)]

%% Compute the numerical Laplacian function of a 2-d function
% Note: The Laplacian of this function should be everywhere == 4
fun = @(x,y) x.^2 + y.^2;
xy = [2 3];
lapval = derivest(@(x) fun(x,xy(2)),xy(1),'d',2) + ...
           derivest(@(y) fun(xy(1),y),xy(2),'d',2)

%% Compute the derivative of a function using a central difference scheme
% Sometimes you may not want your function to be evaluated
% above or below a given point. A 'central' difference scheme will
% look in both directions equally.
[deriv,err] = derivest(@(x) sinh(x),0,'Style','central')

%% Compute the derivative of a function using a forward difference scheme
% But a forward scheme will only look above x0.
[deriv,err] = derivest(@(x) sinh(x),0,'Style','forward')

%% Compute the derivative of a function using a backward difference scheme
% And a backward scheme will only look below x0.
[deriv,err] = derivest(@(x) sinh(x),0,'Style','backward')

%% Although a central rule may put some samples in the wrong places, it may still succeed
[d,e,del]=derivest(@(x) log(x),.001,'style','central')

%% But forcing the use of a one-sided rule may be smart anyway
[d,e,del]=derivest(@(x) log(x),.001,'style','forward')

%% Control the behavior of DERIVEST - forward 2nd order method, with only 1 Romberg term
% Compute the first derivative, also return the final stepsize chosen
[deriv,err,fdelta] = derivest(@(x) tan(x),pi,'deriv',1,'Style','for','MethodOrder',2,'RombergTerms',1)

%% Functions should be vectorized for speed, but its not always easy to do.
[deriv,err] = derivest(@(x) x.^2,0:5,'deriv',1)
[deriv,err] = derivest(@(x) x^2,0:5,'deriv',1,'vectorized','no')


close all; clear all; clc;

% Create the directory for storing results
spx.fs.ensure_dir('bin');

h = spx.data.motion.Hopkins155;
% pre-load all examples
h.load_all_examples();
h.describe();
examples = h.get_2_3_motions();
ne = length(examples);
fprintf('Number of 2, 3 motions: %d\n', ne);
min_angles = zeros(1, ne);
max_angles = zeros(1, ne);
all_ranks = zeros(3, ne);
for i=1:ne
    fprintf('Example (%d): ', i);
    example = examples{i};
    fprintf('%s, %d motions, %d points, %d frames;', example.name, example.num_motions, example.num_points, example.num_frames);
    % The dataset
    X = example.X;
    % average norm
    norms1 = spx.norm.norms_l2_cw(X);
    fprintf('\n');
    %X = spx.la.affine.homogenize(X, 1);
    norms2 = spx.norm.norms_l2_cw(X);
    m1 = mean(norms1);
    m2 = mean(norms2);
    d = (m2 - m1) * 100 / m1 ;
    %fprintf('Mean norm: before: %0.2f, after: %0.2f, diff: %0.2f %%\n', m1, m2, d);
    % now compute the bases for 4 dimensional approximations
    % bases  = spx.la.svd.low_rank_bases(X, example.counts, 2);
    [bases, ranks]  = spx.la.svd.vidal_rank_bases(X, example.counts, 0.1);
    all_ranks(1:example.num_motions, i) = ranks;
    fprintf('Ranks: ');
    fprintf('%d ', ranks);
    fprintf('\n');
    % bases  = spx.la.spaces.bases(X, example.counts);
    angles = spx.la.spaces.smallest_angles_deg(bases);
    off_diag_angles = spx.matrix.off_diag_upper_tri_elements(angles);
    off_diag_angles = sort(off_diag_angles);
    minimum_angle = min(off_diag_angles);
    maximum_angle = max(off_diag_angles);
    fprintf('Angles between subspaces (degrees): \n');
    disp(angles);
    % disp(off_diag_angles');
    % fprintf('Minimum angle: %.2f degrees\n\n', minimum_angle);
    % fprintf('Maximum angle: %.2f degrees\n\n', maximum_angle);
    fprintf('\n');
    min_angles(i) = minimum_angle;
    max_angles(i) = maximum_angle;
end
fprintf ('Minimum angle: %0.2f deg\n', min(min_angles));
fprintf ('Maximum angle: %0.2f deg\n', max(max_angles));
save ('bin/principal_angles','min_angles', 'max_angles', 'all_ranks');


function geometry_test20715 ( )

%*****************************************************************************80
%
%% TEST20715 tests TRIANGLE_POINT_DIST_SIGNED_2D;
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
  ntest = 7;

  ptest = [ ...
     0.25,   0.25; ...
     0.75,   0.25; ...
     1.00,   1.00; ...
    11.00,   0.50; ...
     0.00,   1.00; ...
     0.50, -10.00; ...
     0.60,   0.60 ]';
  t = [ ...
    0.0, 1.0; ...
    0.0, 0.0; ...
    1.0, 0.0 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST20715\n' );
  fprintf ( 1, '  For a triangle in 2D,\n' );
  fprintf ( 1, '  TRIANGLE_POINT_DIST_SIGNED_2D computes signed\n' );
  fprintf ( 1, '    distance to a point;\n' );

  r8mat_transpose_print ( 2, 3, t, '  Triangle vertices:' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '       P       DIST_SIGNED\n' );
  fprintf ( 1, '\n' );

  for i = 1 : ntest

    p(1:2,1) = ptest(1:2,i);

    dist_signed = triangle_point_dist_signed_2d ( t, p );

    fprintf ( 1, '  %10f  %10f  %10f\n', p(1:2,1), dist_signed );

  end

  return
end


% HLINE - Plot 2D lines defined in homogeneous coordinates.
%
% Function for ploting 2D homogeneous lines defined by 2 points
% or a line defined by a single homogeneous vector
%
% Usage:   hline(p1,p2)   where p1 and p2 are 2D homogeneous points.
%          hline(p1,p2,'colour_name')  'black' 'red' 'white' etc
%          hline(l)       where l is a line in homogeneous coordinates
%          hline(l,'colour_name')
%

%  Peter Kovesi
%  School of Computer Science & Software Engineering
%  The University of Western Australia
%  pk @ csse uwa edu au
%  http://www.csse.uwa.edu.au/~pk
%
%  April 2000

function hline(a,b,c)

col = 'blue';  % default colour

if nargin >= 2 & isa(a,'double')  & isa(b,'double')   % Two points specified

  p1 = a./a(3);        % make sure homogeneous points lie in z=1 plane
  p2 = b./b(3);

  if nargin == 3 & isa(c,'char')  % 2 points and a colour specified
    col = c;
  end

elseif nargin >= 1 & isa(a,'double')       % A single line specified

  a = a./a(3);   % ensure line in z = 1 plane (not needed??)

  if abs(a(1)) > abs(a(2))   % line is more vertical
    ylim = get(get(gcf,'CurrentAxes'),'Ylim');
    p1 = hcross(a, [0 1 0]');
    p2 = hcross(a, [0 -1/ylim(2) 1]');
  else                       % line more horizontal
    xlim = get(get(gcf,'CurrentAxes'),'Xlim');
    p1 = hcross(a, [1 0 0]');
    p2 = hcross(a, [-1/xlim(2) 0 1]');
  end

  if nargin == 2 & isa(b,'char') % 1 line vector and a colour specified
    col = b;
  end

else
  error('Bad arguments passed to hline');
end

line([p1(1) p2(1)], [p1(2) p2(2)], 'color', col);


function gen_laguerre_rule ( order, alpha, a, b, filename )

%*****************************************************************************80
%
%% GEN_LAGUERRE_RULE generates a Gauss-Laguerre rule.
%
%  Discussion:
%
%    This program computes a standard or exponentially weighted
%    generalized Gauss-Laguerre quadrature rule and writes it to a file.
%
%    The user specifies:
%    * the ORDER (number of points) in the rule;
%    * ALPHA, the exponent of |X|;
%    * A, the left endpoint of integration;
%    * B, the scale factor in the exponential;
%    * FILENAME, the root name of the output files.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 February 2010
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'GEN_LAGUERRE_RULE\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Compute a generalized Gauss-Laguerre rule for approximating\n' );
  fprintf ( 1, '    Integral ( a <= x < oo ) |x-a|^ALPHA exp(-B*(x-a)) f(x) dx\n' );
  fprintf ( 1, '  of order ORDER.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The user specifies ORDER, ALPHA, A, B, and FILENAME.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  ORDER is the number of points.\n' );
  fprintf ( 1, '  ALPHA is the exponent of |X|.\n' );
  fprintf ( 1, '  A is the left endpoint (typically 0).\n' );
  fprintf ( 1, '  B is the exponential scale factor (typically 1).\n' );
  fprintf ( 1, '  FILENAME is used to generate 3 files:\n' );
  fprintf ( 1, '  * filename_w.txt - the weight file\n' );
  fprintf ( 1, '  * filename_x.txt - the abscissa file.\n' );
  fprintf ( 1, '  * filename_r.txt - the region file.\n' );
%
%  Initialize the parameters.
%
  beta = 0.0;
%
%  Get ORDER.
%
  if ( nargin < 1 )
    order = input ( '  Enter the rule order ORDER.' );
  elseif ( ischar ( order ) )
    order = str2num ( order );
  end
%
%  Get ALPHA.
%
  if ( nargin < 2 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  ALPHA is the exponent of |X| in the weighting function.\n' );
    fprintf ( 1, '  ALPHA is a real number strictly greater than -1.\n' );
    fprintf ( 1, '\n' );
    alpha = input ( '  Enter the value of ALPHA.' );
  elseif ( ischar ( alpha ) )
    alpha = str2num ( alpha );
  end
%
%  Get A.
%
  if ( nargin < 3 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  A is the left endpoint, typically 0.\n' );
    fprintf ( 1, '\n' );
    a = input ( '  Enter the value of A.' );
  elseif ( ischar ( a ) )
    a = str2num ( a );
  end
%
%  Get B.
%
  if (  nargin < 4 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  B is the exponential scale factor, typically 1.\n' );
    fprintf ( 1, '\n' );
    b = input ( '  Enter the value of B.' );
  elseif ( ischar ( b ) )
    b = str2num ( b );
  end
%
%  Get FILENAME:
%
  if ( nargin < 5 )
    fprintf ( 1,  '\n' );
    fprintf ( 1,  '  FILENAME specifies the ''root name'' of the quadrature files).\n' );
    filename = input ( '  Enter FILENAME as a quoted string:' );
  end
%
%  Input summary.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  ORDER = %d\n', order );
  fprintf ( 1, '  ALPHA = %f\n', alpha );
  fprintf ( 1, '  A = %f\n', a );
  fprintf ( 1, '  B = %f\n', b );
  fprintf ( 1, '  FILENAME = "%s".\n', filename );
%
%  Construct the rule.
%
  kind = 5;
  [ x, w ] = cgqf ( order, kind, alpha, beta, a, b );
%
%  Write the rule.
%
  r = zeros ( 2, 1 );
  r(1) = a;
  r(2) = r8_huge ( );
  rule_write ( order, filename, x, w, r );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'GEN_LAGUERRE_RULE:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end
function [ t, wts ] = cdgqf ( nt, kind, alpha, beta )

%*****************************************************************************80
%
%% CDGQF computes a Gauss quadrature formula with default A, B and simple knots.
%
%  Discussion:
%
%    This routine computes all the knots and weights of a Gauss quadrature
%    formula with a classical weight function with default values for A and B,
%    and only simple knots.
%
%    There are no moments checks and no printing is done.
%
%    Use routine EIQFS to evaluate a quadrature computed by CGQFS.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 January 2010
%
%  Author:
%
%    Original FORTRAN77 version by Sylvan Elhay, Jaroslav Kautsky.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Sylvan Elhay, Jaroslav Kautsky,
%    Algorithm 655: IQPACK, FORTRAN Subroutines for the Weights of
%    Interpolatory Quadrature,
%    ACM Transactions on Mathematical Software,
%    Volume 13, Number 4, December 1987, pages 399-415.
%
%  Parameters:
%
%    Input, integer NT, the number of knots.
%
%    Input, integer KIND, the rule.
%    1, Legendre,             (a,b)       1.0
%    2, Chebyshev,            (a,b)       ((b-x)*(x-a))^(-0.5)
%    3, Gegenbauer,           (a,b)       ((b-x)*(x-a))^alpha
%    4, Jacobi,               (a,b)       (b-x)^alpha*(x-a)^beta
%    5, Generalized Laguerre, (a,inf)     (x-a)^alpha*exp(-b*(x-a))
%    6, Generalized Hermite,  (-inf,inf)  |x-a|^alpha*exp(-b*(x-a)^2)
%    7, Exponential,          (a,b)       |x-(a+b)/2.0|^alpha
%    8, Rational,             (a,inf)     (x-a)^alpha*(x+b)^beta
%
%    Input, real ALPHA, the value of Alpha, if needed.
%
%    Input, real BETA, the value of Beta, if needed.
%
%    Output, real T(NT), the knots.
%
%    Output, real WTS(NT), the weights.
%
  parchk ( kind, 2 * nt, alpha, beta );
%
%  Get the Jacobi matrix and zero-th moment.
%
  [ aj, bj, zemu ] = class_matrix ( kind, nt, alpha, beta );
%
%  Compute the knots and weights.
%
  [ t, wts ] = sgqf ( nt, aj, bj, zemu );

  return
end
function [ t, wts ] = cgqf ( nt, kind, alpha, beta, a, b )

%*****************************************************************************80
%
%% CGQF computes knots and weights of a Gauss quadrature formula.
%
%  Discussion:
%
%    The user may specify the interval (A,B).
%
%    Only simple knots are produced.
%
%    The user may request that the routine print the knots and weights,
%    and perform a moment check.
%
%    Use routine EIQFS to evaluate this quadrature formula.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 February 2010
%
%  Author:
%
%    Original FORTRAN77 version by Sylvan Elhay, Jaroslav Kautsky.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Sylvan Elhay, Jaroslav Kautsky,
%    Algorithm 655: IQPACK, FORTRAN Subroutines for the Weights of
%    Interpolatory Quadrature,
%    ACM Transactions on Mathematical Software,
%    Volume 13, Number 4, December 1987, pages 399-415.
%
%  Parameters:
%
%    Input, integer NT, the number of knots.
%
%    Input, integer KIND, the rule.
%    1, Legendre,             (a,b)       1.0
%    2, Chebyshev Type 1,     (a,b)       ((b-x)*(x-a))^(-0.5)
%    3, Gegenbauer,           (a,b)       ((b-x)*(x-a))^alpha
%    4, Jacobi,               (a,b)       (b-x)^alpha*(x-a)^beta
%    5, Generalized Laguerre, (a,+oo)     (x-a)^alpha*exp(-b*(x-a))
%    6, Generalized Hermite,  (-oo,+oo)   |x-a|^alpha*exp(-b*(x-a)^2)
%    7, Exponential,          (a,b)       |x-(a+b)/2.0|^alpha
%    8, Rational,             (a,+oo)     (x-a)^alpha*(x+b)^beta
%    9, Chebyshev Type 2,     (a,b)       ((b-x)*(x-a))^(+0.5)
%
%    Input, real ALPHA, the value of Alpha, if needed.
%
%    Input, real BETA, the value of Beta, if needed.
%
%    Input, real A, B, the interval endpoints.
%
%    Output, real T(NT), the knots.
%
%    Output, real WTS(NT), the weights.
%

%
%  Compute the Gauss quadrature formula for default values of A and B.
%
  [ t, wts ] = cdgqf ( nt, kind, alpha, beta );
%
%  All knots have multiplicity = 1.
%
  mlt = zeros(nt,1);
  mlt(1:nt) = 1;
%
%  NDX(I) = I.
%
  ndx = ( 1 : nt );
%
%  Scale the quadrature rule.
%
  [ t, wts ] = scqf ( nt, t, mlt, wts, nt, ndx, kind, alpha, beta, a, b );

  return
end
function [ aj, bj, zemu ] = class_matrix ( kind, m, alpha, beta )

%*****************************************************************************80
%
%% CLASS_MATRIX computes the Jacobi matrix for a quadrature rule.
%
%  Discussion:
%
%    This routine computes the diagonal AJ and subdiagonal BJ
%    elements of the order M tridiagonal symmetric Jacobi matrix
%    associated with the polynomials orthogonal with respect to
%    the weight function specified by KIND.
%
%    For weight functions 1-7, M elements are defined in BJ even
%    though only M-1 are needed.  For weight function 8, BJ(M) is
%    set to zero.
%
%    The zero-th moment of the weight function is returned in ZEMU.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 January 2010
%
%  Author:
%
%    Original FORTRAN77 version by Sylvan Elhay, Jaroslav Kautsky.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Sylvan Elhay, Jaroslav Kautsky,
%    Algorithm 655: IQPACK, FORTRAN Subroutines for the Weights of
%    Interpolatory Quadrature,
%    ACM Transactions on Mathematical Software,
%    Volume 13, Number 4, December 1987, pages 399-415.
%
%  Parameters:
%
%    Input, integer KIND, the rule.
%    1, Legendre,             (a,b)       1.0
%    2, Chebyshev,            (a,b)       ((b-x)*(x-a))^(-0.5)
%    3, Gegenbauer,           (a,b)       ((b-x)*(x-a))^alpha
%    4, Jacobi,               (a,b)       (b-x)^alpha*(x-a)^beta
%    5, Generalized Laguerre, (a,inf)     (x-a)^alpha*exp(-b*(x-a))
%    6, Generalized Hermite,  (-inf,inf)  |x-a|^alpha*exp(-b*(x-a)^2)
%    7, Exponential,          (a,b)       |x-(a+b)/2.0|^alpha
%    8, Rational,             (a,inf)     (x-a)^alpha*(x+b)^beta
%
%    Input, integer M, the order of the Jacobi matrix.
%
%    Input, real ALPHA, the value of Alpha, if needed.
%
%    Input, real BETA, the value of Beta, if needed.
%
%    Output, real AJ(M), BJ(M), the diagonal and subdiagonal
%    of the Jacobi matrix.
%
%    Output, real ZEMU, the zero-th moment.
%
  temp = eps;

  parchk ( kind, 2 * m - 1, alpha, beta );

  temp2 = 0.5;

  if ( 500.0 * temp < abs ( ( gamma ( temp2 ) )^2 - pi ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'CLASS - Fatal error!\n' );
    fprintf ( 1, '  Gamma function does not match machine parameters.\n' );
    error ( 'CLASS - Fatal error!' );
  end

  bj = zeros(m,1);
  aj = zeros(m,1);

  if ( kind == 1 )

    ab = 0.0;

    zemu = 2.0 / ( ab + 1.0 );

    aj(1:m) = 0.0;

    for i = 1 : m
      abi = i + ab * mod ( i, 2 );
      abj = 2 * i + ab;
      bj(i) = abi * abi / ( abj * abj - 1.0 );
    end
    bj(1:m) =  sqrt ( bj(1:m) );

  elseif ( kind == 2 )

    zemu = pi;

    aj(1:m) = 0.0;

    bj(1) =  sqrt ( 0.5 );
    bj(2:m) = 0.5;

  elseif ( kind == 3 )

    ab = alpha * 2.0;
    zemu = 2.0^( ab + 1.0 ) * gamma ( alpha + 1.0 )^2 ...
      / gamma ( ab + 2.0 );

    aj(1:m) = 0.0;
    bj(1) = 1.0 / ( 2.0 * alpha + 3.0 );
    for i = 2 : m
      bj(i) = i * ( i + ab ) / ( 4.0 * ( i + alpha )^2 - 1.0 );
    end
    bj(1:m) =  sqrt ( bj(1:m) );

  elseif ( kind == 4 )

    ab = alpha + beta;
    abi = 2.0 + ab;
    zemu = 2.0^( ab + 1.0 ) * gamma ( alpha + 1.0 ) ...
      * gamma ( beta + 1.0 ) / gamma ( abi );
    aj(1) = ( beta - alpha ) / abi;
    bj(1) = 4.0 * ( 1.0 + alpha ) * ( 1.0 + beta ) ...
      / ( ( abi + 1.0 ) * abi * abi );
    a2b2 = beta * beta - alpha * alpha;

    for i = 2 : m
      abi = 2.0 * i + ab;
      aj(i) = a2b2 / ( ( abi - 2.0 ) * abi );
      abi = abi^2;
      bj(i) = 4.0 * i * ( i + alpha ) * ( i + beta ) * ( i + ab ) ...
        / ( ( abi - 1.0 ) * abi );
    end
    bj(1:m) =  sqrt ( bj(1:m) );

  elseif ( kind == 5 )

    zemu = gamma ( alpha + 1.0 );

    for i = 1 : m
      aj(i) = 2.0 * i - 1.0 + alpha;
      bj(i) = i * ( i + alpha );
    end
    bj(1:m) =  sqrt ( bj(1:m) );

  elseif ( kind == 6 )

    zemu = gamma ( ( alpha + 1.0 ) / 2.0 );

    aj(1:m) = 0.0;

    for i = 1 : m
      bj(i) = ( i + alpha * mod ( i, 2 ) ) / 2.0;
    end
    bj(1:m) =  sqrt ( bj(1:m) );

  elseif ( kind == 7 )

    ab = alpha;
    zemu = 2.0 / ( ab + 1.0 );

    aj(1:m) = 0.0;

    for i = 1 : m
      abi = i + ab * mod(i,2);
      abj = 2 * i + ab;
      bj(i) = abi * abi / ( abj * abj - 1.0 );
    end
    bj(1:m) =  sqrt ( bj(1:m) );

  elseif ( kind == 8 )

    ab = alpha + beta;
    zemu = gamma ( alpha + 1.0 ) * gamma ( - ( ab + 1.0 ) ) ...
      / gamma ( - beta );
    apone = alpha + 1.0;
    aba = ab * apone;
    aj(1) = - apone / ( ab + 2.0 );
    bj(1) = - aj(1) * ( beta + 1.0 ) / ( ab + 2.0 ) / ( ab + 3.0 );
    for i = 2 : m
      abti = ab + 2.0 * i;
      aj(i) = aba + 2.0 * ( ab + i ) * ( i - 1 );
      aj(i) = - aj(i) / abti / ( abti - 2.0 );
    end

    for i = 2 : m - 1
      abti = ab + 2.0 * i;
      bj(i) = i * ( alpha + i ) / ( abti - 1.0 ) * ( beta + i ) ...
        / ( abti^2 ) * ( ab + i ) / ( abti + 1.0 );
    end

    bj(m) = 0.0;
    bj(1:m) =  sqrt ( bj(1:m) );

  end

  return
end
function [ d, z ] = imtqlx ( n, d, e, z )

%*****************************************************************************80
%
%% IMTQLX diagonalizes a symmetric tridiagonal matrix.
%
%  Discussion:
%
%    This routine is a slightly modified version of the EISPACK routine to
%    perform the implicit QL algorithm on a symmetric tridiagonal matrix.
%
%    The authors thank the authors of EISPACK for permission to use this
%    routine.
%
%    It has been modified to produce the product Q' * Z, where Z is an input
%    vector and Q is the orthogonal matrix diagonalizing the input matrix.
%    The changes consist (essentialy) of applying the orthogonal transformations
%    directly to Z as they are generated.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 January 2010
%
%  Author:
%
%    Original FORTRAN77 version by Sylvan Elhay, Jaroslav Kautsky.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Sylvan Elhay, Jaroslav Kautsky,
%    Algorithm 655: IQPACK, FORTRAN Subroutines for the Weights of
%    Interpolatory Quadrature,
%    ACM Transactions on Mathematical Software,
%    Volume 13, Number 4, December 1987, pages 399-415.
%
%    Roger Martin, James Wilkinson,
%    The Implicit QL Algorithm,
%    Numerische Mathematik,
%    Volume 12, Number 5, December 1968, pages 377-383.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real D(N), the diagonal entries of the matrix.
%
%    Input, real E(N), the subdiagonal entries of the
%    matrix, in entries E(1) through E(N-1).
%
%    Input, real Z(N), a vector to be operated on.
%
%    Output, real D(N), the diagonal entries of the diagonalized matrix.
%
%    Output, real Z(N), the value of Q' * Z, where Q is the matrix that
%    diagonalizes the input symmetric tridiagonal matrix.
%
  itn = 30;

  prec = eps;

  if ( n == 1 )
    return
  end

  e(n) = 0.0;

  for l = 1 : n

    j = 0;

    while ( 1 )

      for m = l : n

        if ( m == n )
          break
        end

        if ( abs ( e(m) ) <= prec * ( abs ( d(m) ) + abs ( d(m+1) ) ) )
          break
        end

      end

      p = d(l);

      if ( m == l )
        break
      end

      if ( j == itn )
        fprintf ( 1, '\n' );
        fprintf ( 1, 'IMTQLX - Fatal error!\n' );
        fprintf ( 1, '  Iteration limit exceeded.\n' );
        error ( 'IMTQLX - Fatal error!' );
      end

      j = j + 1;
      g = ( d(l+1) - p ) / ( 2.0 * e(l) );
      r =  sqrt ( g * g + 1.0 );
      g = d(m) - p + e(l) / ( g + r8_sign ( g ) * abs ( r ) );
      s = 1.0;
      c = 1.0;
      p = 0.0;
      mml = m - l;

      for ii = 1 : mml

        i = m - ii;
        f = s * e(i);
        b = c * e(i);

        if ( abs ( f ) >= abs ( g ) )
          c = g / f;
          r =  sqrt ( c * c + 1.0 );
          e(i+1) = f * r;
          s = 1.0 / r;
          c = c * s;
        else
          s = f / g;
          r =  sqrt ( s * s + 1.0 );
          e(i+1) = g * r;
          c = 1.0 / r;
          s = s * c;
        end

        g = d(i+1) - p;
        r = ( d(i) - g ) * s + 2.0 * c * b;
        p = s * r;
        d(i+1) = g + p;
        g = c * r - b;
        f = z(i+1);
        z(i+1) = s * z(i) + c * f;
        z(i) = c * z(i) - s * f;

      end

      d(l) = d(l) - p;
      e(l) = g;
      e(m) = 0.0;

    end

  end

  for ii = 2 : n

     i = ii - 1;
     k = i;
     p = d(i);

     for j = ii : n
       if ( d(j) < p )
         k = j;
         p = d(j);
       end
     end

     if ( k ~= i )
       d(k) = d(i);
       d(i) = p;
       p = z(i);
       z(i) = z(k);
       z(k) = p;
     end

  end

  return
end
function parchk ( kind, m, alpha, beta )

%*****************************************************************************80
%
%% PARCHK checks parameters ALPHA and BETA for classical weight functions.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 January 2010
%
%  Author:
%
%    Original FORTRAN77 version by Sylvan Elhay, Jaroslav Kautsky.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Sylvan Elhay, Jaroslav Kautsky,
%    Algorithm 655: IQPACK, FORTRAN Subroutines for the Weights of
%    Interpolatory Quadrature,
%    ACM Transactions on Mathematical Software,
%    Volume 13, Number 4, December 1987, pages 399-415.
%
%  Parameters:
%
%    Input, integer KIND, the rule.
%    1, Legendre,             (a,b)       1.0
%    2, Chebyshev,            (a,b)       ((b-x)*(x-a))^(-0.5)
%    3, Gegenbauer,           (a,b)       ((b-x)*(x-a))^alpha
%    4, Jacobi,               (a,b)       (b-x)^alpha*(x-a)^beta
%    5, Generalized Laguerre, (a,inf)     (x-a)^alpha*exp(-b*(x-a))
%    6, Generalized Hermite,  (-inf,inf)  |x-a|^alpha*exp(-b*(x-a)^2)
%    7, Exponential,          (a,b)       |x-(a+b)/2.0|^alpha
%    8, Rational,             (a,inf)     (x-a)^alpha*(x+b)^beta
%
%    Input, integer M, the order of the highest moment to
%    be calculated.  This value is only needed when KIND = 8.
%
%    Input, real ALPHA, BETA, the parameters, if required
%    by the value of KIND.
%
  if ( kind <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'PARCHK - Fatal error!\n' );
    fprintf ( 1, '  KIND <= 0.\n' );
    error ( 'PARCHK - Fatal error!' );
  end
%
%  Check ALPHA for Gegenbauer, Jacobi, Laguerre, Hermite, Exponential.
%
  if ( 3 <= kind && alpha <= -1.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'PARCHK - Fatal error!\n' );
    fprintf ( 1, '  3 <= KIND and ALPHA <= -1.\n' );
    error ( 'PARCHK - Fatal error!' );
  end
%
%  Check BETA for Jacobi.
%
  if ( kind == 4 && beta <= -1.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'PARCHK - Fatal error!\n' );
    fprintf ( 1, '  KIND == 4 and BETA <= -1.0.\n' );
    error ( 'PARCHK - Fatal error!' );
  end
%
%  Check ALPHA and BETA for rational.
%
  if ( kind == 8 )
    tmp = alpha + beta + m + 1.0;
    if ( 0.0 <= tmp || tmp <= beta )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'PARCHK - Fatal error!\n' );
      fprintf ( 1, '  KIND == 8 but condition on ALPHA and BETA fails.\n' );
      error ( 'PARCHK - Fatal error!' );
    end
  end

  return
end
function value = r8_huge ( )

%*****************************************************************************80
%
%% R8_HUGE returns a "huge" real number.
%
%  Discussion:
%
%    The value returned by this function is NOT required to be the
%    maximum representable R8.  This value varies from machine to machine,
%    from compiler to compiler, and may cause problems when being printed.
%    We simply want a "very large" but non-infinite number.
%
%    MATLAB provides a built-in symbolic constant "inf" that can be used
%    if a huge number is really what you want!
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 January 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real VALUE, a huge number.
%
  value = 1.0E+30;

  return
end
function value = r8_sign ( x )

%*****************************************************************************80
%
%% R8_SIGN returns the sign of an R8.
%
%  Discussion:
%
%    The value is +1 if the number is positive or zero, and it is -1 otherwise.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 March 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the number whose sign is desired.
%
%    Output, real VALUE, the sign of X.
%
  if ( 0 <= x )
    value = +1.0;
  else
    value = -1.0;
  end

  return
end
function r8mat_write ( output_filename, m, n, table )

%*****************************************************************************80
%
%% R8MAT_WRITE writes an R8MAT file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 August 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string OUTPUT_FILENAME, the output filename.
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of points.
%
%    Input, real TABLE(M,N), the points.
%

%
%  Open the file.
%
  output_unit = fopen ( output_filename, 'wt' );

  if ( output_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_WRITE - Error!\n' );
    fprintf ( 1, '  Could not open the output file.\n' );
    error ( 'R8MAT_WRITE - Error!' );
  end
%
%  Write the data.
%
%  For smaller data files, and less precision, try:
%
%     fprintf ( output_unit, '  %14.6e', table(i,j) );
%
  for j = 1 : n
    for i = 1 : m
      fprintf ( output_unit, '  %24.16e', table(i,j) );
    end
    fprintf ( output_unit, '\n' );
  end
%
%  Close the file.
%
  fclose ( output_unit );

  return
end
function rule_write ( order, filename, x, w, r )

%*****************************************************************************80
%
%% RULE_WRITE writes a quadrature rule to a file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 February 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer ORDER, the order of the rule.
%
%    Input, string FILENAME, specifies the output files.
%    write files 'filename_w.txt', 'filename_x.txt', 'filename_r.txt' defining
%    weights, abscissas, and region.
%
%    Input, real X(ORDER), the abscissas.
%
%    Input, real W(ORDER), the weights.
%
%    Input, real R(2), the region.
%
  filename_x = strcat ( filename, '_x.txt' );
  filename_w = strcat ( filename, '_w.txt' );
  filename_r = strcat ( filename, '_r.txt' );

  fprintf ( 1, '\n' );
  fprintf ( 1,'  Creating quadrature files.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  "Root" file name is   "%s".\n', filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Weight file will be   "%s".\n', filename_w );
  fprintf ( 1, '  Abscissa file will be "%s".\n', filename_x );
  fprintf ( 1, '  Region file will be   "%s".\n', filename_r );

  r8mat_write ( filename_w, 1, order, w' );
  r8mat_write ( filename_x, 1, order, x' );
  r8mat_write ( filename_r, 1, 2,     r' );

  return
end
function [ t, wts ] = scqf ( nt, t, mlt, wts, nwts, ndx, kind, alpha, ...
  beta, a, b )

%*****************************************************************************80
%
%% SCQF scales a quadrature formula to a nonstandard interval.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 February 2010
%
%  Author:
%
%    Original FORTRAN77 version by Sylvan Elhay, Jaroslav Kautsky.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Sylvan Elhay, Jaroslav Kautsky,
%    Algorithm 655: IQPACK, FORTRAN Subroutines for the Weights of
%    Interpolatory Quadrature,
%    ACM Transactions on Mathematical Software,
%    Volume 13, Number 4, December 1987, pages 399-415.
%
%  Parameters:
%
%    Input, integer NT, the number of knots.
%
%    Input, real T(NT), the original knots.
%
%    Input, integer MLT(NT), the multiplicity of the knots.
%
%    Input, real WTS(NWTS), the weights.
%
%    Input, integer NWTS, the number of weights.
%
%    Input, integer NDX(NT), used to index the array WTS.
%    For more details see the comments in CAWIQ.
%
%    Input, integer KIND, the rule.
%    1, Legendre,             (a,b)       1.0
%    2, Chebyshev Type 1,     (a,b)       ((b-x)*(x-a))^(-0.5)
%    3, Gegenbauer,           (a,b)       ((b-x)*(x-a))^alpha
%    4, Jacobi,               (a,b)       (b-x)^alpha*(x-a)^beta
%    5, Generalized Laguerre, (a,+oo)     (x-a)^alpha*exp(-b*(x-a))
%    6, Generalized Hermite,  (-oo,+oo)   |x-a|^alpha*exp(-b*(x-a)^2)
%    7, Exponential,          (a,b)       |x-(a+b)/2.0|^alpha
%    8, Rational,             (a,+oo)     (x-a)^alpha*(x+b)^beta
%    9, Chebyshev Type 2,     (a,b)       ((b-x)*(x-a))^(+0.5)
%
%    Input, real ALPHA, the value of Alpha, if needed.
%
%    Input, real BETA, the value of Beta, if needed.
%
%    Input, real A, B, the interval endpoints.
%
%    Output, real T(NT), the scaled knots.
%
%    Output, real WTS(NWTS), the scaled weights.
%
  temp = eps;

  parchk ( kind, 1, alpha, beta )

  if ( kind == 1 )

    al = 0.0;
    be = 0.0;

    if ( abs ( b - a ) <= temp )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SCQF - Fatal error!\n' );
      fprintf ( 1, '  |B - A| too small.\n' );
      fprintf ( 1, '  A = %f\n', a );
      fprintf ( 1, '  B = %f\n', b );
      error ( 'SCQF - Fatal error!' );
    end

    shft = ( a + b ) / 2.0;
    slp = ( b - a ) / 2.0;

  elseif ( kind == 2 )

    al = -0.5;
    be = -0.5;

    if ( abs ( b - a ) <= temp )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SCQF - Fatal error!\n' );
      fprintf ( 1, '  |B - A| too small.\n' );
      fprintf ( 1, '  A = %f\n', a );
      fprintf ( 1, '  B = %f\n', b );
      error ( 'SCQF - Fatal error!' );
    end

    shft = ( a + b ) / 2.0;
    slp = ( b - a ) / 2.0;

  elseif ( kind == 3 )

    al = alpha;
    be = alpha;

    if ( abs ( b - a ) <= temp )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SCQF - Fatal error!\n' );
      fprintf ( 1, '  |B - A| too small.\n' );
      fprintf ( 1, '  A = %f\n', a );
      fprintf ( 1, '  B = %f\n', b );
      error ( 'SCQF - Fatal error!' );
    end

    shft = ( a + b ) / 2.0;
    slp = ( b - a ) / 2.0;

  elseif ( kind == 4 )

    al = alpha;
    be = beta;

    if ( abs ( b - a ) <= temp )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SCQF - Fatal error!\n' );
      fprintf ( 1, '  |B - A| too small.\n' );
      fprintf ( 1, '  A = %f\n', a );
      fprintf ( 1, '  B = %f\n', b );
      error ( 'SCQF - Fatal error!' );
    end

    shft = ( a + b ) / 2.0;
    slp = ( b - a ) / 2.0;

  elseif ( kind == 5 )

    if ( b <= 0.0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SCQF - Fatal error!\n' );
      fprintf ( 1, '  B <= 0.\n' );
      fprintf ( 1, '  A = %f\n', a );
      fprintf ( 1, '  B = %f\n', b );
      error ( 'SCQF - Fatal error!' );
    end

    shft = a;
    slp = 1.0 / b;
    al = alpha;
    be = 0.0;

  elseif ( kind == 6 )

    if ( b <= 0.0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SCQF - Fatal error!\n' );
      fprintf ( 1, '  B <= 0.\n' );
      fprintf ( 1, '  A = %f\n', a );
      fprintf ( 1, '  B = %f\n', b );
      error ( 'SCQF - Fatal error!' );
    end

    shft = a;
    slp = 1.0 / sqrt ( b );
    al = alpha;
    be = 0.0;

  elseif ( kind == 7 )

    al = alpha;
    be = 0.0;

    if ( abs ( b - a ) <= temp )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SCQF - Fatal error!\n' );
      fprintf ( 1, '  |B - A| too small.\n' );
      fprintf ( 1, '  A = %f\n', a );
      fprintf ( 1, '  B = %f\n', b );
      error ( 'SCQF - Fatal error!' );
    end

    shft = ( a + b ) / 2.0;
    slp = ( b - a ) / 2.0;

  elseif ( kind == 8 )

    if ( a + b <= 0.0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SCQF - Fatal error!\n' );
      fprintf ( 1, '  A + B <= 0.\n' );
      fprintf ( 1, '  A = %f\n', a );
      fprintf ( 1, '  B = %f\n', b );
      error ( 'SCQF - Fatal error!' );
    end

    shft = a;
    slp = a + b;
    al = alpha;
    be = beta;

  elseif ( kind == 9 )

    al = 0.5;
    be = 0.5;

    if ( abs ( b - a ) <= temp )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SCQF - Fatal error!\n' );
      fprintf ( 1, '  |B - A| too small.\n' );
      fprintf ( 1, '  A = %f\n', a );
      fprintf ( 1, '  B = %f\n', b );
      error ( 'SCQF - Fatal error!' );
    end

    shft = ( a + b ) / 2.0;
    slp = ( b - a ) / 2.0;

  end

  p = slp^( al + be + 1.0 );

  for k = 1 : nt

    t(k) = shft + slp * t(k);
    l = abs ( ndx(k) );

    if ( l ~= 0 )
      tmp = p;
      for i = l : l + mlt(k) - 1
        wts(i) = wts(i) * tmp;
        tmp = tmp * slp;
      end
    end

  end

  return
end
function [ t, wts ] = sgqf ( nt, aj, bj, zemu )

%*****************************************************************************80
%
%% SGQF computes knots and weights of a Gauss Quadrature formula.
%
%  Discussion:
%
%    This routine computes all the knots and weights of a Gauss quadrature
%    formula with simple knots from the Jacobi matrix and the zero-th
%    moment of the weight function, using the Golub-Welsch technique.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 February 2010
%
%  Author:
%
%    Original FORTRAN77 version by Sylvan Elhay, Jaroslav Kautsky.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Sylvan Elhay, Jaroslav Kautsky,
%    Algorithm 655: IQPACK, FORTRAN Subroutines for the Weights of
%    Interpolatory Quadrature,
%    ACM Transactions on Mathematical Software,
%    Volume 13, Number 4, December 1987, pages 399-415.
%
%  Parameters:
%
%    Input, integer NT, the number of knots.
%
%    Input, real AJ(NT), the diagonal of the Jacobi matrix.
%
%    Input, real BJ(NT), the subdiagonal of the Jacobi
%    matrix, in entries 1 through NT-1.  On output, BJ has been overwritten.
%
%    Input, real ZEMU, the zero-th moment of the weight function.
%
%    Output, real T(NT), the knots.
%
%    Output, real WTS(NT), the weights.
%

%
%  Exit if the zero-th moment is not positive.
%
  if ( zemu <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'SGQF - Fatal error!\n' );
    fprintf ( 1, '  ZEMU <= 0.\n' );
    error ( 'SGQF - Fatal error!' );
  end
%
%  Set up vectors for IMTQLX.
%
  wts = zeros ( nt, 1 );

  wts(1) = sqrt ( zemu );
  wts(2:nt) = 0.0;
%
%  Diagonalize the Jacobi matrix.
%
  [ t, wts ] = imtqlx ( nt, aj, bj, wts );

  wts(1:nt) = wts(1:nt).^2;

  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 w = nc_compute ( n, x_min, x_max, x )

%*****************************************************************************80
%
%% NC_COMPUTE computes a Newton-Cotes quadrature rule.
%
%  Discussion:
%
%    For the interval [X_MIN,X_MAX], the Newton-Cotes quadrature rule
%    estimates
%
%      Integral ( X_MIN <= X <= X_MAX ) F(X) dX
%
%    using N abscissas X and weights W:
%
%      Sum ( 1 <= I <= N ) W(I) * F ( X(I) ).
%
%    For the CLOSED rule, the equally spaced abscissas include A and B.
%    For the OPEN rule, the equally spaced abscissas do not include A and B.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 February 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order.
%
%    Input, real X_MIN, X_MAX, the endpoints of the interval.
%
%    Input, real X(N), the abscissas.
%
%    Output, real W(N,1), the weights.
%
  d = zeros ( n, 1 );
  w = zeros ( n, 1 );

  for i = 1 : n
%
%  Compute the Lagrange basis polynomial which is 1 at X(I),
%  and zero at the other nodes.
%
    d(1:n) = 0.0;
    d(i) = 1.0;

    for j = 2 : n
      for k = j : n
        d(n+j-k) = ( d(n+j-k-1) - d(n+j-k) ) / ( x(n+1-k) - x(n+j-k) );
      end
    end

    for j = 1 : n - 1
      for k = 1 : n - j
        d(n-k) = d(n-k) - x(n-k-j+1) * d(n-k+1);
      end
    end
%
%  Evaluate the antiderivative of the polynomial at the endpoints.
%
    yvala = d(n) / n;
    for j = n - 1 : -1 : 1
      yvala = yvala * x_min + d(j) / j;
    end
    yvala = yvala * x_min;

    yvalb = d(n) / n;
    for j = n - 1 : -1 : 1
      yvalb = yvalb * x_max + d(j) / j;
    end
    yvalb = yvalb * x_max;

    w(i) = yvalb - yvala;

  end

  return
end


function a = poisson ( nrow, ncol )

%*****************************************************************************80
%
%% POISSON returns the POISSON matrix.
%
%  Formula:
%
%    if ( I = J )
%      A(I,J) = 4.0
%    elseif ( I = J+1 or I = J-1 or I = J+NROW or I = J-NROW )
%      A(I,J) = -1.0
%    else
%      A(I,J) = 0.0
%
%  Example:
%
%    NROW = NCOL = 3
%
%     4 -1  0 | -1  0  0 |  0  0  0
%    -1  4 -1 |  0 -1  0 |  0  0  0
%     0 -1  4 |  0  0 -1 |  0  0  0
%     ----------------------------
%    -1  0  0 |  4 -1  0 | -1  0  0
%     0 -1  0 | -1  4 -1 |  0 -1  0
%     0  0 -1 |  0 -1  4 |  0  0 -1
%     ----------------------------
%     0  0  0 | -1  0  0 |  4 -1  0
%     0  0  0 |  0 -1  0 | -1  4 -1
%     0  0  0 |  0  0 -1 |  0 -1  4
%
%  Properties:
%
%    A is symmetric: A' = A.
%
%    Because A is symmetric, it is normal.
%
%    Because A is normal, it is diagonalizable.
%
%    A is integral, therefore det ( A ) is integral, and
%    det ( A ) * inverse ( A ) is integral.
%
%    A is persymmetric: A(I,J) = A(N+1-J,N+1-I).
%
%    A results from discretizing Poisson's equation with the
%    5 point operator on a square mesh of N points.
%
%    A has eigenvalues
%
%      LAMBDA(I,J) = 4 - 2 * COS(I*PI/(N+1))
%                      - 2 * COS(J*PI/(M+1)), I = 1 to N, J = 1 to M.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 March 2015
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Gene Golub, Charles Van Loan,
%    Matrix Computations, second edition,
%    Johns Hopkins University Press, Baltimore, Maryland, 1989
%    (Section 4.5.4).
%
%  Parameters:
%
%    Input, integer NROW, NCOL, the number of rows and columns
%    in the grid.
%
%    Output, real A(NROW*NCOL,NROW*NCOL), the matrix.
%
  n = nrow * ncol;

  a = zeros ( n, n );

  i = 0;

  for i1 = 1 : nrow
    for j1 = 1 : ncol

      i = i + 1;

      if ( 1 < i1 )
        j = i - ncol;
        a(i,j) = -1.0;
      end

      if ( 1 < j1 )
        j = i - 1;
        a(i,j) = -1.0;
      end

      j = i;
      a(i,j) = 4.0;

      if ( j1 < ncol )
        j = i + 1;
        a(i,j) = -1.0;
      end

      if ( i1 < nrow )
        j = i + ncol;
        a(i,j) = -1.0;
      end

    end
  end

  return
end


function pdf = weibull_discrete_pdf ( x, a, b )

%*****************************************************************************80
%
%% WEIBULL_DISCRETE_PDF evaluates the discrete Weibull PDF.
%
%  Discussion:
%
%    PDF(X)(A,B) = ( 1 - A )**X**B - ( 1 - A )**(X+1)**B.
%
%    WEIBULL_DISCRETE_PDF(X)(A,1) = GEOMETRIC_PDF(X)(A)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer X, the argument of the PDF.
%    0 <= X
%
%    Input, real A, B, the parameters that define the PDF.
%    0 <= A <= 1,
%    0 < B.
%
%    Output, real PDF, the value of the PDF.
%
  if ( x < 0 )
    pdf = 0.0;
  else
    pdf = ( 1.0 - a )^(x^b) - ( 1.0 - a )^((x+1)^b);
  end

  return
end


function y=stdtpdf(x,mu,sigma2,nu)
% Probability Density Function (PDF) for the Standardized T distribution
%
% USAGE:
%   Y = stdtpdf(X,MU,SIGMA2,NU)
%
% INPUTS:
%   X      - Standardized T random variables
%   MU     - Mean of X, either scalar or size(x)
%   SIGMA2 - Variance of X, either scalar or size(x)
%   NU     - Degree of freedom parameters, either scalar or size(x)
%
% OUTPUTS:
%   Y     - Probability density evaluated at X
%
% COMMENTS:
%   NU>2
%
% REFERENCES:
%   [1] Cassella and Berger (1990) 'Statistical Inference'
%
% See also STDTCDF, STDTINV, STDTRND, STDTLOGLIK, TPDF

% Copyright:
% Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 6    Date: 8/21/2014

[T,K]=size(x);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if K~=1
    error('X must be a column vector');
end

if nargin==4
    if length(mu)~=1 && ~all(size(mu)==[T K])
        error('mu must be either a scalar or the same size as X');
    end
    if any(sigma2<=0)
        error('sigma2 must contain only positive elements')
    end
    if length(sigma2)==1
        sigma2=sigma2*ones(T,K);
    elseif size(sigma2,1)~=T || size(sigma2,2)~=1
        error('sigma2 must be a scalar or a vector with the same dimensions as X');
    end
    if length(nu)>1 || nu<=2
        error('nu must be a scalar greater than 2');
    end
    x=x-mu;
else
    error('Only 4 inputs supported');
end

constant = exp(gammaln( 0.5 * (nu + 1)) - gammaln(0.5 * nu));
y = constant ./ sqrt(pi * (nu - 2) * sigma2) .* (1 + (x-mu) .^ 2.0 / (sigma2 * (nu - 2))) .^ (-(nu + 1) / 2);