% TANDEMO   This file generates the tan(x) - x example from Chapter 1.
%           See Figures 1.2 and 1.3.
%
% C. T. Kelley, October 22, 2002.
%
x0=4.5; tol=1.d-20;
%
% Solve the problem three times.
%
[x,hist]=newtsol(x0,'ftan',tol,tol,1);
[x,histc]=chordsol(x0,'ftan',tol,tol);
[x,hists]=secant(x0,'ftan',tol,tol);
%
% Iteration history for Newton.
% I use handle graphics to get the axis the way I
% want them. Try commenting out "set(p1,'XTick',[0 1 2 3 4 5]);"
% and see what the difference is.
%
maxit=6;
figure(1);
p1=subplot(1,1,1);
semilogy(hist(1:maxit,1),abs(hist(1:maxit,2)))
set(p1,'XTick',[0 1 2 3 4 5],'FontSize',14);
axis([0 5 1.d-16 1]);
xlabel('Nonlinear iterations'); ylabel('Absolute Nonlinear Residual');

%
% Plot 15 iterations for all three methods.
%
figure(2);
maxit=15;
p2=subplot(1,1,1);
semilogy(hist(1:maxit,1),abs(hist(1:maxit,2)),'-',...
histc(1:maxit,1),abs(histc(1:maxit,2)),'--',...
hists(1:maxit,1),abs(hists(1:maxit,2)),'-.');
set(p2,'XTick',[0 1 2 3 4 5 6 7 8 9 10 11 12 13 14],'FontSize',14);
axis([0 14 1.d-16 1]);
legend('Newton','Chord','Secant');
xlabel('Nonlinear iterations'); ylabel('Absolute Nonlinear Residual');


function y = huber_circ( x, DIM, varargin )

%HUBER_CIRC   Huber penalty function with circular symmetry.
%   For a vector X, HUBER_CIRC(X) computes the Huber penalty function
%
%       HUBER_CIRC(X) =   NORM(X,2)^2 if NORM(X,2)<=1,
%                       2*NORM(X,2)-1 if NORM(X,2)>=1.
%
%   For matrices and N-D arrays, the penalty function is applied to the
%   first dimens
%
%   HUBER_CIRC(X,[],M) computes the penalty function with halfwidth M,
%   M.^2.*HUBER_CIRC(X./M). M must be real and positive.
%
%   HUBER_CIRC(X,[],M,T) computes the penalty function with halfwidth M
%   and concomitant scale T:
%
%       HUBER_CIRC(X,[],M,T) = T.*HUBER_CIRC(X./T,[],M) if T > 0
%                              +Inf                     if T <= 0
%
%   See the help file for HUBER for information about this usage.
%
%   If X is a matrix, the penalty function is applied to the columns of the
%   matrix X, and a row vector is returned. If X is an N-D matrix, the
%   penalties are computed along the first non-singleton dimension.
%
%   HUBER_CIRC(X,DIM), HUBER_CIRC(X,DIM,M), and HUBER_CIRC(X,DIM,M,T)
%   computes the penalty along the dimension DIM.
%
%   Disciplined convex programming information:
%       HUBER_CIRC is jointly convex in X and T. It is nonomonotonic in X
%       and nonincreasing in T. Therefore, when used in CVX specifications,
%       X must be affine and T must be concave (or affine). T must be real.
%       X, on the other hand, may be real or complex.

if ~cvx_isaffine( x ),
    error( 'Disciplined convex programming error:\n    HUBER_CIRC is nonmonotonic in X, so X must be affine.', 1 ); %#ok
end
if nargin < 2, DIM = []; end
y = huber_pos( norms( x, DIM ), varargin{:} );

% Copyright 2005-2016 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function [z,p,sig] = stouffer(p,varargin)
% :Usage:
% ::
%
%     [z,p,sig] = stouffer(p,[alph])
%
% :Inputs:
%
%   **p:**
%        p values in 4-D array
%
%        1st 3 dims are within images, dim4 = image
%
%        optional: alpha value for thresholding
%
% :Outputs:
%
%   **z:**
%        stouffer's combined test statistic, compare to normal
%
%   **p:**
%        p-values for combined test
%
%   **sig:**
%        signficance 1 / 0 binary mask, if alpha is specified
%
% :Described in:
%
% Lazar, N. A., Luna, B., Sweeney, J. A., & Eddy, W. F. (2002).
% Combining brains: a survey of methods for statistical pooling
% of information. Neuroimage, 16(2), 538-550.
%
% Stouffer, S. A., Suchman, E. A., DeVinney, L. C., Star, S. A., and
% Williams, R. M. 1949. The American Soldier: Vol. I. Adjustment
% During Army Life. Princeton University Press, Princeton.
%
% ..
%    tor wager
% ..

if length(varargin) > 0, alph = varargin{1};,else,alph = 0;,end

lastdim = length(size(p));
k = size(p,lastdim);

z = sum(norminv(1 - p),lastdim) ./ sqrt(k);
p = normcdf(1-z);

if alph, sig = p <= alph;,end

return


function h = ksizeHall(npd)
%
% Find kernel size according to "plug-in" method of
%     Hall, Marron, Sheather, Jones (91)
%
%

