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

%*****************************************************************************80
%
%% GEGENBAUER_RULE generates a Gauss-Gegenbauer rule.
%
%  Discussion:
%
%    This program computes a standard Gauss-Gegenbauer quadrature rule
%    and writes it to a file.
%
%    The user specifies:
%    * the ORDER (number of points) in the rule;
%    * the ALPHA parameter;
%    * A, the left endpoint;
%    * B, the right endpoint;
%    * FILENAME, the root name of the output files.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 February 2010
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'GEGENBAUER_RULE\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Compute a Gauss-Gegenbauer rule for approximating\n' );
  fprintf ( 1, '    Integral ( A <= x <= B ) ((x-A)(B-X))^ALPHA 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:\n' );
  fprintf ( 1, '  A is the left endpoint;\n' );
  fprintf ( 1, '  B is the right endpoint;\n' );
  fprintf ( 1, '  FILENAME is used to generate 3 files:\n' );
  fprintf ( 1, '    filename_w.txt - the weight file\n' );
  fprintf ( 1, '    filename_x.txt - the abscissa file.\n' );
  fprintf ( 1, '    filename_r.txt - the region file.\n' );
%
%  Initialize the parameters.
%
  beta = 0.0;
%
%  Get ORDER.
%
  if ( nargin < 1 )
    order = input ( '  Enter the rule order ORDER:  ' );
  elseif ( ischar ( order ) )
    order = str2num ( order );
  else
