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

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

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

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

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

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

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

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

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

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

  temp2 = 0.5;

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

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

  if ( kind == 1 )

    ab = 0.0;

    zemu = 2.0 / ( ab + 1.0 );

    aj(1:m) = 0.0;

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

  elseif ( kind == 2 )

    zemu = pi;

    aj(1:m) = 0.0;

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

  elseif ( kind == 3 )

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

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

  elseif ( kind == 4 )

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

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

  elseif ( kind == 5 )

    zemu = gamma ( alpha + 1.0 );

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

  elseif ( kind == 6 )

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

    aj(1:m) = 0.0;

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

  elseif ( kind == 7 )

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

    aj(1:m) = 0.0;

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

  elseif ( kind == 8 )

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

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

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

  end

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

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

  prec = eps;

  if ( n == 1 )
    return
  end

  e(n) = 0.0;

  for l = 1 : n

    j = 0;

    while ( 1 )

      for m = l : n

        if ( m == n )
          break
        end

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

      end

      p = d(l);

      if ( m == l )
        break
      end

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

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

      for ii = 1 : mml

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

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

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

      end

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

    end

  end

  for ii = 2 : n

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

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

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

  end

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

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

  return
end
function value = r8_huge ( )

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

  return
end
function value = r8_sign ( x )

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

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

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

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

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

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

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

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

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

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

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

  parchk ( kind, 1, alpha, beta )

  if ( kind == 1 )

    al = 0.0;
    be = 0.0;

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

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

  elseif ( kind == 2 )

    al = -0.5;
    be = -0.5;

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

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

  elseif ( kind == 3 )

    al = alpha;
    be = alpha;

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

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

  elseif ( kind == 4 )

    al = alpha;
    be = beta;

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

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

  elseif ( kind == 5 )

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

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

  elseif ( kind == 6 )

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

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

  elseif ( kind == 7 )

    al = alpha;
    be = 0.0;

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

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

  elseif ( kind == 8 )

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

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

  elseif ( kind == 9 )

    al = 0.5;
    be = 0.5;

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

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

  end

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

  for k = 1 : nt

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

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

  end

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

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

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

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

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

  return
end
function timestamp ( )

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

  return
end


function w = nc_compute ( n, x_min, x_max, x )

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

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

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

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

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

    w(i) = yvalb - yvala;

  end

  return
end


function a = poisson ( nrow, ncol )

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

  a = zeros ( n, n );

  i = 0;

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

      i = i + 1;

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

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

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

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

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

    end
  end

  return
end


function pdf = weibull_discrete_pdf ( x, a, b )

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

  return
end


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

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

[T,K]=size(x);

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

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

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


%% This file will symbolic solve for the ODE of the Single Pendulum on a Cart system.
% Coded By: K
% Data: 2019/04/27
%%
clc;clear;close all
% Define the symbolic symbols you need
syms x0(t) dx0(t) ddx0 theta(t) dtheta(t) ddtheta M m L F k0 k1 g I1 dum
% Set some variables to zero to simplify the model
I1=0; k0=0; k1=0;
% First define the position of the pendulum mass center
x1=x0+L*sin(theta)
y1=L*cos(theta)

% Now define the velocity of the cart and moving mass
v0=diff(x0,t)
v1x=diff(x1,t)
v1y=diff(y1,t)

% Substitute the variables
v0=subs(v0,diff(x0,t),dx0);
v1x=subs(v1x,diff(x0,t),dx0);
v1x=subs(v1x,diff(theta,t),dtheta);
v1y=subs(v1y,diff(x0,t),dx0);
v1y=subs(v1y,diff(theta,t),dtheta);

simplify(v1x^2+v1y^2)
simplify(v1x)
simplify(v1y)

% Now define the kinetic energy of the system
EngK=simplify(0.5*M*v0^2+0.5*m*simplify(v1x^2+v1y^2)+0.5*I1*diff(theta,t)^2)
EngP=m*g*(cos(theta))*L

% Define the Rayley disspation function
Damp=0.5*k0*v0^2+0.5*k1*dtheta^2;

% Now define the Lagrangian
Lag=simplify(EngK-EngP)

%% Now get the second order ODE
the1Part1=diff(subs(Lag,dtheta,dum),dum);
the1Part1=subs(the1Part1,dum,dtheta);
the1Part1=diff(the1Part1,t);
%
the1Part2=diff(subs(Lag,theta,dum),dum);
the1Part2=subs(the1Part2,dum,theta);
%
the1Part3=diff(subs(Damp,dtheta,dum),dum);
the1Part3=subs(the1Part3,dum,dtheta);
eqn1=the1Part1-the1Part2+the1Part3
%
eqn1=subs(eqn1,diff(dx0(t), t),ddx0);
eqn1=subs(eqn1,diff(x0(t), t, t),ddx0);
eqn1=subs(eqn1,diff(theta(t), t),dtheta);
eqn1=subs(eqn1,diff(dtheta(t), t),ddtheta);

% Second equation
the2Part1=diff(subs(Lag,dx0,dum),dum);
the2Part1=subs(the2Part1,dum,dx0);
the2Part1=diff(the2Part1,t);
%
the2Part2=diff(subs(Lag,x0,dum),dum);
the2Part2=subs(the2Part2,dum,x0);
%
the2Part3=diff(subs(Damp,dx0,dum),dum);
the2Part3=subs(the2Part3,dum,dx0);
eqn2=the2Part1-the2Part2+the2Part3-F
%
eqn2=subs(eqn2,diff(dx0(t), t),ddx0);
eqn2=subs(eqn2,diff(x0(t), t, t),ddx0);
eqn2=subs(eqn2,diff(theta(t), t),dtheta);
eqn2=subs(eqn2,diff(dtheta(t), t),ddtheta);

% Final eqn1 and eqn2
Eqn1=simplify(eqn1)==0
Eqn2=simplify(eqn2)==0

%% Solve for ddtheta and ddx0
eqns=[Eqn1 Eqn2];
vars=[ddtheta ddx0];
[Ans1 Ans2]=solve(eqns,vars);

Ans1=simplify(Ans1)
Ans2=simplify(Ans2)

%% Now rewrite the second order system into first order system
syms z1 z2 z3 z4
thisisddtheta1=simplify(subs(Ans1,[theta(t) x0(t) dtheta(t) dx0(t)],[z1 z2 z3 z4]))
thisisddtheta2=simplify(subs(Ans2,[theta(t) x0(t) dtheta(t) dx0(t)],[z1 z2 z3 z4]))

%% Save this file into Matlab Function
% dtheta=z3, dx=z4, ddtheta=thisistheta1, ddx=thisistheta2
Eqns=[z3;z4;thisisddtheta1;thisisddtheta2]

matlabFunction(Eqns,'File','SinglePendulum_ODE','Vars',{t,[z1 z2 z3 z4],F,M,m,L,g},'Optimize',true)


%% RD_FEM solves a 1D reaction/diffusion problem using finite elements.
%
%  Discussion:
%
%    This script sets up and runs a finite element simulation code
%    for a 1D reaction/diffusion equation.
%
%    The dynamics are given by the following reaction/diffusion equation
%    for the function W(T,X):
%
%      W_t = W_xx + NL(W,c),
%
%    where NL(W,c) is a polynomial in W:
%
%       NL(W,c) = c(1) + c(2) * W + c(3) * W^2 + c(4) * W^3
%
%    with Neumann boundary conditions at X = 0.0 and X = 1.0:
%
%      W_x(T,0.0) = 0.0
%      W_x(T,1.0) = 0.0
%
%    and initial condition at T = 0.0:
%
%      W(0,X) = sin ( pi * X ).
%
%    The problem is to be solved for 0.0 <= T <= 4.0, 0.0 <= X <= 1.0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 April 2011
%
%  Author:
%
%   Eugene Cliff
%
%  Reference:
%
%    Jeffrey Borggaard, John Burkardt, John Burns, Eugene Cliff,
%    Working Notes on a Reaction Diffusion Model: a Finite Element Formulation.
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'RD_FEM:\n' );
  fprintf ( 1, '  Solve a 1D time-dependent reaction/diffusion problem\n' );
  fprintf ( 1, '  with a nonlinear term and Neumann boundary conditions,\n' );
  fprintf ( 1, '  using Neumann boundary conditions.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The equation is discretized using the finite element method.\n' );
%
%  Set initial condition.
%  The first option uses a centered hat.
%
  if ( 0 )
    w_0 = @(x) 1.0 + basic_hat ( 8.0 * x - 4.0 );
  else
    w_0 = @(x) sin(pi*x);
  end
%
%  Set the polynomial coefficients of the reaction term.
%  Here, c = -x + x^3 = x * ( x^2 - 1 )
%
  c = [ 0.0; -1.0; 0.0; 1.0 ];
%
%  Select 101 times for output.
%
  t = ( 0.0 : 0.04 : 4.0 );
%
%  The number of spatial grid points is N+1;
%
  n = 32;
  x = linspace ( 0.0, 1.0, n + 1 );
%
%  T is a vector of 101 times.
%  W is an array of 101x33 function values.
%
  [ T, W ] = rd_lin_spline ( w_0, t, n, c );
%
%  Plot the results.
%
  plot_rd ( t, x, W, c )
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'RD_FEM:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );


function[a]=morseafun(varargin)
%MORSEAFUN  Returns the generalized Morse wavelet amplitude or a-function.
%
%   MORSEAFUN is a low-level function called by many a number of the Morse
%   wavelet functions.
%
%   A=MORSEAFUN(GAMMA,BETA) returns the generalized Morse wavelet
%   amplitude, called "A_{BETA,GAMMA}" by Lilly and Olhede (2009).
%
%   By default, A is chosen such that the maximum of the frequency-
%   domain wavelet is equal to 2, the ``bandpass normalization.''
%
%   A=MORSEAFUN(GAMMA,BETA,'energy') instead returns the coefficient
%   giving the wavelet unit energy.
%
%   A=MORSEAFUN(K,GAMMA,BETA,'energy') returns the unit energy coefficient
%   appropriate for the Kth-order wavelet.  The default choice is K=1.
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2006--2016 J.M. Lilly --- type 'help jlab_license' for details

if strcmpi(varargin{1},'--t')
      morseafun_test;return
end

str='band';
if ischar(varargin{end})
    str=varargin{end};
    varargin=varargin(1:end-1);
end

k=1;
if length(varargin)==3
    k=varargin{1};
    varargin=varargin(2:end);
end

ga=varargin{1};
be=varargin{2};
if strcmpi(str(1:3),'ban')
    om=morsefreq(ga,be);
%    a=frac(2,(om.^be).*exp(-om.^ga));
    a=frac(2,exp(be.*log(om)-om.^ga));
    a(be==0)=2;
elseif strcmpi(str(1:3),'tes')
    a=sqrt(frac(2 *pi*ga.*2.^frac(2*be+1,ga),gamma(frac(2*be+1,ga))));
elseif strcmpi(str(1:3),'ene')
    r=frac(2*be+1,ga);
    a=double((2*pi*ga.*(2.^r).*exp(gammaln(k)-gammaln(k+r-1))).^(1/2));
end

function[]=morseafun_test

ga1=(2:1:9);
be1=(1:1:10);
[ga,be]=meshgrid(ga1,be1);
om=reshape(morsefreq(ga,be),10,8);

