function pyramid_rule ( legendre_order, jacobi_order, filename )

%*****************************************************************************80
%
%% PYRAMID_RULE generates a quadrature rule for a pyramid.
%
%  Discussion:
%
%    This program computes a quadrature rule for a pyramid
%    and writes it to a file.
%
%    The user specifies:
%    * the LEGENDRE_ORDER (number of points in the X and Y dimensions)
%    * the JACOBI_ORDER (number of points in the Z dimension)
%    * FILENAME< the root name of the output files.
%
%    The integration region is:
%
%      - ( 1 - Z ) <= X <= 1 - Z
%      - ( 1 - Z ) <= Y <= 1 - Z
%                0 <= Z <= 1.
%
%    When Z is zero, the integration region is a square lying in the (X,Y)
%    plane, centered at (0,0,0) with "radius" 1.  As Z increases to 1, the
%    radius of the square diminishes, and when Z reaches 1, the square has
%    contracted to the single point (0,0,1).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 July 2009
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PYRAMID_RULE\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Compute a quadrature rule for approximating\n' );
  fprintf ( 1, '  the integral of a function over a pyramid.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The user specifies:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  LEGENDRE_ORDER, the order of the Legendre rule for X and Y.\n' );
  fprintf ( 1, '  JACOBI_ORDER, the order of the Jacobi rule for Z,\n' );
  fprintf ( 1, '  FILENAME, the prefix for the three output files:\n' );
  fprintf ( 1, '\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' );
%
%  Get the Legendre order.
%
  if ( nargin < 1 )
    fprintf ( 1, '\n' );
    legendre_order = input ( '  Enter the Legendre rule order:' );
  elseif ( ischar ( legendre_order ) )
    legendre_order = str2num ( legendre_order );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The requested Legendre order of the rule is %d.\n', ...
    legendre_order );
%
%  Get the Jacobi order.
%
  if ( nargin < 2 )
    fprintf ( 1, '\n' );
    jacobi_order = input ( '  Enter the Jacobi rule order:' );
  elseif ( ischar ( jacobi_order ) )
    jacobi_order = str2num ( jacobi_order );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The requested Jacobi order of the rule is %d.\n', ...
    jacobi_order );
%
%  Get the output option or quadrature file root name:
%
  if ( nargin < 3 )
    fprintf ( 1, '\n' );
    filename = input ( '  Enter the "root name" of the quadrature files).' );
  end

  pyramid_handle ( legendre_order, jacobi_order, filename );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PYRAMID_RULE:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end
function [ x, w ] = jacobi_compute ( order, alpha, beta )

%*****************************************************************************80
%
%% JACOBI_COMPUTE computes the abscissa and weights for Jacobi quadrature.
%
%  Discussion:
%
%    The integration interval is [ -1, 1 ].
%
%    The weight function is w(x) = (1-X)^ALPHA * (1+X)^BETA.
%
%    The integral to approximate is:
%
%      Integral ( -1 <= X <= 1 ) (1-X)^ALPHA * (1+X)^BETA * F(X) dX
%
%    The quadrature formula is:
%
%      Sum ( 1 <= I <= ORDER ) W(I) * F ( X(I) )
%
%    Thanks to Xu Xiang of Fudan University for pointing out that
%    an earlier implementation of this routine was incorrect!
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 February 2008
%
%  Author:
%
%    Original FORTRAN77 version by Arthur Stroud, Don Secrest.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Arthur Stroud, Don Secrest,
%    Gaussian Quadrature Formulas,
%    Prentice Hall, 1966,
%    LC: QA299.4G3S7.
%
%  Parameters:
%
%    Input, integer ORDER, the order of the quadrature rule to be computed.
%
%    Input, real ALPHA, BETA, the exponents of (1-X) and
%    (1+X) in the quadrature rule.  For simple Legendre quadrature,
%    set ALPHA = BETA = 0.0.  -1.0 < ALPHA and -1.0 < BETA are required.
%
%    Output, real X(ORDER), the abscissas.
%
%    Output, real W(ORDER), the weights.
%
  if ( order < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'JACOBI_COMPUTE - Fatal error!\n' );
    fprintf ( 1, '  Illegal value of ORDER = %d\n', order );
    error ( 'JACOBI_COMPUTE - Fatal error!' );
  end
%
%  Check ALPHA and BETA.
%
  if ( alpha <= -1.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'JACOBI_COMPUTE - Fatal error!\n' );
    fprintf ( 1, '  -1.0 < ALPHA is required.\n' );
    error ( 'JACOBI_COMPUTE - Fatal error!' );
  end

  if ( beta <= -1.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'JACOBI_COMPUTE - Fatal error!\n' );
    fprintf ( 1, '  -1.0 < BETA is required.\n' );
    error ( 'JACOBI_COMPUTE - Fatal error!' );
  end
%
%  Set the recursion coefficients.
%
  b = zeros ( order, 1 );
  c = zeros ( order, 1 );
  w = zeros ( order, 1 );
  x = zeros ( order, 1 );

  for i = 1 : order

    if ( alpha + beta == 0.0 || beta - alpha == 0.0 )

      b(i) = 0.0;

    else

      b(i) = ( alpha + beta ) * ( beta - alpha ) / ...
            ( ( alpha + beta + 2 * i ) ...
            * ( alpha + beta + 2 * i - 2 ) );

    end

    if ( i == 1 )

      c(i) = 0.0;

    else

      c(i) = 4.0 * ( i - 1 ) * ( alpha + i - 1 ) * ( beta + i - 1 ) ...
        * ( alpha + beta + i - 1 ) / ( ( alpha + beta + 2 * i - 1 ) ...
        * ( alpha + beta + 2 * i - 2 )^2 * ( alpha + beta + 2 * i - 3 ) );

    end

  end

  delta = r8_gamma ( alpha        + 1.0 ) ...
        * r8_gamma (         beta + 1.0 ) ...
        / r8_gamma ( alpha + beta + 2.0 );

  cc = delta * 2.0^( alpha + beta + 1.0 ) * prod ( c(2:order) );

  for i = 1 : order

    if ( i == 1 )

      an = alpha / order;
      bn = beta / order;

      r1 = ( 1.0 + alpha ) * ( 2.78 / ( 4.0 + order * order ) ...
        + 0.768 * an / order );

      r2 = 1.0 + 1.48 * an + 0.96 * bn + 0.452 * an^2 + 0.83 * an * bn;

      x0 = ( r2 - r1 ) / r2;

    elseif ( i == 2 )

      r1 = ( 4.1 + alpha ) / ...
        ( ( 1.0 + alpha ) * ( 1.0 + 0.156 * alpha ) );

      r2 = 1.0 + 0.06 * ( order - 8.0 ) * ( 1.0 + 0.12 * alpha ) / order;

      r3 = 1.0 + 0.012 * beta * ...
        ( 1.0 + 0.25 * abs ( alpha ) ) / order;

      x0 = x0 - r1 * r2 * r3 * ( 1.0 - x0 );

    elseif ( i == 3 )

      r1 = ( 1.67 + 0.28 * alpha ) / ( 1.0 + 0.37 * alpha );

      r2 = 1.0 + 0.22 * ( order - 8.0 ) / order;

      r3 = 1.0 + 8.0 * beta / ( ( 6.28 + beta ) * order * order );

      x0 = x0 - r1 * r2 * r3 * ( x(1) - x0 );

    elseif ( i < order - 1 )

      x0 = 3.0 * x(i-1) - 3.0 * x(i-2) + x(i-3);

    elseif ( i == order - 1 )

      r1 = ( 1.0 + 0.235 * beta ) / ( 0.766 + 0.119 * beta );

      r2 = 1.0 / ( 1.0 + 0.639 * ( order - 4.0 ) ...
        / ( 1.0 + 0.71 * ( order - 4.0 ) ) );

      r3 = 1.0 / ( 1.0 + 20.0 * alpha / ( ( 7.5 + alpha ) * order * order ) );

      x0 = x0 + r1 * r2 * r3 * ( x0 - x(i-2) );

    elseif ( i == order )

      r1 = ( 1.0 + 0.37 * beta ) / ( 1.67 + 0.28 * beta );

      r2 = 1.0 / ( 1.0 + 0.22 * ( order - 8.0 ) / order );

      r3 = 1.0 / ( 1.0 + 8.0 * alpha / ( ( 6.28 + alpha ) * order * order ) );

      x0 = x0 + r1 * r2 * r3 * ( x0 - x(i-2) );

    end

    [ x0, dp2, p1 ] = jacobi_root ( x0, order, alpha, beta, b, c );

    x(i) = x0;
    w(i) = cc / ( dp2 * p1 );

  end
%
%  Reverse the order of the values.
%
  x(1:order) = x(order:-1:1);
  w(1:order) = w(order:-1:1);

  return
end
function [ p2, dp2, p1 ] = jacobi_recur ( x, order, alpha, beta, b, c )

%*****************************************************************************80
%
%% JACOBI_RECUR finds the value and derivative of a Jacobi polynomial.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 October 2005
%
%  Author:
%
%    Original FORTRAN77 version by Arthur Stroud, Don Secrest.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Arthur Stroud, Don Secrest,
%    Gaussian Quadrature Formulas,
%    Prentice Hall, 1966,
%    LC: QA299.4G3S7.
%
%  Parameters:
%
%    Input, real X, the point at which polynomials are evaluated.
%
%    Input, integer ORDER, the order of the polynomial to be computed.
%
%    Input, real ALPHA, BETA, the exponents of (1+X) and
%    (1-X) in the quadrature rule.
%
%    Input, real B(ORDER), C(ORDER), the recursion
%    coefficients.
%
%    Output, real P2, the value of J(ORDER)(X).
%
%    Output, real DP2, the value of J'(ORDER)(X).
%
%    Output, real P1, the value of J(ORDER-1)(X).
%
  p1 = 1.0;
  dp1 = 0.0;

  p2 = x + ( alpha - beta ) / ( alpha + beta + 2.0 );
  dp2 = 1.0;

  for i = 2 : order

    p0 = p1;
    dp0 = dp1;

    p1 = p2;
    dp1 = dp2;

    p2 = ( x - b(i) ) * p1 - c(i) * p0;
    dp2 = ( x - b(i) ) * dp1 + p1 - c(i) * dp0;

  end

  return
end
function [ x, dp2, p1 ] = jacobi_root ( x, order, alpha, beta, b, c )

%*****************************************************************************80
%
%% JACOBI_ROOT improves an approximate root of a Jacobi polynomial.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 October 2005
%
%  Author:
%
%    Original FORTRAN77 version by Arthur Stroud, Don Secrest.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Arthur Stroud, Don Secrest,
%    Gaussian Quadrature Formulas,
%    Prentice Hall, 1966,
%    LC: QA299.4G3S7.
%
%  Parameters:
%
%    Input, real X, the approximate root.
%
%    Input, integer ORDER, the order of the polynomial to be computed.
%
%    Input, real ALPHA, BETA, the exponents of (1+X) and
%    (1-X) in the quadrature rule.
%
%    Input, real B(ORDER), C(ORDER), the recursion coefficients.
%
%    Output, real X, the improved approximate root.
%
%    Output, real DP2, the value of J'(ORDER)(X).
%
%    Output, real P1, the value of J(ORDER-1)(X).
%
  maxstep = 10;

  eps = r8_epsilon ( );

  for i = 1 : maxstep

    [ p2, dp2, p1 ] = jacobi_recur ( x, order, alpha, beta, b, c );

    d = p2 / dp2;
    x = x - d;

    if ( abs ( d ) <= eps * ( abs ( x ) + 1.0 ) )
      return
    end

  end

  return
end
function [ x, w ] = legendre_compute ( order )