%
%  Get ALPHA.
%
  if ( nargin < 2 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  ALPHA is the exponent of ((x-A)(B-X)) in the integral:\n' );
    fprintf ( 1, '  Note that -1.0 < ALPHA is required.\n' );
    fprintf ( 1, '\n' );
    alpha = input ( '  Enter the value of ALPHA:  ' );
  elseif ( ischar ( alpha ) )
    alpha = str2num ( alpha );
  end
%
%  Get A.
%
  if ( nargin < 3 )
    a = input ( '  Enter the left endpoint A:  ' );
  elseif ( ischar ( a ) )
    a = str2num ( a );
  end
%
%  Get B.
%
  if ( nargin < 4 )
    b = input ( '  Enter the right endpoint B:  ' );
  elseif ( ischar ( b ) )
    b = str2num ( b );
  end
%
%  Get FILENAME.
%
  if ( nargin < 5 )
    fprintf ( 1,  '\n' );
    fprintf ( 1,  '  FILENAME is 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 = 3;
  [ x, w ] = cgqf ( order, kind, alpha, beta, a, b );
%
%  Write the rule.
%
  r = [ a, b ]';
  rule_write ( order, filename, x, w, r );
%
%  Terminate.
%
  fprintf ( 1,  '\n' );
  fprintf ( 1,  'GEGENBAUER_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_sign ( x )

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

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

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

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

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

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

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

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

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

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

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

  parchk ( kind, 1, alpha, beta )

  if ( kind == 1 )

    al = 0.0;
    be = 0.0;

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

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

  elseif ( kind == 2 )

    al = -0.5;
    be = -0.5;

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

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

  elseif ( kind == 3 )

    al = alpha;
    be = alpha;

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

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

  elseif ( kind == 4 )

    al = alpha;
    be = beta;

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

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

  elseif ( kind == 5 )

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

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

  elseif ( kind == 6 )

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

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

  elseif ( kind == 7 )

    al = alpha;
    be = 0.0;

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

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

  elseif ( kind == 8 )

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

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

  elseif ( kind == 9 )

    al = 0.5;
    be = 0.5;

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

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

  end

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

  for k = 1 : nt

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

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

  end

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

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

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

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

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

  return
end
function timestamp ( )

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

  return
end


function value = r8_factorial ( n )

%*****************************************************************************80
%
%% R8_FACTORIAL returns N!.
%
%  Definition:
%
%    N! = Product ( 1 <= I <= N ) I
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 February 1999
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the argument of the function.
%    0 <= N.
%
%    Output, real VALUE, the factorial of N.
%
  if ( n < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_FACTORIAL - Fatal error!\n' );
    fprintf ( 1, '  N < 0.\n' );
    error ( 'R8_FACTORIAL - Fatal error!' );
  end

  value = 1.0;

  for i = 2 : n
    value = value * i;
  end

  return
end


%---------------------------------------------------------
% Low-pass FIR digital differentiator (LPFIRDD) design   -
% via constrained quadratic programming (QP)             -
% [Copt,c]=lpfirdd(N,alpha,beta,r,idraw)                 -
% By Dr Yangquan Chen		019-07-1999                   -
% Email=<yqchen@ieee.org>; URL=http://www.crosswinds.net/~yqchen/
% --------------------------------------------------------
% LPFIRDD: only 1st order derivative estimate
% total taps=2N. c(i)=-c(i+N+1); c(N+1)=0 (central point)
%
%          -N      -N+1            -1                    N
% FIR=c(1)z   +c(2)z    +...+ c(N)z  + 0 + ... + c(2N+1)z
%
%       N
%     ------
%     \                    j   -j
%      >       Copt(j) * (z - z  )
%     /
%     ------
%      j=1
% N: Taps  (N=2, similar to sgfilter(2,2,1,1)
% alpha ~ beta: transit band of frequency
%				    (in percentage of Nyquest freq)
% r: the polynomial order. r<=N Normally, set it to 1.
%---------------------------------------------------------------
function [Copt,bd]=lpfirdd(N,alpha,beta,r,idraw)
% testing parameters
% alpha=1./pi;beta=1.5/pi;N=10;r=1;idraw=1;
if (alpha>beta)
   disp('Error in alpha (alpha<=beta)');return;
end
if ((beta>1) | (beta <0))
   disp('Error in Beta! (beta in [0,1]');return;
end
if ((alpha>1) | (alpha <0))
   disp('Error in Alpha! (Alpha in [0,1]');return;
end
% default r=1
if (r<1); r=1; end
% matrix W
W=zeros(r,N);
for ix=1:N;for jx=1:r;
      W(jx,ix)=ix^(2*jx-1);
end;end
%matrix L
L=zeros(N,1);
if (beta>alpha)
	for ix=1:N
	   L(ix)=(alpha*sin(ix*beta*pi)-beta*sin(ix*alpha*pi))/ix/ix/(beta-alpha);
	end
elseif (beta==alpha)
	for ix=1:N
	   L(ix)=(ix*alpha*pi*cos(ix*alpha*pi)-sin(ix*alpha*pi))/ix/ix;
	end
end
% matrix e
ex=zeros(r,1);ex(1)=1;
% optimal solution
Copt=W'*inv(W*W')*(ex + 2.*W*L/pi)-2.*L/pi
Copt=Copt/2;
% fr plots
if (idraw==1)
   bd=[-fliplr(Copt'),0,Copt']';
	%ad=1;sys_sg=tf(bd',ad,1./Fs);bode(sys_sg)
	Fs=12790;nL=N;nR=N;npts=1000;%w=logspace(0,4,npts);
	w=((1:npts)-1)*pi/npts;
	j=sqrt(-1);ejw=zeros(nL+nR+1,npts);
	for ix=(-nL:nR)
		ejw(ix+nL+1,:)=exp(j*ix*w);
	end
	freq=bd'*ejw;
	figure;subplot(2,1,1)
	plot(w/pi*Fs/2,(abs(freq)));grid on;
	hold on; ax=axis;ax(2)=Fs/2;axis(ax);
	xlabel('freq. (Hz)');ylabel('amplitude (dB)');
	subplot(2,1,2)
	plot(w/pi*Fs/2,180*(angle(freq))/pi );grid on;
	hold on; ax=axis;ax(2)=Fs/2;axis(ax);
   xlabel('freq. (Hz)');ylabel('phase anlge (deg.)');

	figure;subplot(2,1,1);Fs=12600; % Hz for U8
	semilogx(w*Fs/pi/2,20*log10(abs(freq)));grid on;
	hold on; ax=axis;ax(2)=Fs/2;axis(ax);
	semilogx([Fs/2,Fs/2],[ax(3),ax(4)],'o-r');grid on;
	xlabel('freq. (Hz)');ylabel('amplitude (dB)');
	subplot(2,1,2)
	semilogx(w*Fs/pi/2,180*(angle(freq))/pi );grid on;
	hold on; ax=axis;ax(2)=Fs/2;axis(ax);
	semilogx([Fs/2,Fs/2],[ax(3),ax(4)],'o-r');grid on;
	xlabel('freq. (Hz)');ylabel('phase anlge (deg.)');
end
return


function [vFrequency, vAmplitude] = fastfft(vData, SampleRate, Plot)

%FASTFFT   Create useful data from an FFT operation.
%   Usage: [vFrequency, vAmplitude] = fastfft(vData, SampleRate, [Plot])
%
%   (no plot will be shown if the last input == 0 or is not included)
%
%   This function inputs 'vData' as a vector (row or column),
%   'SampleRate' as a number (samples/sec), 'Plot' as anything,
%   and does the following:
%
%     1: Removes the DC offset of the data
%     2: Puts the data through a hanning window
%     3: Calculates the Fast Fourier Transform (FFT)
%     4: Calculates the amplitude from the FFT
%     5: Calculates the frequency scale
%     6: Optionally creates a Bode plot
%
%   Created 7/22/03, Rick Auch, mekaneck@campbellsville.com

%Make vData a row vector
if size(vData,2)==1
    vData = vData';
end

%Calculate number of data points in data
n = length(vData);

%Remove DC Offset
vData = vData - mean(vData);

%Put data through hanning window using hanning subfunction
vData = hanning(vData);

%Calculate FFT
vData = fft(vData);

%Calculate amplitude from FFT (multply by sqrt(8/3) because of effects of hanning window)
vAmplitude = abs(vData)*sqrt(8/3);

%Calculate frequency scale
vFrequency = linspace(0,n-1,n)*(SampleRate/n);

%Limit both output vectors due to Nyquist criterion
DataLimit = ceil(n/2);
vAmplitude = vAmplitude(1:DataLimit);
vFrequency = vFrequency(1:DataLimit);

if exist('Plot', 'var')==1 & Plot~=0
    plot(vFrequency, vAmplitude);
    title('Bode Plot');
    xlabel('Frequency (Hz)');
    ylabel('Amplitude');
end

%------------------------------------------------------------------------------------------
%Hanning Subfunction
function vOutput = hanning(vInput)
% This function takes a vector input and outputs the same vector,
% multiplied by the hanning window function

%Determine the number of input data points
n = length(vInput);

%Initialize the vector
vHanningFunc = linspace(0,n-1,n);

%Calculate the hanning funtion
vHanningFunc = .5*(1-cos(2*pi*vHanningFunc/(n-1)));

%Output the result
vOutput = vInput.*vHanningFunc;


function binv = beta_inv (x, a, b)
%  BETA_INV Inverse of the beta cumulative distribution function (cdf).
%     X = BETA_INV(P,A,B) returns the inverse of the beta cdf with
%     parameters A and B at the values in P.
%
% Copyright (c) 1995-1997, 2005-2007 Kurt Hornik

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

if (nargin ~= 3)
  error('must provide 3 parameters')
end

if (~isscalar (a) || ~isscalar(b))
  if ((size(a,1) ~= size(b,1)) || (size(b,1) ~= size(x,1)))
    error ('betainv: x, a and b must be of common size or scalars');
  end
end

[n,nin] = size(x);
binv = zeros(n,nin);

k = find ((x<0) || (x>1) || (a<0) || (b<0) || isnan(x));
if (any(k))
  binv(k) = NaN;
end

k = find ((x==1) && (a>0) && (b>0));
if (any (k))
  binv(k) = 1;
end

k = find ((x > 0) & (x < 1) & (a > 0) & (b > 0));
if (any (k))
  if (~isscalar(a) || ~isscalar(b))
    a = a(k);
    b = b(k);
    y = a./(a+b);
  else
    y = a/(a + b)*ones(size(k));
  end
  x = x(k);
  l = find (y < eps);
  if (any (l))
    y(l) = sqrt (eps) * ones (length (l), 1);
  end
  l = find (y > 1 - eps);
  if (any (l))
    y(l) = 1 - sqrt (eps) * ones (length (l), 1);
  end

  y_old = y;
  for i = 1 : 10000
    h     = (beta_cdf (y_old, a, b) - x) ./ beta_pdf (y_old, a, b);
    y_new = y_old - h;
    ind   = find (y_new <= eps);
    if (any (ind))
      y_new (ind) = y_old (ind) / 10;
    end
    ind = find (y_new >= 1 - eps);
    if (any (ind))
      y_new (ind) = 1 - (1 - y_old (ind)) / 10;
    end
    h = y_old - y_new;
    if (max (abs (h)) < sqrt (eps))
      break;
    end
    y_old = y_new;
  end

  binv(k) = y_new;
end

end


function [P loglikelihood] = LearnCPDsGivenGraph(dataset, G, labels)
%
% Inputs:
% dataset: N x 10 x 3, N poses represented by 10 parts in (y, x, alpha)
% G: graph parameterization as explained in PA description
% labels: N x 2 true class labels for the examples. labels(i,j)=1 if the
%         the ith example belongs to class j and 0 elsewhere
%
% Outputs:
% P: struct array parameters (explained in PA description)
% loglikelihood: log-likelihood of the data (scalar)
%
% Copyright (C) Daphne Koller, Stanford Univerity, 2012

N = size(dataset, 1);
K = size(labels,2);

loglikelihood = 0;
P.c = zeros(1,K);

% estimate parameters
% fill in P.c, MLE for class probabilities
% fill in P.clg for each body part and each class
% choose the right parameterization based on G(i,1)
% compute the likelihood - you may want to use ComputeLogLikelihood.m
% you just implemented.
%%%%%%%%%%%%%%%%%%%%%%%%%
% YOUR CODE HERE
if size(size(G),2) == 2
	G1 = G; G2 = G;
else
	G1 = reshape(G(:,:,1),10,2);
	G2 = reshape(G(:,:,2),10,2);
end
human = dataset([labels(:,1)==1],:,:);
alien = dataset([labels(:,2)==1],:,:);
P.c = [size(human,1),size(alien,1)];
P.c /= N;
for i = 1:10
	if G1(i,1) == 0
		[P.clg(i).mu_y(1),P.clg(i).sigma_y(1)] = FitGaussianParameters(human(:,i,1));
		[P.clg(i).mu_x(1),P.clg(i).sigma_x(1)] = FitGaussianParameters(human(:,i,2));
		[P.clg(i).mu_angle(1),P.clg(i).sigma_angle(1)] = FitGaussianParameters(human(:,i,3));
	else
		% calc beta and sigma
		[Betay,P.clg(i).sigma_y(1)] = FitLinearGaussianParameters(human(:,i,1),reshape(human(:,G1(i,2),:),size(human,1),3));
		[Betax,P.clg(i).sigma_x(1)] = FitLinearGaussianParameters(human(:,i,2),reshape(human(:,G1(i,2),:),size(human,1),3));
		[Betaangle,P.clg(i).sigma_angle(1)] = FitLinearGaussianParameters(human(:,i,3),reshape(human(:,G1(i,2),:),size(human,1),3));
		P.clg(i).theta(1,:) = [Betay(4),Betay(1:3)',Betax(4),Betax(1:3)',Betaangle(4),Betaangle(1:3)'];

	end
	if G2(i,1) == 0
		[P.clg(i).mu_y(2),P.clg(i).sigma_y(2)] = FitGaussianParameters(alien(:,i,1));
		[P.clg(i).mu_x(2),P.clg(i).sigma_x(2)] = FitGaussianParameters(alien(:,i,2));
		[P.clg(i).mu_angle(2),P.clg(i).sigma_angle(2)] = FitGaussianParameters(alien(:,i,3));
	else
		%calc beta and sigma
		[Betay,P.clg(i).sigma_y(2)] = FitLinearGaussianParameters(alien(:,i,1),reshape(alien(:,G2(i,2),:),size(alien,1),3));
		[Betax,P.clg(i).sigma_x(2)] = FitLinearGaussianParameters(alien(:,i,2),reshape(alien(:,G2(i,2),:),size(alien,1),3));
		[Betaangle,P.clg(i).sigma_angle(2)] = FitLinearGaussianParameters(alien(:,i,3),reshape(alien(:,G2(i,2),:),size(alien,1),3));
		P.clg(i).theta(2,:) = [Betay(4),Betay(1:3)',Betax(4),Betax(1:3)',Betaangle(4),Betaangle(1:3)'];
	end
end
loglikelihood = ComputeLogLikelihood(P,G,dataset);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('log likelihood: %f\n', loglikelihood);


function [P, R] = mdp_example_rand (S, A, is_sparse, mask)

% mdp_example_rand    Generate a random Markov Decision Process
% Arguments -------------------------------------------------------------
%   S = number of states (> 0)
%   A = number of actions (> 0)
%   is_sparse = false to have matrices in plain format, true to have sparse matrices
%               optional (default false).
%   mask(SxS) = matrix with 0 and 1 (0 indicates a place for a zero probability),
%               optional (default, ones(S,S) )
% Evaluation -------------------------------------------------------------
%   P(SxSxA) = transition probability matrix
%   R(SxSxA) = reward matrix

% MDPtoolbox: Markov Decision Processes Toolbox
% Copyright (C) 2009  INRA
% Redistribution and use in source and binary forms, with or without modification,
% are permitted provided that the following conditions are met:
%    * Redistributions of source code must retain the above copyright notice,
%      this list of conditions and the following disclaimer.
%    * Redistributions in binary form must reproduce the above copyright notice,
%      this list of conditions and the following disclaimer in the documentation
%      and/or other materials provided with the distribution.
%    * Neither the name of the <ORGANIZATION> nor the names of its contributors
%      may be used to endorse or promote products derived from this software
%      without specific prior written permission.
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
% ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
% WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
% IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
% INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
% BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
% DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
% LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
% OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
% OF THE POSSIBILITY OF SUCH DAMAGE.

% arguments checking
if S < 1 || A < 1
    disp('--------------------------------------------------------')
    disp('MDP Toolbox ERROR: The number of states S ')
    disp('and the number of actions A must be upper than 1.')
    disp('--------------------------------------------------------')
elseif nargin >= 4 && ( size(mask,1) ~= S || size(mask,2) ~= S )
    disp('--------------------------------------------------------')
    disp('MDP Toolbox ERROR: mask must be a SxS matrix')
    disp('--------------------------------------------------------')
else

    % initialization of optional arguments
    if nargin < 3; is_sparse = false; end;
    if nargin < 4; mask = ones(S,S); end;

    if is_sparse
        % definition of transition matrix : square stochastic matrix
        P = {};
        for a=1:A
            PP = sparse(mask .* rand(S));
            for s=1:S
                PP(s,:) = PP(s,:) / sum( PP(s,:) );
            end;
            P{a} = PP;
        end;

        % definition of reward matrix (values between -1 and +1)
        R = {};
        for a=1:A
	    R{a} = sparse(mask .* ( 2*rand(S) - ones(S,S) ));
        end;
    else
        % definition of transition matrix : square stochastic matrix
        for a=1:A
            P(:,:,a) = mask .* rand(S);
            for s=1:S
                P(s,:,a) = P(s,:,a) / sum( P(s,:,a) );
            end;
        end;

        % definition of reward matrix (values between -1 and +1)
        for a=1:A
	    R(:,:,a) = mask .* ( 2*rand(S) - ones(S,S) );
        end;
    end

end;


function L = gaussianLogLikelihood(noise, mu, varsigma, y)

% GAUSSIANLOGLIKELIHOOD Log-likelihood of data under Gaussian noise model.

% NOISE

N = size(mu, 1);
D = size(mu, 2);
varsigma = varsigma + noise.sigma2;
for i = 1:D
  mu(:, i) = mu(:, i) + noise.bias(i);
end
arg = (y - mu);
arg = arg.*arg./varsigma;

L = - 0.5*sum(sum(log(varsigma))) ...
    - 0.5*sum(sum(arg)) ...
    - 0.5*N*D*log(2*pi);


% ASTROTIK by Francesco Santilli
% f2v computes velocity vector of an orbit at different true anomalies
%
% Usage: V = f2v(orbit,f,mi)
%
% where: orbit = [p e i o w] = 3d orbit elements
%        orbit = [p e w]     = 2d orbit elements
%           p = semi-latus rectum [L] (p>0)
%           e = eccentricity [-] (e>=0)
%           i = inclinaion [rad]
%           o = raan [rad]
%           w = argument of perifocus [rad]
%        f(k) = true anomalies [rad]
%        mi = gravitational parameter [L^3*T^-2] (mi>0)
%        V(k,:) = velocity [L*T^-1]

function V = f2v(orbit,f,mi)

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

    D = check(orbit,1);
    K = check(f,1);
    check(mi,0)

    if ~(D==3 || D==5)
        error('Wrong size of input arguments.')
    end

    d3 = (D==5);
    f = f(:);
    cf = cos(f);
    sf = sin(f);

    % rotation matrix
    if d3
        [p,e,i,o,w] = take(orbit);
        R = rotation([o i w]);
        R = R(1:2,:);
    else
        [p,e,w] = take(orbit);
        cw = cos(w);
        sw = sin(w);
        R = [ cw sw
             -sw cw];
    end

    if mi <= 0
        error('mi must be a strictly positive value.')
    end

    if p <= 0
        error('p must be a stricly positive value.')
    end

    if e < 0
        error('e must be a positive value.')
    end

    % velocity
    v = sqrt(mi/p);
    V = v * [-sf e+cf] * R;

end


function ex9mbvp
%EX9MBVP  Example 9 of the BVP tutorial, solved as a multi-point BVP
%   This boundary value problem is the subject of Chapter 8 of
%   C.C. Lin and L.A. Segel, Mathematics Applied to Deterministic
%   Problems in the Natural Sciences, SIAM, Philadelphia, 1988.
%   The ODEs
%
%      v' = (C - 1)/n
%      C' = (vC - min(x,1))/eta
%
%   are solved on the interval [0, lambda]. The boundary conditions
%   are v(0) = 0, C(lambda) = 1, and continuity of v(x) and C(x) at
%   x = 1.  Example EX9BVP shows how this three-point BVP is
%   reformulated for solution with BVP4C present in MATLAB 6.0.
%   Starting with MATLAB 7.0, BVP4C solves multi-point BVPs directly.
%
%   The quantity of most interest is the emergent osmolarity Os =
%   1/v(lambda).  The parameters are related to another parameter
%   kappa by eta = lambda^2/(n*kappa^2).  Lin and Segel develop an
%   approximate solution for Os valid for "small" n.  Here the BVP is
%   solved for a range of kappa when lambda = 2 and n = 0.005.  The
%   computed Os is compared to the approximation of Lin and Segel.

% Copyright 2004, The MathWorks, Inc.

  % Known parameters, visible in nested functions.
  n = 5e-2;
  lambda = 2;

  % Initial mesh - duplicate the interface point x = 1.
  xinit = [0, 0.25, 0.5, 0.75, 1, 1, 1.25, 1.5, 1.75, 2];   lambda = 2;

  sol = bvpinit(xinit,[1 1]);

  fprintf(' kappa    computed Os  approximate Os \n')
  for kappa = 2:5
    eta = lambda^2/(n*kappa^2);
    % After creating function handles, the new value of eta
    % will be used in nested functions.
    sol = bvp4c(@ex9mode,@ex9mbc,sol);

    K2 = lambda*sinh(kappa/lambda)/(kappa*cosh(kappa));
    approx = 1/(1 - K2);
    computed = 1/sol.y(1,end);
    fprintf('  %2i    %10.3f    %10.3f \n',kappa,computed,approx);
  end

  figure
  plot(sol.x,sol.y(1,:),sol.x,sol.y(2,:),'--')
  legend('v(x)','C(x)')
  title('A three-point BVP.')
  xlabel(['\lambda = ',num2str(lambda),', \kappa = ',num2str(kappa),'.'])
  ylabel('v and C')

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

  function dydx = ex9mode(x,y,region)
  %EX9MODE  ODE function for Example 9 of the BVP tutorial.
  % Here the problem is solved directly, as a three-point BVP.
    dydx = zeros(2,1);
    dydx(1) = (y(2) - 1)/n;
    % The definition of C'(x) depends on the region.
    switch region
      case 1    % x in [0 1]
        dydx(2) = (y(1)*y(2) - x)/eta;
      case 2    % x in [1 lambda]
        dydx(2) = (y(1)*y(2) - 1)/eta;
    end
  end % ex9mode

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

  function res = ex9mbc(YL,YR)
  %EX9MBC  Boundary conditions for Example 9 of the BVP tutorial.
  % Here the problem is solved directly, as a three-point BVP.
    res = [ YL(1,1)             % v(0) = 0
            YR(1,1) - YL(1,2)   % continuity of v(x) at x = 1
            YR(2,1) - YL(2,2)   % continuity of C(x) at x = 1
            YR(2,end) - 1    ]; % C(lambda) = 1
  end % ex9mbc

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

end  % ex9mbvp


function [ value, ifault ] = gammds ( x, p )

%*****************************************************************************80
%
%% GAMMDS computes the incomplete Gamma integral.
%
%  Discussion:
%
%    The parameters must be positive.  An infinite series is used.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 January 2008
%
%  Author:
%
%    Original FORTRAN77 version by Chi Leung Lau.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Chi Leung Lau,
%    Algorithm AS 147:
%    A Simple Series for the Incomplete Gamma Integral,
%    Applied Statistics,
%    Volume 29, Number 1, 1980, pages 113-114.
%
%  Parameters:
%
%    Input, real X, P, the arguments of the incomplete
%    Gamma integral.  X and P must be greater than 0.
%
%    Output, real VALUE, the value of the incomplete
%    Gamma integral.
%
%    Output, integer IFAULT, error flag.
%    0, no errors.
%    1, X <= 0 or P <= 0.
%    2, underflow during the computation.
%
  e = 1.0E-09;
  uflo = 1.0E-37;
%
%  Check the input.
%
  if ( x <= 0.0 )
    ifault = 1;
    value = 0.0;
    return
  end

  if ( p <= 0.0 )
    ifault = 1;
    value = 0.0;
    return
  end
%
%  ALNGAM is the natural logarithm of the gamma function.
%
  arg = p * log ( x ) - alngam ( p + 1.0 ) - x;

  if ( arg < log ( uflo ) )
    value = 0.0;
    ifault = 2;
    return
  end

  f = exp ( arg );

  if ( f == 0.0 )
    value = 0.0;
    ifault = 2;
    return
  end

  ifault = 0;
%
%  Series begins.
%
  c = 1.0;
  value = 1.0;
  a = p;

  while ( 1 )

    a = a + 1.0;
    c = c * x / a;
    value = value + c;

    if ( c <= e * value )
      break;
    end

  end

  value = value * f;

  return
end


%% Examples 10.17, 10.26, and 10.38: Benes-Daum and EKF/ERTS examples
%
% Copyright:
%   2018 - Simo Särkkä and Arno Solin
%
% License:
%   This software is provided under the MIT License. See the accompanying
%   LICENSE file for details.

%%
% Benes and Benes-Daum filter tests
%
    % Check that we have the correct normalization constants and
    % moments. We should have (for x = x(t) with t >= t_k)
    %
    % p(x | Y_k) =
    % 1/sqrt{2 pi P} exp(-(x-m)^2/(2P)) exp(-1/2 P) cosh(x) / cosh(m)
    %
    % and
    %
    %    E[x] = m + tanh(m) P
    %  E[x^2] = m^2 + P^2 + 2 m tanh(m) P + P
    %    V[x] = P + [1 - tanh(m)^2] P^2

    dx = 0.001;
    xx = -15:dx:15;
    dens = @(x,m,P) 1/sqrt(2*pi*P)*exp(-P/2)/cosh(m) * exp(-(x-m).^2/(2*P)) .* cosh(x);

    m = 1;
    P = 5;
    pp = dens(xx,m,P);
    figure(1); clf; plot(xx,pp);

    sum(pp .* dx)
    [sum(xx .* pp * dx)        m+tanh(m)*P]
    [sum(xx.^2 .* pp * dx)  m^2+P^2+2*m*tanh(m)*P+P]
    [sum((xx-m-tanh(m)*P).^2 .* pp * dx) P+(1-tanh(m)^2)*P^2]

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

    steps = 500;
    meas_mod = 20;
    dt = 0.01;
    R = 0.1;

    X = [];
    Y = [];
    T = [];
    x0 = 0;
    x = x0;
    t = 0;
    for k=1:steps
        x = x + tanh(x) * dt + sqrt(dt)*randn;
        if rem(k,meas_mod) == 0
            y = x + sqrt(R)*randn;
        else
            y = NaN;
        end
        t = t + dt;
        X(k) = x;
        Y(k) = y;
        T(k) = t;
    end

    figure(1); clf
    plot(T,X,T,Y,'o');

%%
% Benes-Daum filter
%
    EB = zeros(1,length(Y));
    VB = zeros(1,length(Y));
    MMB = zeros(1,length(Y));
    PPB = zeros(1,length(Y));

    mb0 = 0.1;
    Pb0 = 1;

    mb = mb0;
    Pb = Pb0;

    m0 = mb + Pb * tanh(mb);
    P0 = Pb + (1-tanh(mb)^2) * Pb^2;

    for k=1:length(Y)
        %
        % The Benes-Daum filter
        %
        Pb = Pb + dt;
        if ~isnan(Y(k))
            mb = mb + Pb / (R + Pb)*(Y(k) - mb);
            Pb = Pb - Pb^2/(R + Pb);
        end
        MMB(:,k) = mb;
        PPB(:,k) = Pb;
        EB(:,k) = mb + Pb * tanh(mb);
        VB(:,k) = Pb + (1-tanh(mb)^2) * Pb^2;
    end

    figure(1); clf;
    subplot(2,1,1);
      plot(T,MMB,T,EB);
    subplot(2,1,2);
      plot(T,PPB,T,VB);

    rmse_b = sqrt(mean((EB - X).^2))

%%
% Determine the quantiles of Benes solution
%
    QQ1 = zeros(size(MMB));
    QQ2 = zeros(size(MMB));
    for k=1:length(MMB)
        pp = dens(xx,MMB(:,k),PPB(:,k));
        cu = cumsum(pp ./ sum(pp));
        ind1 = find(cu >= 2.5/100,1);
        ind2 = find(cu >= 97.5/100,1);
        QQ1(k) = xx(ind1);
        QQ2(k) = xx(ind2);
    end

    figure(1); clf;
    plot(T,EB,T,QQ1,'--',T,QQ2,'--');

%%
% EKF solution to the same problem
%
    me = m0;
    Pe = P0;
    MMe = zeros(1,length(Y));
    PPe = zeros(1,length(Y));
    ekf_steps = 5;

    for k=1:length(Y)
        dt2 = dt/ekf_steps;
        for i=1:ekf_steps
            Pe = Pe + dt2 * (2 * (1 - tanh(me)^2) * Pe + 1);
            me = me + dt2 * tanh(me);
        end
        if ~isnan(Y(k))
            me = me + Pe / (R + Pe) * (Y(k) - me);
            Pe = Pe - Pe^2/(R + Pe);
        end
        MME(:,k) = me;
        PPE(:,k) = Pe;
    end

    rmse_e = sqrt(mean((MME - X).^2))

    figure(1); clf;
    subplot(2,2,1);
    plot(T,EB,T,QQ1,'--',T,QQ2,'--');
    subplot(2,2,2);
    plot(T,MME,T,MME-1.96*sqrt(PPE),'--',T,MME+1.96*sqrt(PPE),'--');

    subplot(2,2,3);
    plot(T,EB,T,MME)

    subplot(2,2,4);
    plot(T,VB,T,PPE)

%%
% ERTS Smoother
%
    MMS = MME;
    PPS = PPE;
    ms = MME(end);
    Ps = PPE(end);
    for k=length(Y)-1:-1:1
        m = MME(k);
        P = PPE(k);
        dms = dt * tanh(m) + dt * ((1 - tanh(m)^2) * P + 1)/P * (ms - m);
        dPs = dt * 2 * ((1 - tanh(m)^2) * P + 1)/P * Ps - dt;
        ms = ms - dms;
        Ps = Ps - dPs;
        MMS(k) = ms;
        PPS(k) = Ps;
    end

    rmse_s = sqrt(mean((MMS - X).^2))

    figure(1); clf;
    plot(T,MMS,T,MMS-1.96*sqrt(PPS),'--',T,MMS+1.96*sqrt(PPS),'--');

%%
% Plot the final figures
%

    %
    % Benes-Daum
    %
    figure(1); clf; hold on

    % Plot mean
    h0 = plot(T,EB,'-k');
    set(h0,'LineWidth',1);
    set(h0,'Color',[0.5 0.5 0.5]);

    % Plot uncetainty
    h1 = fill([T'; flipud(T')], [QQ1'; flipud(QQ2')],1);
    set(h1,'EdgeColor',[.7 .7 .7],'FaceColor',[.7 .7 .7])

    % Plot signal
    h2 = plot(T,X,'-k');
    set(h2,'LineWidth',1);

    % Plot mean
    h3 = plot(T,EB,'-k');
    set(h3,'LineWidth',1);
    set(h3,'Color',[0.5 0.5 0.5]);

    % Plot observations
    h4 = plot(T,Y,'+k');

    box on
    xlabel('Time, $t$'), ylabel('Signal, $x(t)$')

    legend([h0, h1, h2, h4],'BD mean','BD 95\% quantiles','Signal','Observations')

    %
    % Benes-Daum vs EKF
    %
    figure(2); clf; hold on

    % Plot BD mean
    h1 = plot(T,EB,'-k');
    set(h1,'LineWidth',1);
    set(h1,'Color',[0.5 0.5 0.5]);

    % Plot EKF mean
    h2 = plot(T,MME,'--k');
    set(h2,'LineWidth',1);
    %set(h2,'Color',[0.0 0.0 0.0]);

    % Plot BD uncertainty
    h3 = fill([T'; flipud(T')], [QQ1'; flipud(QQ2')],1);
    set(h3,'EdgeColor',[.7 .7 .7],'FaceColor',[.7 .7 .7])

    % Plot BD mean
    h4 = plot(T,EB,'-k');
    set(h4,'LineWidth',1);
    set(h4,'Color',[0.5 0.5 0.5]);

    % Plot EKF mean
    h5 = plot(T,MME,'--k');
    set(h5,'LineWidth',1);
    set(h5,'Color',[0.0 0.0 0.0]);

    % Plot EKF uncertainty
    h6 = plot(T,MME-1.96*sqrt(PPE),'-.k',T,MME+1.96*sqrt(PPE),'-.k');
    set(h6,'LineWidth',.5);

    box on
    xlabel('Time, $t$'), ylabel('Signal, $x(t)$')
    legend([h1, h2, h3, h6(1)],'BD mean','EKF mean','BD 95\% quantiles','EKF 95\% quantiles')

    %
    % ERTS
    %
    figure(3); clf; hold on

    % Plot mean
    h0 = plot(T,MMS,'-k');
    set(h0,'LineWidth',1);
    set(h0,'Color',[0.5 0.5 0.5]);

    QQ1S = MMS-1.96*sqrt(PPS);
    QQ2S = MMS+1.96*sqrt(PPS);

    % Plot uncetainty
    h1 = fill([T'; flipud(T')], [QQ1S'; flipud(QQ2S')],1);
    set(h1,'EdgeColor',[.7 .7 .7],'FaceColor',[.7 .7 .7])

    % Plot signal
    h2 = plot(T,X,'-k');
    set(h2,'LineWidth',1);

    % Plot mean
    h3 = plot(T,MMS,'-k');
    set(h3,'LineWidth',1);
    set(h3,'Color',[0.5 0.5 0.5]);

    % Plot observations
    h4 = plot(T,Y,'+k');

    box on
    xlabel('Time, $t$'), ylabel('Signal, $x(t)$')

    legend([h0, h1, h2, h4],'ERTS mean','ERTS 95\% quantiles','Signal','Observations')


function f = calccod(x,y,dim,wantgain,wantmeansub,wantsafe)
%
% USAGE::
%
%   f = calccod(x,y,dim,wantgain,wantmeansub,wantsafe)
%
% <x>,<y> are matrices with the same dimensions
%
% <dim> (optional) is the dimension of interest.
% default to 2 if <x> is a (horizontal) vector and to 1 if not.
% special case is 0 which means to calculate globally.
%
% <wantgain> (optional) is
%
%   - 0 means normal
%   - 1 means allow a gain to be applied to each case of <x>
%     to minimize the squared error with respect to <y>.
%     in this case, there cannot be any NaNs in <x> or <y>.
%   - 2 is like 1 except that gains are restricted to be non-negative.
%     so, if the gain that minimizes the squared error is negative,
%     we simply set the gain to be applied to be 0.
%     default: 0.
%
% <wantmeansub> (optional) is
%
%   - 0 means do not subtract any mean.  this makes it such that
%     the variance quantification is relative to 0.
%   - 1 means subtract the mean of each case of <y> from both
%     <x> and <y> before performing the calculation.  this makes
%     it such that the variance quantification
%     is relative to the mean of each case of <y>.
%     note that <wantgain> occurs before <wantmeansub>.
%     default: 1.
%
% <wantsafe> (optional) is whether to protect against NaNs. Default: 1.
%
% calculate the coefficient of determination (R^2) indicating
% the percent variance in <y> that is explained by <x>.  this is achieved
% by calculating 100*(1 - sum((y-x).^2) / sum(y.^2)).  note that
% by default, we subtract the mean of each case of <y> from both <x>
% and <y> before proceeding with the calculation.
%
% the quantity is at most 100 but can be 0 or negative (unbounded).
% note that this metric is sensitive to DC and scale and is not symmetric
% (i.e. if you swap <x> and <y>, you may obtain different results).
% it is therefore fundamentally different than Pearson's correlation
% coefficient (see calccorrelation.m).
%
% if <wantsafe> is true, NaNs are handled gracefully (a NaN causes
% that data point to be ignored). if you are sure there are no NaNs,
% you can turn off <wantsafe> to increase execution speed.
%
% if there are no valid data points (i.e. all data points are
% ignored because of NaNs), we return NaN for that case.
%
% Note some weird cases:
%
% calccod([],[]) is []
%
% Example::
%
%   x = randn(1,100);
%   calccod(x,x+0.1*randn(1,100))
%

% history:
% 2013/08/18 - fix pernicious case where <x> is all zeros and <wantgain> is 1 or 2.
% 2010/11/28 - add <wantgain>==2 case
% 2010/11/23 - changed the output range to percentages.  thus, the range is (-Inf,100].
%              also, we removed the <wantr> input since it was dumb.

% input
if ~exist('dim','var') || isempty(dim)
  if isvector(x) && size(x,1)==1
    dim = 2;
  else
    dim = 1;
  end
end
if ~exist('wantgain','var') || isempty(wantgain)
  wantgain = 0;
end
if ~exist('wantmeansub','var') || isempty(wantmeansub)
  wantmeansub = 1;
end
if ~exist('wantsafe','var') || isempty(wantsafe)
  wantsafe = 1;
end

% handle weird case up front
if isempty(x)
  f = [];
  return;
end

% handle 0 case
if dim==0
  x = x(:);
  y = y(:);
  dim = 1;
end

% handle gain
if wantgain
  % to get the residuals, we want to do something like y-x*inv(x'*x)*x'*y
  temp = 1./dot(x,x,dim) .* dot(x,y,dim);
  if wantgain==2
    temp(temp < 0) = 0;  % if the gain was going to be negative, rectify it to 0.
  end
  x = bsxfun(@times,x,temp);
end

% propagate NaNs (i.e. ignore invalid data points)
if wantsafe
  x(isnan(y)) = NaN;
  y(isnan(x)) = NaN;
end

% handle mean subtraction
if wantmeansub
  mn = nanmean(y,dim);
  y = bsxfun(@minus,y,mn);
  x = bsxfun(@minus,x,mn);
end

% finally, compute it
f = 100*(1 - zerodiv(nansum((y-x).^2,dim),nansum(y.^2,dim),NaN,0));

% JUNK:
%
% % <wantr> (optional) is whether to apply signedarraypower(f,0.5)
% %   at the very end, giving something like a correlation coefficient (r).
% %   default: 1.
%
% if ~exist('wantr','var') || isempty(wantr)
%   wantr = 1;
% end
%
% if wantr
%   f = signedarraypower(f,0.5);
% end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% calculation of the 'epsgg' matrix for circular holes using
%%% analytical expression; the matrix is symmetric, i.e E'=E
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function epsi=epsgg(r, na, nb, b1, b2, N1, N2)
N = N1*N2;
epsilon=zeros(N1,N2);
epsi=zeros(N,N);
%%% filling factor, ratio between the area occupied by the cylinders and area of the unit cell
f=(2*pi/sqrt(3))*r^2;

for  l=1:N1
      for m=1:N2
          for  n=1:N1
              for p=1:N2
                  GGx=(l-n)*b1(1)+(m-p)*b2(1);
                  GGy=(l-n)*b1(2)+(m-p)*b2(2);
                  GG=sqrt(GGx^2+GGy^2); x=2*pi*GG;
                  %%% GG is a scalar which changes with iteration and goes into Bessel function argument
                  if (GG~=0)
                      epsilon(p,n)=2*f*(na^2-nb^2)*besselj(1,x*r)/(x*r);
                  else
                      epsilon(p,n)=f*na^2+(1-f)*nb^2;
                  end ;
              end;
           end;
           u=(l-1)*N2+m; %%% this is the line index
           epsi(u,:)=reshape(epsilon,1,N);
        end;
end;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%  LU 分解的紧凑方式
function [L,U,x] = my_lu(A,b)

[m,n]=size(A);
if (m~=n)
    fprintf('\n Error: A 不是方阵!\n'); return;
end

L = eye(n);
U = zeros(n);

for k = 1:n
    U(k,k) = A(k,k)-sum(L(k,1:k-1)'.*U(1:k-1,k));
    if (U(k,k) == 0)
        fprintf('\n Error: 第 %d 个主元＝0 !\n',k); return;
    end
    for j = k+1:n
        U(k,j) = A(k,j) - sum(L(k,1:k-1)'.*U(1:k-1,j));
        L(j,k) = (A(j,k) - sum( L(j,1:k-1)'.*U(1:k-1,k) ))/U(k,k);
    end
end

% 求解Ly=b 和 Ux=y
y(1) = b(1);
for i=2:n
    summary = 0;
    for t=1:i-1
        summary = summary + L(i,t) * y(t);
    end
    y(i)=b(i) - summary;
end
x(n) = y(n)/U(n,n);
for i = n-1 : -1 : 1
    summary = 0;
    for t=i+1:n
        summary = summary + U(i,t) * x(t);
    end
    x(i)=(y(i) - summary)/U(i,i);
end

% END


% Program to form Admittance And Impedance Bus Formation....

function ybus = ybus();  % Returns ybus

linedata = linedata6(); % Calling "linedata6.m" for Line Data...
fb = linedata(:,1);     % From bus number...
tb = linedata(:,2);     % To bus number...
r = linedata(:,3);      % Resistance, R...
x = linedata(:,4);      % Reactance, X...
b = linedata(:,5);      % Ground Admittance, B/2...
z = r + i*x;            % Z matrix...
y = 1./z;               % To get inverse of each element...
b = i*b;                % Make B imaginary...

nbus = max(max(fb),max(tb));    % no. of buses...
nbranch = length(fb);           % no. of branches...
ybus = zeros(nbus,nbus);        % Initialise YBus...

 % Formation of the Off Diagonal Elements...
 for k=1:nbranch
     ybus(fb(k),tb(k)) = -y(k);
     ybus(tb(k),fb(k)) = ybus(fb(k),tb(k));
 end

 % Formation of Diagonal Elements....
 for m=1:nbus
     for n=1:nbranch
         if fb(n) == m | tb(n) == m
             ybus(m,m) = ybus(m,m) + y(n) + b(n);
         end
     end
 end
 ybus;                  % Bus Admittance Matrix
 zbus = inv(ybus);      % Bus Impedance Matrix


function [trans,rot,scale,skew] = affineDecompose(A)
%
% [trans,rot,scale,skew] = affineDecompose(A)
%
% Decomposes an affine (orthogonal linear) transformation matrix into its
% four componets: translations, rotations scales and skews.
%
% (See affineBuild for more info.)
%
% HISTORY:
%   2004.03.12 RFD (bob@white.stanford.edu) shamelessly copied the core
%   algorithm from spm99's spm_imatrix.m.
%   2005.07.08 ras (sayres at stanford edu) imported into mrVista 2.0

% Translations and zooms
%-----------------------------------------------------------------------
R         = A(1:3,1:3);
C         = chol(R'*R);
if(size(A,2)>3)
    trans = A(1:3,4)';
else
    trans = [0 0 0];
end
scale = diag(C)';
if det(R)<0, scale(1)=-scale(1);end % Fix for -ve determinants

% Shears
%-----------------------------------------------------------------------
C         = diag(diag(C))\C;
skew      = C([4 7 8]);
R0        = affineBuild([0 0 0], [0 0 0], scale, skew);
R0        = R0(1:3,1:3);
R1        = R/R0;

% This just leaves rotations in matrix R1
%-----------------------------------------------------------------------
%[          c5*c6,           c5*s6, s5]
%[-s4*s5*c6-c4*s6, -s4*s5*s6+c4*c6, s4*c5]
%[-c4*s5*c6+s4*s6, -c4*s5*s6-s4*c6, c4*c5]

rot(2) = asin(rang(R1(1,3)));
if (abs(rot(2))-pi/2).^2 < 1e-9,
	rot(1) = 0;
	rot(3) = atan2(-rang(R1(2,1)), rang(-R1(3,1)/R1(1,3)));
else,
	c    = cos(rot(2));
	rot(1) = atan2(rang(R1(2,3)/c), rang(R1(3,3)/c));
	rot(3) = atan2(rang(R1(1,2)/c), rang(R1(1,1)/c));
end;

return;

% There may be slight rounding errors making b>1 or b<-1.
function a = rang(b)
a = min(max(b, -1), 1);
return;


function C = partitions(M,K)
%PARTITIONS Find all partitions of a set.
% C = PARTITIONS(N), for scalar N, returns a cell array, wherein each cell
% has a various number of other cells representing the partitions of the
% set of numbers {1,2,3,...N}.  The length of C is the Bell number for N.
% C = PARTITIONS(N), for vector N, returns the partitions of the vector
% elements treated as members of a set.
% C = PARTITIONS(N), for cell N, returns the partitions of the cell
% elements treated as members of a set.
% PARTITIONS(N,K) returns only the partitions of which have K members.
%
% K must be less than or equal to N for scalar N, or length(N).
%
%
% Examples:
%
%     C = partitions(4); % Find the partitions of set {1 2 3 4}
%     home  % Makes for nice display for small enough N.  Else use clc.
%     partdisp(C) % File accompanying PARTITIONS
%
%     C = partitions({'peanut','butter','yummy'});
%     home
%     partdisp(C)
%
%     C = partitions([5 7 8 50],3); % partitions of set {5 7 8 50}
%     home
%     partdisp(C,3)
%
%     C = partitions(['a','b','c','d']); % for longer chars, use {}, not []
%     home
%     partdisp(C)
%
% Class support for inputs N,K:
%      float: double, single, char (N only)
%
% See also, nchoosek, perms, npermutek (On the FEX)
%
% Author: Matt Fig,
% Contact: popkenai@yahoo.com
% Date: 5/17/2009

Kflg = false;  % No K passed in.
Cflg = false;  % A set not passed in.

if length(M)>1
    N = length(M); % User passed {'here','there','everywhere'} for example.
    Cflg = true;
else
    if iscell(M)  % User passed {2} for example.
        error('Set arguments must have more than one element.')
    end

    N = M;
end

if nargin>1
    Kflg = true;  % Used in while loop below, K passed in.
    S = stirling(N,K);  % The number of partitions, Stirling number.
    C = cell(S,min(1,K)); % Main Cell.
    cnt = 1;  % Start the while loop counter.
else
    K = 0;  % Since user doesn't want only certain partitions.
    S = ceil(sum((1:2*N).^N./cumprod(1:2*N))/exp(1)); % Bell number.
    C = cell(S,1); % Main Cell.

    if Cflg
        C{1} = {M};
    else
        C{1} = {1:N}; % First one is easy.
    end

    cnt = 2; % Start the while loop counter.
end

if K~=ceil(K) || numel(K)~=1
    error('Second argument must be an integer of length 1.  See help.')
end

if N<0 || K>N || K<0
   error('Arguments must be greater than zero, and K cannot exceed N.')
end

if N==0
    C = {{}}; % Easy case.
    return
end

if K==1
    if Cflg
        C{1} = {M};  % Easy case.
    else
        C{1} = {1:N};  % Easy case.
    end
    return
end

if Cflg
    NV = M;
else
    NV = 1:M; % Vector: base partition, RGF indexes into this guy.
end

NU = 1; % Number of unique indices in current partition.
stp1 = N; % Controls assigning of indices.
RGF = ones(1,N); % Holds the indexes.
BLD = cell(1,N); % Smaller cell array will be used in creating larger.

while cnt<=S
    idx1 = N; % Index into RGF.
    stp2 = RGF(idx1); % Works with stp1.

    while stp1(stp2)==1
        RGF(idx1) = 1;  % Assign value to RGF.
        idx1 = idx1 - 1; % Need to increment idx1 for translation below.
        stp2 = RGF(idx1); % And set this guy for stp1 assign below.
    end

    NU = NU + idx1 - N;  % Get provisional number of unique vals.
    stp1(1) = stp1(1) + N - idx1;

    if stp2==NU % Increment the number of unique elements.
        NU = NU +1;
        stp1(NU) = 0;
    end

    RGF(idx1) = stp2 + 1;  % Increment this position.
    stp1(stp2) = stp1(stp2) - 1;  % Translate indices of these two.
    stp1(stp2+1) = stp1(stp2+1) + 1; % Re-assign stopper.

    if NU==(~Kflg * NU + Kflg * K)
    % We could use:  C{cnt} = accumarray(RGF',NV',[],@(x) {x});   (SLOW!!)
    % or the next lines to C{cnt} = TMP;
        TMP = BLD(1:NU); % Build subcell of correct size.
        TMP{1} = NV(RGF==1); % These first two are always here.... no loop.
        TMP{2} = NV(RGF==2);

        for ii = 3:NU % Build the rest of cell array, if any.
            TMP{ii} = NV(RGF==ii);
        end

        C{cnt} = TMP;  % Assign cell at jj.
        cnt = cnt + 1;
    end
end

function S = stirling(N,K)
% Calculate the Stirling number of the second kind. Subfunc to partitions.

for ii = K:-1:0
    S(ii+1) = (-1)^ii * prod(1:K) / (prod(1:(K-ii)) *...
              (prod(1:ii)))*(K - ii)^N;  %#ok
end

S = 1/prod(1:K) * sum(S);


function [ alpha ] = LiMapS(s, D, DINV, gamma, maxIter)
%
% [ alpha ] = LIMAPS(s, D, DINV, gamma)
%
%   Returns the sparsest vector alpha which satisfies underdetermined
%   system of equations  D*alpha=s, using  Lipschitzian Mapping for Sparsity
%   Recovery algorithm.
%   The dictionary matrix  D should have more columns than rows. The
%   number of rows of dictionary D  must be equal to the length of the
%   column observation vector(or matrix) s.
%
%     The observations signal(s) s and the dictionary D necessarily, must
%   be provided by the user. The other parameters have defult values, or
%   may be provided by the user.
%
%   This algorithm work fine with matrix too, returning the sparsest matrix
%   alpha that satisfy the underdetermined system of equations D*alpha=s.
%   Then the dictionary D is of size [n x m], s must be of size [ s x T ] and
%   as consiquence the coefficients matirx alpha is of size [ m x T]
%
%   s:
%       The observation vector (matrix)
%
%   D:
%       The dictionary
%
%   DINV (optional):
%       The pseudo-inverse of the dictionary matrix D
%
%   gamma (optional):
%       The increase factor (default 1.01)
%
%   maxIter (optional):
%       Maximum number of iterations (default 1000)
%
% Authors: Alessandro Adamo and Giuliano Grossi
% Version: 1.0
% Last modified: 1 Aug. 2011.
%
% WEB PAGE:
% ------------------
%    http://dalab.dsi.unimi.it/limaps
%
% HISTORY:
%--------------
% Version 1.0: 1 Aug. 2011
%        first official version.
%

if(nargin<2||nargin>5)
    error('Wrong parameters number');
end
if(nargin==2)
    DINV=pinv(D);
    gamma=1.01;
    maxIter=1000;
end
if(nargin==3)
    gamma=1.01;
    maxIter=1000;
end
if(nargin==4)
    maxIter=1000;
end

%% --  INITIALIZATION  --
alpha = DINV*s;

lambda = 1/max(abs(alpha(:)));
epsilon=1e-5;                    % stopping criteria
alpha_min=1e-2;

%% --  CORE  --
for i=1:maxIter

    alphaold=alpha;
    % apply sparsity constraction mapping: increase sparsity
    beta = alpha.*(1-exp(-lambda.*abs(alpha)));

    beta(abs(beta)<alpha_min)=0;

    % apply the orthogonal projection
    alpha = beta-DINV*(D*beta-s);

    % update the lambda coefficient
    lambda = lambda*gamma;

    % check the stopping criteria
    if (norm(alpha(:)-alphaold(:))<epsilon)
        break;
    end

    if (mod(i,100))
        if(sum(abs(1-exp(-lambda*abs(alpha)))>1/lambda)>size(D,1))
           break;
        end
    end

end

%% -- REFINEMENT --

% threshold the coefficients
alpha(abs(alpha)<alpha_min) = 0;

% final refinements of the solution
for i=1:size(alpha,2);
    alpha_ind = alpha(:,i)~=0;
    D1 = D(:,alpha_ind);
    alpha(alpha_ind,i) = alpha(alpha_ind,i) - pinv(D1)*(D1*alpha(alpha_ind,i)-s(:,i));
end

end


function F = spm_Fcdf(x,v,w)
% Cumulative Distribution Function (CDF) of F (Fisher-Snedecor) distribution
% FORMAT F = spm_Fcdf(x,df)
% FORMAT F = spm_Fcdf(x,v,w)
%
% x  - F-variate   (F has range [0,Inf) )
% df - Degrees of freedom, concatenated along last dimension
%      Eg. Scalar (or column vector) v & w. Then df=[v,w];
% v  - Shape parameter 1 /   numerator degrees of freedom (v>0)
% w  - Shape parameter 2 / denominator degrees of freedom (w>0)
% F  - CDF of F-distribution with [v,w] degrees of freedom at points x
%__________________________________________________________________________
%
% spm_Fcdf implements the Cumulative Distribution Function of the F-distribution.
%
% Definition:
%--------------------------------------------------------------------------
% The CDF F(x) of the F distribution with degrees of freedom v & w,
% defined for positive integer degrees of freedom v & w, is the
% probability that a realisation of an F random variable X has value
% less than x F(x)=Pr{X<x} for X~F(v,w). The F-distribution is defined
% for v>0 & w>0, and for x in [0,Inf) (See Evans et al., Ch16).
%
% Variate relationships: (Evans et al., Ch16 & 37)
%--------------------------------------------------------------------------
% The square of a Student's t variate with w degrees of freedom is
% distributed as an F-distribution with [1,w] degrees of freedom.
%
% For X an F-variate with v,w degrees of freedom, w/(w+v*X^2) has
% distributed related to a Beta random variable with shape parameters
% w/2 & v/2.
%
% Algorithm:
%--------------------------------------------------------------------------
% Using the relationship with the Beta distribution: The CDF of the
% F-distribution with v,w degrees of freedom is related to the
% incomplete beta function by:
%       Pr(X<x) = 1 - betainc(w/(w+v*x^2),w/2,v/2)
% See Abramowitz & Stegun, 26.6.2; Press et al., Sec6.4 for
% definitions of the incomplete beta function. The relationship is
% easily verified by substituting for w/(w+v*x^2) in the integral of the
% incomplete beta function.
%
% MATLAB's implementation of the incomplete beta function is used.
%
%
% References:
%--------------------------------------------------------------------------
% Evans M, Hastings N, Peacock B (1993)
%       "Statistical Distributions"
%        2nd Ed. Wiley, New York
%
% Abramowitz M, Stegun IA, (1964)
%       "Handbook of Mathematical Functions"
%        US Government Printing Office
%
% Press WH, Teukolsky SA, Vetterling AT, Flannery BP (1992)
%       "Numerical Recipes in C"
%        Cambridge
%
%__________________________________________________________________________
% Copyright (C) 1992-2013 Wellcome Trust Centre for Neuroimaging

% Andrew Holmes
% $Id: spm_Fcdf.m 5602 2013-08-12 13:35:52Z guillaume $

%-Format arguments, note & check sizes
%--------------------------------------------------------------------------
if nargin<2, error('Insufficient arguments'), end

%-Unpack degrees of freedom v & w from single df parameter (v)
if nargin<3
    vs = size(v);
    if prod(vs)==2
        %-DF is a 2-vector
        w = v(2); v = v(1);
    elseif vs(end)==2
        %-DF has last dimension 2 - unpack v & w
        nv = prod(vs);
        w  = reshape(v(nv/2+1:nv),vs(1:end-1));
        v  = reshape(v(1:nv/2)   ,vs(1:end-1));
    else
        error('Can''t unpack both df components from single argument')
    end
end

%-Check argument sizes
ad = [ndims(x);ndims(v);ndims(w)];
rd = max(ad);
as = [[size(x),ones(1,rd-ad(1))];...
      [size(v),ones(1,rd-ad(2))];...
      [size(w),ones(1,rd-ad(3))]];
rs = max(as);
xa = prod(as,2)>1;
if sum(xa)>1 && any(any(diff(as(xa,:)),1))
    error('non-scalar args must match in size'), end

%-Computation
%--------------------------------------------------------------------------
%-Initialise result to zeros
F = zeros(rs);

%-Only defined for strictly positive v & w. Return NaN if undefined.
md = ( ones(size(x))  &  v>0  &  w>0 );
if any(~md(:))
    F(~md) = NaN;
    warning('Returning NaN for out of range arguments');
end

%-Non-zero where defined and x>0
Q  = find( md  &  x>0 );
if isempty(Q), return, end
if xa(1), Qx=Q; else Qx=1; end
if xa(2), Qv=Q; else Qv=1; end
if xa(3), Qw=Q; else Qw=1; end

%-Compute
F(Q) = 1 - betainc(w(Qw)./(w(Qw) + v(Qv).*x(Qx)),w(Qw)/2,v(Qv)/2);


% SHIFT_TEST_1D.M
% Plot how step responses vary at level 1 to 4 with 16 shifted steps
% for DT CWT and DWT.
%
% Nick Kingsbury, Cambridge University, May 2002.

% Generate 16 step functions as columns of matrix X.
n = 120;
x = cumsum([zeros(n,16);diag(ones(16,1));zeros(n,16)]) - 0.5;

% Specify DT CWT filters here.
biort = 'near_sym_b';
qshift = 'qshift_b';

% Forward DT CWT on each column of x.
[Yl,Yh] = dtwavexfm(x,4,biort,qshift);

% Inverse DT CWT, one subband at a time, using gain_mask to select subbands.
z1 = dtwaveifm(Yl*0,Yh,biort,qshift,[1 0 0 0]);
z01 = dtwaveifm(Yl*0,Yh,biort,qshift,[0 1 0 0]);
z001 = dtwaveifm(Yl*0,Yh,biort,qshift,[0 0 1 0]);
z0001 = dtwaveifm(Yl*0,Yh,biort,qshift,[0 0 0 1]);
z0000 = dtwaveifm(Yl,Yh,biort,qshift,[0 0 0 0]);

% Check for perfect reconstruction: abs(error) < 1e-12.
z = z1 + z01 + z001 + z0001 + z0000;
DTCWT_error = max(abs(z(:)-x(:)))

% Plot 16 input steps, x, and 16 step responses at each level, z1 to z0000.
setfig(1)
set(gcf,'DefaultTextFontSize',12,'Color',[1 1 1]);
set(gcf,'numbertitle','off','name',['Step responses at 16 adjacent shifts']);
subplot('position',[0.15 0.1 0.3 0.8]);
sc = 1.2; % Scale offsets so sets of curves do not overlap.
on = ones(size(x,2)-1,1)/5;
offset = sc*cumsum([0;on;1;on;1;on;1;on;1;on;1;on]/4).'; % Offsets for each curve.
zdtcwt = [x+0.5 z1 z01 z001 z0001 z0000-0.5] - ones(size(x,1),1)*offset;
tt = -40:40;  % Limits of plot horizontally.
plot(tt,zdtcwt(n+8+tt,:),'-b');
axis off
text(0,-7*sc,'(a) Dual Tree CWT','horiz','c')
xpos = -42; % Position of text labels.
text(xpos,0*sc,'Input','horiz','r','vert','m');
text(xpos,-0.9*sc,'Wavelets','horiz','r','vert','m');
text(xpos,-1.4*sc,'Level 1','horiz','r','vert','m');
text(xpos,-2.4*sc,'Level 2','horiz','r','vert','m');
text(xpos,-3.4*sc,'Level 3','horiz','r','vert','m');
text(xpos,-4.4*sc,'Level 4','horiz','r','vert','m');
text(xpos,-5.5*sc,'Scaling fn','horiz','r','vert','m');
text(xpos,-6*sc,'Level 4','horiz','r','vert','m');
drawnow

% Now do DWT for comparison.

% Specify DWT filters here.
biort = 'antonini';

% Forward DWT on each column of x.
[Yl,Yh] = wavexfm(x,4,biort);

% Inverse DWT, one subband at a time, using gain_mask to select subbands.
z1 = waveifm(Yl*0,Yh,biort,[1 0 0 0]);
z01 = waveifm(Yl*0,Yh,biort,[0 1 0 0]);
z001 = waveifm(Yl*0,Yh,biort,[0 0 1 0]);
z0001 = waveifm(Yl*0,Yh,biort,[0 0 0 1]);
z0000 = waveifm(Yl,Yh,biort,[0 0 0 0]);

% Check for perfect reconstruction: abs(error) < 1e-12.
z = z1 + z01 + z001 + z0001 + z0000;
DWT_error = max(abs(z(:)-x(:)))

subplot('position',[0.47 0.1 0.3 0.8]);
on = ones(size(x,2)-1,1)/5;
offset = sc*cumsum([0;on;1;on;1;on;1;on;1;on;1;on]/4).';
zdwt = [x+0.5 z1 z01 z001 z0001 z0000-0.5] - ones(size(x,1),1)*offset;
tt = -40:40;
plot(tt,zdwt(n+8+tt,:),'-r');
axis off
% xlabel('(b) Real DWT')
text(0,-7*sc,'(b) Real DWT','horiz','c')

return


function h = p13_h ( n, x )

%*****************************************************************************80
%
%% P13_H evaluates the Hessian for problem 13.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 October 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the values of the variables.
%
%    Output, real H(N,N), the N by N Hessian matrix.
%
  h = zeros ( n, n );

  s1 = sum ( cos ( x(1:n) ) );

  for j = 1 : n
    h(j,j) = sin ( x(j) );
  end

  s2 = 0.0;

  for j = 1 : n

    th = cos ( x(j) );
    t = ( n + j ) - h(j,j) - s1 - j * th;
    s2 = s2 + t;
    for k = 1 : j - 1
      h(j,k) = 2.0 * ( sin ( x(k) ) * ( ( n + j + k ) * h(j,j) - th ) - ...
        h(j,j) * cos ( x(k) ) );
    end

    h(j,j) =  ( j * ( j + 2 ) + n ) * h(j,j) * h(j,j) + th * ...
      ( th -  ( 2 * j + 2 ) * h(j,j) ) + t * ( j * th + h(j,j) );

  end

  for j = 1 : n
    h(j,j) = 2.0 * ( h(j,j) + cos ( x(j) ) * s2 );
  end

  for i = 1 : n
    h(i,i+1:n) = h(i+1:n,i);
  end

  return
end


function Y = fft2_mid0(X)
% fft2_mid0 -- 2d fft with argument [-pi,pi] (instead of [0,2pi])
% Usage:
%   Y = fft_mid0(X)
% Inputs:
%   X	Array(n)
% Outputs:
%   Y   Array(n)
% Description:
%  Performs 1d fft with grid (-n/2):(n/2-1) on both time
%  and frequency side.
%
% y(k) = sum_{-n/2 <= t1,t2 < n/2} exp(-i 2pi/n (k1 t1 + k2 t2)) x(t1,t2),
%                                         (-n/2) <= k1,k2 < n/2
%

Y = fftshift(fft2(fftshift(X)));


function [lat, lon, alt] = ecf_to_geodetic(x, y, z)
%ECF_TO_GEODETIC Convert ECF coordinates to geodetic
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////
%
% Convert ECF (Earth Centered Fixed) coordinates to geodetic latitude,
% longitude, and altitude.
%
% USAGE:
%   pos_lla = ecf_to_geodetic(pos_ecf)
%   [lat, lon, alt] = geodetic_to_ecf(pos_ecf_x, pos_ecf_y, pos_ecf_z)
%
% INPUTS:
%   pos_ecf - required : ecf x, y, z coordinates                      [m, m, m]
%
% OUTPUTS:
%   pos_lla - required : geodetic latitude, longitude, and altitude   [deg, deg, m]
%
% NOTES:
%   Zhu, J. Conversion of Earth-centered, Earth-fixed coordinates to
%   geodetic coordinates. IEEE Transactions on Aerospace and Electronic
%   Systems, 30, 3 (July 1994), 957-962.
%
% VERSION:
%   1.0
%     - Sean Hatch 20070911
%     - initial version
%   1.1
%     - Wade Schwartzkopf 20130708
%     - vectorized and componentwise data handling
%
% TODO:

% define constants
e2 = 6.6943799901377997e-3;  % eccentricity squared of Earth (WGS 84 value)
a = 6378137.0;               % semimajor radius of the Earth (WGS 84 value)

% calculate derived constants
e4 = e2 .* e2;
ome2 = 1.0 - e2;
a2 = a .* a;
b = a .* sqrt(ome2);         % semiminor radius of the Earth
b2 = b .* b;
e_b2 = (a2 - b2) ./ b2;

% Handle different forms in input arguments
if nargin==3 % Componentwise inputs, separate arguments for X,Y,Z
    % Nothing to do.  Processing uses this form.
elseif size(x,1)==3 % Array of 3-element vectors
    z = x(3,:);
    y = x(2,:);
    x = x(1,:);
elseif numel(x)==3 % Horizontal 3-element vector
    z = x(3);
    y = x(2);
    x = x(1);
else
    error('WGS_84_NORM:INVALID_INPUTS', 'Invalid inputs.');
end

% calculate intermediates
z2 = z .* z;
r2 = (x .* x) + (y .* y);
r = sqrt(r2);

% Check for invalid solution
valid = ((a .* r) .* (a .* r) + (b .* z) .* (b .* z) > (a2 - b2) .* (a2 - b2));
lon = nan(size(x)); % Default values for invalid solutions
lat = nan(size(x));
alt = nan(size(x));

% calculate longitude
lon(valid) = atan2(y(valid), x(valid))*180/pi; % atan2d not available until MATLAB 2012b

% calculate intermediates
F = 54.0 .* b2 .* z2;
G = r2 + ome2 .* z2 - e2 .* (a2 - b2);
c = e4 .* F .* r2 ./ (G .* G .* G);
s = (1.0 + c + sqrt(c .* c + 2 .* c)) .^ (1 ./ 3);
templ = s + 1.0 ./ s + 1.0;
P = F ./ (3.0 .* templ .* templ .* G .* G);
Q = sqrt(1.0 + 2.0 .* e4 .* P);
r0 = -P .* e2 .* r ./ (1.0 + Q) + ...
    sqrt(abs(0.5 .* a2 .* (1.0 + 1.0 ./ Q) - ...
    P .* ome2 .* z2 ./ (Q .* (1.0 + Q)) - 0.5 .* P .* r2));
temp2 = r - e2 .* r0;
U = sqrt(temp2 .* temp2 + z2);
V = sqrt(temp2 .* temp2 + ome2 .* z2);
z0 = b2 .* z ./ (a .* V);

% calculate latitude
lat(valid) = atan2(z(valid) + e_b2 .* z0(valid), r(valid))*180/pi; % atan2d not available until MATLAB 2012b

% calculate altitude
alt(valid) = U(valid) .* (1.0 - b2 ./ (a .* V(valid)));

if nargout < 2 % Matrix/vector form, rather than componentwise form, was requested
    lat = [lat; lon; alt];
end

end

% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////


% [mu,S] = weighted_mean_cov(x,w)
% x is n x d, w is n x 1
function [mu,S] = weighted_mean_cov(x,w)

%[n,d] = size(x);

z = sum(w);
mu = sum(bsxfun(@times,x,w(:)),1)/z;

diffs = bsxfun(@minus,x,mu);
diffs = bsxfun(@times,diffs,sqrt(w));
S = (diffs'*diffs)/z;


function [ o, x, w ] = epn_lag_01_1 ( n )

%*****************************************************************************80
%
%% EPN_LAG_01_1 implements a precision 1 rule for region EPN_LAG.
%
%  Discussion:
%
%    The rule has order O = 1.
%
%    The rule has precision P = 1.
%
%    EPN is the N-dimensional positive space [0,+oo)^N with exponential
%    or Laguerre weight function:
%
%      w(x(1:n)) = exp ( - sum ( x(1:n) ) )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 January 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the spatial dimension.
%
%    Input, integer O, the order.
%
%    Output, real X(N,O), the abscissas.
%
%    Output, real W(O), the weights.
%
  o = 1;

  expon = 0;
  value1 = ep1_lag_monomial_integral ( expon );
  volume = value1 ^ n;

  expon = 1;
  value2 = ep1_lag_monomial_integral ( expon );

  x = zeros ( n, o );
  w = zeros ( o, 1 );

  k = 0;
%
%  1 point.
%
  k = k + 1;
  x(1:n,k) = value2 / value1;
  w(k) = volume;

  return
end


%% computing the bounds from the mean and standard deviations from dynare
prob=0.9;
Params={
	'alp'  ,  'beta'     ,    0.356000, 0.02
	'bet'  ,  'beta'     ,    0.993000, 0.002
	'gam'  ,  'normal'   ,    0.008500, 0.003
	'mst'  ,  'normal'   ,    1.000200, 0.007
	'rho'  ,  'beta'     ,    0.129000, 0.223
	'psi'  ,  'beta'     ,    0.650000, 0.05
	'del'  ,  'beta'     ,    0.010000, 0.005
	'sig_a',  'inv_gamma',    0.035449, inf
	'sig_m',  'inv_gamma',    0.008862, inf
	};
npars=size(Params,1);
bounds=nan(npars,2);
for ii=1:npars
    bounds(ii,:)=distributions.find_bounds(Params{ii,2},Params{ii,3},Params{ii,4},prob);
end
disp(bounds)
%% recover means and standard deviations
MOMS=nan(npars,2);
for ii=1:npars
    [a,b,moments,fval]=distributions.(Params{ii,2})(bounds(ii,1),bounds(ii,2),prob);
    MOMS(ii,1)=moments.mean;
    MOMS(ii,2)=moments.sd;
end
%% display and compare
disp('================ Means and Standard Deviations ================ ')
disp(num2cell(MOMS))
disp('================ Discrepancies ================')
disp(abs(cell2mat(Params(:,3:end))-MOMS))


function zi = pwl_interp_2d ( nxd, nyd, xd, yd, zd, ni, xi, yi )

%*****************************************************************************80
%
%% PWL_INTERP_2D: piecewise linear interpolant to data defined on a 2D grid.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 October 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NXD, NYD, the number of X and Y data values.
%
%    Input, real XD(NXD), YD(NYD), the sorted X and Y data.
%
%    Input, real ZD(NXD,NYD), the Z data.
%
%    Input, integer NI, the number of interpolation points.
%
%    Input, real XI(NI), YI(NI), the coordinates of the
%    interpolation points.
%
%    Output, real ZI(NI), the value of the interpolant.
%
  zi = zeros ( ni, 1 );

  for k = 1 : ni

    i = r8vec_bracket5 ( nxd, xd, xi(k) );
    if ( i == -1 )
      zi(k) = r8_huge ( );
      continue
    end

    j = r8vec_bracket5 ( nyd, yd, yi(k) );
    if ( j == -1 )
      zi(k) = r8_huge ( );
      continue
    end

    if ( yi(k) < yd(j+1) ...
      + ( yd(j) - yd(j+1) ) * ( xi(i) - xd(i) ) / ( xd(i+1) - xd(i) ) )

      dxa = xd(i+1) - xd(i);
      dya = yd(j)   - yd(j);

      dxb = xd(i)   - xd(i);
      dyb = yd(j+1) - yd(j);

      dxi = xi(k)   - xd(i);
      dyi = yi(k)   - yd(j);

      det = dxa * dyb - dya * dxb;

      alpha = ( dxi * dyb - dyi * dxb ) / det;
      beta =  ( dxa * dyi - dya * dxi ) / det;
      gamma = 1.0 - alpha - beta;

      zi(k) = alpha * zd(i+1,j) + beta * zd(i,j+1) + gamma * zd(i,j);

    else

      dxa = xd(i)   - xd(i+1);
      dya = yd(j+1) - yd(j+1);

      dxb = xd(i+1) - xd(i+1);
      dyb = yd(j)   - yd(j+1);

      dxi = xi(k)   - xd(i+1);
      dyi = yi(k)   - yd(j+1);

      det = dxa * dyb - dya * dxb;

      alpha = ( dxi * dyb - dyi * dxb ) / det;
      beta =  ( dxa * dyi - dya * dxi ) / det;
      gamma = 1.0 - alpha - beta;

      zi(k) = alpha * zd(i,j+1) + beta * zd(i+1,j) + gamma * zd(i+1,j+1);

    end

  end

  return
end


% Studies the recovery probability of rank aware thresholding
% with sparsity level and number of channels in joint recovery.

close all;
clear all;
clc;
rng('default');
% Create the directory for storing images
[status_code,message,message_id] = mkdir('bin');
% Signal space
N = 256;
% Number of measurements
M = 32;
% Number of signals
Ss = [1, 2, 4, 8, 16, 32];
% Sparsity levels
Ks = 2:30;
num_trials = 200;
num_ks = length(Ks);
num_ss = length(Ss);
success_with_k = zeros(num_ss, num_ks);

snr_threshold = 100;

for ns=1:num_ss
    % Current sparsity level
    S = Ss(ns);
    for nk=1:num_ks
        K = Ks(nk);
        num_successes = 0;
        for nt=1:num_trials
            % Sensing matrix
            Phi = spx.dict.simple.gaussian_dict(M, N);
            % Sparse signal generator
            gen  = spx.data.synthetic.SparseSignalGenerator(N, K, S);
            % Gaussian distributed non-zero samples
            X = gen.gaussian;
            % Measurement vectors
            Y = Phi * X;
            % Create the solver for rank aware thresholding
            solver = spx.pursuit.joint.RankAwareThresholding(Phi, K);
            result = solver.solve(Y);
            % Solution vectors
            X_Rec = result.Z;
            % Comparison
            cs = spx.commons.SparseSignalsComparison(X, X_Rec, K);
            % Reconstruction SNR
            snr = cs.cum_signal_to_noise_ratio;
            success = snr > snr_threshold;
            num_successes = num_successes + success;
            fprintf('S: %d, K=%d, trial=%d, residual: %e, SNR: %f dB\n'...
                , S, K, nt, cs.cum_difference_norm, snr);
        end
        success_with_k(ns, nk) = num_successes / num_trials;
    end
end

save ('bin/figure_1_ra_thresholding_success_with_K_S.mat');


function circle_segment_test13 ( )

%*****************************************************************************80
%
%% CIRCLE_SEGMENT_TEST13 demonstrates GAUSS for quadrature rules.
%
%  Discussion:
%
%    Some recursion coefficients ALPHA and BETA are listed in Kautsky
%    and Elhay.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 January 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference
%
%    Jaroslav Kautsky, Sylvan Elhay,
%    Calculation of the Weights of Interpolatory Quadratures,
%    Numerische Mathematik,
%    Volume 40, Number 3, October 1982, pages 407-422.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CIRCLE_SEGMENT_TEST13\n' );
  fprintf ( 1, '  GAUSS computes the points and weights for a\n' );
  fprintf ( 1, '  Gauss quadrature rule, given the ALPHA and BETA\n' );
  fprintf ( 1, '  recursion coefficients.\n' );
%
%  Legendre rule.
%
  n = 10;

  alpha = zeros ( n, 1 );
  beta = zeros ( n, 1 );

  for i = 1 : n
    alpha(i) = 0.0;
    if ( i == 1 )
      beta(i) = 2.0;
    else
      beta(i) = 1.0 / ( 4.0 - 1.0 / ( i - 1.0 )^2 );
    end
  end

  ab(1:n,1) = alpha(1:n);
  ab(1:n,2) = beta(1:n);

  xw = gauss ( n, ab );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  LEGENDRE RULE\n' );
  fprintf ( 1, '  Point   Weight\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %12g  %12g\n', xw(i,1), xw(i,2) );
  end
%
%  Hermite rule.
%
  n = 10;

  alpha = zeros ( n, 1 );
  beta = zeros ( n, 1 );

  for i = 1 : n
    alpha(i) = 0.0;
    if ( i == 1 )
      beta(i) = sqrt ( pi );
    else
      beta(i) = ( i - 1 ) / 2;
    end
  end

  ab(1:n,1) = alpha(1:n);
  ab(1:n,2) = beta(1:n);

  xw = gauss ( n, ab );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  HERMITE RULE\n' );
  fprintf ( 1, '  Point   Weight\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %12g  %12g\n', xw(i,1), xw(i,2) );
  end
%
%  Laguerre rule.
%
  n = 10;

  alpha = zeros ( n, 1 );
  beta = zeros ( n, 1 );

  for i = 1 : n
    alpha(i) = 2.0 * i - 1.0;
    if ( i == 1 )
      beta(i) = 1;
    else
      beta(i) = ( i - 1 )^2;
    end
  end

  ab(1:n,1) = alpha(1:n);
  ab(1:n,2) = beta(1:n);

  xw = gauss ( n, ab );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  LAGUERRE RULE\n' );
  fprintf ( 1, '  Point   Weight\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %12g  %12g\n', xw(i,1), xw(i,2) );
  end

  return
end


% StackExchange Mathematics Q3537129
% https://math.stackexchange.com/questions/3537129
% Projection of z onto the Affine Half Space {x∣Ax=b,x≥0}.
% References:
%   1.
% Remarks:
%   1.  sa
% TODO:
% 	1.  ds
% Release Notes
% - 1.0.000     27/12/2020
%   *   First release.

%% General Parameters

subStreamNumberDefault = 2088;0; %<! Set to 0 for Random

run('InitScript.m');

figureIdx           = 0;
figureCounterSpec   = '%04d';

generateFigures = ON;

%% Parameters

numRows         = 10;
numCols         = 20;

% Solvers parameters
numIterations   = 500;
stopThr         = 1e-6;

%% Load / Generate Data

mA = randn(numRows, numCols);
vB = randn(numRows, 1);
vZ = randn(numCols, 1);

mAA     = mA * mA.';
mInvAA  = pinv(mAA);

hObjFun = @(vX) 0.5 * sum((vX - vZ) .^ 2);

hP1 = @(vX) vX - (mA.' * mInvAA * ((mA * vX) - vB)); %<! Projection onto the 1st constraint set
hP2 = @(vX) max(vX, 0); %<! Projection onto the 2nd constraint set

hP = @(vX) OrthogonalProjectionOntoConvexSets({hP1; hP2}, vX, numIterations, stopThr);

solverIdx       = 0;
cMethodString   = {};

%% Solution by CVX

solverString = 'CVX';

% cvx_solver('SDPT3'); %<! Default, Keep numRows low
% cvx_solver('SeDuMi');
% cvx_solver('Mosek'); %<! Can handle numRows > 500, Very Good!
% cvx_solver('Gurobi');

hRunTime = tic();

cvx_begin('quiet')
% cvx_begin()
    % cvx_precision('best');
    variable vX(numCols, 1);
    minimize( 0.5 * sum_square(vX - vZ) );
    subject to
        mA * vX == vB;
        vX >= 0;
cvx_end

runTime = toc(hRunTime);

% vX = mX(:);

disp([' ']);
disp([solverString, ' Solution Summary']);
disp(['The ', solverString, ' Solver Status - ', cvx_status]);
disp(['The Optimal Value Is Given By - ', num2str(hObjFun(vX))]);
disp(['The Optimal Argument Is Given By - [ ', num2str(vX(:).'), ' ]']);
disp(['The Run Time Is Given By - ', num2str(runTime), ' [Sec]']);
disp([' ']);

sCvxSol.vXCvx     = vX;
sCvxSol.cvxOptVal = hObjFun(vX);

%% Solution by Dykstra's Projection Algorithm

solverIdx                   = solverIdx + 1;
cLegendString{solverIdx}    = ['Projected Gradient Method'];

hRunTime = tic();

vX = hP(vZ);

runTime = toc(hRunTime);

disp([' ']);
disp([cLegendString{solverIdx}, ' Solution Summary']);
disp(['The Optimal Value Is Given By - ', num2str(hObjFun(vX))]);
disp(['The Optimal Argument Is Given By - [ ', num2str(vX(:).'), ' ]']);
disp(['The Run Time Is Given By - ', num2str(runTime), ' [Sec]']);
disp([' ']);

%% Restore Defaults

% set(0, 'DefaultFigureWindowStyle', 'normal');
% set(0, 'DefaultAxesLooseInset', defaultLoosInset);