% Copyright (C) 2003 Alexander Ihler; distributable under GPL -- see README.txt

  x = getPoints(npd);
  [N1,N2] = size(x);
  sig = std(x,0,2);                     % estimate sigma (standard)
  lamS= .7413 * iqr(x')';               % find sigma by interquartile range lam
  if (max(lamS)==0) lamS=sig; end;      % replace sigma est. if possible
  BW = 1.0592 * lamS * N2^(-1/(4+N1));
  BW = repmat(BW,[1,N2]);

  dX = repmat(permute(x,[1,3,2]),[1,N2,1]);  % compute Xi-Xj for all i,j
  for i=1:N2,
    dX(:,:,i) = (dX(:,:,i)-x)./BW;
  end;
  for i=1:N2, dX(:,i,i) = 2e22; end;
  dX = reshape(dX,[N1,N2*N2]);

%  use that to find I2 and I3
  I2=h_findI2(N2,dX,BW(:,1));      % I2 = \hat R( f^(2) ) = \int f^(2)^2 dx
  I3=h_findI3(N2,dX,BW(:,1));      % I3 = \hat R( f^(3) )

% for Gaussian Kernel, we evaluate to find:
%  RK = 1.0/(2^N1) * 1.0/pi^(N1/2.0);    % R(K) = \int K^2(x) dx
%  mu2 = 1.0;                            % \mu_i = \int x^i K(x) dx
%  mu4 = 3.0^N1;
  switch (npd.type)
    case 0, RK = 0.282095;   mu2 = 1.000000;   mu4 = 3.000000; % Gauss
    case 1, RK = 0.600000;   mu2 = 0.199994;   mu4 = 0.085708; % Epanetch
    case 2, RK = 0.250002;   mu2 = 1.994473;   mu4 = 23.299070;% Laplace
  end;

  J1 = RK/mu2^2 .* 1./I2;
  J2 = (mu4 * I3) ./ (20 * mu2) .* 1./I2;
  h  = (J1/N2).^(1.0/5) + J2.*(J1/N2).^(3.0/5);

% Let us estimate R(f^(p)) by R( \hat f^(p) )
%   (f is the original density to be est'd;  f^(p) is its pth derivative)
%   Let L be the kernel function for this second estimator, with bandwidth alpha
%
% Ip = \int f^(p)^2_\alpha(x) dx
%    = [(-1)^p/n^2] \sum_i \sum_j L^(p)_\alpha * L^(p)_\alpha
%    = [(-1)^p/(n^2 \alpha^(2p+1))]  \sum_i \sum_j (L^(p) * L^(p))( (Xi-Xj)/\alpha )
%
% Take L to be a Gaussian kernel; we then evaluate L^(p) by:
%
  % L^(p)(x) = (-1)^p H_p(x) L(x)
  % H_p(x) = x H_{p-1}(x) - (p-1)H_{p-2}(x)
  % p=2 =>
  %   H_2(x) = x H_1(x) - H_0(x) = x * x - 1
  %   =>  L^(2)(x) = (x^2 - 1) * L(x)
  % p=3 =>
  %   H_3(x) = x H_2(x) - 2*H_1(x) = x*(x^2-1) - 2*x
  %   =>  L^(3)(x) = (x^3 - 3x) * L(x)
  %

function I2 = h_findI2(n,dXa,alpha)
%%  load ksizeHSJM.mat;
%%  xInd = fix(Nquant * (dXa-Xmin) / (Xmax-Xmin));
%%  xInd = max(xInd,1); xInd = min(xInd,Nquant);
%%  s = sum( L2data(xInd) ,2);
%  s = sum( (dXa.^2 -1) .* 1/sqrt(2*pi) .* exp(-.5*dXa.^2) , 2);
  s = sum( (dXa.^2 -1) .* 1/sqrt(2*pi) .* repmat(exp(-.5*sum(dXa.^2,1)),[size(dXa,1),1]) , 2);
  I2=s./((n*(n-1))*alpha.^5);

function I3 = h_findI3(n,dXb,beta)
%%  load ksizeHSJM.mat;
%%  xInd = fix(Nquant * (dXb-Xmin) / (Xmax-Xmin));
%%  xInd = max(xInd,1); xInd = min(xInd,Nquant);
%%  s = sum( L3data(xInd) , 2);
%  s = sum( (dXb.^3 -3*dXb) .* 1/sqrt(2*pi) .* exp(-.5*dXb.^2) , 2);
  s = sum( (dXb.^3 -3*dXb) .* 1/sqrt(2*pi) .* repmat(exp(-.5*sum(dXb.^2,1)),[size(dXb,1),1]) , 2);
  I3  = -s./((n*(n-1))*beta.^7);


function [lambda_vec, error_train, error_val] = ...
    validationCurve(X, y, Xval, yval)
%VALIDATIONCURVE Generate the train and validation errors needed to
%plot a validation curve that we can use to select lambda
%   [lambda_vec, error_train, error_val] = ...
%       VALIDATIONCURVE(X, y, Xval, yval) returns the train
%       and validation errors (in error_train, error_val)
%       for different values of lambda. You are given the training set (X,
%       y) and validation set (Xval, yval).
%

% Selected values of lambda (you should not change this)
lambda_vec = [0 0.001 0.003 0.01 0.03 0.1 0.3 1 3 10]';

% You need to return these variables correctly.
error_train = zeros(length(lambda_vec), 1);
error_val = zeros(length(lambda_vec), 1);

% ====================== YOUR CODE HERE ======================
% Instructions: Fill in this function to return training errors in
%               error_train and the validation errors in error_val. The
%               vector lambda_vec contains the different lambda parameters
%               to use for each calculation of the errors, i.e,
%               error_train(i), and error_val(i) should give
%               you the errors obtained after training with
%               lambda = lambda_vec(i)
%
% Note: You can loop over lambda_vec with the following:
%
%       for i = 1:length(lambda_vec)
%           lambda = lambda_vec(i);
%           % Compute train / val errors when training linear
%           % regression with regularization parameter lambda
%           % You should store the result in error_train(i)
%           % and error_val(i)
%           ....
%
%       end
%
%

for i = 1:length(lambda_vec)
    lambda = lambda_vec(i);
    [theta] = trainLinearReg(X, y, lambda);
    error_train(i) = linearRegCostFunction(X, y, theta, 0);
    error_val(i) = linearRegCostFunction(Xval, yval, theta, 0);
end

% =========================================================================

end


function chebyshev1_rule ( order, a, b, filename )

%*****************************************************************************80
%
%% CHEBYSHEV1_RULE generates a Gauss-Chebyshev type 1 quadrature rule.
%
%  Discussion:
%
%    This program computes a standard Gauss-Chebyshev type 1 quadrature rule
%    and writes it to a file.
%
%    The user specifies:
%    * the ORDER (number of points) in the rule;
%    * A, the left endpoint;
%    * B, the right endpoint;
%    * FILENAME, the root name of the output files.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 February 2010
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CHEBYSHEV1_RULE\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Compute a Gauss-Chebyshev type 1 rule for approximating\n' );
  fprintf ( 1, '    Integral ( A <= x <= B ) f(x) / sqrt ( ( x - A ) * ( B - x ) ) dx\n' );
  fprintf ( 1, '  of order ORDER.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The user specifies ORDER, A, B, and FILENAME.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  ORDER is the number of points;\n' );
  fprintf ( 1, '  A is the left endpoint;\n' );
  fprintf ( 1, '  B is the right endpoint;\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.
%
  alpha = 0.0;
  beta = 0.0;
%
%  Get ORDER.
%
  if ( nargin < 1 )
    order = input ( '  Enter the rule order ORDER:  ' );
  elseif ( ischar ( order ) )
    order = str2num ( order );
  end
%
%  Get A.
%
  if ( nargin < 2 )
    a = input ( '  Enter the left endpoint A:  ' );
  elseif ( ischar ( a ) )
    a = str2num ( a );
  end
%
%  Get B.
%
  if ( nargin < 3 )
    b = input ( '  Enter the right endpoint B:  ' );
  elseif ( ischar ( b ) )
    b = str2num ( b );
  end
%
%  Get FILENAME.
%
  if ( nargin < 4 )
    fprintf ( 1,  '\n' );
    fprintf ( 1,  '  FILENAME specifies the ''root name'' of the quadrature files).\n' );
    filename = input ( '  Enter the value of FILENAME as a quoted string:  ' );
  end
%
%  Input summary.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  ORDER = %d\n', order );
  fprintf ( 1, '  A = %f\n', a );
  fprintf ( 1, '  B = %f\n', b );
  fprintf ( 1, '  FILENAME = "%s".\n', filename );
%
%  Construct the rule.
%
  kind = 2;
  alpha = 0.0;
  beta = 0.0;
  [ x, w ] = cgqf ( order, kind, alpha, beta, a, b );
%
%  Write the rule.
%
  r = [ a, b ]';
  rule_write ( order, filename, x, w, r );
%
%  Terminate.
%
  fprintf ( 1,  '\n' );
  fprintf ( 1,  'CHEBYSHEV1_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 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,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 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,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 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,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_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.6f', table(i,j) );
%
  for j = 1 : n
    for i = 1 : m
      fprintf ( output_unit, '  %24.16f', 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 value = r8_exponential_01_pdf ( rval )

%*****************************************************************************80
%
%% R8_EXPONENTIAL_01_PDF: PDF of a standard exponential distribution.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2013
%
%  Author:
%
%    John Burkardt.
%
%  Parameters:
%
%    Input, real RVAL, the point where the PDF is evaluated.
%
%    Output, real VALUE, the value of the PDF.
%
  if ( rval < 0.0 )
    value = 0.0;
  else
    value = exp ( - rval );
  end

  return
end


function square_monte_carlo_test01 ( )

%*****************************************************************************80
%
%% SQUARE_MONTE_CARLO_TEST01 estimates integrals over the unit square in 2D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 January 2014
%
%  Author:
%
%    John Burkardt
%
  e_test = [ ...
    0, 0; ...
    2, 0; ...
    0, 2; ...
    4, 0; ...
    2, 2; ...
    0, 4; ...
    6, 0 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'SQUARE_MONTE_CARLO_TEST01\n' );
  fprintf ( 1, '  Use SQUARE01_SAMPLE to estimate integrals \n' );
  fprintf ( 1, '  along the interior of the unit square in 2D.\n' );

  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, '         N' );
  fprintf ( 1, '           1' );
  fprintf ( 1, '              X^2' );
  fprintf ( 1, '             Y^2' );
  fprintf ( 1, '             X^4' );
  fprintf ( 1, '           X^2Y^2' );
  fprintf ( 1, '             Y^4' );
  fprintf ( 1, '           X^6\n' );
  fprintf ( 1, '\n' );

  n = 1;

  while ( n <= 65536 )

    [ x, seed ] = square01_sample ( n, seed );
    fprintf ( 1, '  %8d', n );

    for j = 1 : 7

      e(1:2) = e_test(1:2,j);

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

      result = square01_area ( ) * sum ( value(1:n) ) / n;

      fprintf ( 1, '  %14.6g', result );

    end

    fprintf ( 1, '\n' );

    n = 2 * n;

  end

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

  for j = 1 : 7

    e(1:2) = e_test(1:2,j);

    result = square01_monomial_integral ( e );
    fprintf ( 1, '  %14.6g', result );

  end

  fprintf ( 1, '\n' );

  return
end


function dz = cartPoleDynamicsHumanReadable(z,u,p)
% dz = cartPoleDynamicsHumanReadable(z,u,p)
%
% This function computes the first-order dynamics of the cart-pole.
%
% INPUTS:
%   z = [4, n] = [x;q;dx;dq] = state of the system
%   u = [1, n] = horizontal force applied to the cart
%   p = parameter struct
%       .g = gravity
%       .m1 = cart mass
%       .m2 = pole mass
%       .l = pendulum length
% OUTPUTS:
%   dz = dz/dt = time derivative of state
%
%

% x = z(1,:);   %Cart position (Not used in dynamics)
q = z(2,:);   % pendulum (pole) angle, measure from gravity vector
dx = z(3,:);  % cart velocity
dq = z(4,:);  %pendulum angle rate

% Unpack the physical parameters
l = p.l;  %Pendulum length
m1 = p.m1; % cart mass
m2 = p.m2; % pole mass
g = p.g;  %Gravity acceleration

% The following expressions were derived using the symbolic math toolbox,
% in the script: Derive_cartPole.m
% Typically I use the automatically generated code, but I've written out
% the code in a more readable way here for tutorial purposes.

mEff = m1 + m2 - m2.*cos(q).^2;
centr = dq.^2.*l.*m2.*sin(q);
ddx = (u + centr + g.*m2.*cos(q).*sin(q))./mEff;
ddq = -(u.*cos(q) + (m1 + m2)*g*sin(q) + centr.*cos(q))./(l.*mEff);

dz = [dx;dq;ddx;ddq];

end


function [M_right_up, M_right_down] = index_Matrix(N)
n = log2(N);
M_right_up = zeros(N/2, n);
M_right_down = zeros(N/2, n);
for i = 1 : n
    for j = 1 : 2^(i - 1)
        M_right_up((j - 1) * N/2^i + 1 : j * N/2^i, i) = (1 : N/2^i)' + (j - 1) * N/2^(i - 1);
    end
    M_right_down(:, i) = M_right_up(:, i) + 2^(n - i);
end
M_right_up = M_right_up(:, end : -1 : 1);
M_right_down = M_right_down(:, end : -1 : 1);


function [ vX ] = ProxL1NormSum( vY, paramGamma, paramB )
% ----------------------------------------------------------------------------------------------- %
% [ vX ] = ProxL1NormSum( vY, paramGamma, paramB )
%   Solving the Least Squares Problem with L1 Regualarization (LASOO) with
%   Linear Equality Constraints (Sum of Elements) - Prox Operator.
% Input:
%   - vY            -   Input Vector.
%                       Structure: Vector (Column).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
%   - paramGamma    -   Parameter Gamma - L2 Regularization Factor.
%                       Sets the coefficient of the L1 Term.
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range: [0, inf).
%   - paramB        -   Parameter B - Equality Constarint Parameter.
%                       Sets the scalar constarint of the equality
%                       condition.
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
% Output:
%   - vX            -   Output Vector.
%                       The vector whichi minizes teh objective function
%                       and its sum of elements equals to paramB.
%                       Structure: Vector (Column).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
% References
%   1.  Efficient Solvers for Sparse Subspace Clustering - https://arxiv.org/abs/1804.06291.
%   2.  https://math.stackexchange.com/a/2886715/33.
% Remarks:
%   1.  S
% TODO:
%   1.  U.
% Release Notes:
%   -   1.0.000     18/08/2018  Royi Avital
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

FALSE   = 0;
TRUE    = 1;

OFF     = 0;
ON      = 1;

debugMode = OFF;

numElements = size(vY, 1);

vB      = sort([vY - paramGamma; vY + paramGamma], 'ascend');
iMin    = 1; %<! Bisection Lower Bound
iMax    = (2 * numElements) + 1; %<! Bisection Upper Bound
maxItr  = ceil(log2(iMax)) + 1;

if(debugMode == ON)
    vParamBeta = linspace(vB(1), vB(end), 1000);

    for ii = 1:length(vParamBeta)
        vZ(ii) = sum( ProxL1Norm(vY - vParamBeta(ii), paramGamma) ) - paramB;
    end
    for ii = 1:length(vB)
        vT(ii) = sum( ProxL1Norm(vY - vB(ii), paramGamma) ) - paramB;
    end

    figure();
    plot(vParamBeta, vZ);
    hold('on');
    plot(vB, vT, '*');
end

% Bisection
for ii = 1:maxItr
    if((iMax - iMin) <= 1)
        break;
    end
    idxJ = round((iMin + iMax) / 2);
    idxJ = min(max(idxJ, iMin + 1), iMax - 1);

    vX = ProxL1Norm(vY - vB(idxJ), paramGamma);
    if(sum(vX) > paramB)
        iMin = idxJ;
    else
        iMax = idxJ;
    end
end

if(debugMode == ON)
    plot(vB(iMin), sum( ProxL1Norm(vY - vB(iMin), paramGamma) ) - paramB, 'o');
    plot(vB(iMax), sum( ProxL1Norm(vY - vB(iMax), paramGamma) ) - paramB, 'o');
end

% Once the section is found there a closed form solution.
% Within the section [vB(iMin), vB(iMax)] the function has constant slope
% and for any paramBeta \in (vB(iMin), vB(iMax)) the function sign(vY -
% paramBeta) is constant.
% Hence sign(vY - paramBeta) .* max(abs(vY - paramBeta) - paramGamma), 0)
% equals to (Only at the support, where max(abs(vY - paramBeta) -
% paramGamma), 0) doesn't vanish) vY - paramBeta - paramGamam * sign().
% Looking when this sum equals to paramB happens:
% 1 / length(vSupport) * sumvY(vSupport) -

paramBeta   = (vB(iMin) + vB(iMax)) / 2;
vX          = ProxL1Norm(vY - paramBeta, paramGamma);
vS          = (vX ~= 0);
paramBeta   = (sum(vY(vS) - paramGamma * sign(vX(vS))) - paramB) / sum(vS);
vX          = ProxL1Norm(vY - paramBeta, paramGamma);

end

function [ vX ] = ProxL1Norm( vY, paramLambda )

vX = sign(vY) .* max(abs(vY) - paramLambda, 0);

end


function pdf = pearson_05_pdf ( x, a, b, c )

%*****************************************************************************80
%
%% PEARSON_05_PDF evaluates the Pearson 5 PDF.
%
%  Formula:
%
%    PDF(X)(A,B) = A**B * ( X - C )**(-B-1)
%      * exp ( - A / ( X - C ) ) / Gamma ( B )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the argument of the PDF.
%    C < X
%
%    Input, real A, B, C, the parameters of the PDF.
%    0.0 < A, 0.0 < B.
%
%    Output, real PDF, the value of the PDF.
%
  if ( x <= c )
    pdf = 0.0;
  else
    pdf = a^b * ( x - c )^( - b - 1.0 ) * exp ( - a / ( x - c ) ) / gamma ( b );
  end

  return
end


function y = spm_detrend(x,p)
% Polynomial detrending over columns
% FORMAT y = spm_detrend(x,p)
% x   - data matrix
% p   - order of polynomial [default: 0]
%
% y   - detrended data matrix
%__________________________________________________________________________
%
% spm_detrend removes linear and nonlinear trends from column-wise data
% matrices.
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_detrend.m 7271 2018-03-04 13:11:54Z karl $

% Check for cell arrays
%-------------------------------------------------------------------------
if iscell(x)
    if nargin == 1
        p = 0;
    end
    y     = x;
    for i = 1:numel(x)
        y{i} = spm_detrend(x{i},p);
    end
    return
end

% defaults
%--------------------------------------------------------------------------
[m,n] = size(x);
if ~m || ~n
    y = [];
    return
end
if nargin == 1
    p = 0;
end

% centre columns
%--------------------------------------------------------------------------
if ~p
    y = x - ones(m,1)*mean(x);
    return
end

% polynomial adjustment
%--------------------------------------------------------------------------
G     = zeros(m,p + 1);
for i = 0:p
    d = (1:m).^i;
    G(:,i+1) = d(:);
end
y     = x - G*(pinv(full(G))*x);


function [x y] = BipolarCoordinates(Xi,Eta,a)

% a is radii that defies height of the domain
% a > 0

% Check the input arguments and set default values

if nargin == 2
    a = 1. ;
end
r = Xi ;
s = pi*(Eta-1/2) ;

x = (a*sinh(r))/(cosh(r)+cos(s)) ;
y = (a*sin(s))/(cosh(r)+cos(s)) ;


function a_cum = i4vec_cum ( n, a )

%*****************************************************************************80
%
%% I4VEC_CUM computes the cumulative sum of the entries of an I4VEC.
%
%  Discussion:
%
%    An I4VEC is a vector of I4's.
%
%  Example:
%
%    Input:
%
%      A = (/ 1, 2, 3, 4 /)
%
%    Output:
%
%      A_CUM = (/ 1, 3, 6, 10 /)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 December 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Input, integer A(N), the vector to be summed.
%
%    Output, integer A_CUM(1:N), the cumulative sum of the entries of A.
%
  a_cum(1) = a(1);

  for i = 2 : n
    a_cum(i) = a_cum(i-1) + a(i);
  end

  return
end


function qwv_2d_test02 ( )

%*****************************************************************************80
%
%% QWV_2D_TEST02 tests QWV_2D for Padua points.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 May 2014
%
%  Author:
%
%    John Burkardt
%
  a = -1.0;
  b = +1.0;
  c = -1.0;
  d = +1.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'QWV_2D_TEST02:\n' );
  fprintf ( 1, '  Compute the weights associated with an interpolatory\n' );
  fprintf ( 1, '  quadrature rule defined by N=(T+1)*(T+2)/2 points,\n' );
  fprintf ( 1, '  exact for polynomials of total degree T or less.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  X Interval = [%g,%g]\n', a, b );
  fprintf ( 1, '  Y Interval = [%g,%g]\n', c, d );

  for t = 0 : 10

    n = ( ( t + 1 ) * ( t + 2 ) ) / 2;

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Degree T = %d\n', t );
    fprintf ( 1, '  Number of points N = %d\n', n );

    [ x, y ] = padua_point_set ( t );
%
%  Compute the weights.
%
    w = qwv_2d ( t, n, a, b, c, d, x, y );

    r8vec_print_16 ( n, w, '  Weights:' );

  end

  return
end


function [xi,w]=firstOrder3DCubPoints()
%%FIRSTORDER3DCUBPOINTS Generate first-order cubature points
%               for integration over a 3-dimensional cube with vertices of
%               (1,-1,-1), (-1,-1,-1), (-1,1,-1), (1,1,-1), (1,-1,1),
%               (-1,-1,1), (-1,1,1), and (1,1,1).
%
%INPUTS: None
%
%OUTPUTS: xi This is a 3XnumCubPoints set of points for the standard
%            cube.
%          w A 1XnumCubPoints set of cubature weights. This sums to the
%            volume of the standard cube (8).
%
%This function implements the points given in [1] (1 point). This point is
%just the origin with a weight of 8.
%
%REFERENCES:
%[1] F. D. Witherden and P. E. Vincent, "On the identification of symmetric
%    quadrature rules for finite element methods," Computer and Mathematics
%    with Applications, vol. 69, no. 10, pp. 1232-1241, May 2015.
%
%October 2022 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

w=8;
xi=[0;0;0];

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 p = er_ftest(dof1, dof2, F, dof2max)
%
% p = er_ftest(dof1, dof2, F, <dof2max>)
%
% Computes p-value given F-value. p and F can be vectors.
% dof1 = dof of numerator (Number of Rows in RM)
% dof2 = dof of denominator (DOF)
%
% Ref: Numerical Rec in C, pg 229.
%
% ras, 05/05; based on Ftest in fs-fast code.

if(nargin ~= 3 & nargin ~= 4)
    msg = 'Usage: p = FTest(dof1, dof2, F, <dof2max>)';
    error(msg);
    return;
end

if(length(dof1) > 1)
    error('dof1 must be a scalar');
end
if(length(dof2) > 1)
    error('dof2 must be a scalar');
end

if(nargin == 4) dof2 = min(dof2,dof2max); end

z = dof2./(dof2 + dof1 * F);

% 08/05/04: temp debug stuff, b/c of a funny session
if any(z(:) < 0 | z(:) > 1 | isnan(z(:))) | ~isreal(z)
    fprintf('any < 0?: %i \n',any(z(:) < 0))
    fprintf('any > 1?: %i \n',any(z(:) > 1))
    fprintf('Isnan?: %i \n',any(isnan(z(:))))
    fprintf('~Isreal?: %i \n',~isreal(z))
    z = 0.5 * ones(size(z)); % temp hack so that betainc doesn't fail
end
% fprintf('DEBUG: Z ranges from %f to %f \n',min(z(:)),max(z(:)));

p = betainc(z, dof2/2, dof1/2);

return;


function value = index_to_level_closed ( dim_num, t, order, level_max )

%*****************************************************************************80
%
%% INDEX_TO_LEVEL_CLOSED determines the level of a point given its index.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 November 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    A Sparse Grid Stochastic Collocation Method for Partial Differential
%    Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2309-2345.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer T(DIM_NUM), the grid indices of a point in a 1D closed rule.
%    0 <= T(I) <= ORDER.
%
%    Input, integer ORDER, the order of the rule.
%
%    Input, integer LEVEL_MAX, the level with respect to which the
%    index applies.
%
%    Output, integer VALUE, the first level on which
%    the point associated with the given index will appear.
%
  value = 0;

  for dim = 1 : dim_num

    s = i4_modp ( t(dim), order );

    if ( s == 0 )

      level = 0;

    else

      level = level_max;

      while ( mod ( s, 2 ) == 0 )
        s = floor ( s / 2 );
        level = level - 1;
      end

    end

    if ( level == 0 )
      level = 1;
    elseif ( level == 1 )
      level = 0;
    end

    value = value + level;

  end

  return
end


function [ x,y ] = latlong2xy( lat, long, ref_lat, ref_long )
% converts lat/long coordinates to cartesian x,y, output in km
% ref_lat and ref_long is the geodetic reference point for plane approximation of the earth surface

earth_circumf = 40074; % km

y = (lat - ref_lat)/360 * earth_circumf;
x = (long - ref_long)/360 * cos(ref_lat*pi/180) * earth_circumf;

end


function alpha = dfaScalingExponent(x, minBoxSize, maxBoxSize, pflag)
%
% varargout = dfaScalingExponent(xminBoxSize, midBoxSize, maxBoxSize, pflag)
% calculates the detrended fluctuation analysis estimate of the scaling
% exponent alpha.
%
% INPUTS
%         x          : A Nx1 vector containing the series to be analyzed
%         minBoxSize : Smallest box width (default: 4)
%         maxBoxSize : Largest box width (default: N/4)
%         pflag      : (Optional) pflag=1 plot,  pflag=0
% OUTPUTS
%         alpha      : estimate of scaling exponent, +
%                      minBoxSize <= n <= maxBoxSize
%
% The raw time series x(i) is first integrated to give y(i); i=1,...,N.
% For each length scale, n, y(i) is divided into segments of equal length, n.
% In each segment, the data is detrended by subtracting the local linear least
% squares fit, yn(k).  The root-mean-square fluctuation of this integrated
% and detrended time series is given by
% F(n) = sqrt( (1/N) sum_{k=1}^N [y(k) - yn(k)]^2 )
% We calculate the average fluctuation F(n) for each segment n.
% If the scaling approximately given by F(n) = c n^alpha,
% we can estimate alpha by calculating the slope of log F(n) versus log n.
% Such a linear relationship on a log-log plot indicates the presence of
% power law (fractal) scaling.
% A log-log plot of F(n) against n is provided when pflag=1.  Default: plag=0.
% Peng C-K, Buldyrev SV, Havlin S, Simons M, Stanley HE, Goldberger AL.
% Mosaic organization of DNA nucleotides. Phys Rev E 1994;49:1685-1689.
%
%
% 09-20-2017 Modified by Giulia Da Poian (GDP) to be included in the Physionet
%            HRV Toolkit for Matlab. (Original function name: dfa)
%
%	REPO:
%       https://github.com/cliffordlab/PhysioNet-Cardiovascular-Signal-Toolbox
% Copyright (c) 2005 Patrick E. McSharry (patrick@mcsharry.net)
%
%   LICENSE:
%       This software is offered freely and without warranty under
%       the GNU (v3 or later) public license. See license file for
%       more information

if nargin < 2 || isempty(minBoxSize)
    minBoxSize = 4;
end
if nargin < 3 || isempty(maxBoxSize)
    maxBoxSize = length(x)/4;
end
if nargin < 4
   pflag = 0;
end

if size(x,1)<size(x,2)
    x=x';
end

N = length(x);
y = cumsum(x);

n1 = round(log2(minBoxSize)); % modified GDP, was 3
n2 = round(log2(maxBoxSize)); % modified GDP, was n2 = round(log2(N/2))
ns = (2.^(n1:n2))';           % modified GDP, was ns =[2.^[n1:n2] N]'

nn = length(ns);
F = zeros(nn,1);
for n=1:nn
   t = trend(y, ns(n));
   z = y - t;
   F(n) = sqrt(mean(z.^2));

end

lns = log10(ns);
lF = log10(F);
A = ones(nn,2);
A(:,2) = lns;
a = pinv(A)*lF;
alpha = a(2);
lFpred = A*a;

if pflag == 1
    figure;
    loglog(10.^lns, 10.^lF,'b.-','MarkerSize',16);
    hold on;
    loglog(10.^[lns(1) lns(nn)], 10.^[lFpred(1) lFpred(nn)],'k');
    xlabel('n');
    ylabel('F(n)');
    title(['F(n) ~ n^{\alpha} with \alpha = ' num2str(a(2)) ]);
end

end % dfaScalingExponent function

function t = trend(y, n)
    N = length(y);
    t = zeros(N,1);
    r = floor(N/n);
    for i=1:r
       v = y((i-1)*n+1:i*n);
       t((i-1)*n+1:i*n) = linfit(v);
    end
    v = y(r*n+1:N);
    t(r*n+1:N) = linfit(v);
end % trend function

function up = linfit(v)
    k = length(v);
    A = ones(k,2);
    u = [1:k]';
    A(:,2) = u;
    a = pinv(A)*v;
    up = A*a;
end % linfit function


function [seminnrank,U,V] = seminonnegativerank(M);
% [seminnrank,U,V] = seminonnegativerank(M);
%
% Computes the semi-nonnegative rank and a corresponding factorization of
% matrix M: M = UV, where U has r columns and V >= 0 has r rows and
% r = semi-nonnegative rank of M.
%
% See Corollary 3 in
% N. Gillis, A. Kumar, Exact and Heuristic Algorithms for Semi-Nonnegative
% Matrix Factorization, arXiv, 2014
%
% ****** Input ******
%   M     : m-by-n matrix
%
% ****** Output ******
%   seminnrank  : the (numerical) semi-nonnegative rank of M
%   (U,V)       : M = UV, V >= 0 and U (resp. V) has 'seminnrank' columns
%                  (resp. rows)

r = rank(M);
if issparse(M) ~= 1
    [U,S,V] = svd(M);
    V = V(:,1:r)';
else
    [U,S,V] = svds(M,r);
    V = V';
end

[y,eflag] = linsys_semiNMF(V,0);
if eflag == 1
    seminnrank = r;
    if nargout >= 2
        V = LPinitSemiNMF(M,r);
        U = M/V;
    end
else
    seminnrank = r + 1;
    if nargout >= 2
        [U,V] = SVDinitSemiNMF(M,r+1);
    end
end


function K=fitTwoLines(x,y, showimage)
% K=fitTwoLines(x,y, showimage)
% This fits two adjecent lines into data [x,y] which are partitioed as
% x=[x1,x2] and y=[y1,y2]. It determines K which gives best fit for
% partition x1=x(1:K), x2=x(K:end) ...
% Used for estimation of the 'kink' in the graph.
if ~exist('showimage','var')
    showimage = 0;
end
n=length(y);

for ind_includeMidPoint=0:1 % when 0 then middle point is included (shared by both lines)
    index =0;
    for ii=1:n-ind_includeMidPoint
        index = index + 1;
        x1=x(1:ii);
        y1=y(1:ii);
        x2=x(ii+ind_includeMidPoint:n);
        y2=y(ii+ind_includeMidPoint:n);

        [pbest,perror,nchi]=nonlinft('linearfunction' ,x1,y1,ones(size(x1)),[-1 1e-5],[1 1]);
        pbest1(index,:,ind_includeMidPoint+1)=pbest;
        perror1(index,:,ind_includeMidPoint+1)=perror;
        nchi1(index,ind_includeMidPoint+1)=nchi*(numel(y1)-1); % to compensate for degrees of freedom
        [pbest,perror,nchi]=nonlinft('linearfunction' ,x2,y2,ones(size(x2)),[-1 1e-5],[1 1]);
        pbest2(index,:,ind_includeMidPoint+1)=pbest;
        perror2(index,:,ind_includeMidPoint+1)=perror;
        nchi2(index,ind_includeMidPoint+1)=nchi*(numel(y2)-1); % to compensate for degrees of freedom
    end
end
nn=squeeze((sqrt(perror2(:,1,:).^2+perror2(:,2,:).^2)));
objectiveFunction = (nchi1.^2+nchi2.^2);
mo=min(objectiveFunction,[],2); % minimum the two either including (1) or not (2) the middle point
[del, ixmo]=min(objectiveFunction./(1./nn),[],2); %whether to choose (1) or (2). This is weighted by error of the estimation...
% [m,mx]=min(objectiveFunction);
% [m,mx]=min(mo)+ixmo-1;
% K=x(mx+1);
[m,mx]=min(mo(1:end-1));
K=x(mx+ixmo(mx)-1);

if showimage
    figure;
    plot(x(1:end), mo)
    vline2(K,'r','K');
    xlabel ('x')
    ylabel ('objective function')

    figure;
    plot(x,y,'-o')
    hold on
    plot(x(1:mx), pbest1(mx,1,ixmo(mx))*x(1:mx)+pbest1(mx,2,ixmo(mx)),'g')
    plot(x(mx+ixmo(mx)-1:n), pbest2(mx,1, ixmo(mx))*x(mx+ixmo(mx)-1:n)+pbest2(mx,2,ixmo(mx)),'r')
    xlabel('x')
    ylabel('y')
end


% Copyright (C) 2016, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE 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.
%
% ARTE 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 ARTE.  If not, see <http://www.gnu.org/licenses/>.
function direct_kinematics_symbolic_KUKA_LBR
% link lengths

syms q1 q2 q3 q4 q5 q6 Q7
robot = load_robot('KUKA', 'LBR_IIWA_R820_7DOF')

d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

% matrices DH
A01 = dh_sym(q1, d(1), a(1), alpha(1));
A12 = dh_sym(q2, d(2), a(2), alpha(2));
A23 = dh_sym(q3, d(3), a(3), alpha(3));
A34 = dh_sym(q4, d(4), a(4), alpha(4));
A45 = dh_sym(q5, d(5), a(5), alpha(5));
A56 = dh_sym(q6, d(6), a(6), alpha(6));
A67 = dh_sym(q7, d(7), a(7), alpha(7));

A02 = A01*A12;
A03 = A02*A23;
A04 = A03*A34;
A05 = A04*A45;
A06 = A05*A56;
A07 = A06*A67;

A07 = simplify(A07)

A03 = simplify(A03)

function A = dh_sym(theta, d, a, alpha)
syms q1 q2 q3 q4 q5 q6
% avoid almost zero elements in cos(alpha) and sin(alpha)
ca = cos(alpha);
sa = sin(alpha);
if abs(ca) < 1e-6
    ca = 0;
end
if abs(sa) < 1e-6
    sa = 0;
end

A=[cos(theta)  -ca*sin(theta)   sa*sin(theta)   a*cos(theta);
   sin(theta)   ca*cos(theta)  -sa*cos(theta)   a*sin(theta);
            0         sa             ca             d;
            0         0               0             1];


function circle_inout ( )

%*****************************************************************************80
%
%% CIRCLE_INOUT plots data in and out of a circle.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 April 2011
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CIRCLE_INOUT:\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Make a scatterplot of two sets of data representing\n' );
  fprintf ( 1, '  points in the unit square that are also in or not in\n' );
  fprintf ( 1, '  the unit circle.\n' );
%
%  Read the data.
%
  xy_in = load ( 'circle_in.txt' );
  [ n_in, dim ] = size ( xy_in );

  xy_out = load ( 'circle_out.txt' );
  [ n_out, dim ] = size ( xy_out );

  n = n_in + n_out;
%
%  Report on the estimate for pi:
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of points inside the circle = %d\n', n_in );
  fprintf ( 1, '  Number outside                     = %d\n', n_out );
  fprintf ( 1, '  Total                              = %d\n', n );
  fprintf ( 1, '  Estimate for PI                    = %d\n', 4 * n_in / n );
%
%  Set up points on a circle
%
  n = 50;
  center = [ 0.0, 0.0 ];
  radius = 1.0;
  theta1 = 0.0;
  theta2 = 90.0;

  xy_circ = circle_arc ( n, center, radius, theta1, theta2 );
%
%  Plot the data.
%
  plot ( xy_in(:,1),  xy_in(:,2),  'b.', ...
         xy_out(:,1), xy_out(:,2), 'r.', ...
         xy_circ(:,1), xy_circ(:,2), 'r-', 'LineWidth', 2 );
  grid on
  axis ( [ 0.0, 1.0, 0.0, 1.0 ] )
  axis equal
  axis square
  xlabel ( '<--- X --->' );
  ylabel ( '<--- Y --->' );
  title ( 'Random points inside/outside the unit circle' );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CIRCLE_INOUT:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  return
end
function xy_circ = circle_arc ( n, center, radius, theta1, theta2 )

%*****************************************************************************80
%
%% CIRCLE_ARC samples points on a circular arc.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 April 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of sample points.
%
%    Input, real CENTER(2), the center of the circle.
%
%    Input, real RADIUS, the radius of the circle.
%
%    Input, real THETA1, THETA2, the angular coordinates of the first and
%    last points on the arc, in degrees.
%
%    Output, real XY_CIRC(N,2), points along the arc.
%
  t = pi * linspace ( theta1, theta2, n ) / 180;
  t = t';

  xy_circ = zeros ( n, 2 );

  xy_circ(:,1) = center(1) + radius * cos ( t(:,1) );
  xy_circ(:,2) = center(2) + radius * sin ( t(:,1) );

  return
end


function s = hermite_integral ( p )

%*****************************************************************************80
%
%% HERMITE_INTEGRAL evaluates a monomial Hermite integral.
%
%  Discussion:
%
%    Integral ( -oo < x < +oo ) x^p exp(-x^2) dx
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 November 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer P, the exponent.
%    0 <= P.
%
%    Output, real S, the value of the integral.
%
  if ( mod ( p, 2 ) == 0 )
    s = r8_factorial2 ( p - 1 ) * sqrt ( pi ) / 2.0 ^ ( p / 2 );
  else
    s = 0.0;
  end

  return
end


function A = minimum_spanning_tree(C1, C2)
%
% Find the minimum spanning tree using Prim's algorithm.
% C1(i,j) is the primary cost of connecting i to j.
% C2(i,j) is the (optional) secondary cost of connecting i to j, used to break ties.
% We assume that absent edges have 0 cost.
% To find the maximum spanning tree, used -1*C.
% See Aho, Hopcroft & Ullman 1983, "Data structures and algorithms", p 237.

% Prim's is O(V^2). Kruskal's algorithm is O(E log E) and hence is more efficient
% for sparse graphs, but is implemented in terms of a priority queue.

% We partition the nodes into those in U and those not in U.
% closest(i) is the vertex in U that is closest to i in V-U.
% lowcost(i) is the cost of the edge (i, closest(i)), or infinity is i has been used.
% In Aho, they say C(i,j) should be "some appropriate large value" if the edge is missing.
% We set it to infinity.
% However, since lowcost is initialized from C, we must distinguish absent edges from used nodes.

n = length(C1);
if nargin==1, C2 = zeros(n); end
A = zeros(n);

closest = ones(1,n);
used = zeros(1,n); % contains the members of U
used(1) = 1; % start with node 1
C1(find(C1==0))=inf;
C2(find(C2==0))=inf;
lowcost1 = C1(1,:);
lowcost2 = C2(1,:);

for i=2:n
  ks = find(lowcost1==min(lowcost1));
  k = ks(argmin(lowcost2(ks)));
  A(k, closest(k)) = 1;
  A(closest(k), k) = 1;
  lowcost1(k) = inf;
  lowcost2(k) = inf;
  used(k) = 1;
  NU = find(used==0);
  for ji=1:length(NU)
    for j=NU(ji)
      if C1(k,j) < lowcost1(j)
	lowcost1(j) = C1(k,j);
	lowcost2(j) = C2(k,j);
	closest(j) = k;
      end
    end
  end
end


%gs_top_to_bottom   test top_to_bottom Gauss-Seidel iteration
%   IFISS scriptfile: HCE; 28 January 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage

% Top-to-bottom Gauss-Seidel iterative solution of system Asupg x = fsupg
% starting with zero initial guess, for problem defined on n x n grid.

% For Figure 4.4 of Chapter 4:
%    generate benchmark problem with cd_testproblem (using prescribed
%    outflow for Example 3.1.1), plot residual using command
%       semilogy(stats(:,1),stats(:,2)/stats(1,2));

Q = triu(Asupg,-1);
xgs = zeros(length(fsupg),1);

nf = norm(fsupg);
r = fsupg - Asupg*xgs;
nr = norm(r);
its = 0;
stats = [its,nr];
fprintf('\n%5i %15.3e\n', its, nr);

tol = 1.d-6;

[L,U] = lu(Q);
while nr/nf > tol,
   xgs = xgs + U\(L\r);
   r = fsupg - Asupg*xgs;
   nr = norm(r);
   its = its + 1;
   stats = [stats;[its,nr]];
   fprintf('%5i %15.3e\n', its, nr);
end


%Finite Element Method 101-2
%National Taiwan University
%2D Elasticity problem

%%clear memory
close all; clc; clear all;
format long;
%% Load Mesh for Lab 10
[nodeCoordinates,elementNodes]=Mesh_Lab10('T3');
% node coordinates are given in mm
NodePerElement=3;
numberNodes=length(nodeCoordinates);
numberElements=length(elementNodes);

%% Import BCs

% Essential BC's
GDof = 2*numberNodes;
prescribedDof = [1,2,3,4];

% Natural BC's
force = zeros(GDof,1);
force(end) = -10; % 10 [kN]

%% Import material and section properties
E = 3E7; % [GPa]
poisson = 0.3; %[-]
thickness = 1; % [mm]

%% Evalute force vector
%force=formForceVectorT3(GDof,naturalBCs,surfaceOrientation,...
%    elementNodes,nodeCoordinates,P,thickness);

%% Construct Stiffness matrix for T3 element
D=E/(1-poisson^2)*[1 poisson 0;poisson 1 0;0 0 (1-poisson)/2];

stiffness=formStiffness2D(GDof,numberElements,...
    elementNodes,numberNodes,nodeCoordinates,D,thickness);

%% solution
displacements=solution(GDof,prescribedDof,stiffness,force);

%% output displacements
outputDisplacements(displacements, numberNodes, GDof);
scaleFactor=1.E5;
drawingMesh(nodeCoordinates+scaleFactor*[displacements(1:2:2*numberNodes) ...
    displacements(2:2:2*numberNodes)],elementNodes,'T3','r--');

% Computes elements stresses
for e=1:numberElements
  numNodePerElement = length(elementNodes(e,:));
  numEDOF = 2*numNodePerElement;
  elementDof=zeros(1,numEDOF);
  for i = 1:numNodePerElement
      elementDof(2*i-1)=2*elementNodes(e,i)-1;
      elementDof(2*i)=2*elementNodes(e,i);
  end

  %  B matrix
  x1 = nodeCoordinates(elementNodes(e,1),1);
  y1 = nodeCoordinates(elementNodes(e,1),2);
  x2 = nodeCoordinates(elementNodes(e,2),1);
  y2 = nodeCoordinates(elementNodes(e,2),2);
  x3 = nodeCoordinates(elementNodes(e,3),1);
  y3 = nodeCoordinates(elementNodes(e,3),2);
  A = 1/2*det([1 x1 y1; 1 x2 y2; 1 x3 y3]);
  B = 1/(2*A).*[y2-y3 0 y3-y1 0 y1-y2 0;
                        0 x3-x2 0 x1-x3 0 x2-x1;
                        x3-x2 y2-y3 x1-x3 y3-y1 x2-x1 y1-y2];

  stress=D*B*displacements(elementDof);
  vonmises=sqrt(0.5*((stress(1)-(stress(2)))^2+(stress(2))^2+(stress(1))^2+6*(stress(3))^2));
  fprintf('\n Stress in element % u \n',e)
  fprintf('Sigma_xx : %0 .6f \n',stress(1))
  fprintf('Sigma_yy : %0 .6f \n',stress(2))
  fprintf('Sigma_xy : %0 .6f \n',stress(3))
  fprintf('Vonmises : %0 .6f \n',vonmises)
end


function [h,h1,h2] = maxflatI(K,M)
% [h,h1,h2] = maxflatI(K,M)
% Maxflat Type-I FIR filter
%   2K zeros at z=-1
%   2M zeros away from z=-1
%   h = conv(h1,h2);
%   h1 : all zeros at z=-1
%   h2 : all other zeros
%
% Note: if K = M+1, then h is halfband.
%
% Reference:
% O. Herrmann, "On the approximation problem in Nonrecursive
% Digital Filter Design", IEEE Trans. on Circuit Theory,
% Vol. 18, No. 3, May 1971, pp. 411-413
%
% % Example
% [h,h1,h2] = maxflatI(4,6);

% Ivan Selesnick
% selesi@nyu.edu
% NYU - School of Engineering

h2 = 1;
hi = 1;
c  = 1;
for k = 1:M
   hi = conv(hi,[-1 2 -1]/4);
   c  = c*(K-1+k)/k;
   h2 = [0 h2 0] + c*hi;
end

h1 = 1;
for k = 1:2*K
   h1 = conv(h1,[1 1]/2);
end

h = conv(h1,h2);


function [ g ] = gsp_design_regular(G, param)
%GSP_DESIGN_REGULAR Create a regular filterbank
%   Usage: g = gsp_design_regular( G );
%          g = gsp_design_regular( G, param );
%
%   Inputs parameters:
%       G       : Graph structure or lmax
%       param   : Structure of optional parameters
%
%   Outputs parameters:
%       g       : filterbank
%
%   This function creates a parseval filterbank of $2$ filters. The low-pass
%   filter is defined by a function $f_l(x)$ between $0$ and $2$. For
%   $d = 0$.
%
%   ..    f_l(x) = sin(pi/4*x)
%
%   .. math:: f_{l}= \sin\left( \frac{\pi}{4} x \right)
%
%   For $d = 1$
%
%   ..    f_l(x) = sin( pi/4 * (1+sin(pi/2*(x-1))) )
%
%   .. math:: f_{l}= \sin\left( \frac{\pi}{4} \left( 1+ \sin\left(\frac{\pi}{2}(x-1)\right) \right) \right)
%
%   For $d = 2$
%
%   ..    f_l(x) = sin( pi/4 * ( 1 + sin( pi/2 * sin(pi/2*(x-1) ) ) )
%
%   .. math:: f_{l}= \sin\left( \frac{\pi}{4} \left( 1+ \sin\left(\frac{\pi}{2} \sin\left(\frac{\pi}{2}(x-1)\right)\right) \right) \right)
%
%   And so on for the other degrees $d$.
%
%   The high pass filter is adapted to obtain a tight frame.
%
%   This function will compute the maximum eigenvalue of the laplacian. To
%   be more efficient, you can precompute it using::
%
%       G = gsp_estimate_lmax(G);
%
%   Example:::
%
%         G = gsp_sensor(100);
%         G = gsp_estimate_lmax(G);
%         g = gsp_design_regular(G);
%         gsp_plot_filter(G,g);
%         [A,B] = gsp_filterbank_bounds(G,g)
%
%   *param* is an optional structure containing the following fields
%
%   * *param.verbose*: verbosity level. 0 no log - 1 display warnings.
%     (default 1)
%   * *param.d*: Degree. See equation for mor informations. (default 3)
%

% Author: Nathanael Perraudin, David Shuman
% Date  : 21 June 2014
% Testing: test_filter

if nargin < 2
    param = struct;
end

if ~isfield(param,'verbose'), param.verbose = 1; end
if ~isfield(param,'d'), param.d = 3; end

if isstruct(G)
    if ~isfield(G,'lmax')
        if param.verbose
            fprintf('GSP_DESIGN_REGULAR has to compute lmax \n')
        end
        G = gsp_estimate_lmax(G);
    end
   lmax = G.lmax;
else
   lmax = G;
end

d = param.d;

g = cell(2,1);
g{1} = @(x) regular(x*(2/lmax),d);
g{2} = @(x) real(sqrt(1-(regular(x*(2/lmax),d)).^2));

end

function y = regular(val,d)

if d==0
    y = sin(pi/4*val);
else
    output = sin(pi*(val-1)/2);
    for k=2:d
        output = sin(pi*output/2);
    end
    y = sin(pi/4*(1+output));
end

end


function y = test_h(x,index)
%TEST_H       Some test functions collected from Coconut
%
%   y = test_h(x,index)
%
%Input index specifies test function. Copyright see below.
%

% written  04/04/04     S.M. Rump
%

switch index

  case 1     % source see bottom of file
    y = x(3)-1 + x(1).^2 + x(2).^2 + (x(3)+x(4)).^2 + sin(x(3)).^2 + x(1).^2*x(2).^2 + ...
        x(4)-3 + sin(x(3)).^2 + (x(4)-1).^2 + x(2).^4 + x(3).^4 + (x(4)+x(1)).^2 + ...
        (x(1)-4 + sin(x(4)).^2 + x(2).^2*x(3).^2).^2 + sin(x(4)).^4;

  case 2     % source see bottom of file
    N = length(x);      % model problem: N = 1000, initial x=ones(N,1);
    I = 1:N-4;
    y = sum( (-4*x(I)+3.0).^2 ) + sum( ( x(I).^2 + 2*x(I+1).^2 + ...
              3*x(I+2).^2 + 4*x(I+3).^2 + 5*x(N).^2 ).^2 );

end

% function 1 taken from   http://www.sor.princeton.edu/~rvdb/ampl/nlmodels/cute/allinitu.mod
% # AMPL Model by Hande Y. Benson
% #
% # Copyright (C) 2001 Princeton University
% # All Rights Reserved
% #
% # Permission to use, copy, modify, and distribute this software and
% # its documentation for any purpose and without fee is hereby
% # granted, provided that the above copyright notice appear in all
% # copies and that the copyright notice and this
% # permission notice appear in all supporting documentation.
%
% #   Source:
% #   N. Gould, private communication.
%
% #   SIF input: Nick Gould, June 1990.
%
% #   classification OUR2-AY-4-0
%
% var x{1..4};
%
% minimize f:
% x[3]-1 +
% x[1]^2+
% x[2]^2 + (x[3]+x[4])^2 +
% sin(x[3])^2 + x[1]^2*x[2]^2 + x[4]-3 +
% sin(x[3])^2 +
% (x[4]-1)^2 +
% (x[2]^2)^2+
% (x[3]^2 + (x[4]+x[1])^2)^2 +
% (x[1]-4 + sin(x[4])^2 + x[2]^2*x[3]^2)^2 +
% sin(x[4])^4;
%
% solve;
% display f;
% display x;

% function 2 taken from   http://www.sor.princeton.edu/~rvdb/ampl/nlmodels/cute/bdqrtic.mod
% # AMPL Model by Hande Y. Benson
% #
% # Copyright (C) 2001 Princeton University
% # All Rights Reserved
% #
% # Permission to use, copy, modify, and distribute this software and
% # its documentation for any purpose and without fee is hereby
% # granted, provided that the above copyright notice appear in all
% # copies and that the copyright notice and this
% # permission notice appear in all supporting documentation.
%
% #   Source: Problem 61 in
% #   A.R. Conn, N.I.M. Gould, M. Lescrenier and Ph.L. Toint,
% #   "Performance of a multifrontal scheme for partially separable
% #   optimization",
% #   Report 88/4, Dept of Mathematics, FUNDP (Namur, B), 1988.
%
% #   SIF input: Ph. Toint, Dec 1989.
%
% #   classification SUR2-AN-V-0
%
% param N:=1000;
% var x{1..N} := 1.0;
%
% minimize f:
% sum {i in 1..N-4} (-4*x[i]+3.0)^2 + sum {i in 1..N-4} (x[i]^2+2*x[i+1]^2+3*x[i+2]^2+4*x[i+3]^2+5*x[N]^2)^2;
%
% solve;
% display f;
% display x;


%% UNO Rosenbrock
clc
%Objective
obj = @(x) (1-x(1))^2 + 100 *(x(2)-x(1)^2)^2;
%Setup Options
opts = optiset('solver','ipopt','display','iter');
%Build & Solve
Opt = opti('obj',obj,'ndec',2,'options',opts)
x0 = [0 0]';
[x,fval,exitflag,info]= solve(Opt,x0)
%Plot
% plot(Opt,[],1)

%% NLP1 Hock & Schittkowski #71
clc
%Objective & Gradient
obj = @(x) x(1)*x(4)*sum(x(1:3)) + x(3);
grad = @(x) [ x(1)*x(4) + x(4)*sum(x(1:3));
              x(1)*x(4);
              x(1)*x(4) + 1;
              x(1)*sum(x(1:3)) ];
%Linear Constraints
lb = ones(4,1);
ub = 5*ones(4,1);
%Nonlinear Constraints
nlcon = @(x) [ prod(x);
               sum(x.^2)];
nljac = @(x) [ prod(x)./x';
                2*x' ];
nlrhs = [25 40]';
nle = [1 0]'; % (>=, ==)
%Setup Options
opts = optiset('solver','ipopt','warnings','on','display','iter','solverOpts',ipoptset('linear_solver','pardiso'));
%Build & Solve
Opt = opti('obj',obj,'grad',grad,'nlmix',nlcon,nlrhs,nle,'nljac',nljac,'bounds',lb,ub,'options',opts)
x0 = [1 5 5 1]';
[x,fval,exitflag,info]= solve(Opt,x0)

info.Lambda

%%
clc
prob = amplRead('opf_662bus');
opts = optiset('solver','ipopt','solverOpts',ipoptset('linear_solver','pardiso'),'display','off');
opts2 = optiset('solver','ipopt','solverOpts',ipoptset('linear_solver','ma57'),'display','off');
opts3 = optiset('solver','ipopt','solverOpts',ipoptset('linear_solver','mumps'),'display','off');
O = opti(prob,opts)
O2 = opti(prob,opts2);
O3 = opti(prob,opts3);

[x,f,e,i] = solve(O);
[x2,f,e,i2] = solve(O2);
[x3,f,e,i3] = solve(O3);

norm(x-x2)

i
i2
i3

%%
clc
clear funcs opts

x0 = [0; 0];

funcs.objective = @(x) (1-x(1))^2 + 100 *(x(2)-x(1)^2)^2;
funcs.gradient = @(x)[2*x(1)-400*x(1)*(x(2)-x(1)^2)-2,200*x(2)-200*x(1)^2];

opts.ipopt.print_level = 5;
opts.ipopt.hessian_approximation = 'limited-memory';
opts.ipopt.ma57_pivot_order = 2;
opts.ipopt.linear_solver = 'pardiso';

[x,f] = ipopt(x0,funcs,opts)

f.eval

%%

prob = amplRead('ch3.nl')
opts = optiset('solver','ipopt','display','iter','solverOpts',ipoptset('linear_solver','pardiso'));

Opt = opti(prob,opts)

x = solve(Opt)

asl('close')


%--------------------------------------------------------
% FIR filter design via local moving window LS fitting  -
% A magic smooth and derivative formula generator.      -
% By Dr Yangquan Chen		08-07-1999                   -
% Email=<yqchen@ieee.org>; URL=http://www.crosswinds.net/~yqchen/
% -------------------------------------------------------
% Purpose: general FIR design via local LS fitting.
% total taps = nL+nR+1
% Format: function [c]=sgfilter(nL,nR,M,id)
%          -nL      -nL+1           -1                nR
% FIR=c(1)z   +c(2)z     +...+c(nL)z  +...+c(nL+nR+1)z
%
%       nR
%     ------
%     \                   j
%      >       c(nL+1+j) z
%     /
%     ------
%      j=-nL
% M: the order of LS fit at the moving window of [-nL, ... , nR]
% id: index for the derivative order
%		0: smooth filter,
%		1: 1st order differentiator,
% 		2: 2nd order differentiator, ...
% NOTE: M>=id, set M=(2~4)*(id+1) for reliably results.
%		  to do LS fit, M<nL+nR+1.
%---------------------------------------------------------------
function [c]=sgfilter(nL,nR,M,id)
% Savitzky-Golay smoothing filter.
if (id>M)
   disp('Error in id! (id<M)');return;
end
if (M>(nL+nR))
   disp('Error in M! (M<=nL+nR)');return;
end

A=zeros(nL+nR+1,M+1);
for i=-nL:nR;
   for j=0:M;
      A(i+nL+1,j+1)=i^j;
   end
end
h=zeros(M+1,1);
%h(1)=1;
h(id+1)=1;
b=inv(A'*A)*h;
c=zeros(nL+nR+1,1);
for n=-nL:nR
   nm=n.^[0:M];
   c(n+nL+1)=nm*b;
end
% coefficient for smoothing
return


%       Created by Dwight Nwaigwe  April 2011
%	This program uses the ensemble kalman filter to estimate a system's state.
%	The state is x_new=f(x,u)+w, where u some input, w the
%	Gaussian distributed process noise, and f is a nonlinear function. The measurement
%	is y_new=h(x)+v where h is a nonlinear function and v Gaussian distributed measurement noise.

%       The algorithm used in this code is referenced from the following:
%       S Gillijns et. al., "What Is the Ensemble Kalman Filter and How Well Does it Work?"
%       Proceedings of the 2006 American Control Conference,
%       Minneapolis, Minnesota, USA, June 14-16, 2006, pp 4448-4453.

function [x_tr,x_estbar,ybar]= ensemblekfilter(f,h,x_tr,x_ini,w,z,num_iterations)

%      Example

%      The state and measurement in this example  is taken from Dan Simon, "Kalman Filtering",
%      Embedded Systems Programming,2001.
%
%
%	syms  x1 x2;   %variables must be named x1...xn
%	f=[x1+.1*x2+.005;x2+.1];
%	h=[x1];
%	x_tr=[1;1]; %initial value of state
%	x_ini=ones(2,20); %ensemble of initial estimate of the state
%	w=[10^-3; .02];  %process noise standard deviation
%	z=[10];  %measurement noise standard deviation
%	num_iterations=600;
% 	num_members=20;
% 	[a,b,c]=ensemblekfilter(f,h,x_tr,x_ini,w,z,num_iterations);

[dummy,num_members]=size(x_ini);
p1=length(f);
m1=length(h);
var_vector=[];
xvec=[];
x_estvec=[];
yvec=[];
x_est=x_ini;

for j=1:p1  %create vector containing variables x1 to xn
  eval(sprintf(' syms x%d', j));
  var_vector=[var_vector sprintf('x%d ',j)];
  end
var_vector=strcat('[',var_vector);
var_vector=strcat(var_vector,']');

Zcov=eye(m1); %create measurement noise covariance matrix
for j=1:m1
  Zcov(j,j)=z(j)^2;
end

for i=1:num_iterations

   x_tr=subs(f,var_vector,x_tr)+w.*randn(p1,1); %compute true value of state at next time step

   for j=1:num_members
     W(:,j)=w.*randn(p1,1);                          %create process noise
     Z(:,j)=z.*randn(m1,1);                          %create measurement noise
     x_est(:,j)=subs(f,var_vector,x_est(:,j))+W(:,j);      %forecast state
     y(:,j)=subs(h,var_vector,x_tr)+Z(:,j);                 %make measurement
     y_for(:,j)=subs(h,var_vector,x_est(:,j));              %forecast measurement
   end

   x_estbar=mean(x_est,2);
   ybar=mean(y,2);
   y_forbar=mean(y_for,2);

   for j=1:p1
     Ex(j,:)=[x_est(j,:)-x_estbar(j)];
   end

   for j=1:m1
     Ey(j,:)=[y_for(j,:)-y_forbar(j)];
   end

   Pxy=Ex*Ey'/(num_members-1);
   Pyy=Ey*Ey'/(num_members-1)+Zcov;                     %The addition of Zcov to Pyy is not done in Gillijns et. al but I use it here in case num_members=2 or Pyy is nearly singular
   K=Pxy*inv(Pyy);
   x_est=x_est+K*(y-y_for);
   xvec=[xvec x_tr];
   x_estvec=[x_estvec x_estbar];
   yvec=[yvec ybar];
   if i==num_iterations
   x_estbar=mean(x_est,2);
   end
end


function n = fitNormal(data, show_graph)
%FITNORMAL - Fit a plane to the set of coordinates
%
%For a passed list of points in (x,y,z) cartesian coordinates,
%find the plane that best fits the data, the unit vector
%normal to that plane with an initial point at the average
%of the x, y, and z values.
%
% :param data: Matrix composed of of N sets of (x,y,z) coordinates
%              with dimensions Nx3
% :type data: Nx3 matrix
%
% :param show_graph: Option to display plot the result (default false)
% :type show_graph: logical
%
% :return n: Unit vector that is normal to the fit plane
% :type n: 3x1 vector

	if nargin == 1
		show_graph = false;
	end

	for i = 1:3
		X = data;
		X(:,i) = 1;

		X_m = X' * X;
		if det(X_m) == 0
			can_solve(i) = 0;
			continue
		end
		can_solve(i) = 1;

		% Construct and normalize the normal vector
		coeff = (X_m)^-1 * X' * data(:,i);
		c_neg = -coeff;
		c_neg(i) = 1;
		coeff(i) = 1;
		n(:,i) = c_neg / norm(coeff);

	end

	if sum(can_solve) == 0
		error('Planar fit to the data caused a singular matrix.')
		return
	end

	% Calculating residuals for each fit
	center = mean(data);
	off_center = [data(:,1)-center(1) data(:,2)-center(2) data(:,3)-center(3)];
	for i = 1:3
		if can_solve(i) == 0
			residual_sum(i) = NaN;
			continue
		end

		residuals = off_center * n(:,i);
		residual_sum(i) = sum(residuals .* residuals);

	end

	% Find the lowest residual index
	best_fit = find(residual_sum == min(residual_sum));

	% Possible that equal mins so just use the first index found
	n = n(:,best_fit(1));

	if ~show_graph
		return
	end

	range = max(max(data) - min(data)) / 2;
	mid_pt = (max(data) - min(data)) / 2 + min(data);
	xlim = [-1 1]*range + mid_pt(1);
	ylim = [-1 1]*range + mid_pt(2);
	zlim = [-1 1]*range + mid_pt(3);

	L=plot3(data(:,1),data(:,2),data(:,3),'ro','Markerfacecolor','r'); % Plot the original data points
	hold on;
	set(get(L, 'Parent'),'DataAspectRatio',[1 1 1],'XLim',xlim,'YLim',ylim,'ZLim',zlim);

	norm_data = [mean(data); mean(data) + (n' * range)];

	% Plot the original data points
	L=plot3(norm_data(:,1),norm_data(:,2),norm_data(:,3),'b-','LineWidth',3);
	set(get(get(L,'parent'),'XLabel'),'String','x','FontSize',14,'FontWeight','bold')
	set(get(get(L,'parent'),'YLabel'),'String','y','FontSize',14,'FontWeight','bold')
	set(get(get(L,'parent'),'ZLabel'),'String','z','FontSize',14,'FontWeight','bold')
	title(sprintf('Normal Vector: <%0.3f, %0.3f, %0.3f>',n),'FontWeight','bold','FontSize',14)
	grid on;
	axis square;
	hold off;
end


function [isinside,pt,coord]=linextriangle(p0,p1,plane)
%  [isinside,pt,coord]=linextriangle(p0,p1,plane)
%
%  calculate the intersection of a 3d line (passing two points)
%  with a plane (determined by 3 points)
%
%  author: Qianqian Fang <q.fang at neu.edu>
%  date: 12/12/2008
%
% parameters:
%      p0: a 3d point in form of (x,y,z)
%      p1: another 3d point in form of (x,y,z), p0 and p1 determins the line
%      plane: a 3x3 matrix, each row is a 3d point in form of (x,y,z)
%             this is used to define a plane
% outputs:
%      isinside: a boolean variable, 1 for the intersection is within the
%               3d triangle determined by the 3 points in plane; 0 is outside
%      pt: the coordinates of the intersection pint
%      coord: 1x3 vector, if isinside=1, coord will record the barycentric
%          coordinate for the intersection point within the triangle;
%          otherwise it will be all zeros.
%
% for degenerated lines or triangles, this will stop
%
% Please find more information at http://iso2mesh.sf.net/cgi-bin/index.cgi?metch
%
% this function is part of "metch" toobox, see COPYING for license

[a,b,c,d]=getplanefrom3pt(plane);

if(a*a+b*b+c*c==0.0)
        error('degenerated plane');
end

dl_n=sum([a b c].*(p1-p0));

if(dl_n==0.0)
        error('degenerated line');
end

% solve for the intersection point
t=-(a*p0(1)+b*p0(2)+c*p0(3)+d)/dl_n;
pt=p0+(p1-p0)*t;

dist=sum(abs(diff(plane)));
[md,imax]=sort(dist);
if(md(2)==0.0)
        error('degenerated triangle');
end
goodidx=imax(2:end);

ptproj=pt(goodidx);
mat0=[plane(:,goodidx)',ptproj';1 1 1 1];

isinside=0;
coord=[0 0 0];

det1=det(mat0(:,[4 2 3],:));
det2=det(mat0(:,[1 4 3],:));
if(det1*det2<0)
        return;
end
det3=det(mat0(:,[1 2 4],:));
if(det2*det3<0)
        return;
end
if(det1*det3<0)
        return;
end
isinside=1;
det0=det(mat0(:,1:3));

coord=[det1 det2 det3]/det0;


function p=dir_pdf(x,a)
%DIR_PDF   Probability density function of uniform Dirichlet
%          distribution
%
%       Description:
%       P = DIR_PDF(X, A) returns the pdf of Dirichlet distribution
%       with A at X
%
% Copyright (c) 2000 Aki Vehtari

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

p=exp(gammaln(sum(a))-sum(gammaln)+sum(log(x).^(a-1)));


function imResult = blendMode_Screen(A, B, offsetW, offsetH)
%% Screen blending mode: the values of the pixels in the two layers are
%   inverted, multiplied, and then inverted again. This yields the opposite
%   effect to multiply. The result is a brighter picture.
%
% Input:
%       A       -       Base Image
%       B       -       Top Image
%   offsetW     -   move picture B horizontally in respect to the top-left
%                   corner of picture A. Default value = 1.
%   offsetH     -   move picture B vertically in respect to the top-left
%                   corner of picture A. Default value = 1.
%
% Output:
%       imResult    -   Result of the blending, having the same size of the
%                       Base Image A.
%

%% Check Input
a = size(A);
b = size(B);
blendMode_checkInput(nargin, a, b, func2str(@blendMode_Screen));

if nargin < 3
    offsetW = 1;
    offsetH = 1;
end

if nargin < 4
    offsetH = 1;
end

%% Implementation
imResult = A;

if (((offsetW ~= 1) || (offsetH ~= 1)) || (sum(a == b) ~= length(a)))
    [A, B] = blendMode_ResizeImages(A, B, a, b, offsetW, offsetH);
end

C = 1 - (1 - A) .* (1 - B);

if (((offsetW ~= 1) || (offsetH ~= 1)) || (sum(a == b) ~= length(a)))
    imResult = blendMode_CreateResult(imResult, C, offsetW, offsetH);
else
    imResult = C;
end

end


function value = cc_abscissa ( order, i )

%*****************************************************************************80
%
%% CC_ABSCISSA returns the I-th abscissa of the Clenshaw Curtis rule.
%
%  Discussion:
%
%    Our convention is that the abscissas are numbered from left to
%    right.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 March 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer ORDER, the order of the rule.
%
%    Input, integer I, the index of the desired abscissa.  1 <= I <= ORDER.
%
%    Output, real VALUE, the value of the I-th abscissa in the
%    rule of order ORDER.
%
  if ( order < 1 )
    value = - Inf;
  elseif ( i < 1 | order < i )
    value = - Inf;
  elseif ( order == 1 )
    value = 0.0;
  elseif ( 2 * ( order - i ) == order - 1 )
    value = 0.0;
  else
    value = cos ( ( order - i ) * pi / ( order - 1 ) );
  end

  return
end


function fe = spline_pchip_val ( n, x, f, d, ne, xe )

%*****************************************************************************80
%
%% SPLINE_PCHIP_VAL evaluates a piecewise cubic Hermite function.
%
%  Description:
%
%    This routine may be used by itself for Hermite interpolation, or as an
%    evaluator for SPLINE_PCHIP_SET.
%
%    This routine evaluates the cubic Hermite function at the points XE.
%
%    Most of the coding between the call to CHFEV and the end of
%    the IR loop could be eliminated if it were permissible to
%    assume that XE is ordered relative to X.
%
%    CHFEV does not assume that X1 is less than X2.  Thus, it would
%    be possible to write a version of SPLINE_PCHIP_VAL that assumes a strictly
%    decreasing X array by simply running the IR loop backwards
%    and reversing the order of appropriate tests.
%
%    The present code has a minor bug, which I have decided is not
%    worth the effort that would be required to fix it.
%    If XE contains points in [X(N-1),X(N)], followed by points less than
%    X(N-1), followed by points greater than X(N), the extrapolation points
%    will be counted (at least) twice in the total returned in IERR.
%
%    The evaluation will be most efficient if the elements of XE are
%    increasing relative to X; that is, for all J <= K,
%      X(I) <= XE(J)
%    implies
%      X(I) <= XE(K).
%
%    If any of the XE are outside the interval [X(1),X(N)],
%    values are extrapolated from the nearest extreme cubic,
%    and a warning error is returned.
%
%    This routine was originally named "PCHFE".
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 August 2005
%
%  Author:
%
%    Original FORTRAN77 version by Fred Fritsch.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Fred Fritsch, Ralph Carlson,
%    Monotone Piecewise Cubic Interpolation,
%    SIAM Journal on Numerical Analysis,
%    Volume 17, Number 2, April 1980, pages 238-246.
%
%  Parameters:
%
%    Input, integer N, the number of data points.  N must be at least 2.
%
%    Input, real X(N), the strictly increasing independent
%    variable values.
%
%    Input, real F(N), the function values.
%
%    Input, real D(N), the derivative values.
%
%    Input, integer NE, the number of evaluation points.
%
%    Input, real XE(NE), points at which the function is to
%    be evaluated.
%
%    Output, real FE(NE), the values of the cubic Hermite
%    function at XE.
%

%
%  Check arguments.
%
  if ( n < 2 )
    ierr = -1;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'SPLINE_PCHIP_VAL - Fatal error!\n' );
    fprintf ( 1, '  Number of data points less than 2.\n' );
    error ( 'SPLINE_PCHIP_VAL - Fatal error!' );
  end

  for i = 2 : n
    if ( x(i) <= x(i-1) )
      ierr = -3;
      fprintf ( 1, '\n' );
      fprintf ( 1, 'SPLINE_PCHIP_VAL - Fatal error!\n' );
      fprintf ( 1, '  X array not strictly increasing.\n' );
      error ( 'SPLINE_PCHIP_VAL - Fatal error!' );
    end
  end

  if ( ne < 1 )
    ierr = -4;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'SPLINE_PCHIP_VAL - Fatal error!\n' );
    fprintf ( 1, '  Number of evaluation points less than 1.\n' );
    fe = [];
    return
  end

  ierr = 0;
%
%  Loop over intervals.
%  The interval index is IL = IR-1.
%  The interval is X(IL) <= X < X(IR).
%
  j_first = 1;
  ir = 2;

  while ( 1 )
%
%  Skip out of the loop if have processed all evaluation points.
%
    if ( ne < j_first )
      break
    end
%
%  Locate all points in the interval.
%
    j_save = ne + 1;

    for j = j_first : ne
      if ( x(ir) <= xe(j) )
        j_save = j;
        if ( ir == n )
          j_save = ne + 1;
        end
        break
      end
    end
%
%  Have located first point beyond interval.
%
    j = j_save;

    nj = j - j_first;
%
%  Skip evaluation if no points in interval.
%
    if ( nj ~= 0 )
%
%  Evaluate cubic at XE(J_FIRST:J-1).
%
      [ fe(j_first:j-1), next, ierc ] = chfev ( x(ir-1), x(ir), f(ir-1), ...
        f(ir), d(ir-1), d(ir),  nj, xe(j_first:j-1) );

      if ( ierc < 0 )
        ierr = -5;
        fprintf ( 1, '\n' );
        fprintf ( 1, 'SPLINE_PCHIP_VAL - Fatal error!\n' );
        fprintf ( 1, '  Error return from CHFEV.\n' );
        error ( 'SPLINE_PCHIP_VAL - Fatal error!' );
      end
%
%  In the current set of XE points, there are NEXT(2) to the right of X(IR).
%
      if ( next(2) ~= 0 )

        if ( ir < n )
          ierr = -5;
          fprintf ( 1, '\n' );
          fprintf ( 1, 'SPLINE_PCHIP_VAL - Fatal error!\n' );
          fprintf ( 1, '  IR < N.\n' );
          error ( 'SPLINE_PCHIP_VAL - Fatal error!' );
        end
%
%  These are actually extrapolation points.
%
        ierr = ierr + next(2);

      end
%
%  In the current set of XE points, there are NEXT(1) to the left of X(IR-1).
%
      if ( next(1) ~= 0 )
%
%  These are actually extrapolation points.
%
        if ( ir <= 2 )
          ierr = ierr + next(1);
        else

          j_new = -1;

          for i = j_first : j-1
            if ( xe(i) < x(ir-1) )
              j_new = i;
              break
            end
          end

          if ( j_new == -1 )
            ierr = -5;
            fprintf ( 1, '\n' );
            fprintf ( 1, 'SPLINE_PCHIP_VAL - Fatal error!\n' );
            fprintf ( 1, '  Could not bracket the data point.\n' );
            error ( 'SPLINE_PCHIP_VAL - Fatal error!' );
          end
%
%  Reset J.  This will be the new J_FIRST.
%
          j = j_new;
%
%  Now find out how far to back up in the X array.
%
          for i = 1 : ir-1
            if ( xe(j) < x(i) )
              break
            end
          end
%
%  At this point, either XE(J) < X(1) or X(i-1) <= XE(J) < X(I) .
%
%  Reset IR, recognizing that it will be incremented before cycling.
%
          ir = max ( 1, i-1 );

        end

      end

      j_first = j;

    end

    ir = ir + 1;

    if ( n < ir )
      break
    end

  end

  return
end


% DEMO  --  Chebyshev Polynomial Interpolation and Differentiation
% UPDATED  --  October 28, 2013
% Written by Matthew Kelly, Cornell University
%
% This script demonstrates how chebyshev interpolants can be used to get
% accurate derivate information.
%

%% General Settings
clear; clc;

%What order should the approximation be?
order = 34;  %Low = 25, %Med = 1000, %High = 100000

%How many points should be used for error calculations and plotting?
nTime = 1000;

%What domain should we be looking at?
d = [0,1.3];

%Time for use in plots
time = linspace(d(1),d(2),nTime);

% %The following version of time will yield more accurate results and a
% %faster runtime because the points in time do not line up exactly with
% the chebyshev grid points.
% time = linspace(d(1)-1e-4, d(2)+2e-4,nTime);

%% DEMO  --  Fit to analytic function

%Set up analytic function
IO.domain = d;
IO.userFunc = @testFunction;

%Get the values at each of the chebyshev nodes
f = chebyshevFit(IO, order);         %Chebyshev Values
tic
[y,Dy,DDy,DDDy] = chebyshevInterpolate(f,time,d);   %Approximation
toc

%get the exact (analytic) solution for each derivative
[g,Dg,DDg,DDDg] = testFunction(time);

%Show results
figure(401); clf;
subplot(4,2,1);
    plot(time,y,'b-','LineWidth',2)
    title(['function approximation - order ' num2str(order)]);
subplot(4,2,3)
    plot(time,Dy,'b-','LineWidth',2)
    title(['derivative approximation - order ' num2str(order)]);
subplot(4,2,5)
    plot(time,DDy,'b-','LineWidth',2)
    title(['second derivative approximation - order ' num2str(order)]);
subplot(4,2,7)
    plot(time,DDDy,'b-','LineWidth',2)
    title(['third derivative approximation - order ' num2str(order)]);
subplot(4,2,2);
    semilogy(time,abs(g-y))
    title('error in function');
subplot(4,2,4)
    semilogy(time,abs(Dg-Dy))
    title('error in derivative');
subplot(4,2,6)
    semilogy(time,abs(DDg-DDy))
    title('error in second derivative');
subplot(4,2,8)
    semilogy(time,abs(DDDg-DDDy))
    title('error in third derivative');