%*****************************************************************************80
%
%% LEGENDRE_COMPUTE computes a Legendre quadrature rule.
%
%  Discussion:
%
%    The integration interval is [ -1, 1 ].
%
%    The weight function is w(x) = 1.0.
%
%    The integral to approximate:
%
%      Integral ( -1 <= X <= 1 ) F(X) dX
%
%    The quadrature rule:
%
%      Sum ( 1 <= I <= ORDER ) W(I) * F ( X(I) )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 January 2008
%
%  Author:
%
%    Original FORTRAN77 version by Philip Davis, Philip Rabinowitz.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Philip Davis, Philip Rabinowitz,
%    Methods of Numerical Integration,
%    Second Edition,
%    Dover, 2007,
%    ISBN: 0486453391,
%    LC: QA299.3.D28.
%
%  Parameters:
%
%    Input, integer ORDER, the order of the rule.
%    ORDER must be greater than 0.
%
%    Output, real X(ORDER), the abscissas of the rule.
%
%    Output, real W(ORDER), the weights of the rule.
%    The weights are positive, symmetric, and should sum to 2.
%
  if ( order < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'LEGENDRE_COMPUTE - Fatal error!\n' );
    fprintf ( 1, '  Illegal value of ORDER = %d\n', order );
    error ( 'LEGENDRE_COMPUTE - Fatal error!' );
  end

  w = zeros ( order, 1 );
  x = zeros ( order, 1 );

  e1 = order * ( order + 1 );

  m = floor ( ( order + 1 ) / 2 );

  for i = 1 : floor ( ( order + 1 ) / 2 )

    mp1mi = m + 1 - i;

    t = ( 4 * i - 1 ) * pi / ( 4 * order + 2 );

    x0 = cos(t) * ( 1.0 - ( 1.0 - 1.0 / ( order ) ) / ( 8 * order * order ) );

    pkm1 = 1.0;
    pk = x0;

    for k = 2 : order
      pkp1 = 2.0 * x0 * pk - pkm1 - ( x0 * pk - pkm1 ) / k;
      pkm1 = pk;
      pk = pkp1;
    end

    d1 = order * ( pkm1 - x0 * pk );

    dpn = d1 / ( 1.0 - x0 * x0 );

    d2pn = ( 2.0 * x0 * dpn - e1 * pk ) / ( 1.0 - x0 * x0 );

    d3pn = ( 4.0 * x0 * d2pn + ( 2.0 - e1 ) * dpn ) / ( 1.0 - x0 * x0 );

    d4pn = ( 6.0 * x0 * d3pn + ( 6.0 - e1 ) * d2pn ) / ( 1.0 - x0 * x0 );

    u = pk / dpn;
    v = d2pn / dpn;
%
%  Initial approximation H:
%
    h = - u * ( 1.0 + 0.5 * u * ( v + u * ( v * v - d3pn / ( 3.0 * dpn ) ) ) );
%
%  Refine H using one step of Newton's method:
%
    p = pk + h * ( dpn + 0.5 * h * ( d2pn + h / 3.0 ...
      * ( d3pn + 0.25 * h * d4pn ) ) );

    dp = dpn + h * ( d2pn + 0.5 * h * ( d3pn + h * d4pn / 3.0 ) );

    h = h - p / dp;

    xtemp = x0 + h;

    x(mp1mi) = xtemp;

    fx = d1 - h * e1 * ( pk + 0.5 * h * ( dpn + h / 3.0 ...
      * ( d2pn + 0.25 * h * ( d3pn + 0.2 * h * d4pn ) ) ) );

    w(mp1mi) = 2.0 * ( 1.0 - xtemp * xtemp ) / fx / fx;

  end

  if ( mod ( order, 2 ) == 1 )
    x(1) = 0.0;
  end
%
%  Shift the data up.
%
  nmove = floor ( ( order + 1 ) / 2 );
  ncopy = order - nmove;

  for i = 1 : nmove
    iback = order + 1 - i;
    x(iback) = x(iback-ncopy);
    w(iback) = w(iback-ncopy);
  end
%
%  Reflect values for the negative abscissas.
%
  for i = 1 : order - nmove
    x(i) = - x(order+1-i);
    w(i) = w(order+1-i);
  end

  return
end
function pyramid_handle ( legendre_order, jacobi_order, filename )

%*****************************************************************************80
%
%% PYRAMID_HANDLE computes the requested pyramid rule and outputs it.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 July 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer LEGENDRE_ORDER, JACOBI_ORDER, the orders
%    of the component Legendre and Jacobi rules.
%
%    Input, string FILENAME, the rootname for the files,
%    write files 'file_w.txt' and 'file_x.txt', and 'file_r.txt', weights,
%    abscissas, and region.
%
  dim_num = 3;
%
%  Compute the factor rules.
%
  [ legendre_x, legendre_w ] = legendre_compute ( legendre_order );

  jacobi_alpha = 2.0;
  jacobi_beta = 0.0;

  [ jacobi_x, jacobi_w ] = jacobi_compute ( jacobi_order, jacobi_alpha, ...
    jacobi_beta );
%
%  Compute the pyramid rule.
%
  pyramid_order = legendre_order * legendre_order * jacobi_order;

  volume = 4.0 / 3.0;

  pyramid_w = zeros ( pyramid_order, 1 );
  pyramid_x = zeros ( dim_num, pyramid_order );

  l = 0;
  for k = 1 : jacobi_order
    xk = ( jacobi_x(k) + 1.0 ) / 2.0;
    wk = jacobi_w(k) / 2.0;
    for j = 1 : legendre_order
      xj = legendre_x(j);
      wj = legendre_w(j);
      for i = 1 : legendre_order
        xi = legendre_x(i);
        wi = legendre_w(i);
        l = l + 1;
        pyramid_w(l) = wi * wj * wk / 4.0 / volume;
        pyramid_x(1,l) = xi * ( 1.0 - xk );
        pyramid_x(2,l) = xj * ( 1.0 - xk );
        pyramid_x(3,l) =              xk;
      end
    end
  end

  pyramid_r(1:dim_num,1) = [ -1.0, -1.0, 0.0 ]';
  pyramid_r(1:dim_num,2) = [ +1.0, -1.0, 0.0 ]';
  pyramid_r(1:dim_num,3) = [ -1.0, +1.0, 0.0 ]';
  pyramid_r(1:dim_num,4) = [ +1.0, +1.0, 0.0 ]';
  pyramid_r(1:dim_num,5) = [  0.0,  0.0, 1.0 ]';
%
%  Write the rule to files.
%
  filename_w = strcat ( filename, '_w.txt' );
  filename_x = strcat ( filename, '_x.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,       pyramid_order, pyramid_w' );
  r8mat_write ( filename_x, dim_num, pyramid_order, pyramid_x );
  r8mat_write ( filename_r, dim_num, 5,             pyramid_r );

  return
end
function value = r8_epsilon ( )

%*****************************************************************************80
%
%% R8_EPSILON returns the R8 roundoff unit.
%
%  Discussion:
%
%    The roundoff unit is a number R which is a power of 2 with the
%    property that, to the precision of the computer's arithmetic,
%      1 < 1 + R
%    but
%      1 = ( 1 + R / 2 )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 August 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real VALUE, the roundoff unit.
%
  value = eps;

  return
end
function value = r8_gamma ( x )

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

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

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

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

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

    if ( res ~= zero )

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

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

    else

      res = xinf;
      value = res;
      return

    end

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

  elseif ( y < twelve )

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

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

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

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

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

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

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

    end

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

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

    else

      res = xinf;
      value = res;
      return

    end

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

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

  value = res;

  return
end
function 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 timestamp ( )

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

  return
end


function lam = line_adj_eigenvalues ( n )

%*****************************************************************************80
%
%% LINE_ADJ_EIGENVALUES returns the eigenvalues of the LINE_ADJ matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Output, real LAM(N), the eigenvalues.
%
  lam = zeros ( n, 1 );

  for i = 1 : n
    angle = i * pi / ( n + 1 );
    lam(i) = 2.0 * cos ( angle );
  end

  return
end


function sftpack_test04 ( )

%*****************************************************************************80
%
%% TEST04 tests R8VEC_SQCTB and R8VEC_SQCTF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 February 2010
%
%  Author:
%
%    John Burkardt
%
  n = 256;

  ahi = 5.0;
  alo = 0.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST04\n' );
  fprintf ( 1, '  For real slow quarter wave cosine transforms,\n' );
  fprintf ( 1, '  R8VEC_SQCTF does a forward transform;\n' );
  fprintf ( 1, '  R8VEC_SQCTB does a backward transform.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The number of data items is N = %d\n', n );
%
%  Set the data values.
%
  seed = 123456789;

  [ x, seed ] = r8vec_uniform ( n, alo, ahi, seed );

  r8vec_print_part ( n, x, 10, '  The original data:' );
%
%  Compute the coefficients.
%
  y = r8vec_sqctf ( n, x );

  r8vec_print_part ( n, y, 10, '  The cosine coefficients:' );
%
%  Now compute inverse transform of coefficients.  Should get back the
%  original data.

  x = r8vec_sqctb ( n, y );

  r8vec_print_part ( n, x, 10, '  The retrieved data:' );

  return
end


%% Projection onto the intersection of two sets
% Our goal is to find a point in the intersection of two convex sets C1 and C2.
% A similar goal is to find the projection of an arbitrary point onto the
% intersection (or if the intersection is empty, find the point(s) in C1
% that are as close as possible to C2). In this demo, the intersection
% of C1 and C2 is a singleton, so the concepts are the same.
%
% We assume that we know how to project on the sets C1 and C2 individually,
% but not onto C1 intersect C2.
%
% The basic algorithm to solve this problem is the alternating projection method,
% first studied by John von Neumann for the case where C1 and C2 are affine
% spaces. This algorithm can be extended to arbitrary convex sets, although
% you may not converge to the *projection* of the original point.
%
% This algorithm is very simple: starting at some point y, and with x <-- y,
% we update
%   x <-- Proj_{C1}( Proj_{C2}( x ) )
% where Proj_{C1} is the projector onto the set C1.
%
% Better methods (which actually give you the projection of y onto the intersection)
% are based on Dykstra's algorithm (not to be confused with the Dijkstra algorithm
% described here: http://en.wikipedia.org/wiki/Dijkstra%27s_algorithm ).
%
% Dykstra's algorithm also uses only Proj_{C1} and Proj_{C2}, but with a few
% intermediate steps. For an overview, see the 2011 book by Raydan:
% http://www.amazon.com/Alternating-Projection-Methods-Fundamentals-Algorithms/dp/1611971934
%
% Another overview of both alternating projection and Dykstra are in the book chapter
% "Proximal splitting methods in signal processing" by P. L. Combettes and J.-C. Pesquet,
% in the book 'Fixed-Point Algorithms for Inverse Problems in Science and Engineering',
% (ed.: H. H. Bauschke, R. S. Burachik, P. L. Combettes, V. Elser, D. R. Luke, and
% H. Wolkowicz, Editors), pp. 185-212. Springer, New York, 2011.
% The chapter can be downloaded at http://www.ann.jussieu.fr/~plc/prox.pdf
%
% The purpose of this demo is to show the above two methods, and also
% how to formulate this with TFOCS. The advantage of solving this with TFOCS
% is that we can use an accelerated solver (Nesterov-style) and use large
% step-sizes and line search (whereas Dykstra has no step-size parameter).
% In the first example, it is especially apparent that TFOCS avoids
% the slow convergence that both the alternating projection and Dykstra
% algorithms suffer from. In the second example, the performance
% of alternating projections and TFOCS are quite similar.

%% Mathematical formulation
% For a given point y, and two convex sets C1 and C2, we wish to find
% the closest point x to y, such that x is in both C1 and C2. We write this
% as:
%
% $$ \textrm{minimize}_{x\in C_1 \cap C_2} \,\, \mu/2||x\textrm{--}y||_2^2 $$
%
% The parameter $\mu > 0$ is arbitrary and does not affect the answer.
%
% This fits naturally into the TFOCS formulation since the primal
% problem is already strongly convex.

%% Demo with two polygon sets in 2D that intersect at (0,0)
% Set the dimension to 2
N = 2;  % 2D is best for visualization

%%
% Define some 2D polygons

mx = 1;
x1a = [1;.1];
x1b = [1;.2];
xx1 = [0,x1a(1),2*mx,x1b(1)];
yy1 = [0,x1a(2),2*mx,x1b(2)];

x2a = [1;.3];
x2b = [1;.35];
xx2 = [0,x2a(1),2*mx,x2b(1)];
yy2 = [0,x2a(2),2*mx,x2b(2)];

%%
% The solution is at x = (0,0), so our error function is simple

err = @(x) norm(x);
%%
% plot the regions

figure
red = [255,153,153]/255;
blue = [204,204,255]/255;
fill( xx1, yy1,red);
hold on
fill( xx2, yy2,blue);

xlim( [0,mx] ); ylim( [0,.5*mx] );
axis equal
text(.5,.07,'set C1');
text(.5,.22,'set C2');

%%
% Make some operators that will be used with TFOCS

addpath ~/Dropbox/TFOCS/  % modify this to wherever it is installed on your computer
op1     = project2DCone(x1a, x1b );
op2     = project2DCone(x2a, x2b );

offset1 = 0;
offset2 = 0;
op1_d   = prox_dualize(op1);
op2_d   = prox_dualize(op2);

% and some simpler operators that we will use with alternating projections
%   and with Dykstra
proj1   = @(x) callandmap( op1, 2, x, 1); % gamma is irrelevant
proj2   = @(x) callandmap( op2, 2, x, 1 ); % gamma is irrelevant

% for all methods, we need to specify a starting point

x0      = [1; 1/4];

%% solve in TFOCS
% We can pick any mu and should get the same result, although due to
% some scaling issues in stopping criteria, it does have a small effect.
mu = 1e-6;