dom=0.01;
omgrid=permute((0:dom:20)',[3 2 1]);
omgrid=vrep(omgrid,length(ga1),2);
omgrid=vrep(omgrid,length(be1),1);

omgrid=omgrid.*vrep(om,size(omgrid,3),3);
a=morseafun(ga,be,'energy');

begrid=vrep(be,size(omgrid,3),3);
gagrid=vrep(ga,size(omgrid,3),3);
agrid=vrep(a,size(omgrid,3),3);

psi=agrid.*omgrid.^begrid.*exp(-omgrid.^gagrid);
psiint=vsum(psi.^2,3).*dom.*om./(2*pi);

reporttest('MORSEAFUN unit energy', allall(abs(psiint-1)<1e-2))

a2=morseafun(ga,be,'test');
reporttest('MORSEAFUN unit energy, alternate formulation', aresame(a,a2,1e-6));


function [ fea, out ] = ex_laplace1( varargin )
%EX_LAPLACE1 2D Laplace equation example on a unit square.
%
%   [ FEA, OUT ] = EX_LAPLACE1( VARARGIN ) Laplace equation on a unit square
%   with exact solution 2*y/((1+x)^2+y^2). Accepts the following property/value pairs.
%
%       Input       Value/{Default}        Description
%       -----------------------------------------------------------------------------------
%       igrid       scalar 1/{0}           Cell type (0=quadrilaterals, 1=triangles)
%       hmax        scalar {1/10}          Max grid cell size
%       refsol      string {2*y/((1+x)^2+y^2)}   Reference solution
%       sfun        string {sflag1}        Shape function
%       iphys       scalar 0/{1}           Use physics mode to define problem    (=1)
%                                          or directly define fea.eqn/bdr fields (=0)
%       iplot       scalar 0/{1}           Plot solution (=1)
%                                                                                .
%       Output      Value/(Size)           Description
%       -----------------------------------------------------------------------------------
%       fea         struct                 Problem definition struct
%       out         struct                 Output struct

% Copyright 2013-2022 Precise Simulation, Ltd.

cOptDef = { ...
  'igrid',    0; ...
  'hmax',     0.1; ...
  'refsol',   '2*y/((1+x)^2+y^2)'; ...
  'sfun',     'sflag1'; ...
  'iphys',    1; ...
  'icub',     2; ...
  'iplot',    1; ...
  'fid',      1 };
[got,opt] = parseopt(cOptDef,varargin{:});
fid       = opt.fid;

% Geometry definition.
gobj = gobj_rectangle();
fea.geom.objects = { gobj };

% Grid generation.
if ( opt.igrid==-1 )
  fea.grid = rectgrid(round(1/opt.hmax));
  fea.grid = quad2tri( fea.grid );
elseif ( opt.igrid==0 )
  fea.grid = rectgrid(round(1/opt.hmax));
else
  fea.grid = gridgen(fea,'hmax',opt.hmax,'fid',fid);
end
n_bdr = max(fea.grid.b(3,:));           % Number of boundaries.

% Problem definition.
fea.sdim  = { 'x' 'y' };                % Coordinate names.
if ( opt.iphys==1 )

  fea = addphys(fea,@poisson);          % Add Poisson equation physics mode.
  fea.phys.poi.sfun = { opt.sfun };     % Set shape function.
  fea.phys.poi.eqn.coef{3,4} = { 0 };   % Set source term coefficient to zero.
  fea.phys.poi.bdr.coef{1,end} = repmat({opt.refsol},1,n_bdr);   % Set Dirichlet boundary coefficient to reference solution.
  fea = parsephys(fea);                 % Check and parse physics modes.

else

  fea.dvar  = { 'u' };                  % Dependent variable name.
  fea.sfun  = { opt.sfun  };            % Shape function.

  % Define equation system.
  fea.eqn.a.form = { [2 3;2 3] };       % First row indicates test function space   (2=x-derivative + 3=y-derivative),
                                        % second row indicates trial function space (2=x-derivative + 3=y-derivative).
  fea.eqn.a.coef = { 1 };               % Coefficient used in assembling stiffness matrix.

  fea.eqn.f.form = { 1 };               % Test function space to evaluate in right hand side (1=function values).
  fea.eqn.f.coef = { 0 };               % Coefficient used in right hand side.

  % Define boundary conditions.
  fea.bdr.d     = cell(1,n_bdr);
 [fea.bdr.d{:}] = deal(opt.refsol);     % Assign reference solution to all boundaries (Dirichlet).

  fea.bdr.n     = cell(1,n_bdr);        % No Neumann boundaries ('fea.bdr.n' empty).

end

% Parse and solve problem.
fea       = parseprob(fea);             % Check and parse problem struct.
fea.sol.u = solvestat(fea,'fid',fid,'icub',opt.icub);   % Call to stationary solver.

% Postprocessing.
s_err = ['abs(',opt.refsol,'-u)'];
if ( opt.iplot>0 )
  figure
  subplot(2,1,1)
  postplot(fea,'surfexpr','u')
  title('Solution u')
  subplot(2,1,2)
  postplot(fea,'surfexpr',s_err)
  title('Error')
end

% Error checking.
if ( size(fea.grid.c,1)==4 )
  xi = [0;0];
else
  xi = [1/3;1/3;1/3];
end
err = evalexpr0(s_err,xi,1,1:size(fea.grid.c,2),[],fea);
ref = evalexpr0('u',xi,1,1:size(fea.grid.c,2),[],fea);
err = sqrt(sum(err.^2)/sum(ref.^2));

if( ~isempty(fid) )
  fprintf(fid,'\nL2 Error: %f\n',err)
  fprintf(fid,'\n\n')
end

out.err  = err;
out.pass = out.err<0.01;
if ( nargout==0 )
  clear fea out
end


%The basic idea behind the false position method is similar to the bisection
%method in that we continuously shrink the interval the root lies on until
%the algorithm converges on the root. Unlike the bisection method, the false
%position method does not halve the interval with each iteration. Instead of
%using the midpoint of a and b to create the new interval, the false position
%method uses the x-intercept of the line connecting f(a) and f(b). This
%algorithm converges faster than the bisection method.

%INPUTS:
%Function handle f
%endpoint a
%endpoint b
%maximum tolerated error

%OUTPUTS:
%An approximated value for the root of f within the defined interval.

%Written by MatteoRaso

function y = false_position(f, a, b, error)
  if ~(f(a) < 0)
    disp("f(a) must be less than 0")
  elseif ~(f(b) > 0)
    disp("f(b) must be greater than zero")
  else
    c = 100000;
    while abs(f(c)) > error
      %Formula for the x-intercept
      c = -f(b) * (b - a) / (f(b) - f(a)) + b;
      if f(c) < 0
        a = c;
      else
        b = c;
      endif
      disp(f(c))
    endwhile
    x = ["The root is approximately located at ", num2str(c)];
    disp(x)
    y = c;
  endif
endfunction


function [w3j,jmin, jmax] = Wigner3j_new(j2, j3, m1, m2, m3)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%	This subroutine will calculate the Wigner 3j symbols
%
%		j  j2 j3
%		m1 m2 m3
%
%	for all allowable values of j. The returned values in the array j are
%	calculated only for the limits
%
%		jmin = max(|j2-j3|, |m1|)
%		jmax = j2 + j3
%
%	To be non-zero, m1 + m2 + m3 = 0. In addition, it is assumed that all j and m are
%	integers. Returned values have a relative error less than ~1.d-8 when j2 and j3
%	are less than 103 (see below). In practice, this routine is probably usable up to 165.
%
%	This routine is based upon the stable non-linear recurence relations of Luscombe and
%	Luban (1998) for the "non classical" regions near jmin and jmax. For the classical
%	region, the standard three term recursion relationship is used (Schulten and Gordon 1975).
%	Note that this three term recursion can be unstable and can also lead to overflows. Thus
%	the values are rescaled by a factor "scalef" whenever the absolute value of the 3j coefficient
%	becomes greater than unity. Also, the direction of the iteration starts from low values of j
%	to high values, but when abs(w3j(j+2)/w3j(j)) is less than one, the iteration will restart
%	from high to low values. More efficient algorithms might be found for specific cases
%	(for instance, when all m's are zero).
%
%	Verification:
%
%	The results have been verified against this routine run in quadruple precision.
%	For 1.e7 acceptable random values of j2, j3, m2, and m3 between -200 and 200, the relative error
%	was calculated only for those 3j coefficients that had an absolute value greater than
%	1.d-17 (values smaller than this are for all practical purposed zero, and can be heavily
%	affected by machine roundoff errors or underflow). 853 combinations of parameters were found
%	to have relative errors greater than 1.d-8. Here I list the minimum value of max(j2,j3) for
%	different ranges of error, as well as the number of times this occured
%
%	1.d-7 < error  <=1.d-8 = 103	# = 483
%	1.d-6 < error <= 1.d-7 =  116	# = 240
%	1.d-5 < error <= 1.d-6 =  165	# = 93
%	1.d-4 < error <= 1.d-5 = 167	# = 36
%
%	Many times (maybe always), the large relative errors occur when the 3j coefficient
%	changes sign and is close to zero. (I.e., adjacent values are about 10.e7 times greater
%	in magnitude.) Thus, if one does not need to know highly accurate values of the 3j coefficients
%	when they are almost zero (i.e., ~1.d-10)  this routine is probably usable up to about 160.
%
%	These results have also been verified for parameter values less than 100 using a code
%	based on the algorith of de Blanc (1987), which was originally coded by Olav van Genabeek,
%	and modified by M. Fang (note that this code was run in quadruple precision, and
%	only calculates one coefficient for each call. I also have no idea if this code
%	was verified.) Maximum relative errors in this case were less than 1.d-8 for a large number
%	of values (again, only 3j coefficients greater than 1.d-17 were considered here).
%
%	The biggest improvement that could be made in this routine is to determine when one should
%	stop iterating in the forward direction, and start iterating from high to low values.
%
%	Calling parameters
%		IN
%			j2, j3, m1, m2, m3 	Integer values.
%		OUT
%			w3j			Array of length jmax - jmin + 1.
%			jmin, jmax		Minimum and maximum values
%						out output array.
%	Dependencies: None
%
%	Written by Mark Wieczorek August (2004)
%
%	August 2009: Based on the suggestions of Roelof Rietbroek, the calculation of RS has been slightly
%	modified so that division by zero will not cause a run time crash (this behavior depends on how the
%	compiler treats IEEE floating point exceptions). These values were never used in the original code
%	when this did occur.
%
%	Copyright (c) 2005-2009, Mark A. Wieczorek
%	All rights reserved.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%	implicit none
%	integer, intent(in) ::	j2, j3, m1, m2, m3
%	integer, intent(out) ::	jmin, jmax
%	real*8, intent(out) ::	w3j(:)
%	real*8 ::		wnmid, wpmid, scalef, denom, rs(j2+j3+1), &
%				wl(j2+j3+1), wu(j2+j3+1), xjmin, yjmin, yjmax, zjmax, xj, zj
%	integer :: 		j, jnum, jp, jn, k, flag1, flag2, jmid

%% some basic constants

flag1 = 0;
flag2 = 0;

scalef = 1;

jmin = max(abs(j2-j3), abs(m1));
jmax = j2 + j3;
jnum = jmax - jmin + 1;

w3j = zeros(1,jnum);

%% some simple functions
jindex = @(j) j-jmin+1;

a = @(j) sqrt((j.^2 - (j2-j3).^2) .* ((j2+j3+1).^2 - j.^2) * (j.^2-m1^2));

y= @(j) -(2*j+1) .* ( m1 .* (j2.*(j2+1) - j3*(j3+1)) - (m3-m2).*j.*(j+1));

x = @(j) j .* a(j+1);

z = @(j) (j+1) .* a(j);

%% exclude some basic cases	which give zero
if abs(m2) > j2 || abs(m3) > j3
  return
elseif m1 + m2 + m3 ~= 0
  return
elseif jmax < jmin
  return
end

%% Only one term is present

if jnum == 1

  w3j = 1 / sqrt(2 * jmin + 1);

  if (w3j < 0 && (-1)^(j2-j3+m2+m3) > 0) || ...
      (w3j > 0 && (-1)^(j2-j3+m2+m3) < 0)
    w3j = -w3j;
  end
  return

end

%% more then one term
%
% Calculate lower non-classical values for [jmin, jn]. If the second term
%	can not be calculated because the recursion relationsips give rise to a
%	1/0,  set flag1 to 1.  If all m's are zero,  this is not a problem
%	as all odd terms must be zero.
%

rs = 0;
wl = 0;

xjmin = x(jmin);
yjmin = y(jmin);

if (m1 == 0 && m2 == 0 && m3 == 0) % All m's are zero

  wl(jindex(jmin)) = 1;
  wl(jindex(jmin+1)) = 0;
  jn = jmin+1;

elseif yjmin == 0 % The second terms is either zero

  if xjmin == 0   % or undefined
    flag1 = 1;
    jn = jmin;
  else
    wl(jindex(jmin)) = 1;
    wl(jindex(jmin+1)) = 0;
    jn = jmin+1;
  end

elseif xjmin * yjmin >= 0 % The second term is outside of the non-classical region

  wl(jindex(jmin)) = 1;
  wl(jindex(jmin+1)) = -yjmin / xjmin;
  jn = jmin+1;

else							% Calculate terms in the non-classical region

  rs(jindex(jmin)) = -xjmin / yjmin;

  jn = jmax;
  for j = jmin + 1:jmax-1

    denom =  y(j) + z(j)*rs(jindex(j-1));
    xj = x(j);
    if abs(xj) > abs(denom) || xj * denom >= 0 || denom == 0
      jn = j-1;
      break
    else
      rs(jindex(j)) = -xj / denom;
    end

  end

  wl(jindex(jn)) = 1;

  for k = 1:jn - jmin

    wl(jindex(jn-k)) = wl(jindex(jn-k+1)) * rs(jindex(jn-k));

  end

  if jn == jmin	% Calculate at least two terms so that

    wl(jindex(jmin+1)) = -yjmin / xjmin;		% these can be used in three term
    jn = jmin+1;					% recursion

  end

end

if jn == jmax					% All terms are calculated

  w3j = wl;

  % normalize
  norm = sum((2*(jmin:jmax)+1) .* w3j.^2);
  w3j = w3j ./ sqrt(norm);

  % fix sign
  if (w3j(end) < 0 && (-1)^(j2-j3+m2+m3) > 0) || ...
      (w3j(end) > 0 && (-1)^(j2-j3+m2+m3) < 0)
    w3j = -w3j;
  end

  return
end

%%
%
% 	Calculate upper non-classical values for [jp, jmax].
%	If the second last term can not be calculated because the
%	recursion relations give a 1/0,  set flag2 to 1.
%	(Note, I don't think that this ever happens).
%

wu = 0;

yjmax = y(jmax);
zjmax = z(jmax);

if (m1 == 0 && m2 == 0 && m3 == 0)

  wu(jindex(jmax)) = 1;
  wu(jindex(jmax-1)) = 0;
  jp = jmax-1;

elseif yjmax == 0

  if zjmax == 0
    flag2 = 1;
    jp = jmax;
  else
    wu(jindex(jmax)) = 1;
    wu(jindex(jmax-1)) = - yjmax / zjmax;
    jp = jmax-1;
  end

elseif yjmax * zjmax >= 0

  wu(jindex(jmax)) = 1;
  wu(jindex(jmax-1)) = - yjmax / zjmax;
  jp = jmax-1;

else
  rs(jindex(jmax)) = -zjmax / yjmax;

  jp = jmin;
  for j = jmax-1:-1:jn

    denom = y(j) + x(j)*rs(jindex(j+1));
    zj = z(j);
    if abs(zj) > abs(denom) || zj * denom >= 0 || denom == 0
      jp = j+1;
      break
    else
      rs(jindex(j)) = -zj / denom;
    end

  end

  wu(jindex(jp)) = 1;

  for k=1:jmax - jp
    wu(jindex(jp+k)) = wu(jindex(jp+k-1))*rs(jindex(jp+k));
  end

  if jp == jmax
    wu(jindex(jmax-1)) = - yjmax / zjmax;
    jp = jmax-1;
  end

end

%%
%
% 	Calculate classical terms for [jn+1, jp-1] using standard three
% 	term rercusion relationship. Start from both jn and jp and stop at the
% 	midpoint. If flag1 is set,  perform the recursion solely from high to
% 	low values. If flag2 is set,  perform the recursion solely from low to high.

if flag1 == 0

  jmid = ceil((jn + jp)/2);

  for j = jn : jmid - 1

    wl(jindex(j+1)) = - (z(j)*wl(jindex(j-1)) +y(j)*wl(jindex(j))) / x(j);

    if abs(wl(jindex(j+1))) > 1 				% watch out for overflows.
      wl(jindex(jmin):jindex(j+1)) = wl(jindex(jmin):jindex(j+1)) / scalef;
    end

    % if values are decreasing
    if (abs(wl(jindex(j+1)) / wl(jindex(j-1))) < 1 && wl(jindex(j+1)) ~= 0)

      %  stop upward iteration
      jmid = j+1;	% and start with the downward
      break	% iteration.
    end
  end

  wnmid = wl(jindex(jmid));

  if (abs(wnmid/wl(jindex(jmid-1))) < 1e-6 && ...
      wl(jindex(jmid-1)) ~= 0) 				% Make sure that the stopping

    wnmid = wl(jindex(jmid-1));					% midpoint value is not a zero,
    jmid = jmid - 1;							% or close to it%
  end

  for j=jp:-1:jmid+1
    wu(jindex(j-1)) = - (x(j)*wu(jindex(j+1)) + y(j)*wu(jindex(j)) ) / z(j);
    if (abs(wu(jindex(j-1))) > 1)
      wu(jindex(j-1):jindex(jmax)) = wu(jindex(j-1):jindex(jmax)) / scalef;
    end

  end

  wpmid = wu(jindex(jmid));

  % rescale two sequences to common midpoint

  if jmid == jmax
    w3j(1:jnum) = wl(1:jnum);
  elseif jmid == jmin
    w3j(1:jnum) = wu(1:jnum);
  else
    w3j(1:jindex(jmid)) = wl(1:jindex(jmid)) * wpmid / wnmid;
    w3j(jindex(jmid+1):jindex(jmax)) = wu(jindex(jmid+1):jindex(jmax));
  end

elseif (flag1 == 1 && flag2 == 0) 	% iterature in downward direction only

  for j=jp:-1:jmin+1
    wu(jindex(j-1)) = - (x(j)*wu(jindex(j+1)) + y(j)*wu(jindex(j)) ) / z(j);
    if (abs(wu(jindex(j-1))) > 1)
      wu(jindex(j-1):jindex(jmax)) = wu(jindex(j-1):jindex(jmax)) / scalef;
    end
  end

  w3j(1:jnum) = wu(1:jnum);

elseif flag2 == 1 && flag1 == 0 % iterature in upward direction only

  for j = jn:jp-1
    wl(jindex(j+1)) = - (z(j)*wl(jindex(j-1)) +y(j)*wl(jindex(j))) / x(j);
    if abs(wl(jindex(j+1))) > 1
      wl(jindex(jmin):jindex(j+1)) = wl(jindex(jmin):jindex(j+1))/ scalef;
    end
  end

  w3j = wl;

elseif flag1 == 1 && flag2 == 1

  error('Can not calculate function for input values');

end

% normalize
norm = sum((2*(jmin:jmax)+1) .* w3j.^2);
w3j = w3j ./ sqrt(norm);

% fix sign
if (w3j(end) < 0 && (-1)^(j2-j3+m2+m3) > 0) || ...
    (w3j(end) > 0 && (-1)^(j2-j3+m2+m3) < 0)
  w3j = -w3j;
end

end


function s = QuinticTimeScaling(Tf, t)
% *** CHAPTER 9: TRAJECTORY GENERATION ***
% Takes Tf: Total time of the motion in seconds from rest to rest,
%       t: The current time t satisfying 0 < t < Tf.
% Returns s: The path parameter s(t) corresponding to a fifth-order
%            polynomial motion that begins and ends at zero velocity and
%            zero acceleration.
% Example Input:
%
% clear; clc;
% Tf = 2;
% t = 0.6;
% s = QuinticTimeScaling(Tf,t)
%
% Output:
% s =
%    0.1631

s = 10 * (t / Tf) ^ 3 - 15 * (t / Tf) ^ 4 + 6 * (t / Tf) ^ 5;
end


function K = knPoly(X, Y, o, c)
% Polynomial kernel k(x,y)=(x'y+c)^o
% Input:
%   X: d x nx data matrix
%   Y: d x ny data matrix
%   o: order of polynomial
%   c: constant
% Ouput:
%   K: nx x ny kernel matrix
% Written by Mo Chen (sth4nth@gmail.com).
if nargin < 4
    c = 0;
end

if nargin < 3
    o = 3;
end

if nargin < 2 || isempty(Y)
    K = (dot(X,X,1)+c).^o;            % norm in kernel space
else
    K = (X'*Y+c).^o;
end


% runpca() -  perform principal component analysis (PCA) using singular value
%             decomposition (SVD) using Matlab svd() or svds()
%                        >> inv(eigvec)*data = pc;
% Usage:
%    >> [pc,eigvec,sv] = runpca(data);
%    >> [pc,eigvec,sv] = runpca(data,num,norm)
%
% Inputs:
%   data   - input data matrix (rows are variables, columns observations)
%   num    - number of principal comps to return  {def|0|[] -> rows in data}
%   norm   - 1/0 = do/don't normalize the eigvec's to be equivariant
%                                                {def|0 -> no normalization}
% Outputs:
%   pc     - the principal components, i.e.        >> inv(eigvec)*data = pc;
%   eigvec - the inverse weight matrix (=eigenvectors). >> data = eigvec*pc;
%   sv     - the singular values (=eigenvalues)
%
% Author: Colin Humphries, CNL / Salk Institute, 1997
%
% See also: runica()

% Copyright (C) Colin Humphries, CNL / Salk Institute, Aug, 1997
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

% 01/31/00 renamed runpca() and improved usage message -sm
% 01-25-02 reformated help & license, added links -ad

function [pc,M,S] = runpca(data,N,norm)

BIG_N = 50; % for efficiency, switch to sdvs() when BIG_N<=N or N==rows

if nargin < 1
  help runpca
  return
end

rows = size(data,1);

% remove the mean
for i = 1:rows
  data(i,:) = data(i,:) - mean(data(i,:));
end

if nargin < 3
  norm = 0;
elseif isempty(norm)
  norm = 0;
end

if nargin < 2
  N = 0;
end
if isempty(N)
  N = 0;
end

if N == 0  | N == rows
  N = rows;
  [U,S,V] = svd(data',0);   % performa SVD
  if norm == 0
    pc = U';
    M = (S*V')';
  else % norm
    pc = (U*S)';
    M = V;
  end
else
  if N > size(data,1)
    error('N must be <= the number of rows in data.')
  end
  %if N <= BIG_N | N == rows
  %[U,S,V] = svd(data',0);
  %else
  [U,S,V] = svds(data',N);
  %end
  if norm == 0
    pc = U';
    M = (S*V')';
  else % norm
    pc = (U*S)';
    M = V;
  end
  %if N > BIG_N & N < rows
  %pc = pc(1:N,:);
  %M = M(:,1:N);
  %end
end
%S = diag(S(1:N,1:N));


% Thanks for Carlos Lopez for this file and many other enhancements to the mp toolbox.
%quick and dirty tests
precision=1000;%notice that tol~=(.5)^precision;
a42=mp(rand(4,2),precision);%It will be nice to use mprand, but it doesn't work
b42=mp(rand(4,2),precision);

%2) direct/inverse pairs
c_mp=norm(asin(sin(a42))-a42);disp(['2.1 sin/asin' setstr(9) num2str(c_mp)])
c_mp=norm(acos(cos(a42))-a42);disp(['2.2 cos/acos' setstr(9) num2str(c_mp)])
c_mp=norm(atan(tan(a42))-a42);disp(['2.3 tan/atan' setstr(9) num2str(c_mp)])
c_mp=norm(asinh(sinh(a42))-a42);disp(['2.4 sinh/asinh' setstr(9) num2str(c_mp)])
c_mp=norm(acosh(cosh(a42))-a42);disp(['2.5 cosh/acosh' setstr(9) num2str(c_mp)])
c_mp=norm(atanh(tanh(a42))-a42);disp(['2.6 tanh/atanh' setstr(9) num2str(c_mp)])
c_mp=norm(exp(log(a42))-a42);disp(   ['2.7 exp/log   ' setstr(9) num2str(c_mp)])
c_mp=norm(sqrt(a42.^2)-a42);disp(    ['2.8 sqrt/^2   ' setstr(9) num2str(c_mp)])

%4) Catastrophic cancellation
z=sqrt(117)-sqrt(116);zExact=1/(sqrt(117)+sqrt(116));
disp('4.1 Improvement in the catastrophic cancellation of sqrt(117)-sqrt(116)')
disp([setstr(9) 'Standard double precision:' num2str(abs(z/zExact-1)) ])
for precision=300:100:1000
 zmp=sqrt(mp(117,precision))-sqrt(mp(116,precision));zmpExact=1/(sqrt(mp(117,precision))+sqrt(mp(116,precision)));
 disp([setstr(9) 'Multiple precision = ' num2str(precision) ':'  num2str(abs((zmp/zmpExact-1)))])
end

disp('4.2 Accuracy of tan(pi/4)-1')
for precision=300:100:1000
 zmp=mp('pi',precision)/4;
 disp([setstr(9) 'Multiple precision = ' num2str(precision) ':'  num2str(((tan(zmp)-1)))])
end
disp('4.3 Accuracy of 4*atan2(-1,1)+pi')
for precision=300:100:1000
 zmp=mp('pi',2*precision);%Request a higher precision pi in order to compare
 one=mp(1,precision);
 disp([setstr(9) 'Multiple precision = ' num2str(precision) ':'  ...
       num2str(((atan2(-one,one)*4+zmp)))])
end

%5) Test matrix; we will only test that the accuracy
%of an exact inverse times the matrix is closer to the identity in mp rather than in double
n=14;
disp(['5.1 Hilbert matrix and its exact inverse.'])
disp(['    norm in double: ' num2str([norm(hilb(n)*invhilb(n)-eye(n))])])
for precision=300:100:1000
 mp_set_defaults(precision);
 %5.1 Hilbert
 one=mp(1);

 J = 1:n;
 J = J(ones(n,1),:);
 I = J';
 E = mp(ones(n,n));
 mpHilb = E./(I+J-one);

 %Now the inverse; this is a slightly modified version of invhilb.m to produce mp values
 p = n;
 H = mp(zeros(n,n));
 for k = 1:n
  i=mp(k);
  if k > 1, p = ((n-i+one)*p*(n+i-one))/(k-one)^2; end
  r = p*p;
  H(k,k) = r/(2*i-one);
  for j = i+1:n
   %             r = -((n-j+one)*r*(n+j-one))/(j-one)^2;
   num = -((n-j+one)*r*(n+j-one));
   den= (j-one)^2;
   r = num/den;
   H(k,j) = r/(i+j-one);
   H(j,k) = r/(i+j-one);
  end
 end
 disp(['precision=' num2str(precision) ':' setstr(9) num2str(norm(H*mpHilb-eye(n)))]);
end
%check also some auxiliary functions (unrelated with precision!)
X = mp([2 8 4;7 3 9]);
%see "help min" for full documentation
if all(min(X,[],1) == [2 3 4]), disp('Test 6.1 passed'), else, disp('Test 6.1 failed'),end
if all(min(X,[],2) == [2;3]), disp('Test 6.2 passed'), else, disp('Test 6.2 failed'),end
if all(min(X,5)==[2 5 4;5 3 5]), disp('Test 6.3 passed'), else, disp('Test 6.3 failed'),end

if all(max(X,[],1)==[7 8 9]), disp('Test 6.4 passed'), else, disp('Test 6.4 failed'),end
if all(max(X,[],2)==[8;9]), disp('Test 6.5 passed'), else, disp('Test 6.5 failed'),end
if all(max(X,5)==[5 8 5;7 5 9]),disp('Test 6.6 passed'), else, disp('Test 6.6 failed'),end

X = mp([0 1 2;3 4 5]);
if all(sum(X,1) == [3 5 7]), disp('Test 6.7 passed'), else, disp('Test 6.7 failed'),end
if all(sum(X,2) == [ 3;12]), disp('Test 6.8 passed'), else, disp('Test 6.8 failed'),end

X = mp([3 7 5;0 4 2]);
if all(sort(X,1) == [0 4 2;3 7 5] ), disp('Test 6.9 passed'), else, disp('Test 6.9 failed'),end
if all(sort(X,2) == [3 5 7;0 2 4] ), disp('Test 6.10 passed'), else, disp('Test 6.10 failed'),end


function [v,usediters] = projfunc( s, k1, k2, nn )

% Solves the following problem:
% Given a vector s, find the vector v having sum(abs(v))=k1
% and sum(v.^2)=k2 which is closest to s in the euclidian sense.
% If the binary flag nn is set, the vector v is additionally
% restricted to being non-negative (v>=0).
%
% Written 2.7.2004 by Patrik O. Hoyer
%

% Problem dimension
N = length(s);

% If non-negativity flag not set, record signs and take abs
if ~nn,
    isneg = s<0;
    s = abs(s);
end

% Start by projecting the point to the sum constraint hyperplane
v = s + (k1-sum(s))/N;

% Initialize zerocoeff (initially, no elements are assumed zero)
zerocoeff = [];

j = 0;
while 1,

    % This does the proposed projection operator
    midpoint = ones(N,1)*k1/(N-length(zerocoeff));
    midpoint(zerocoeff) = 0;
    w = v-midpoint;
    a = sum(w.^2);
    b = 2*w'*v;
    c = sum(v.^2)-k2;
    alphap = (-b+real(sqrt(b^2-4*a*c)))/(2*a);
    v = alphap*w + v;

    if all(v>=0),
	% We've found our solution
	usediters = j+1;
	break;
    end

    j = j+1;

    % Set negs to zero, subtract appropriate amount from rest
    zerocoeff = find(v<=0);
    v(zerocoeff) = 0;
    tempsum = sum(v);
    v = v + (k1-tempsum)/(N-length(zerocoeff));
    v(zerocoeff) = 0;

end

% If non-negativity flag not set, return signs to solution
if ~nn,
    v = (-2*isneg + 1).*v;
end

% Check for problems
if max(max(abs(imag(v))))>1e-10,
    error('Somehow got imaginary values!');
end


function uv=spherAng2Uv(azEl,systemType,includeW,Ms,Muv)
%%SPHERANG2UV Convert azimuth and elevation in spherical coordinates into a
%             [u;v] or [u;v;w] direction cosine vector in 3D. Direction
%             cosines u and v are the first two elements of a unit vector
%             in 3D.
%
%INPUTS: azEl A 2XN set of N points in [azimuth;elevation] in radians to
%           convert to direction cosines.
%  systemType An optional parameter specifying the axis from which the
%           angles are measured in radians. Possible values are
%           0 (The default if omitted) Azimuth is measured
%             counterclockwise from the x-axis in the x-y plane.
%             Elevation is measured up from the x-y plane (towards the
%             z-axis). This is consistent with common spherical
%             coordinate systems for specifying longitude (azimuth) and
%             geocentric latitude (elevation).
%           1 Azimuth is measured counterclockwise from the z-axis in the
%             z-x plane. Elevation is measured up from the z-x plane
%             (towards the y-axis). This is consistent with some spherical
%             coordinate systems that use the z-axis as the boresight
%             direction of the radar.
%           2 This is the same as 0 except instead of being given
%             elevation, one is given the angle away from the z-axis, which
%             is (pi/2-elevation).
%           3 This is the same as 0 except azimuth is measured clockwise
%             from the y-axis in the x-y plane instead of counterclockwise
%             from the x-axis. This coordinate system often arises when
%             given "bearings" in a local East-North-Up coordinate system,
%             where the bearing directions are measured East of North.
%  includeW An optional boolean value indicating whether a third direction
%           cosine component should be included. The u and v direction
%           cosines are two parts of a 3D unit vector. Generally, one might
%           assume that the target is in front of the sensor, so the third
%           component would be positive and is not needed. However, the
%           third component can be included if ambiguity exists. The
%           default if this parameter is omitted or an empty matrix is
%           passed is false.
%    Ms,Muv If either the spherical coordinate system or the u-v coordinate
%           system is rotated compared to the global Cartesian coordinate
%           system, these optional 3X3 matrices provide the rotations. Ms
%           is a 3X3 matrix to go from the alignment of a global
%           Cartesian coordinate system to that in which the spherical
%           coordinates are computed. Similarly, Muv is a rotation matrix
%           to go from the alignment of a global Cartesian cordinate system
%           to that in which the u-v(-w) coordinates are computed. If
%           either of these in omitted or an empty matrix is passed, then
%           the missing one is replaced with the identity matrix.
%
%OUTPUTS: uv A 2XN (without w) or 3XN (with w) set of direction cosines
%            values corresponding to the specified angles.
%
%Direction cosines and spherical coordinate systems are discussed in [1].
%
%EXAMPLE:
%In this example, a spherical value is converted to uv coordiantes and then
%back, demonostrating the consistency of the functions spherAng2Uv and
%uv2SpherAng. The relative error should be about zero.
% zSpher=[0.4;0.7];
% systemType=3;
% includeW=true;
% Ms=Euler2Ang2RotMat(0.1,0.2,'xy');
% Muv=Euler1Ang2RotMat(0.25,'z');
% convBack=uv2SpherAng(spherAng2Uv(zSpher,systemType,includeW,Ms,Muv),systemType,Ms,Muv);
% relErr=max(abs((convBack-zSpher)./zSpher))
%
%REFERENCES:
%[1] D. F. Crouse, "Basic tracking using nonlinear 3D monostatic and
%    bistatic measurements," IEEE Aerospace and Electronic Systems
%    Magazine, vol. 29, no. 8, Part II, pp. 4-53, Aug. 2014.
%
%June 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<5||isempty(Muv))
    Muv=eye(3,3);
end

if(nargin<4||isempty(Ms))
    Ms=eye(3,3);
end

if(nargin<3||isempty(includeW))
    includeW=false;
end

if(nargin<2||isempty(systemType))
    systemType=0;
end

azimuth=azEl(1,:);
elevation=azEl(2,:);

if(systemType==2)
    elevation=pi/2-elevation;
    systemType=0;
elseif(systemType==3)
    azimuth=pi/2-azimuth;
    systemType=0;
end

switch(systemType)
    case 0
        uv=Muv*Ms'*[cos(azimuth).*cos(elevation);
                    sin(azimuth).*cos(elevation);
                    sin(elevation)];
    case 1
        uv=Muv*Ms'*[sin(azimuth).*cos(elevation);
                    sin(elevation);
                    cos(azimuth).*cos(elevation)];
    otherwise
        error('Invalid system type specified.')
end

if(includeW==false)
    uv=uv(1:2,:);
end
end

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


%% Example 8.6: Simulating from a trigonometric nonlinear SDE
%
% Copyright:
%   2018 - Simo Särkkä and Arno Solin
%
% License:
%   This software is provided under the MIT License. See the accompanying
%   LICENSE file for details.

%% Gaussian approximation

  % Lock random seed
  if exist('rng') % Octave doesn't have rng
    rng(1,'twister')
  else
    randn('state',1);
    rand('state',1);
  end

  % Parameters
  tspan = 0:1:10;

  % The model
  f = @(x,t) -(1/10)^2*sin(x).*cos(x).^3;
  L = @(x,t) 1/10*cos(x).^2;

  % The derivatives
  df  = @(x,t) -1/100*cos(x).^2.*(2*cos(2*x)-1);
  ddf = @(x,t) 1/100*(sin(2*x) + 2*sin(4*x));
  dL  = @(x,t) -1/5*sin(x).*cos(x);
  ddL = @(x,t) -1/5*cos(2*x);

  % Exact solution
  solfun = @(wt,x0) atan(1/10*wt +tan(x0));

  % Intial
  x0 = 1;

%% Sample

  x = zeros(1,10000);
  xem   = x;
  xw20  = x;
  xw20g = x;

  % Lock random seed
  if exist('rng') % Octave doesn't have rng
    rng(1,'twister')
  else
    randn('state',1);
    rand('state',1);
  end

  for j=1:size(x,2)

    % Use Euler-Maruyama
    foo = eulermaruyama_weak(f,L,tspan,x0,1);
    xem(:,j) = foo(:,end);

    % Report
    if rem(j,100)==0, j, end

  end

  % Lock random seed
  if exist('rng') % Octave doesn't have rng
    rng(1,'twister')
  else
    randn('state',1);
    rand('state',1);
  end

  for j=1:size(x,2)

    % Weak order 2.0
    foo = w20scalar({f,df,ddf},{L,dL,ddL},tspan,x0,1,false);
    xw20(:,j) = foo(:,end);

    % Report
    if rem(j,100)==0, j, end

  end

  % Lock random seed
  if exist('rng') % Octave doesn't have rng
    rng(1,'twister')
  else
    randn('state',1);
    rand('state',1);
  end

  for j=1:size(x,2)

    % Weak order 2.0 (Gaussian increments)
    foo = w20scalar({f,df,ddf},{L,dL,ddL},tspan,x0,1,true);
    xw20g(:,j) = foo(:,end);

    % Store
    x(:,j) = foo(:,end);

    % Report
    if rem(j,100)==0, j, end

  end

%% Visualize

  % Lock random seed
  if exist('rng') % Octave doesn't have rng
    rng(1,'twister')
  else
    randn('state',1);
    rand('state',1);
  end

  % Samples from the exact solution
  xe = solfun(sqrt(tspan(end))*randn(1,200000),x0);

  % Bins
  nbins = 64;
  t = linspace(min(xe),max(xe)+.1,nbins);

  figure(2); clf; hold on

    % Show solution
    n = histc(xe,t);
    fill(t,n/numel(xe),1,'FaceColor',[.7 .7 .7],'EdgeColor',[.7 .7 .7])

    % Show solution w2.0
    n = histc(xw20,t);
    stairs(t-(t(2)-t(1))/2,n/numel(xw20),'-k')

    % Limits
    xlim([.35 1.25])
    lims = ylim;

    % Label
    xlabel('$x$')

    % Ticks
    set(gca,'XTick',0:.2:1.2)

  figure(3); clf; hold on

    % Show solution
    n = histc(xe,t);
    fill(t,n/numel(xe),1,'FaceColor',[.7 .7 .7],'EdgeColor',[.7 .7 .7])

    % Show solution w2.0 (Gaussian increments)
    n = histc(xw20g,t);
    stairs(t-(t(2)-t(1))/2,n/numel(xw20g),'-k')

    % Limits
    %xlim([min(t) max(t)])
    xlim([.35 1.25])
    ylim(lims)

    % Show legend
    legend('Exact','Weak order $2.0$')

    % Label
    xlabel('$x$')

    % Ticks
    set(gca,'XTick',0:.2:1.2)


function [lambda,v] = eig3(M,a12,a13,a22,a23,a33,varargin)
% eigenvalue and vectors of a symmetric 3x3 matrix
%
% Syntax
%
%   lambda = eig3(M)
%
%   lambda = eig3(a11,a12,a13,a22,a23,a33)
%
%
%   [v,lambda] = eig3(a11,a12,a13,a22,a23,a33)
%
% Input
%  M - array of symmetric 3x3 matrix
%  a11, a12,a13,a22,a23,a33 - vector of matrix elements
%
% Output
%  lambda - eigen values
%  v - eigen vectors
%

% get input
if nargin == 1
  a11 = M(1,1,:); a12 = M(1,2,:); a13 = M(1,3,:);
  a22 = M(2,2,:); a23 = M(2,3,:); a33 = M(3,3,:);
else
  a11 = M;
end

s = size(a11);

% input should be column vectors
a11 = a11(:).'; a12 = a12(:).'; a13 = a13(:).';
a22 = a22(:).'; a23 = a23(:).'; a33 = a33(:).';

% Given a real symmetric 3x3 matrix A, compute the eigenvalues
p1 = a12.^2 +a13.^2 + a23.^2;

q = (a11 + a22 + a33)/3;
p2 = (a11 - q).^2 + (a22 - q).^2 + (a33 - q).^2 + 2 * p1;
p = sqrt(p2 / 6);

%r = det(A-q*Id) / 2 / p^3;
r = (a11-q) .* ( (a22-q) .* (a33-q) - a23.^2) + ...
  a12 .* (a13 .* a23 - a12 .* (a33-q)) + ...
  a13 .* (a12 .* a23 - (a22-q) .* a13);
r = r / 2 ./ p.^3;

% In exact arithmetic for a symmetric matrix  -1 <= r <= 1
% but computation error can leave it slightly outside this range.
phi = acos(r) / 3;
phi(r <= -1) = pi / 3;
phi(r >= 1) = 0;

% the eigenvalues satisfy eig3 <= eig2 <= eig1
lambda = zeros(3,numel(a11));
lambda(1,:) = q + 2 * p .* cos(phi + (2*pi/3));
lambda(3,:) = q + 2 * p .* cos(phi);
lambda(2,:) = 3 * q - lambda(1,:) - lambda(3,:);     % since trace(A) = eig1 + eig2 + eig3

if nargout > 1

  % this is only required for matlab versions prior to 2015
  b11 = repmat(a11,3,1); b12 = repmat(a12,3,1); b13 = repmat(a13,3,1);
  b22 = repmat(a22,3,1); b23 = repmat(a23,3,1); b33 = repmat(a33,3,1);

  v = vector3d(b12 .* b23 - b13 .* (b22 - lambda),...
    b13 .* b12 - (b11 - lambda) .* b23,...
    (b11 - lambda) .* (b22 - lambda) - b12 .* b12,'antipodal');

  v = v.normalize;

  % fallback for special cases
  % TODO: this should be done better
  id = ~(abs(det(v(1,:),v(2,:),v(3,:))) > (1 - 1e-5)) | isnan(lambda(1,:));
  x = v.x; y = v.y; z = v.z;
  for k = find(id)

    [V,l] = eig([a11(k) a12(k) a13(k);...
      a12(k) a22(k) a23(k); ...
      a13(k) a23(k) a33(k)]);

    x(:,k) = V(1,:); y(:,k) = V(2,:); z(:,k) = V(3,:);
    lambda(:,k) = diag(l);

  end
  v.x = x; v.y = y; v.z = z;

  %a1 = vector3d(a11-lambda(1),a12,a13);
  %a2 = vector3d(a12,a22-lambda(1),23);
  %a3 = vector3d(a13,a23,33-lambda(1));

  %cross(a2,a3)

  % return only the largest eigen vector
  if check_option(varargin,'largest'), v = reshape(v(3,:),s); end

  % for some reason Matlab eig function changes to order outputs if called
  % with two arguments - so we should do the same
  [lambda,v] = deal(v,lambda);

end

end

function test

% generate random symmetric 3x3 matrixes
N = 10
a = rand(6,N);

tic
[V1,lambda1] = eig3(a(1,:),a(2,:),a(3,:),a(4,:),a(5,:),a(6,:));
toc

tic
lambda2 = zeros(3,N);
V2 = vector3d.zeros(3,N)
for i = 1:N
  A = [a(1,i),a(2,i),a(3,i);a(2,i),a(4,i),a(5,i);a(3,i),a(5,i),a(6,i)];
  [V,lambda2(:,i)] = eig(A,'vector');
  V2(1,i) = vector3d(V(:,1));
  V2(2,i) = vector3d(V(:,2));
  V2(3,i) = vector3d(V(:,3));
end
toc

norm(lambda1 - flipud(lambda2)) ./ N
max(angle(V1(:),V2(:)))

end


function [dphi,dlambda,h] = togeod(a,finv,X,Y,Z)
%TOGEOD   Subroutine to calculate geodetic coordinates
%         latitude, longitude, height given Cartesian
%         coordinates X,Y,Z, and reference ellipsoid
%         values semi-major axis (a) and the inverse
%         of flattening (finv).

%  The units of linear parameters X,Y,Z,a must all agree (m,km,mi,ft,..etc)
%  The output units of angular quantities will be in decimal degrees
%  (15.5 degrees not 15 deg 30 min).  The output units of h will be the
%  same as the units of X,Y,Z,a.

%  Copyright (C) 1987 C. Goad, Columbus, Ohio
%  Reprinted with permission of author, 1996
%  Fortran code translated into MATLAB
%  Kai Borre 03-30-96
% Changed according to Matlab ver. 6.5.1, 9 February 2004

h = 0;
tolsq = 1.e-10;
maxit = 1000;
% compute radians-to-degree factor
rtd = 180/pi;
% compute square of eccentricity
if finv < 1.e-20
   esq = 0;
else
   esq = (2-1/finv)/finv;
end
oneesq = 1-esq;
% first guess
% P is distance from spin axix
P = sqrt(X^2+Y^2);
% direct calculation of longitude
if P > 1.e-20
   dlambda = atan2(Y,X)*rtd;
else
   dlambda = 0;
end;
if (dlambda < 0)
   dlambda = dlambda + 360;
end
% r is distance from origin (0,0,0)
r = sqrt(P^2+Z^2);
if r > 1.e-20
   sinphi = Z/r;
else
   sinphi = 0;
end
dphi = asin(sinphi);
% initial value of height  =  distance from origin minus
% approximate distance from origin to surface of ellipsoid
if r < 1.e-20
   h = 0;
   return;
end;
h = r-a*(1-sinphi*sinphi/finv);
% iterate
for i = 1:maxit
   sinphi = sin(dphi);
   cosphi = cos(dphi);
   % compute radius of curvature in prime vertical direction
   N_phi = a/sqrt(1-esq*sinphi*sinphi);
   % compute residuals in P and Z
   dP = P - (N_phi + h) * cosphi;
   dZ = Z - (N_phi*oneesq + h) * sinphi;
   % update height and latitude
   h = h+(sinphi*dZ+cosphi*dP);
   dphi = dphi+(cosphi*dZ-sinphi*dP)/(N_phi + h);
   % test for convergence
   if (dP*dP + dZ*dZ < tolsq)
      break;
   end

   % Not Converged--Warn user
   if i == maxit
      fprintf([' Problem in TOGEOD, did not converge in %2.0f',...
            ' iterations\n'],i)
   end;
end;
dphi = dphi*rtd;
%%%%%%%% end togeod.m  %%%%%%%%%%%%%%%%%%%%%%


% KEYWORDs: normal log pdf
% this program returns log of f(x)=normal(x; mean_par, variance_par) pdf function and its derivative,
% the pdf f(x) is defined up to the normalization constant
% USAGE: [log_function, log_function_prime]=log_normal_pdf(x, alpha, beta)
% INPUTs: "x" is vector of argument values
%         "function_parameters" is a vector of parameters of function f(x), function_parameters(1)=mean
%                         and function_parameters(2)=variance are parameters of the normal distribution
% OUTPUTs: "log_function" is log[f(x)]
%          "log_function_prime" is (d/dx)log[f(x)]
% NOTES: pdf is log-concave
% Last modified on Jul 15, 2007

function [log_function, log_function_prime]=log_normal_pdf(x, function_parameters)

mean_par=function_parameters(1);
variance_par=function_parameters(2);

log_function=(-0.5/variance_par)*(x-mean_par).^2;
log_function_prime=(-1/variance_par)*(x-mean_par);


function [z,ze]=doubleLengthSum(x,xe,y,ye)
%%DOUBLELENGTHSUM Given two doublelength values, which could, for example,
%       be returned by exactPairMult or exactPairSum, find their sum as
%       another doublelength value.
%
%INPUTS: x,xe A real doublelength floating point number. The exact number
%              is x+xe (added with infinite precision) and
%              abs(xe)<=abs(x+xe)*2^(-t)/(1+2^(-t)), where t is the number
%              of bits in the mantissa, which is 53, since it is assumed
%              that these values are floating point doubles.
%        y, ye A second real doublelength floating point number that should
%              be added to x,xe.
%
%OUTPUTS: z, ze The doublelength floating point number that is the sum of
%               (x,xe) and (y,ye). If the inputs satisfy the proper
%               splitting of the number into two parts, (the relation in
%               size between abs(xe) and abs(x+xe) and similarly for y and
%               ye), then this will also satisfy the splitting of the
%               numbers. This does not necessarily hold if denormalized
%               numbers are encountered.
%
%This function implements the Add2 algorithm of [1].
%
%Note that this assumes that the processor rounding mode has been set to
%round to "nearest," which is the default in Matlab, but which can be
%changed using the function setProcRoundingMode.
%
%EXAMPLE:
% [z,ze]=doubleLengthSum(1,1e-30,1e20,1e-1)
%One gets z=1e20 and ze=1.1. The 1e-30 component is truncated away, because
%it is too small to represent with two doubles.
%
%REFERENCES:
%[1] T. J. Dekker, "A Floating Point Technique for Extending the Available
%    Precision," Numerische Mathematik, vol. 18, no. 3, Jun. 1971, pp.
%    224-242.
%
%November 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

r=x+y;
if(abs(x)>abs(y))
    s=x-r+y+ye+xe;
else
    s=y-r+x+xe+ye;
end

z=r+s;
ze=r-z+s;

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 val=BesselI1RatioApprox(kappa,algorithm)
%%BESSELI1RATIOAPPROX Approximate the ratio of modified Bessel functions of
%              the first kind of the form x=I_{1}(kappa)/I_{0}(kappa).
%              For a more exact solution, use the function BesseliRatio,
%              which can also handle different subscripts (orders) for the
%              modified Bessel functions of the first kind.
%
%INPUTS: kappa The real argument of the modified Bessel function of the
%              first kind kappa>=0.
%    algorithm A parameter selecting the approximation that is used.
%              Possible values are:
%              0 (The default if omitted or an empty matrix is passed) Use
%                the approximation used in Equations 8 and 10 of [1].
%                Equation 8 in [1] is the first two terms of an expansion
%                given on page 290 of [2].
%              1 Use the approximations in Equations 5 and 7 of [3].
%                Equation 5 of [3] is related to a Taylor series expansion
%                given on page 289 of [2].
%
%OUTPUTS: x An approximation to the ratio I_{1}(kappa)/I_{0}(kappa).
%
%REFERENCES:
%[1] G. Stienne, S. Reboul, M. Azmani, J. B. Choquel, and M. Benjelloun, A
%    multi-temporal multi-sensor circular fusion filter," Information
%    Fusion, vol. 18, pp. 86-100, Jul. 2014.
%[2] S. R. Jammalamadaka and A. SenGupta, Topics in Circular Statistics.
%    Singapore: World Scientific, 2001.
%[3] G. Stienne, S. Reboul, J. B. Choquel, and M. Benjelloun, "Circular
%    data processing tools applied to a phase open loop architecture for
%    multi-channel signals tracking," in Position Location and Navigation
%    Symposium, Myrtle Beach, SC, 23-26 Apr. 2012, pp. 633-642.
%
%April 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

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

    switch(algorithm)
        case 0%The method of [1].
            if(kappa>=0.6)
                val=1-1/(2*kappa);
            else
                val=exp(-1/(2*kappa));
            end
        case 1
            if(kappa>=0.6)
                val=(1-3/(8*kappa)-15/(128*kappa^2))/(1+1/(8*kappa)+9/(128*kappa^2));
            else
                 val=kappa/2;
            end
        otherwise
            error('Unknown Algorithm Specified.')
    end

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


function [r, s, t] = xyztorst(X, Y, Z)

% function [r,s,t] = xyztorst(x, y, z)
% Purpose : Transfer from (x,y,z) in equilateral tetrahedron
%           to (r,s,t) coordinates in standard tetrahedron

v1 = [-1,-1/sqrt(3), -1/sqrt(6)]; v2 = [ 1,-1/sqrt(3), -1/sqrt(6)];
v3 = [ 0, 2/sqrt(3), -1/sqrt(6)]; v4 = [ 0, 0/sqrt(3),  3/sqrt(6)];

% back out right tet nodes
rhs = [X';Y';Z'] - 0.5*(v2'+v3'+v4'-v1')*ones(1,length(X));
A = [0.5*(v2-v1)',0.5*(v3-v1)',0.5*(v4-v1)'];
RST = A\[rhs];
r = RST(1,:)'; s = RST(2,:)'; t = RST(3,:)';
return;


function [h,g,a,info]=wfilt_lemarie(N)
%WFILT_LEMARIE  Battle and Lemarie filters
%   Usage: [h,g,a]=wfilt_lemarie(N)
%
%   Input parameters:
%         N     : Filter length, must be even.
%
%   `[h,g,a]=wfilt_lemarie(N)` calculates $N$ (even) truncated coeficients
%   of orthonormal Battle-Lemarie wavelets. Filter coefficients are obtained
%   by frequency domain sampling and trunctating the impulse response.
%   Due to the truncation, the filterbank might not achieve a perfect
%   reconstruction. The filetrs are included nevertheless since they were
%   the original ones used in the first MRA paper.
%
%   Examples:
%   ---------
%   :::
%     wfiltinfo('lemarie50');
%
%   References: mallat89atheory

% Original copyright goes to:
% Copyright (C) 1994, 1995, 1996, by Universidad de Vigo
% Author: Jose Martin Garcia
% e-mail: Uvi_Wave@tsc.uvigo.es

if rem(N,2)~=0
    error('%s: Filter length must be even.',upper(mfilename));
end

num_coefs = N;
L = 1024;
H = wfreq_lemarie(L);
hh=real(ifft(H{1},L));
hh=[ hh(L-floor(num_coefs/2)+1:L) hh(1:ceil(num_coefs/2))];
hh=hh/norm(hh);

g{1} = (hh);
g{2} = -(-1).^(1:length(hh)).*g{1}(end:-1:1);

g = cellfun(@(gEl) struct('h',gEl,'offset',-floor(numel(gEl)/2)),g,'UniformOutput',0);

h = g;
a= [2;2];
info.istight = 1;

function [H,G] = wfreq_lemarie(L)
%WFREQ_LEMARIE  Battle and Lemarie filters frequency resp. sampling
%   Usage: [H,G]=wfreq_lemarie(L)
%
%   Input parameters:
%         N     : Number of samples of the frequency response.
%
%   `[H,G]=wfreq_lemaire(L)` calculates $L$ samples of the Battle and
%   Lemarie filters frequency responses.
%
%   References: mallat89atheory
%
%

% Original copyright goes to:
% Copyright (C) 1994, 1995, 1996, by Universidad de Vigo
% Author: Jose Martin Garcia
% e-mail: Uvi_Wave@tsc.uvigo.es

% frequency axis
w=[0:2*pi/L:2*pi*(1-1/L)];
w(1)=eps;
w(L/2+1)=w(L/2+1)+1e-15;

% calculation of frequency response of analysis lowpass filter
num=0;den=0;
K=36;
for k=-K:K,
	num=1./((w+2*pi*k).^8)+num;
	den=1./((2*w+2*pi*k).^8)+den;
end
H = cell(2,1);
H{1}=sqrt(num./(2.^8*den));
H{1}(1)=1;

H{2} = fftshift(H{1});
G = cell(2,1);
G{1} = fliplr(H{1});
G{2} = fliplr(H{2});


function value = r8mat_norm_l1 ( m, n, a )

%*****************************************************************************80
%
%% R8MAT_NORM_L1 returns the matrix L1 norm of an R8MAT.
%
%  Discussion:
%
%    The matrix L1 norm is defined as:
%
%      value = max ( 1 <= J <= N ) sum ( 1 <= I <= M ) abs ( A(I,J) ).
%
%    The matrix L1 norm is derived from the vector L1 norm, and
%    satisifies:
%
%      vec_norm_l1 ( A * x ) <= mat_norm_l1 ( A ) * vec_norm_l1 ( x ).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the number of rows in A.
%
%    Input, integer N, the number of columns in A.
%
%    Input, real A(M,N), the matrix whose L1 norm is desired.
%
%    Output, real VALUE, the L1 norm of A.
%
  value = 0.0;

  for j = 1 : n
    value = max ( value, sum ( abs ( a(1:m,j) ) ) );
  end

  return
end


function value = i4_is_prime ( n )

%*****************************************************************************80
%
%% I4_IS_PRIME reports whether an integer is prime.
%
%  Discussion:
%
%    A simple, unoptimized sieve of Erasthosthenes is used to
%    check whether N can be divided by any integer between 2
%    and SQRT(N).
%
%    Note that negative numbers, 0 and 1 are not considered prime.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the integer to be tested.
%
%    Output, logical VALUE, is TRUE if N is prime, and FALSE
%    otherwise.
%
  if ( n <= 0 )
    value = 0;
    return
  end

  if ( n == 1 )
    value = 0;
    return
  end

  if ( n <= 3 )
    value = 1;
    return
  end

  nhi = floor ( sqrt ( n ) );

  for i = 2 : nhi
    if ( mod ( n, i ) == 0 )
      value = 0;
      return
    end
  end

  value = 1;

  return
end


function P = legendrepol(N, a, b)
%-------------------------------------------------------------------------
%
%       P = legendrepol(N)
%
% returns the Legendre polynomials up to order N, orthogonal on
% the interval [-1,1]. Each row contain the polynomial coefficients in
% descending order, i.e. the first row is P0 = [0 0 .... 1].
%
%       P = legendrepol(N),xmin,xmax)
%
% returns the Legendre polynomial orthogonal on [xmin, xmax].
%-------------------------------------------------------------------------

% T. Wik, April 2012

% L is the Legendre polynomials orthogonal on (-1,1):
if N<1
	L = 1;
else
	L 		   = zeros(N+1,N+1);
	L(1,N+1)   = 1;              % L0
	L(2,N:N+1) = [1 0];          % L1
   for n=1:N-1                   % L2 to LN
   	L(n+2,N-n:N+1) 	= 1/(n+1)* ...
                   	 ((2*n+1)*[L(n+1,N+1-n:N+1) 0]-n*[0 0 L(n,N+2-n:N+1)]);
   end
end

if nargin == 3
    if a > b
        error('legendrepol:wrongArguments','xmin > xmax !')
    end
% Transform such that P is orthogonal on (xmin,xmax):
    P = zeros(N+1);
    A = zeros(N+1);

    for n = 1:N+1
        A(end-n+1,end-n+1:end) = binomial(n-1,2/(b-a),-(a+b)/(b-a));
        Lambda  = diag(L(n,:));
        P(n,:) = sum(Lambda*A);
    end
elseif nargin == 1
        P = L;
else
    error('legendrepol:wrongArguments','Use one or three arguments!')
end

function P = binomial(n,a,b)
%------------------------------------------------------
%
% function P = binomial(n,a,b)
%
% Returns the polynomial coefficients p(i) for
%
% P(x) = (ax + b)^n
%      = p(1)x^n + p(2)x^(n-1) + ... + p(n)x + p(n+1)
%
%------------------------------------------------------

% First binomial coefficients for (x+1)^n

if n==0, P=1; return, end
p = zeros(n,n+1);

p(1,1:2) = [1 1];

for j = 2:n
    p(j,1) = 1;
    for k = 2:n
        p(j,k) = p(j-1,k-1) + p(j-1,k);
    end
    p(j,n+1) = 1;
end

% Adjust for a and b

for k = 1:n+1;
    P(k) = p(n,k)*a^(n-k+1)*b^(k-1);
end


function [MSC]=coherence_MVDR(x1,x2,L,K);

%% This program computes the coherence function between 2 signals
%% x1 and x2 with the MVDR method.
%% This algorithm is based on the paper by the same authors:
%% J. Benesty, J. Chen, and Y. Huang, "A generalized MVDR spectrum,"
%% IEEE Signal Processing letters, vol. 12, pp. 827-830, Dec. 2005.

%% x1, first signal vector of length n
%% x2, second signal vector of length n
%% L is the length of MVDR filter or window length
%% K is the resolution (the higher K, the better the resolution)

%initialization
xx1     = zeros(L,1);
xx2     = zeros(L,1);
r11     = zeros(L,1);
r22     = zeros(L,1);
r12     = zeros(L,1);
r21     = zeros(L,1);

%construction of the Fourier Matrix
F       = zeros(L,K);
l       = [0:L-1]';
f       = exp(2*pi*l*j/K);
for k = 0:K-1
    F(:,k+1) = f.^k;
end
F       = F/sqrt(L);

%number of samples, equal to the lenght of x1 and x2
n       = length(x1);

for i = 1:n
    xx1 = [x1(i);xx1(1:L-1)];
    xx2 = [x2(i);xx2(1:L-1)];
    r11 = r11 + xx1*conj(xx1(1));
    r22 = r22 + xx2*conj(xx2(1));
    r12 = r12 + xx1*conj(xx2(1));
    r21 = r21 + xx2*conj(xx1(1));
end
r11 = r11/n;
r22 = r22/n;
r12 = r12/n;
r21 = r21/n;
%
R11 = toeplitz(r11);
R22 = toeplitz(r22);
R12 = toeplitz(r12,conj(r21));
%
%for regularization
Dt1     = 0.01*r11(1)*diag(diag(ones(L)));
Dt2     = 0.01*r22(1)*diag(diag(ones(L)));
%
Ri11    = inv(R11 + Dt1);
Ri22    = inv(R22 + Dt2);
Rn12    = Ri11*R12*Ri22;
%
Si11    = real(diag(F'*Ri11*F));
Si22    = real(diag(F'*Ri22*F));
S12     = diag(F'*Rn12*F);
%
%Magnitude squared coherence function
MSC     = real(S12.*conj(S12))./(Si11.*Si22);