opts    = struct('debug',false,'printEvery',20,'maxIts',200);
opts.errFcn{1}  = @(f,d,x) err(x);
opts.errFcn{2}  = @(f,d,x) recordPoints(x);
recordPoints(); % zero-out counter. We use this to plot the points later

opts.tol        = 1e-15;

affineOperator  = {eye(N),offset1;eye(N),offset2};
dualProx        = {op1_d,op2_d};
% Solve in TFOCS:
[x,out,optsOut] = tfocs_SCD( [], affineOperator, dualProx , mu, x0, [], opts );

% Record the path:
path    = recordPoints();
path    = [x0,path];

figure
subplot(1,2,1);
fill( xx1, yy1,red);
hold on
fill( xx2, yy2,blue);
xlim( [0,mx] ); ylim( [0,.5*mx] );
text(.5,.07,'set C1');
text(.5,.22,'set C2');
plot( path(1,:), path(2,:),'ko-','linewidth',2 )
title('Path for TFOCS solving the intersection of 2 polygons');

subplot(1,2,2);
semilogy( out.err(:,1) ,'o-'); xlabel('iterations'); ylabel('error');
title('Error for TFOCS method');
set(gcf, 'Position', [400 200 800 400]);
%% solve via alternating projection method
maxIts  = 500;
x       = x0;
path    = [x0];
errHist = [];
for k = 1:maxIts
    % basic alternating projection method:
    x   = proj1(x);
    path= [path, x];
    x   = proj2(x);
    path= [path, x];
    errHist     = [ errHist; err(x) ];
    if ~mod(k,50)
        fprintf('Iter %4d, error is %.2e\n', k, errHist(end) );
    end
end
figure
subplot(1,2,1);
fill( xx1, yy1,red);
hold on
fill( xx2, yy2,blue);
xlim( [0,mx] ); ylim( [0,.5*mx] );
text(.5,.07,'set C1');
text(.5,.22,'set C2');
plot( path(1,:), path(2,:),'ko-','linewidth',1 )
title('Path for alternating projection method solving the intersection of 2 polygons');

subplot(1,2,2);
semilogy( errHist, 'o-' ); xlabel('iterations'); ylabel('error');
errHist_1 = errHist;
title('Error for alternating projection method');
set(gcf, 'Position', [400 200 800 400]);
%% solve via Dykstra's algorithm

maxIts  = 500;
x       = x0;
[p,q]   = deal( 0*x0 );
p       = -.25*[1;1];
path    = [x0];
errHist = [];
for k = 1:maxIts
    % If x + p is feasible, the y = (x+p), so p=x+p-y = 0.
    y   = proj1( x + p );
    p   = x + p - y;
    x   = proj2( y + q );
    q   = y + q - x;
    path = [path,y,x];
    errHist     = [ errHist; err(x) ];
    if ~mod(k,50)
        fprintf('Iter %4d, error is %.2e\n', k, errHist(end) );
    end
end
figure
subplot(1,2,1);
fill( xx1, yy1,red);
hold on
fill( xx2, yy2,blue);
xlim( [0,mx] ); ylim( [0,.5*mx] );
text(.5,.07,'set C1');
text(.5,.22,'set C2');
plot( path(1,:), path(2,:),'ko-' )
title('Path for Dykstra''s algo solving the intersection of 2 polygons');

subplot(1,2,2);
semilogy( errHist, 'o-' ); xlabel('iterations'); ylabel('error');
title('Error for Dykstra''s method');
set(gcf, 'Position', [400 200 800 400]);

%%
% With Dykstra's algo (and alternating projections), we get
% very slow convergence, because of the very low angle between the
% two sets. Here is a zoom in on the graph of the iterates:
figure
fill( xx1, yy1,red);
hold on
fill( xx2, yy2,blue);
xlim( [0,mx] ); ylim( [0,.5*mx] );
text(.5,.07,'set C1');
text(.5,.22,'set C2');
plot( path(1,:), path(2,:),'ko-' )
title('Path for Dykstra''s algo solving the intersection of 2 polygons (zoom)');
xlim( [.25,.4] );
ylim( [.058,.1])
%% Plot all the errors together
figure
semilogy( out.err(:,1),'-' ,'linewidth',3);
hold all
semilogy( errHist_1,'-','linewidth',3);
semilogy( errHist,'--','linewidth',3);
legend('TFOCS','Alternating projection','Dykstra');
xlabel('iteration'); ylabel('error');
%% Demo with two circles that intersect at (0,0)
center1     = [0;.5];
radius1     = .5;
center2     = -center1;
radius2     = radius1;

figure
rectangle('Position',[-radius1,0,2*radius1,2*radius1],'Curvature',[1,1],'FaceColor',red)
rectangle('Position',[-radius1,-2*radius2,2*radius1,2*radius1],'Curvature',[1,1],'FaceColor',blue)
axis equal
hold on
text(0,.5,'set C1');
text(0,-.5,'set C2');

offset1     = center1;
offset2     = center2;

op1_d       = prox_l2(radius1);
op2_d       = prox_l2(radius2);
x0          = [1; .2];

% and some simpler operators that we will use with alternating projections
%   and with Dykstra
proj1   = @(x) radius1*(x-center1)/norm(x-center1) + center1;
proj2   = @(x) radius2*(x-center2)/norm(x-center2) + center2;
%% solve in TFOCS
opts.maxIts     = 300;
affineOperator  = {eye(N),offset1;eye(N),offset2};
dualProx        = {op1_d,op2_d};
% Solve in TFOCS:
[x,out,optsOut] = tfocs_SCD( [], affineOperator, dualProx , mu, x0, [], opts );

% Record the path:
path    = recordPoints();
path    = [x0,path];
% Plot:
figure
subplot(1,3,1);
rectangle('Position',[-radius1,0,2*radius1,2*radius1],'Curvature',[1,1],'FaceColor',red)
rectangle('Position',[-radius1,-2*radius2,2*radius1,2*radius1],'Curvature',[1,1],'FaceColor',blue)
hold on
text(0,.5,'set C1');
text(0,-.5,'set C2');
plot( path(1,:), path(2,:),'ko-','linewidth',2 )
title('Path for TFOCS solving the intersection of 2 circles');

subplot(1,3,2);
rectangle('Position',[-radius1,0,2*radius1,2*radius1],'Curvature',[1,1],'FaceColor',red)
rectangle('Position',[-radius1,-2*radius2,2*radius1,2*radius1],'Curvature',[1,1],'FaceColor',blue)
hold on
plot( path(1,:), path(2,:),'ko-','linewidth',2 )
xlim([0,.15]);
ylim([-.1,.1]);
title('(zoom)');

subplot(1,3,3);
semilogy( out.err,'o-')
title('Error of iterates for TFOCS method');
set(gcf, 'Position', [400 200 800 400]);
%% solve via alternating projection method
maxIts  = 300;
x       = x0;
path    = [x0];
errHist = [];
for k = 1:maxIts
    % basic alternating projection method:
    x   = proj1(x);
    path= [path, x];
    x   = proj2(x);
    path= [path, x];
    errHist     = [ errHist; err(x) ];
    if ~mod(k,50)
        fprintf('Iter %4d, error is %.2e\n', k, errHist(end) );
    end
end
figure
subplot(1,3,1);
rectangle('Position',[-radius1,0,2*radius1,2*radius1],'Curvature',[1,1],'FaceColor',red)
rectangle('Position',[-radius1,-2*radius2,2*radius1,2*radius1],'Curvature',[1,1],'FaceColor',blue)
hold on
text(0,.5,'set C1');
text(0,-.5,'set C2');
plot( path(1,:), path(2,:),'ko-' )
title('Path for alternating projection method solving the intersection of 2 circles');

subplot(1,3,2);
rectangle('Position',[-radius1,0,2*radius1,2*radius1],'Curvature',[1,1],'FaceColor',red)
rectangle('Position',[-radius1,-2*radius2,2*radius1,2*radius1],'Curvature',[1,1],'FaceColor',blue)
hold on
plot( path(1,:), path(2,:),'ko-','linewidth',2 )
xlim([0,.15]);
ylim([-.1,.1]);
title('(zoom)');

subplot(1,3,3);
semilogy( errHist,'o-')
errHist_1 = errHist;
title('Error of iterates for alternating projection method');
set(gcf, 'Position', [400 200 800 400]);
%% solve via Dykstra's algorithm

maxIts  = 300;
x       = x0;
[p,q]   = deal( 0*x0 );
path    = [x0];
errHist = [];
for k = 1:maxIts
    % If x + p is feasible, the y = (x+p), so p=x+p-y = 0.
    y   = proj1( x + p );
    p   = x + p - y;
    x   = proj2( y + q );
    q   = y + q - x;
    path = [path,y];
    path = [path,x];
    errHist     = [ errHist; err(x) ];
    if ~mod(k,50)
        fprintf('Iter %4d, error is %.2e\n', k, errHist(end) );
    end
end
figure
subplot(1,3,1);
rectangle('Position',[-radius1,0,2*radius1,2*radius1],'Curvature',[1,1],'FaceColor',red)
rectangle('Position',[-radius1,-2*radius2,2*radius1,2*radius1],'Curvature',[1,1],'FaceColor',blue)
hold on
text(0,.5,'set C1');
text(0,-.5,'set C2');
plot( path(1,:), path(2,:),'ko-' )
title('Path for Dykstra''s algo solving the intersection of 2 circles');

subplot(1,3,2);
rectangle('Position',[-radius1,0,2*radius1,2*radius1],'Curvature',[1,1],'FaceColor',red)
rectangle('Position',[-radius1,-2*radius2,2*radius1,2*radius1],'Curvature',[1,1],'FaceColor',blue)
hold on
plot( path(1,:), path(2,:),'ko-','linewidth',2 )
xlim([0,.15]);
ylim([-.1,.1]);
title('(zoom)');

subplot(1,3,3);
semilogy( errHist,'o-')
title('Error of iterates for Dykstra''s algo');
set(gcf, 'Position', [400 200 800 400]);

%% Plot all the errors together
figure
semilogy( out.err(:,1),'-' ,'linewidth',3);
hold all
semilogy( errHist_1,'-','linewidth',3);
semilogy( errHist,'--','linewidth',3);
legend('TFOCS','Alternating projection','Dykstra');
xlabel('iteration'); ylabel('error');

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


function j = jacobi_symbol ( q, p )

%*****************************************************************************80
%
%% JACOBI_SYMBOL evaluates the Jacobi symbol (Q/P).
%
%  Definition:
%
%    If P is prime, then
%
%      Jacobi Symbol (Q/P) = Legendre Symbol (Q/P)
%
%    Else
%
%      let P have the prime factorization
%
%        P = Product ( 1 <= I <= N ) P(I)^E(I)
%
%      Jacobi Symbol (Q/P) =
%
%        Product ( 1 <= I <= N ) Legendre Symbol (Q/P(I))^E(I)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 July 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Daniel Zwillinger,
%    CRC Standard Mathematical Tables and Formulae,
%    30th Edition,
%    CRC Press, 1996, pages 86-87.
%
%  Parameters:
%
%    Input, integer Q, an integer whose Jacobi symbol with
%    respect to P is desired.
%
%    Input, integer P, the number with respect to which the Jacobi
%    symbol of Q is desired.  P should be 2 or greater.
%
%    Output, integer J, the Jacobi symbol (Q/P).
%    Ordinarily, J will be -1, 0 or 1.
%    -2, not enough factorization space.
%    -3, an error during Legendre symbol calculation.
%

%
%  P must be greater than 1.
%
  if ( p <= 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'JACOBI_SYMBOL - Fatal error!\n' );
    fprintf ( 1, '  P must be greater than 1.\n' );
    j = -2;
    return
  end
%
%  Decompose P into factors of prime powers.
%
  [ nfactor, factor, power, nleft ] = i4_factor ( p );

  if ( nleft ~= 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'JACOBI_SYMBOL - Fatal error!\n' );
    fprintf ( 1, '  Not enough factorization space.\n' );
    j = -2;
    error ( 'JACOBI_SYMBOL - Fatal error!' );
  end
%
%  Force Q to be nonnegative.
%
  qq = q;

  while ( qq < 0 )
    qq = qq + p;
  end
%
%  For each prime factor, compute the Legendre symbol, and
%  multiply the Jacobi symbol by the appropriate factor.
%
  j = 1;
  for i = 1 : nfactor
    pp = factor(i);
    l = legendre_symbol ( qq, pp );
    if ( l < -1 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'JACOBI_SYMBOL - Fatal error!\n' );
      fprintf ( 1, '  Error during Legendre symbol calculation.\n' );
      j = -3;
      error ( 'JACOBI_SYMBOL - Fatal error!' );
    end
    j = j * l^power(i);
  end

  return
end


% Exercise 4.31: Design of a cantilever beam (GP)
% Boyd & Vandenberghe "Convex Optimization"
% Almir Mutapcic - 01/30/06
% Updated to use GP mode 02/08/06
% (a figure is generated)
%
% We have a segmented cantilever beam with N segments. Each segment
% has a unit length and variable width and height (rectangular profile).
% The goal is minimize the total volume of the beam, over all segment
% widths w_i and heights h_i, subject to constraints on aspect ratios,
% maximum allowable stress in the material, vertical deflection y, etc.
%
% The problem can be posed as a geometric program (posynomial form)
%     minimize    sum( w_i* h_i)
%         s.t.    w_min <= w_i <= w_max,       for all i = 1,...,N
%                 h_min <= h_i <= h_max
%                 S_min <= h_i/w_i <= S_max
%                 6*i*F/(w_i*h_i^2) <= sigma_max
%                 6*F/(E*w_i*h_i^3) == d_i
%                 (2*i - 1)*d_i + v_(i+1) <= v_i
%                 (i - 1/3)*d_i + v_(i+1) + y_(i+1) <= y_i
%                 y_1 <= y_max
%
% with variables w_i, h_i, d_i, (i = 1,...,N) and v_i, y_i (i = 1,...,N+1).
% (Consult the book for other definitions and a recursive formulation of
% this problem.)

% optimization variables
N = 8;

% constants
wmin = .1; wmax = 100;
hmin = .1; hmax = 6;
Smin = 1/5; Smax = 5;
sigma_max = 1;
ymax = 10;
E = 1; F = 1;

cvx_begin gp
  % optimization variables
  variables w(N) h(N) v(N+1) y(N+1);

  % objective is the total volume of the beam
  % obj = sum of (widths*heights*lengths) over each section
  % (recall that the length of each segment is set to be 1)
  minimize( w'*h )
  subject to
    % non-recursive formulation
    d = 6*F*ones(N,1)./(E*ones(N,1).*w.*h.^3);
    for i = 1:N
      (2*i-1)*d(i) + v(i+1) <= v(i); %#ok
      (i-1/3)*d(i) + v(i+1) + y(i+1) <= y(i); %#ok
    end

    % constraint set
    wmin <= w    <= wmax; %#ok
    hmin <= h    <= hmax; %#ok
    Smin <= h./w <= Smax; %#ok
    6*F*[1:N]'./(w.*(h.^2)) <= sigma_max; %#ok
    y(1) <= ymax; %#ok
cvx_end

% display results
disp('The optimal widths and heights are: ');
w, h
fprintf('The optimal minimum volume of the beam is %3.4f.\n', sum(w.*h));

% plot the 3D model of the optimal cantilever beam
figure, clf
cantilever_beam_plot([h; w])


function d = sqdist(x, y)
% L2 dist from x to y
% x: [1 x d]
% y: [n x d]
% d: [n x 1]

d = sqrt(sum((repmat(x, size(y,1), 1) - y).^2, 2));


function [A,b,x] = foxgood(n)
%FOXGOOD Test problem: severely ill-posed problem.
%
% [A,b,x] = foxgood(n)
%
% This is a model problem which does not satisfy the
% discrete Picard condition for the small singular values.
% The problem was first used by Fox & Goodwin.

% Reference: C. T. H. Baker, "The Numerical Treatment of
% Integral Equations", Clarendon Press, Oxford, 1977; p. 665.

% Discretized by simple quadrature (midpoint rule).

% Per Christian Hansen, IMM, 03/16/93.

% Initialization.
h = 1/n; t = h*((1:n)' - 0.5);

A = h*sqrt((t.^2)*ones(1,n) + ones(n,1)*(t.^2)');
x = t; b = ((1+t.^2).^1.5 - t.^3)/3;


function [ f, g, H ] = opt12_fgh ( x, flag )

%*****************************************************************************80
%
%% OPT12_FGH evaluates F, G and H for test case #2.
%
%  Discussion:
%
%    This is the Beale function.
%
%    Suggested initial values for X include:
%
%      X init = ( 1, 1 )
%
%      X init = ( 1, 4 ) (may have trouble converging)
%
%    The optimizing value is
%
%      X* = ( 3.0, 0.5 )
%
%    and the optimal function value is
%
%      F(X*) = 0.
%
%  Modified:
%
%    28 January 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Evelyn Beale,
%    On an Iterative Method for Finding a Local Minimum of a Function
%    of More than One Variable,
%    Technical Report 25,
%    Statistical Techniques Research Group,
%    Princeton University, 1958.
%
%    Richard Brent,
%    Algorithms for Minimization with Derivatives,
%    Dover, 2002,
%    ISBN: 0-486-41998-3,
%    LC: QA402.5.B74.
%
%  Parameters:
%
%    Input, real X(2), the evaluation point.
%
%    Input, string FLAG, indicates what must be computed.
%    'f' means only the value of F is needed,
%    'g' means only the value of G is needed,
%    'all' means F, G and H (if appropriate) are needed.
%    It is acceptable to behave as though FLAG was 'all'
%    on every call.
%
%    Output, real F, the optimization function.
%
%    Output, real G(2,1), the gradient column vector.
%
%    Output, real H(2,2), the Hessian matrix.
%
  n = length ( x );

  if ( n ~= 2 )
    fprintf ( '\n' );
    fprintf ( 'OPT12_FGH - Fatal error!\n' );
    fprintf ( '  The input vector X should have length 2.\n'),
    fprintf ( '  Instead, it has length = %d.\n', n );
    keyboard
  end

  f1 = 1.5   - x(1) * ( 1.0 - x(2)    );
  f2 = 2.25  - x(1) * ( 1.0 - x(2) * x(2) );
  f3 = 2.625 - x(1) * ( 1.0 - x(2) * x(2) * x(2) );

  f = f1 * f1 + f2 * f2 + f3 * f3;

  df1dx1 = - ( 1.0 - x(2) );
  df1dx2 = x(1);
  df2dx1 = - ( 1.0 - x(2) * x(2) );
  df2dx2 = 2.0 * x(1) * x(2);
  df3dx1 = - ( 1.0 - x(2) * x(2) * x(2) );
  df3dx2 = 3.0 * x(1) * x(2) * x(2);

  g(1,1) = 2.0 * ( f1 * df1dx1 + f2 * df2dx1 + f3 * df3dx1 );
  g(2,1) = 2.0 * ( f1 * df1dx2 + f2 * df2dx2 + f3 * df3dx2 );

  d2f1dx12 = 1.0;
  d2f1dx21 = 1.0;

  d2f2dx12 = 2.0 * x(2);
  d2f2dx21 = 2.0 * x(2);
  d2f2dx22 = 2.0 * x(1);

  d2f3dx12 = 3.0 * x(2) * x(2);
  d2f3dx21 = 3.0 * x(2) * x(2);
  d2f3dx22 = 6.0 * x(1) * x(2);

  H(1,1) = 2.0 * ( df1dx1 * df1dx1 ...
                 + df2dx1 * df2dx1 ...
                 + df3dx1 * df3dx1 );

  H(1,2) = 2.0 * ( df1dx2 * df1dx1 + f1 * d2f1dx12 ...
                 + df2dx2 * df2dx1 + f2 * d2f2dx12 ...
                 + df3dx2 * df3dx1 + f3 * d2f3dx12 );

  H(2,1) = 2.0 * ( df1dx1 * df1dx2 + f1 * d2f1dx21 ...
                 + df2dx1 * df2dx2 + f2 * d2f2dx21 ...
                 + df3dx1 * df3dx2 + f3 * d2f3dx21 );

  H(2,2) = 2.0 * ( df1dx2 * df1dx2 ...
                 + df2dx2 * df2dx2 + f2 * d2f2dx22 ...
                 + df3dx2 * df3dx2 + f3 * d2f3dx22 );


clear all; close all; clc

n=200; L=8;
x=linspace(0,L,n);
x1=x(1:100);   % train
x2=x(101:200); % test
n1=length(x1);
n2=length(x2);
ftrain=(x1.^2).';  % train parabola x=[0,4]
ftest=(x2.^2).';   % test parbola x=[4,5]
figure(1), subplot(3,1,1),
plot(x1,ftrain,'r',x2,ftest,'b','Linewidth',[2])
legend('','','Location','Northwest')
legend boxoff

M=30; % number of model terms
Eni=zeros(100,M); Ene=zeros(100,M);
for jj=1:M
    for j=1:jj
        phi_i(:,j)=(x1.').^(j-1); % interpolation key
        phi_e(:,j)=(x2.').^(j-1); % extrapolation key
    end

    f=(x.^2).';
    for j=1:100
        fni=(x1.^2+0.1*randn(1,n1)).'; % interpolation
        fne=(x2.^2+0.1*randn(1,n2)).'; % extrapolation

        ani=pinv(phi_i)*fni; fnai=phi_i*ani;
        Eni(j,jj)=norm(ftrain-fnai)/norm(ftrain);

        fnae=phi_e*ani;  % use loadings from x in [0,4]
        Ene(j,jj)=norm(ftest-fnae)/norm(ftest);
    end
end

subplot(3,2,3), boxplot(Eni), axis([0.5 30.5 0 0.7]), set(gca,'Xlim',[0.5 30.5],'Xtick',1:30,'Xticklabel',{'1','','','','5','','','','','10','','','','','15','','','','','20','','','','','25','','','','','30'})
subplot(3,2,4), boxplot(Eni), axis([0.5 30.5 0 0.02]), set(gca,'Xlim',[0.5 30.5],'Xtick',1:30,'Xticklabel',{'1','','','','5','','','','','10','','','','','15','','','','','20','','','','','25','','','','','30'})
subplot(3,2,5), boxplot(Ene), set(gca,'Xlim',[0.5 30.5],'Xtick',1:30,'Xticklabel',{'1','','','','5','','','','','10','','','','','15','','','','','20','','','','','25','','','','','30'})
subplot(3,2,6), boxplot(log(Ene+1)), axis([0.5 30.5 0 30]), set(gca,'Xtick',1:30,'Xticklabel',{'1','','','','5','','','','','10','','','','','15','','','','','20','','','','','25','','','','','30'})


function [dmodedr, dmodeds] = GradSimplex2DP(a,b,id,jd)

% function [dmodedr, dmodeds] = GradSimplex2DP(a,b,id,jd)
% Purpose: Return the derivatives of the modal basis (id,jd) on the 2D simplex at (a,b).

fa = JacobiP(a, 0, 0, id);     dfa = GradJacobiP(a, 0, 0, id);
gb = JacobiP(b, 2*id+1,0, jd); dgb = GradJacobiP(b, 2*id+1,0, jd);

% r-derivative
% d/dr = da/dr d/da + db/dr d/db = (2/(1-s)) d/da = (2/(1-b)) d/da
dmodedr = dfa.*gb;
if(id>0)
  dmodedr = dmodedr.*((0.5*(1-b)).^(id-1));
end

% s-derivative
% d/ds = ((1+a)/2)/((1-b)/2) d/da + d/db
dmodeds = dfa.*(gb.*(0.5*(1+a)));
if(id>0)
 dmodeds = dmodeds.*((0.5*(1-b)).^(id-1));
end

tmp = dgb.*((0.5*(1-b)).^id);
if(id>0)
  tmp = tmp-0.5*id*gb.*((0.5*(1-b)).^(id-1));
end
dmodeds = dmodeds+fa.*tmp;

% Normalize
dmodedr = 2^(id+0.5)*dmodedr; dmodeds = 2^(id+0.5)*dmodeds;
return;


function p = predict(theta, X)
%PREDICT Predict whether the label is 0 or 1 using learned logistic
%regression parameters theta
%   p = PREDICT(theta, X) computes the predictions for X using a
%   threshold at 0.5 (i.e., if sigmoid(theta'*x) >= 0.5, predict 1)  %'

m = size(X, 1); % Number of training examples

% You need to return the following variables correctly
p = zeros(m, 1);

% ====================== YOUR CODE HERE ======================
% Instructions: Complete the following code to make predictions using
%               your learned logistic regression parameters.
%               You should set p to a vector of 0's and 1's
%

p = round(sigmoid(X * theta));

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

end


function y = coshminus1(x)
%COSHMINUS1 Hyperbolic cosine minus one.
%
%   COSHMINUS1(X) is COSH(X)-1 calculated in a way that is numerically better
%   when X is close zero.
%
%   This illustrates the difference
%
%      x = 5e-8*(-1:0.01:1);
%      plot(x, cosh(x)-1, 'b-', x, coshminus1(x), 'r-');
%
%   See also ACOSHPLUS1.

%   Author:      Peter J. Acklam
%   Time-stamp:  2003-10-13 14:57:06 +0200
%   E-mail:      pjacklam@online.no
%   URL:         http://home.online.no/~pjacklam

   % check number of input arguments
   error(nargchk(1, 1, nargin));

   y = 2 * sinh(x / 2).^2;


% vgg_fit_hplane_to_x  Fitting hyperplane to set of points.
%
% SYNOPSIS
% [A,e] = vgg_fit_hplane_to_x(s), where
%
%   s ... double(N+1,N+1), inv. covariance matrix s = x'*x, the columns of which
%      are (N+1)-vectors of homog. coordinates of points in N-space (s(end,:)==1)
%   A ... double(1,N+1), fitted hyperplane (homog. coordinates).
%     Homogeneous part of A is normalized, norm(A(1,1:N))==1.
%   e ... double(N,1), eignevalues of the fit
%
% Hyperplane A minimizes sum of squared orthogonal distances of points to it.
%
% EXAMPLE
% Let e be (2,?) vector of detected edgels in nonhomog. coords. Then fitting a straight
% line l to this set of edgels is:
%   e(end,:) = 1;
%   l = vgg_fit_hplane_to_x(e*e');

function [A,e] = vgg_fit_hplane_to_x(s)

N = size(s,1) - 1; % space dimension
c = s(1:N,end)/s(end,end); % centroid

[U,e,V] = svd(s(1:N,1:N)-c*s(end,1:N),0);

A = U(:,end)';
A = [A -A*c];
e = diag(e);

return


function fx = p10_fx ( x )

%*****************************************************************************80
%
%% P10_FX evaluates the Repeller.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 July 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X(*), the point at which F is to be evaluated.
%
%    Output, real FX(*), the value of the function at X.
%
  fx = 20.0 * x ./ ( 100.0 * x .* x + 1.0 );

  return
end


function [ bvec3, overflow ] = bvec_add ( n, bvec1, bvec2 )

%*****************************************************************************80
%
%% BVEC_ADD adds two binary vectors.
%
%  Discussion:
%
%    A BVEC is an integer vector of binary digits, intended to
%    represent an integer.  BVEC(1) is the units digit, BVEC(N-1)
%    is the coefficient of 2**(N-2), and BVEC(N) contains sign
%    information.  It is 0 if the number is positive, and 1 if
%    the number is negative.
%
%  Example:
%
%    N = 4
%
%    BVEC1    dec  BVEC2    dec  BVEC3    dec
%    -------  ---  -------  ---  -------  ---
%    1 0 0 0   1   1 1 0 0   3   0 0 1 0   4
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 December 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the length of the vectors.
%
%    Input, integer BVEC1(N), BVEC2(N), the vectors to be added.
%
%    Output, integer BVEC3(N), the sum of the two input vectors.
%
%    Output, logical OVERFLOW, is true if the sum overflows.
%
  base = 2;
  overflow = 0;

  bvec3(1:n) = floor ( bvec1(1:n) ) + floor ( bvec2(1:n) );

  for i = 1 : n
    while ( base <= bvec3(i) )
      bvec3(i) = bvec3(i) - base;
      if ( i < n )
        bvec3(i+1) = bvec3(i+1) + 1;
      else
        overflow = 1;
      end
    end
  end

  return
end


function [xn,yn] = randvertex(x,y,npix)
%RANDVERTEX Adds random noise to the vertices of a polygon.
%	[XN,YN] = RANDVERTEX[X,Y,NPIX] adds uniformly distributed noise to
%	the coordinates of vertices of a polygon. The coordinates of the
%	vertices are input in X and Y, and NPIX is the maximum number of
%	pixel locations by which any pair (X(i),Y(i)) is allowed to deviate.
%	For example, if NPIX = 1, the location of any X(i) will not deviate
%	by more than one pixel location in the x-direction, and similarly for
%	Y(i). Noise is added independently to the two coordinates.
%
%   Copyright 2002-2020 Gatesmark
%
%   This function, and other functions in the DIPUM Toolbox, are based
%   on the theoretical and practical foundations established in the
%   book Digital Image Processing Using MATLAB, 3rd ed., Gatesmark
%   Press, 2020.
%
%   Book website: http://www.imageprocessingplace.com
%   License: https://github.com/dipum/dipum-toolbox/blob/master/LICENSE.txt

% Convert to columns.
x = x(:);
y = y(:);

% Preliminary calculations.
L = length(x);
xnoise = rand(L,1);
ynoise = rand(L,1);
xdev = npix*xnoise.*sign(xnoise - 0.5);
ydev = npix*ynoise.*sign(ynoise - 0.5);

% Add noise and round.
xn = round(x + xdev);
yn = round(y + ydev);

% All pixel locations must be no less than 1.
xn = max(xn,1);
yn = max(yn,1);


function r8blt_print ( n, ml, a, title )

%*****************************************************************************80
%
%% R8BLT_PRINT prints a R8BLT matrix.
%
%  Discussion:
%
%    The R8BLT storage format is appropriate for a banded lower triangular matrix.
%    The matrix is assumed to be zero below the ML-th subdiagonal.
%    The matrix is stored in an ML+1 by N array, in which the diagonal
%    appears in the first row, followed by successive subdiagonals.
%    Columns are preserved.
%
%  Example:
%
%    N = 5, ML = 2
%
%    A11   0   0   0   0
%    A21 A22   0   0   0
%    A31 A32 A33   0   0
%      0 A42 A43 A44   0
%      0   0 A53 A54 A55
%                --- ---
%                    ---
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 April 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer ML, the lower bandwidth.
%
%    Input, real A(ML+1,N), the R8BLT matrix.
%
%    Input, string TITLE, a title to be printed.
%
  r8blt_print_some ( n, ml, a, 1, 1, n, n, title );

  return
end


% Compute the values of parameter theta (controlling sparsity) that should
% be expected to give each sparsity level. Return upper and lower bounds
% each sparsity level k=[1, ..., n-1] neighbors/node.
%
% USAGE: [theta_l, theta_u, Z_sorted] = gsp_compute_theta_bounds(Z)
%                                       gsp_compute_theta_bounds(Z, geom_mean)
%                                       gsp_compute_theta_bounds(Z, geom_mean, is_sorted)
%
% geom_mean:    use geometric mean instead of arithmetic mean? default: 0
% is_sorted:    is Z already sorted? default: 0
%
%
%
%
%
% code author: Vassilis Kalofolias
% date: Aug 2016

function [theta_l, theta_u, Z_sorted] = gsp_compute_theta_bounds(Z, geom_mean, is_sorted)

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

if nargin < 3
    is_sorted = 0;
end

% Z is the zero-diagonal pairwise distance matrix between nodes

if ismatrix(Z)

    if is_sorted
        Z_sorted = Z;
    else
        n = length(Z);
        % don't take into account the diagonal of Z
        Z_sorted = zeros(n, n-1);
        for i=1:n
            Z_sorted(i, :) = sort(Z(i, [1:i-1, i+1:n]), 2, 'ascend');
        end
    end
    [m, n] = size(Z_sorted);

    B_k = cumsum(Z_sorted, 2);       % cummulative sum for each row
    K_mat = repmat((1:n), m, 1);

    %% Theoretical intervals of theta for each desired sparsity level:
    if geom_mean == 0
        theta_u = mean(1./sqrt(K_mat.*Z_sorted.^2 - B_k.*Z_sorted));
    else
    % try geometric mean instead of arithmetic:
        theta_u = exp(mean(log(1./sqrt(K_mat.*Z_sorted.^2 - B_k.*Z_sorted))));
    end
    theta_l = [theta_u(2:end), 0];
end


fprintf('METODO NEWTON-RAPHSON\n'); 			%TITULO
syms f(x) %crea una variable simbolica
f(x)=input('Ingrese la funcion: ');            		% se ingresara la funcion
r0=input('ingrese la primera aproximacion: ');  	%se ingresa el primer punto o raiz
tol=input('Ingrese la tolerancia: ');           	%valor del error minimo aceptable
n=input('Ingrese numero maximo de iteraciones: '); 	%hasta cuantas iteraciones
df=diff(f,x);       %derivada de f(x)
error=100;          %para ingresar al while
i=0;                 %contador
ri=r0;
fprintf('iteraciones\t\t\t\tri\t\t\t\t\tri+1\t\t\t\terror\n');    	%el encabezado a imprimir
fprintf('%i\t\t\t\t%4.11f\t\t\t\t---------\t\t\t\t-----------\n',i,r0); %imprime nuestros datos inciales
while(error>=tol && i<=n)
   i=i+1;
   rim1=ri-(f(ri)/df(ri)); %hallamos la nueva raiz
   error=abs(((ri-rim1)/rim1)*100);     				%hallamos el error al tratar de encontrar la raiz
   fprintf('%i\t\t\t\t%4.11f\t\t\t\t%4.11f\t\t\t\t%4.11f\n',i,r0,rim1,error); %se imprimen los valores hallados por el metodo newton
   ri=rim1; 								%en caso no termine el bucle se vuelve a repetir con una nueva raiz
end
fprintf('La raiz es %4.11f y el error es %4.11f\n',rim1,error);     	%se imprimen los datos finales


function [ x, w ] = ncoh_compute ( n )

%*****************************************************************************80
%
%% NCOH_COMPUTE computes a Newton-Cotes "open half" quadrature rule.
%
%  Discussion:
%
%    The input value N is used to define N equal subintervals of [-1,+1].
%    The I-th abscissa is the center of the I-th subinterval.
%
%    The integral:
%
%      Integral ( X_MIN <= X <= X_MAX ) F(X) dx
%
%    The quadrature rule:
%
%      Sum ( 1 <= I <= N ) W(I) * F ( X(I) ).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 July 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order.
%
%    Output, real X(N), the abscissas.
%
%    Output, real W(N), the weights.
%
  x = zeros ( n, 1 );

  x_min = -1.0;
  x_max =  1.0;

  for i = 1 : n
    x(i) = ( ( 2 * n - 2 * i + 1 ) * x_min   ...
           + (         2 * i - 1 ) * x_max ) ...
           / ( 2 * n             );
  end

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

  return
end


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% function B = getGradientNodal(omega,m);
%
% Builds the gradient operator B for a domain defined by omega
% and a nodal grid discretization defined by m:
%
%
% EXAMPLE FOR 2D:
%
% o--x--o--x--o--x--o
% |     |     |     |
% +     +     +     +
% |     |     |     |
% o--x--o--x--o--x--o
% |     |     |     |       WHERE:
% +     +     +     +       'o'  - uc       (nodal)
% |     |     |     |       'x'  - d_1 uc      (staggered-2)
% o--x--o--x--o--x--o       '+'  - d_2 uc      (staggered-1)
%
% Note: the matrices are simple, it is size that matters.
% =============================================================================

function B = getGradientNodal(omega,m)
if nargin == 0,
  help(mfilename);
  runMinimalExample;
  return;
end;

h      =  (omega(2:2:end)-omega(1:2:end))./m;
dim    =  size(omega,2)/2;
id     =  @(i) speye(m(i)+1);   % (m(i)+1) identity matrix
                             % short difference operator
dx    =  @(i) spdiags(ones(m(i),1)*[-1,1],[0,1],m(i),m(i)+1)/h(i);
switch dim
    case 2
      B = [kron(id(2),dx(1));kron(dx(2),id(1))];
      z = sparse(size(B,1),size(B,2));
      B = sparse([B z; z B]);
    case 3
        B = [
        kron(id(3),kron(id(2),dx(1)))
        kron(id(3),kron(dx(2),id(1)))
        kron(dx(3),kron(id(2),id(1)))
        ];
        z = sparse(size(B,1),size(B,2));
        B = sparse([B z z; z B z; z z B]);
    otherwise
        error('Dimension must be either 2 or 3.')
end
%------------------------------------------------------------------------------
function runMinimalExample

% 2D
omega = [0,3,0,5]; m = [10,13];
B2 = getGradientNodal(omega,m);
figure(1); clf;
subplot(1,2,1)
spy(B2);
title(sprintf('%s matrix (2D)',mfilename));

% 3D
omega = [0,1,0,2,0,3]; m = [4,5,6];
B2 = getGradientNodal(omega,m);
subplot(1,2,2);
spy(B2);
title(sprintf('%s matrix (3D)',mfilename));
%==============================================================================


function res=accurateSumK(p,K)
%%ACCURATESUMK Sum the elements in a vector in a manner that reduces finite
%             precision errors when summing large and small values as
%             compared to sequentially summing the terms. In comparison to
%             the accurateSum function, this function allows for K-fold
%             precision (K=2 is fixed in accurateSum with algorithm 0).
%
%INPUTS: p An nX1 or 1Xn vector of real values.
%        K The multiple of the standard double precision desired for
%          intermediate results. If omitted or an empty matrix is passed,
%          the default is K=3. Values larger than n are clipped to n.
%
%OUTPUTS: res The value of the sum of the values in p.
%
%This function implements algorithm 4.8 in [1].
%
%EXAMPLE:
%The simplest example demonstrates how using higher intermediate precision
%can make sorting the input less important. We compare this function to the
%unsorted accurateSum function.
% a=[1e100;2;1;eps();eps();eps();-1e100];
% res0=accurateSumK(a)
% res1=accurateSum(a,[],false)%false makes it unsorted.
%One gets res0=3.000000000000001 and res=3. res0 is more accurate.
%
%REFERENCES:
%[1] T. Ogita, S. M. Rump, and S. Oishi, "Accurate sum and dot product,"
%    SIAM Journal on Scientific Computing, no. 6, pp. 1955-1988, 2005.
%
%November 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<2||isempty(K))
    K=3;
end

n=length(p);
K=min(K,n);
for k=1:(K-1)
    p=VecSum(p);
end

res=p(1);
for i=2:n
    res=res+p(i);
end
end

function p=VecSum(p)
    n=length(p);
    for k=2:n
        [p1,p2]=exactPairSum(p(k),p(k-1));
        p(k)=p1;
        p(k-1)=p2;
    end
end

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


function legendre_exactness ( n, x, w, p_max )

%*****************************************************************************80
%
%% LEGENDRE_EXACTNESS: quadrature rule exactness for Legendre integral.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 May 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of points in the rule.
%
%    Input, real X(N), the quadrature points.
%
%    Input, real W(N), the quadrature weights.
%
%    Input, integer P_MAX, the maximum exponent.
%    0 <= P_MAX.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Quadrature rule of order %d for the Legendre integral.\n', n );
  fprintf ( 1, '  Degree          Relative Error\n' );
  fprintf ( 1, '\n' );

  for p = 0 : p_max

    e = legendre_monomial_quadrature ( n, x, w, p );

    fprintf ( 1, '  %6d  %24.16f\n', p, e );

  end

  return
end


function [ center, eccent, parity ] = tree_arc_center ( nnode, inode, jnode )

%*****************************************************************************80
%
%% TREE_ARC_CENTER computes the center, eccentricity, and parity of a tree.
%
%  Discussion:
%
%    A tree is an undirected graph of N nodes, which uses N-1 edges,
%    and is connected.
%
%    A graph with N-1 edges is not guaranteed to be a tree, and so this
%    routine must first check that condition before proceeding.
%
%    The edge distance between two nodes I and J is the minimum number of
%    edges that must be traversed in a path from I and J.
%
%    The eccentricity of a node I is the maximum edge distance between
%    node I and the other nodes J in the graph.
%
%    The radius of a graph is the minimum eccentricity over all nodes
%    in the graph.
%
%    The diameter of a graph is the maximum eccentricity over all nodes
%    in the graph.
%
%    The center of a graph is the set of nodes whose eccentricity is
%    equal to the radius, that is, the set of nodes of minimum eccentricity.
%
%    For a tree, the center is either a single node, or a pair of
%    neighbor nodes.
%
%    The parity of the tree is 1 if the center is a single node, or 2 if
%    the center is 2 nodes.
%
%    The center of a tree can be found by removing all "leaves", that is,
%    nodes of degree 1.  This step is repeated until only 1 or 2 nodes
%    are left.
%
%    Thanks to Alexander Sax for pointing out that a previous version of the
%    code was failing when the tree had an odd parity, that is, a single
%    center node, 15 April 2013.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 June 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NNODE, the number of nodes.
%
%    Input, integer INODE(NNODE-1), JNODE(NNODE-1), the edges of
%    the tree.  Edge I connects nodes INODE(I) and JNODE(I).
%
%    Output, integer CENTER(2).  CENTER(1) is the index of the
%    first node in the center.  CENTER(2) is 0 if there is only one node
%    in the center, or else the index of the second node.
%
%    Output, integer ECCENT, the eccentricity of the nodes in
%    the center, and the radius of the the tree.
%
%    Output, integer PARITY, the parity of the tree, which is
%    normally 1 or 2.
%
  eccent = 0;
  center(1) = 0;
  center(2) = 0;
  parity = 0;

  if ( nnode <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TREE_ARC_CENTER - Fatal error!\n' );
    fprintf ( 1, '  NNODE <= 0.\n' );
    error ( 'TREE_ARC_CENTER - Fatal error!' );
  elseif ( nnode == 1 )
    eccent = 0;
    center(1) = 1;
    center(2) = 0;
    parity = 1;
    return
  elseif ( nnode == 2 )
    eccent = 1;
    center(1) = 1;
    center(2) = 2;
    parity = 2;
    return
  end
%
%  Is this graph really a tree?
%
  nedge = nnode - 1;
  result = graph_arc_is_tree ( nedge, inode, jnode, nnode );

  if ( result == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TREE_ARC_CENTER - Fatal error!\n' );
    fprintf ( 1, '  This graph is NOT a tree.\n' );
    error ( 'TREE_ARC_CENTER - Fatal error!' );
  end
%
%  Compute the degrees.
%
  degree = graph_arc_degree ( nnode, nedge, inode, jnode );
%
%  Defoliate the tree.
%
  nnode2 = nnode;

  while ( 1 )

    eccent = eccent + 1;
%
%  Find and mark the leaves.
%
    nleaf = 0;

    for i = 1 : nnode

      if ( degree(i) == 1 )
        nleaf = nleaf + 1;
        list(nleaf) = i;
      end

    end
%
%  Delete the leaves.
%
    for ileaf = 1 : nleaf

      i = list(ileaf);

      iedge = 0;
      j = 0;

      while ( 1 )

        iedge = iedge + 1;

        if ( nedge < iedge )
          fprintf ( 1, '\n' );
          fprintf ( 1, 'TREE_ARC_CENTER - Fatal error!\n' );
          fprintf ( 1, '  Data or algorithm failure.\n' );
          error ( 'TREE_ARC_CENTER - Fatal error!' );
        end

        if ( inode(iedge) == i )
          j = jnode(iedge);
          inode(iedge) = - inode(iedge);
          jnode(iedge) = - jnode(iedge);
        elseif ( jnode(iedge) == i )
          j = inode(iedge);
          inode(iedge) = - inode(iedge);
          jnode(iedge) = - jnode(iedge);
        end

        if ( j ~= 0 )
          break
        end

      end

      degree(i) = -1;
      nnode2 = nnode2 - 1;
      degree(j) = degree(j) - 1;
%
%  If the other node has degree 0, we must have just finished
%  stripping all leaves from the tree, leaving a single node.
%  Don't kill it here.  It is our odd center.
%
%     if ( degree(j) == 0 )
%       nnode2 = nnode2 - 1;
%     end

    end
%
%  Find the remaining nodes.
%
    nnode2 = 0;

    for i = 1 : nnode

      if ( 0 <= degree(i) )
        nnode2 = nnode2 + 1;
        list(nnode2) = i;
      end

    end
%
%  If at least 3, more pruning is required.
%
    if ( nnode2 < 3 )
      break
    end

  end
%
%  If only one or two nodes left, we are done.
%
  parity = nnode2;

  center(1:nnode2) = list(1:nnode2);
  inode(1:nedge) = abs ( inode(1:nedge) );
  jnode(1:nedge) = abs ( jnode(1:nedge) );

  return
end


function value = r8poly_value_horner ( m, c, x )

%*****************************************************************************80
%
%% R8POLY_VALUE_HORNER evaluates a polynomial using Horner's method.
%
%  Discussion:
%
%    The polynomial
%
%      p(x) = c0 + c1 * x + c2 * x^2 + ... + cm * x^m
%
%    is to be evaluated at the value X.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 January 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the degree.
%
%    Input, real C(M+1), the polynomial coefficients.
%    C(I) is the coefficient of X^(I-1).
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the polynomial value.
%
  value = c(m+1);
  for i = m : -1 : 1
    value = value * x + c(i);
  end

  return
end


function [ f_rec , X ] = abel_inversion(h,R,upf,plot_results)
% This function calculates a Fourier-based Abel inversion based on the
% method described in [1]. Assuming cylindrical symmetry, the distribution
% function f_rec can be reconstructed from the measured profile h .
%
% The main idea is a Fourier-series-like expansion of the unknown
% distribution function f(r) into
% f(r) = sum_{n=lof}^{upf} (A_n * f_n(r))                               (1)
% where the lower frequency is set to 1 and the upper frequency upf is
% important for noise-filtering.
%
%
% INPUT:  H - dataset to be analyzed (1xN - matrix).
%           For optimum results, H(1) should be the center and H(N) the
%           edge of the investigated object and H(N) should be approx zero.
%           If no input H is given, a sampla dataset will be created.
%         R - radius of given system (i.e. distance between H(1) and H(N))
%         UPF  - upper frequency limit. Defines the number of cosinus
%           expansions (and critically determines computation time).
%           Choosing a very low value (e.g. 4) results in a low-pass
%           filtering effect, reducing noise (but also potential features)
%         PLOT_RESULTS - set to 1 to plot the results
%
% OUTPUT: F_REC - reconstructed density profile
%         X - x-vector containing spatial coordinates for F_REC
%
%
%     [1] G. Pretzler, Z. Naturforsch. 46a, 639 (1991)
%
% See also: COMPUTE_EXPANSION, GENERATE_TEST_DATA, SOLVE_LSQ
%
%                                         written by C. Killer, Sept. 2013

%% format data / generate sample data

% create sample data based on a polynomial distribution function
% if no data input is given
if ~exist('h', 'var') || isempty(h)
    [X,h,R]=generate_test_data;
    plot_results=1;
else
    X=linspace(0,R-0.01,length(h))';
end

% default value for number of expansion elements
if ~exist('upf', 'var'); upf=10; end;

% avoiding problems if this flag is not given in input
if ~exist('plot_results', 'var'); plot_results=0; end;

%% calculate series expansions fn and corresponding integrals hn

[fn,hn] = compute_expansion( X,upf,R );

%% Solving optimization problem

% guess some initial values for optimisation
x0=1*ones(upf+1,1);

% solve for amplitudes A
A = solve_lsq(h,hn,x0);

% create vector for resulting reconstructed distribution
f_rec=zeros(length(h),1);

% special case for n=0 (where f_0(r) = 1)
f_rec = f_rec + A(1)*1;

% iterate eq. (1) for n=1:upf
for c=2:length(x0)
    f_rec = f_rec + A(c).*fn(:,c);
end

if plot_results==1
    figure; % normalized profiles for better comparison
    set(gca,'linewidth',1.5,'fontsize',16)
    plot(X,f_rec./max(f_rec),'k','Linewidth',1.5);
    hold on; plot(X,h./max(h),'b','Linewidth',1.5);
    grid on; box on;
    title(sprintf('number of cos-expansions: %i',upf))
    legend('reconstructed distribution','measured profile','Location','SouthWest')
end


function p = circle_imp_points_arc_2d ( r, center, theta1, theta2, n )

%*****************************************************************************80
%
%% CIRCLE_IMP_POINTS_ARC_2D returns N points on an arc of an implicit circle in 2D.
%
%  Discussion:
%
%    The first point is
%      ( CENTER(1) + R * COS ( THETA1 ), CENTER(2) + R * SIN ( THETA1 ) );
%    The last point is
%      ( CENTER(1) + R * COS ( THETA2 ), CENTER(2) + R * SIN ( THETA2 ) );
%    and the intermediate points are evenly spaced in angle between these,
%    and in counterclockwise order.
%
%    An implicit circle in 2D satisfies the equation:
%
%      ( X - CENTER(1) )^2 + ( Y - CENTER(2) )^2 = R^2
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real R, the radius of the circle.
%
%    Input, real CENTER(2,1), the center of the circle.
%
%    Input, real THETA1, THETA2, the angular coordinates of
%    the first and last points to be drawn, in radians.
%
%    Input, integer N, the number of points desired.  N must be at least 1.
%
%    Output, real P(2,N), the points on the circle.
%

%
%  THETA3 is the smallest angle, no less than THETA1, which
%  coincides with THETA2.
%
  theta3 = theta1 + r8_modp ( theta2 - theta1, 2.0 * pi );

  for i = 1 : n

    if ( 1 < n )
      theta = ( ( n - i     ) * theta1   ...
              + (     i - 1 ) * theta3 ) ...
              / ( n     - 1 );
    else
      theta = 0.5 * ( theta1 + theta3 );
    end

    p(1,i) = center(1,1) + r * cos ( theta );
    p(2,i) = center(2,1) + r * sin ( theta );

  end

  return
end


function [vector] = createUnitVector(vector)
% Normalize a [1xn] or [nx1] vector
%Description: This function will simplify the process of creating a unit
%vector in one direction by dividing a vector by its length.
%Use: [vectorOut] = fcn_createUnitVector(vectorIn[1xn])
%  or [vectorOut] = fcn_createUnitVector(vectorIn[mx1])

%Author: Jim West
%Date: 7/12/2010
%Updated 8/31/2010
%Made a heavy revision to this script based on feedback from John D'Errico
[r c] = size(vector);

%Convert the vector to columns
if (r > c && c == 1);
    vector = vector';
elseif (r < c && r==1);
    %do Nothing
else
    error('Input vector must be [mx1] or [1xn]');
end

%Process the unit transform
vector = vector./norm(vector);

%Check if the result is illogical, or has an error
if not(sqrt(sum(vector.^2)) < 1.00001) && not(sqrt(sum(vector.^2)) > 0.99999)
    error(['There has been a calculation error, because the square root of the sum of the squares does not equal 1']);
end

%Check if the result is NaN
if sum(isnan(vector)) >= 1
    error(['The vector is ill-conditioned or dividing by zero with a result of ', num2str(vector)]);
end

%If originally in rows, convert back.
if r > c; vector = vector'; end


function ndvi = NDVI( red,nir )
%NDVI Calculate Normalized Difference Vegetation Index (NDVI) using NIR and
%     Red bands.
%
% Syntax
%
%     ndvi = NDVI(red,nir)
%
% Description
%
%     This function calculates Normalized Difference Vegetation Index (NDVI)
%     using NIR and Red bands (as following equation). This range is between
%     -1 and 1.
%     NDVI=(NIR-Red)/(NIR+Red).
%
% Input arguments
%
%     red         Red band
%     nir         Near-infrared band
%
% Output arguments
%
%     ndvi        Normalized Difference Vegetation Index
%
%
% Author: Shi Qiu (shi.qiu@ttu.edu)
% Date: 19. October, 2017

    % calculate NDVI
    ndvi=(nir-red)./(nir+red);

    % fix unnormal pixels
    ndvi((nir+red)==0)=0.01;

end


%SIGM Sigmoid map
%
%   W = W*SIGM(SCALE)
%
%   B = SIGM(A,SCALE)
%   B = A*SIGM([],SCALE)
%   B = A*SIGM(SCALE)
%
% INPUT
%   A        Dataset (optional)
%   SCALE    Scaling parameter (optional, default: 1)
%
% OUTPUT
%   W        Sigmoid mapping, or
%   B        Dataset A mapped by sigmoid mapping
%
% DESCRIPTION
% Sigmoidal transformation, useful to transform a map to classifier,
% producing posterior probability estimates. The parameter SCALE scales the
% data first (A/SCALE), before the transformation. Default: SCALE = 1, i.e.
% no scaling.
%
% SEE ALSO (PRTools Guide)
% DATASETS, MAPPINGS, CLASSC

% Copyright: R.P.W. Duin, duin@ph.tn.tudelft.nl
% Faculty of Applied Sciences, Delft University of Technology
% P.O. Box 5046, 2600 GA Delft, The Netherlands

function out = sigm (varargin)

  argin = shiftargin(varargin,'scalar');
  argin = setdefaults(argin,[],1);

  if mapping_task(argin,'definition')
    out = define_mapping(argin,'fixed','Sigmoidal Mapping');

  else			% Evaluate

    [a,scale] = deal(argin{:});
		out = 1./(1+exp(-a/scale));

	end

	return


classdef snr
    %SNR helper functions for calculating SNR

    properties
    end

    methods(Static)
        function result = SNR(signal, noise)
            sigEnergy = sum(signal .* conj(signal));
            noiseEnergy = sum(noise .* conj(noise));
            result=10*log10(sigEnergy./noiseEnergy);
        end
        function result = recSNRdB(signal, reconstruction)
            result=20*log10(norm(signal)/norm(signal-reconstruction));
        end

        function result = energyDB(signal)
            result = 20*log10(norm(signal));
        end
        function result = recSNR(signal, reconstruction)
            noise = (signal - reconstruction);
            result = (signal' * signal) / (noise'*noise);
        end
        function result = normalizedErrorEnergy(signal, reconstruction)
            noise = (signal - reconstruction);
            result = (noise' * noise) / (signal'*signal);
        end

        function result = recSNRsdB(signals, reconstructions)
            ratios = spx.snr.recSNRs(signals, reconstructions);
            result= 10 * log10(ratios);
        end

        function result = recSNRs(signals, reconstructions)
            sigEngergies =  spx.snr.energies(signals);
            noiseEnergies =  spx.snr.energies(signals - reconstructions);
            result = sigEngergies ./ noiseEnergies;
        end

        function result = energies(signals)
            signals = abs(signals);
            signals = signals .^2;
            result = sum(signals, 1);
        end

        function result = energiesDB(signals)
            energies = spx.snr.energies(signals);
            result = 10* log10(energies);
        end
    end

end


function [Az El] = RaDec2AzEl(Ra,Dec,lat,lon,time)
% Programed by Darin C. Koblick 01/23/2010
%--------------------------------------------------------------------------
% External Function Call Sequence:
% [Az El] = RaDec2AzEl(0,0,0,-104,'1992/08/20 12:14:00')
%
% Worked Example: pg. 262 Vallado
%[Az El] = RaDec2AzEl(294.9891115,-20.8235624,39.007,-104.883,'1994/05/14 13:11:20.59856')
%[210.7514  23.9036] = RaDec2AzEl(294.9891115,-20.8235624,39.007,-104.883,'1994/05/14 13:11:20.59856')
%
% Worked Example: http://www.stargazing.net/kepler/altaz.html
% [Az El] = RaDec2AzEl(344.95,42.71667,52.5,-1.91667,'1997/03/14 19:00:00')
% [311.92258 22.40100] = RaDec2AzEl(344.95,42.71667,52.5,-1.91667,'1997/03/14 19:00:00')
%
% [Beta,el] = RaDec2AzEl(alpha_t,delta_t,phi,lamda,'yyyy/mm/dd hh:mm:ss')
%
% Function Description:
%--------------------------------------------------------------------------
% RaDec2AzEl will take the Right Ascension and Declination in the topocentric
% reference frame, site latitude and longitude as well as a time in GMT
% and output the Azimuth and Elevation in the local horizon
% reference frame.
%
% Inputs:                                                       Format:
%--------------------------------------------------------------------------
% Topocentric Right Ascension (Degrees)                         [N x 1]
% Topocentric Declination Angle (Degrees)                       [N x 1]
% Lat (Site Latitude in degrees -90:90 -> S(-) N(+))            [N x 1]
% Lon (Site Longitude in degrees -180:180 W(-) E(+))            [N x 1]
% UTC (Coordinated Universal Time YYYY/MM/DD hh:mm:ss)          [N x 1]
%
% Outputs:                                                      Format:
%--------------------------------------------------------------------------
% Local Azimuth Angle   (degrees)                               [N x 1]
% Local Elevation Angle (degrees)                               [N x 1]
%
%
% External Source References:
% Fundamentals of Astrodynamics and Applications
% D. Vallado, Second Edition
% Example 3-5. Finding Local Siderial Time (pg. 192)
% Algorithm 28: AzElToRaDec (pg. 259)
% -------------------------------------------------------------------------

%Example 3-5
[yyyy mm dd HH MM SS] = datevec(datenum(time,'yyyy/mm/dd HH:MM:SS'));
JD = juliandate(yyyy,mm,dd,HH,MM,SS);
T_UT1 = (JD-2451545)./36525;
ThetaGMST = 67310.54841 + (876600*3600 + 8640184.812866).*T_UT1 ...
+ .093104.*(T_UT1.^2) - (6.2*10^-6).*(T_UT1.^3);
ThetaGMST = mod((mod(ThetaGMST,86400*(ThetaGMST./abs(ThetaGMST)))/240),360);
ThetaLST = ThetaGMST + lon;

%Equation 4-11 (Define Siderial Time LHA)
LHA = mod(ThetaLST - Ra,360);

%Equation 4-12 (Elevation Deg)
El = asind(sind(lat).*sind(Dec)+cosd(lat).*cosd(Dec).*cosd(LHA));

%Equation 4-13 / 4-14 (Adaptation) (Azimuth Deg)
%Az = mod(atand(-(sind(LHA).*cosd(Dec)./(cosd(lat).*sind(Dec) - sind(lat).*cosd(Dec).*cosd(LHA)))),360);
Az = mod(atan2(-sind(LHA).*cosd(Dec)./cosd(El),...
    (sind(Dec)-sind(El).*sind(lat))./(cosd(El).*cosd(lat))).*(180/pi),360);

function jd = juliandate(year, month, day, hour, min, sec)
YearDur = 365.25;
for i = length(month):-1:1
    if (month(i)<=2)
        year(i)=year(i)-1;
        month(i)=month(i)+12;
    end
end
A = floor(YearDur*(year+4716));
B = floor(30.6001*(month+1));
C = 2;
D = floor(year/100);
E = floor(floor(year/100)*.25);
F = day-1524.5;
G = (hour+(min/60)+sec/3600)/24;
jd =A+B+C-D+E+F+G;


function dw2dx2 = r8poly_lagrange_2 ( npol, xpol, xval )

%*****************************************************************************80
%
%% R8POLY_LAGRANGE_2 evaluates the second derivative of the Lagrange factor.
%
%  Formula:
%
%    W(X)  = Product ( 1 <= I <= NPOL ) ( X - XPOL(I) )
%
%    W'(X) = Sum ( 1 <= J <= NPOL )
%            Product ( I /= J ) ( X - XPOL(I) )
%
%    W"(X) = Sum ( 1 <= K <= NPOL )
%            Sum ( J =/ K )
%            Product ( I /= K, J ) ( X - XPOL(I) )
%
%    For a set of points XPOL(I), 1 <= I <= NPOL, the IPOL-th Lagrange basis
%    polynomial L(IPOL)(X), has the property:
%
%      L(IPOL)( XPOL(J) ) = delta ( IPOL, J )
%
%    and may be expressed as:
%
%      L(IPOL)(X) = W(X) / ( ( X - XPOL(IPOL) ) * W'(XPOL(IPOL)) )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 January 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NPOL, the number of abscissas.
%    NPOL must be at least 1.
%
%    Input, real XPOL(NPOL), the abscissas, which should be distinct.
%
%    Input, real XVAL, the point at which the Lagrange factor is to be
%    evaluated.
%
%    Output, real DW2DX2, the second derivative of W with respect to XVAL.
%
  dw2dx2 = 0.0;

  for k = 1 : npol

    for j = 1 : npol

      if ( j ~= k )
        term = 1.0;

        for i = 1: npol
          if ( i ~= j && i ~= k )
            term = term * ( xval - xpol(i) );
          end
        end

        dw2dx2 = dw2dx2 + term;

      end

    end

  end

  return
end


function IMs = slinv2x2(Ms)
%SLINV2X2 Computes inverse matrices for 2 x 2 matrices in a fast way
%
% $ Syntax $
%   - IMs = slinv2x2(Ms)
%
% $ Arguments $
%   - Ms:       the 2 x 2 matrix (matrices)
%   - IMs:      the computed inverse matrix (matrices)
%
% $ Description $
%   - IMs = slinv2x2(Ms) computes the inverse of 2x2 matrices. If Ms is
%     a 2 x 2 matrix, then r is its inverse.
%     Or, if Ms is a 2 x 2 x ... array, then r would be a 2 x 2 x ... array
%     storing the inverse matrices.
%
% $ Remarks $
%   - The function uses the following formula for fast calculation of
%     inverse of 2 x 2 matrices:
%       inv(A) = [a22, -a12; -a21, a11] / det(A)
%
% $ History $
%   - Created by Dahua Lin on Apr 22nd, 2006
%

%% parse and verify input arguments

if size(Ms, 1) ~= 2 || size(Ms, 2) ~= 2
    error('sltoolbox:invalidarg', 'Ms should be set of 2 x 2 matrices');
end

%% compute

if ndims(Ms) == 2       % single matrix
    IMs = [Ms(4), -Ms(2); -Ms(3), Ms(1)] / (Ms(1) * Ms(4) - Ms(2) * Ms(3));

else                    % a set of matrices

    IMs = zeros(size(Ms));

    % compute adjacent matrix
    IMs(1,1,:) = Ms(2,2,:);
    IMs(1,2,:) = -Ms(1,2,:);
    IMs(2,1,:) = -Ms(2,1,:);
    IMs(2,2,:) = Ms(1,1,:);

    % compute the determinants
    d = sldet2x2(Ms);

    % scale
    d = reshape(d, [1, 1, size(d)]);
    IMs = slmul(IMs, 1 ./ d);

end


% KSVD running file
% in this file a synthetic test of the K-SVD algorithm is performed. First,
% a random dictionary with normalized columns is being generated, and then
% a set of data signals, each as a linear combination of 3 dictionary
% element is created, with noise level of 20SNR. this set is given as input
% to the K-SVD algorithm.

% a different mode for activating the K-SVD algorithm is until a fixed
% error is reached in the Sparse coding stage, instead until a fixed number of coefficients is found
% (it was used by us for the
% denoising experiments). in order to switch between those two modes just
% change the param.errorFlag (0 - for fixed number of coefficients, 1 -
% until a certain error is reached).

param.L = 3;   % number of elements in each linear combination.
param.K = 50; % number of dictionary elements
param.numIteration = 50; % number of iteration to execute the K-SVD algorithm.

param.errorFlag = 0; % decompose signals until a certain error is reached. do not use fix number of coefficients.
%param.errorGoal = sigma;
param.preserveDCAtom = 0;

%%%%%%% creating the data to train on %%%%%%%%
N = 1500; % number of signals to generate
n = 20;   % dimension of each data
SNRdB = 20; % level of noise to be added
[param.TrueDictionary, D, x] = gererateSyntheticDictionaryAndData(N, param.L, n, param.K, SNRdB);
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%% initial dictionary: Dictionary elements %%%%%%%%
param.InitializationMethod =  'DataElements';

param.displayProgress = 1;
disp('Starting to  train the dictionary');

[Dictionary,output]  = KSVD(D,param);

disp(['The KSVD algorithm retrived ',num2str(output.ratio(end)),' atoms from the original dictionary']);

[Dictionary,output]  = MOD(D,param);

disp(['The MOD algorithm retrived ',num2str(output.ratio(end)),' atoms from the original dictionary']);


%CHERNOFFM Suboptimal discrimination linear mapping (Chernoff mapping)
%
%	W = CHERNOFFM(A,N,R)
%
% INPUT
%	A   Dataset
%	N   Number of dimensions to map to, N < C, where C is the number of classes
%	    (default: min(C,K)-1, where K is the number of features in A)
%	R   Regularization variable, 0 <= r <= 1, default is r = 0, for r = 1 the
%	    Chernoff mapping is (should be) equal to the Fisher mapping
%
% OUTPUT
% 	W   Chernoff mapping
%
% DESCRIPTION
% Finds a mapping of the labeled dataset A onto an N-dimensional linear
% subspace such that it maximizes the heteroscedastic Chernoff criterion
% (also called the Chernoff mapping).
%
% REFERENCE
% M. Loog and R.P.W. Duin, Linear Dimensionality Reduction via a Heteroscedastic
% Extension of LDA: The Chernoff Criterion, IEEE Transactions on pattern
% analysis and machine intelligence, vol. PAMI-26, no. 6, 2004, 732-739.
%
% SEE ALSO (PRTools Guide)
% MAPPINGS, DATASETS, FISHERM, NLFISHERM, KLM, PCA

% Copyright: M. Loog, marco@isi.uu.nl
% Image Sciences Institute, University Medical Center Utrecht
% P.O. Box 85500, 3508 GA Utrecht, The Netherlands

% $Id: chernoffm.m,v 1.3 2010/02/08 15:31:48 duin Exp $

function W = chernoffm(a,n,r)

		if (nargin < 3)
        r = 0;
	end

	if (nargin < 2)
		prwarning (4, 'number of dimensions to map to not specified, assuming min(C,K)-1');
		n = [];
	end

	% If no arguments are specified, return an untrained mapping.

	if (nargin < 1) | (isempty(a))
		W = prmapping('chernoffm',{n,r});
		W = setname(W,'Chernoff mapping');
		return
	end

	isvaldset(a,2,2); % at least 2 objects per class, 2 classes

	% If N is not given, set it.

	[m,k,c] = getsize(a); if (isempty(n)), n = min(k,c)-1; end
	if (n >= m)
		error('The dataset is too small for the requested dimensionality')
	end

	% To simplify computations, the within scatter W is set to the identity matrix
    % therefore A is centered and sphered by KLMS.

	v = klms(a); a = a*v;
	k = size(v,2); % dimensionalities may have changed for small training sets

	% Now determine a solution to the Chernoff criterion

	[U,G] = meancov(a);

	CHERNOFF = zeros(k);
    p = getprior(a);

    for i = 1:c-1 % loop over all class pairs
        for j = i+1:c
            p_i = p(i)/(p(i)+p(j)); p_j = p(j)/(p(i)+p(j));
            G_i = (1-r)*G(:,:,i) + r*eye(k); G_j = (1-r)*G(:,:,j) + r*eye(k);
            G_ij = p_i*G_i + p_j*G_j;
            m_ij = sqrtm(real(prinv(G_ij)))*(U(i,:) - U(j,:))';
            CHERNOFF = CHERNOFF + p(i) * p(j) * ...
                ( m_ij * m_ij' ...
                + 1/(p_i*p_j) * ( logm(G_ij) ...
                - p_i * logm(G_i) - p_j * logm(G_j) ) );
        end
    end

	[F,V] = preig(CHERNOFF);
	[dummy,I] = sort(-diag(V));
	I = I(1:n);
	rot = F(:,I);
	off = -mean(a*F(:,I));
	preW = affine(rot,off,a);
	W = v*preW;					% Construct final mapping.
	W = setname(W,'Chernoff mapping');

return


function K = levelsetCurvature(phi,mode)
%levelsetCurvature Computes the curvature of a level set function.
%   K = levelsetCurvature(PHI,MODE) computes the curvature, K, of level
%   set function PHI. If MODE = 'Cu' the curvature is not normalized
%   (this is the default). If MODE = 'Cn', the curvature is normalized
%   by dividing it by its maximum value. This function is based on Eqs.
%   (12-61) and (12-62) of DIPUM3E.
%
%   Copyright 2002-2020 Gatesmark
%
%   This function, and other functions in the DIPUM Toolbox, are based
%   on the theoretical and practical foundations established in the
%   book Digital Image Processing Using MATLAB, 3rd ed., Gatesmark
%   Press, 2020.
%
%   Book website: http://www.imageprocessingplace.com
%   License: https://github.com/dipum/dipum-toolbox/blob/master/LICENSE.txt

% PRELIMINARIES
% Set default.
if nargin == 1
    mode = 'Cu';
end
[M,N] = size(phi);
phi = double(phi);

% PAD WITH 1'S TO BE ABLE TO COMPUTE THE DERIVATIVE AT IMAGE BORDERS.
phip = padarray(phi,[1 1],1,'both');

% COMPUTE THE CENTRAL DIFFERENCES.
phix = 0.5*(phip(3:end,2:N+1) - phip(1:M,2:N+1));
phiy = 0.5*(phip(2:M+1,3:end) - phip(2:M+1,1:N));
phixx = phip(3:end,2:N+1) + phip(1:M,2:N+1) - 2*phi;
phiyy = phip(2:M+1,3:end) + phip(2:M+1,1:N) - 2*phi;
phixy = 0.25.*(phip(3:end,3:end) - phip(1:M,3:end)...
                            - phip(3:end,1:N) + phip(1:M,1:N));
% COMPUTE THE CURVATURE.
DEN = (phix.^2 + phiy.^2 + eps).^1.5;
K = (phixx.*(phiy.^2) - 2*(phix.*phiy.*phixy) + phiyy.*(phix.^2))./DEN;

% CHECK FOR NORMALIZATION.
if isequal(mode,'Cn')
    K = K/max(abs(K(:)));
end

% COMPENSATE FOR BORDER EFFECTS BY SETTING THE FIRST AND LAST ROWS AND
% COLUMNS EQUAL TO THEIR IMMEDIATE PREDECESSORS.
% CURVATURE
% Rows.
K(:,1) = K(:,2);
K(:,end) = K(:,end-1);
% Columns.
K(1,:) = K(2,:);
K(end,:) = K(end-1,:);


function result = polygon_1 ( n, v )

%*****************************************************************************80
%
%% POLYGON_INTEGRAL_1 integrates the function 1 over a polygon.
%
%  Discussion
%
%    The polygon is bounded by the points (X(1:N), Y(1:N)).
%
%    INTEGRAL = 0.5 * SUM ( 1 <= I <= N )
%      ( X(I) + X(I-1) ) * ( Y(I) - Y(I-1) )
%
%    where X(0) and Y(0) should be replaced by X(N) and Y(N).
%
%    Note that the integral of 1 over a polygon is the area of the polygon.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    S F Bockman,
%    Generalizing the Formula for Areas of Polygons to Moments,
%    American Mathematical Society Monthly,
%    1989, pages 131-132.
%
%  Parameters:
%
%    Input, integer N, the number of vertices of the polygon.
%    N should be at least 3 for a nonzero result.
%
%    Input, real V(2,N), the coordinates of the vertices
%    of the polygon.  These vertices should be given in counter-clockwise order.
%
%    Output, real RESULT, the value of the integral.
%
  result = 0.0;

  if ( n < 3 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'POLYGON_INTEGRAL_1 - Warning!\n' );
    fprintf ( 1, '  The number of vertices must be at least 3.\n' );
    fprintf ( 1, '  The input value of N = %d\n', n );
    error ( 'POLYGON_INTEGRAL_1 - Fatal error!' );
  end

  for i = 1 : n

    if ( i == 1 )
      im1 = n;
    else
      im1 = i - 1;
    end

    result = result + 0.5 * ( v(1,i) + v(1,im1) ) * ( v(2,i) - v(2,im1) );

  end

  return
end


clear();

CONVOLUTION_SHAPE_FULL  = 1;
CONVOLUTION_SHAPE_SAME  = 2;
CONVOLUTION_SHAPE_VALID = 3;

maxThr = 1e-9;

tic();
for numElementsSignal = 8:21

    vI = rand(numElementsSignal, 1);

    for numElementsKernel = 1:7

        vH = rand(numElementsKernel, 1);

        for convShape = 1:3

            switch(convShape)
                case(CONVOLUTION_SHAPE_FULL)
                    numElementsOut  = numElementsSignal + numElementsKernel - 1;
                    convShapeString = 'full';
                case(CONVOLUTION_SHAPE_SAME)
                    numElementsOut  = numElementsSignal;
                    convShapeString = 'same';
                case(CONVOLUTION_SHAPE_VALID)
                    numElementsOut  = numElementsSignal - numElementsKernel + 1;
                    convShapeString = 'valid';
            end

            vORef   = conv2(vI, vH, convShapeString);
            mK      = GenerateToeplitzConvMatrixEx(vH, numElementsSignal, convShape);
            % mKK      = full(CreateConvMtx1D(vH, numElementsSignal, convShape));
            vO      = reshape(mK * vI, numElementsOut, 1);

            disp([' ']);
            disp(['Validating solution for the following parameters:']);
            disp(['Signal Size - [', num2str(numElementsSignal), ' x 1]']);
            disp(['Kernel Size - [', num2str(numElementsKernel), ' x 1]']);
            disp(['Convolution Shape - ', convShapeString]);

            vE = vO - vORef;
            maxAbsDev = max(abs(vE(:)));
            if(maxAbsDev >= maxThr)
                disp([' ']);
                disp(['Validation Failed']);
                disp([' ']);
            end
            assert(maxAbsDev < maxThr);

        end
    end
end

toc();


%VEX Convert skew-symmetric matrix to vector
%
% V = VEX(S) is the vector which has the corresponding skew-symmetric
% matrix S.
%
% In the case that S (2x2) =
%
%           | 0  -v |
%           | v   0 |
%
% then V = [v].  In the case that S (3x3) =
%
%           |  0  -vz   vy |
%           | vz    0  -vx |
%           |-vy   vx    0 |
%
% then V = [vx; vy; vz].
%
% Notes::
% - This is the inverse of the function SKEW().
% - Only rudimentary checking (zero diagonal) is done to ensure that the
%   matrix is actually skew-symmetric.
% - The function takes the mean of the two elements that correspond to
%   each unique element of the matrix.
% - The matrices are the generator matrices for so(2) and so(3).
%
% References::
% - Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p25+43.
%
% See also SKEW, VEXA.

% Copyright (C) 1993-2019 Peter I. Corke
%
% This file is part of The Spatial Math Toolbox for MATLAB (SMTB).
%
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, including without limitation the rights
% to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
% of the Software, and to permit persons to whom the Software is furnished to do
% so, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in all
% copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
% FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
% COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
% IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
% CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
%
% https://github.com/petercorke/spatial-math

function v = vex(S)
%     if trace(abs(S)) > 10*eps
%         error('SMTB:vex:badarg', 'argument is not skew symmetric tr=%g', trace(abs(S)));
%     end
    if all(size(S) == [3 3])
        v = 0.5*[S(3,2)-S(2,3); S(1,3)-S(3,1); S(2,1)-S(1,2)];
    elseif all(size(S) == [2 2])
        v = 0.5*(S(2,1)-S(1,2));
    else
        error('SMTB:vex:badarg', 'argument must be a 2x2 or 3x3 matrix');
    end