function [ nxy, bxy, fxy ] = pce_legendre_linear_assemble ( n, p )

%*****************************************************************************80
%
%% PCE_LEGENDRE_LINEAR_ASSEMBLE assembles a particular stochastic Galerkin matrix.
%
%  Discussion:
%
%    We wish to analyze a stochastic PDE of the form:
%
%      -div A(X,Y) grad U(X,Y) = F(X)
%
%    where
%
%      U is an unknown scalar function,
%      A is a given diffusion function,
%      F is a given right hand side function,
%      X represents dependence on spatial variables,
%      Y represents dependence on stochastic variables.
%
%    We let X be a space of finite element functions generated by piecewise linear
%    functions associated with a particular triangular dissection of the unit square.
%
%    We let Y be the space of polynomials over R^N with total degree at most P.
%
%    We seek solutions U in X cross Y using a polynomial chaos expansion approach.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 May 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of factors in the probability space.
%
%    Input, integer P, the maximum degree of the basis functions for the
%    probability space.
%
%    Output, integer NXY, the order of the matrix BXY.  NXY = NX * NY.
%
%    Output, real sparse BXY(NXY,NXY), the matrix.
%
%    Output, real FXY(NXY), the right hand side.
%
%  Local Parameters:
%
%    Local, real AREA_X, the area of the current element.
%
%    Local, integer BASIS_X_NUM, the number of finite element basis functions (3).
%
%    Local, integer BASIS_Y_DEGREE(1:N), describes a particular basis function
%    in Y, in terms of the degrees of its components.
%
%    Local, integer BASIS_Y_H(*), auxilliary "external" memory needed by
%    SUBCOMP_NEXT.
%
%    Local, logical BASIS_Y_MORE, is set FALSE before the first call to
%    SUBCOMP_NEXT.  SUBCOMP_NEXT returns the next subcomposition, and sets the
%    value of BASIS_Y_MORE to TRUE if there are even more subpositions that can
%    be produced, or FALSE if there are no more.
%
%    Local, integer BASIS_Y_T(*), auxilliary "external" memory needed by
%    SUBCOMP_NEXT.
%
%    Local, real BVEC(1:N,1:POINT_X_NUM), stores the KL expansion coefficients
%    of orders 1 through N at the spatial points POINT_X_P(1:2,1:POINT_X_NUM).
%
%    Local, real BZERO(1:POINT_X_NUM), stores the KL expansion coefficients
%    of order 0 at the spatial points POINT_X_P(1:2,1:POINT_X_NUM).
%
%    Local, real CLK(1:QUAD_X_NUM), contains the integral of the KL expansion
%    function for given values of the L-th probability basis function and the
%    K-th probability test function evaluated at the quadrature points in an
%    element.
%
%    Local, integer ELEMENT_X, the element being considered.
%
%    Local, integer ELEMENT_X_NUM, the number of elements in the finite element
%    problem.
%
%    Local, integer ELEMENT_X_NODE(1:3,1:ELEMENT_X_NUM);
%    ELEMENT_X_NODE(I,J) is the global index of local node I in element J.
%
%    Local, integer ELEMENT_X1_NUM, the number of (pairs) of elements in the
%    first spatial dimension in the finite element problem.
%
%    Local, integer ELEMENT_X2_NUM, the number of (pairs) of elements in the
%    second spatial dimension in the finite element problem.
%
%    Local, real FVEC(1:QUAD_X_NUM), stores the values of the function F which
%    is the right hand side of the PDE, at the quadrature points in an element.
%
%    Local, integer I, the index of the finite element test function
%    PHI(1:NX)(X).
%
%    Local, integer J, the index of the finite element basis function
%    PHI(1:NX)(X).
%
%    Local, integer K, the index of the probability space test function
%    PSI(1:NY)(Y).
%
%    Local, integer L, the index of the probability space basis function
%    PSI(1:NY)(Y).
%
%    Local, integer NODE_X_NUM, the number of nodes.
%
%    Local, real NODE_X_P(1:2,1:NODE_X_NUM), the coordinates of nodes.
%
%    Local, integer NODE_X1_NUM, the number of nodes in the first space
%    direction.
%
%    Local, integer NODE_X2_NUM, the number of nodes in the second space
%    direction.
%
%    Local, integer NX, the dimension of the finite element space.
%    NX = NODE_X_NUM for a finite element problem involving a scalar variable U.
%
%    Local, integer NY, = ( N + P)! / N! / P! = the dimension of the space Y,
%    the space of polynomials over R^N of total degree at most P.
%
%    Local, real PHI(1:BASIS_X_NUM,1:QUAD_X_NUM), the finite element basis
%    functions, evaluated at all the quadrature points in a particular element.
%
%    Local, real PHI_DX1(1:BASIS_X_NUM,1:QUAD_X_NUM), the derivative of the
%    finite element basis functions with respect to the first spatial variable,
%    evaluated at all the quadrature points in a particular element.
%
%    Local, real PHI_DX2(1:BASIS_X_NUM,1:QUAD_X_NUM), the derivative of the
%    finite element basis functions with respect to the second spatial variable,
%    evaluated at all the quadrature points in a particular element.
%
%    Local, real PHYS_X_P(1:2,1:QUAD_X_NUM), the "physical" coordinates of the
%    quadrature points in the current element.
%
%    Local, integer QUAD_X, the index of the current X quadrature point.
%
%    Local, integer QUAD_X_NUM, the order of the X quadrature rule.
%
%    Local, real QUAD_X_P(1:2,1:QUAD_X_NUM), the points for the X quadrature
%    rule.
%
%    Local, real QUAD_X_W(1:QUAD_X_NUM), the weights for the X quadrature rule.
%
%    Local, real T3(1:2,1:3), the coordinates of the nodes that define the
%    current element.
%
%    Local, real TABLE(1:P+1,1:P+1), a table of the values of integrals of the
%    1D basis functions in Z.  TABLE(D1+1,D2+1)
%    = Integral ( -1 <= Z <= +1 ) Z * PSI(D1)(Z) * PSI(D2)(Z) dZ.
%
%    Local, integer TEST_X_NUM, the number of finite element test functions (3).
%
%    Local, integer TEST_Y_DEGREE(1:N), describes a particular test function in
%    Y, in terms of the degrees of its components.
%
%    Local, integer TEST_Y_H(*), auxilliary "external" memory needed by
%    SUBCOMP_NEXT.
%
%    Local, logical TEST_Y_MORE, is set FALSE before the first call to
%    SUBCOMP_NEXT.  SUBCOMP_NEXT returns the next subcomposition, and sets the
%    value of TESST_Y_MORE to TRUE if there are even more subpositions that can
%    be produced, or FALSE if there are no more.
%
%    Local, integer TEST_Y_T(*), auxilliary "external" memory needed by
%    SUBCOMP_NEXT.
%
  FALSE = 0;
%
%  Some setup for the finite element calculation.
%
  basis_x_num = 3;
  test_x_num = 3;
  node_x1_num = 11;
  node_x2_num = 11;
  node_x_num = node_x1_num * node_x2_num;
  nx = node_x_num;

  node_x_p = grid_nodes_01 ( node_x1_num, node_x2_num );

  element_x1_num = node_x1_num - 1;
  element_x2_num = node_x2_num - 1;
  element_x_num = 2 * element_x1_num * element_x2_num;
  element_x_node = grid_t3_element ( element_x1_num, element_x2_num );

  quad_x_num = 13;
  [ quad_x_p, quad_x_w ] = triangle_rule_13 ( );
%
%  Some setup for the probability space.
%
  ny = i4_choose ( n + p, n );

  e = 1;
  table = legendre_linear_product ( p, e );

  for i = 1 : p + 1
    for j = 1 : p + 1
      if ( abs ( table(i,j) ) < 10000 * eps )
        table(i,j) = 0.0;
      end
    end
  end
%
%  Define the order of the matrix BXY,
%  Define BXY as a sparse matrix;
%  Define FXY as a column vector.
%
  nxy = nx * ny;
  bxy = sparse ( [], [], [], nxy, nxy );
  fxy(1:nxy,1) = 0.0;
%
%  LOOP L:
%  Generate the L-th basis function PSI(D,Y) in Y space.
%
  basis_y_degree(1:n) = 0;
  basis_y_more = FALSE;
  basis_y_h = [];
  basis_y_t = [];
  basis_y_n2 = [];
  basis_y_more2 = [];
  l = 0;

  while ( 1 )

    l = l + 1;

    [ basis_y_degree, basis_y_more, basis_y_h, basis_y_t, basis_y_n2, basis_y_more2 ] = ...
      subcomp_next ( p, n, basis_y_degree, basis_y_more, basis_y_h, basis_y_t, basis_y_n2, ...
      basis_y_more2 );
%
%  LOOP K:
%  Generate the K-th test function PSI(D,Y) in Y space.
%
    test_y_degree(1:n) = 0;
    test_y_more = FALSE;
    test_y_h = [];
    test_y_t = [];
    test_y_n2 = [];
    test_y_more2 = [];
    k = 0;

    while ( 1 )

      k = k + 1;

      [ test_y_degree, test_y_more, test_y_h, test_y_t, test_y_n2, test_y_more2 ] = ...
        subcomp_next ( p, n, test_y_degree, test_y_more, test_y_h, test_y_t, test_y_n2, ...
        test_y_more2 );

      for element_x = 1 : element_x_num
%
%  For ALL the quadrature points in this element, evaluate:
%  * PHI the basis functions;
%  * PHI_DX1 and PHI_DX2, basis function derivatives;
%  * PHYS_X_P, the physical coordinates of the quadrature points;
%  * BZERO and BVEC, the coefficient functions in the KL expansion for CLK;
%  * CLK, the KL function integrated against the probability basis and test functions;
%  * FVEC, the right hand side of the PDE.
%
        t3(1:2,1:3) = node_x_p(1:2,element_x_node(1:3,element_x));

        area_x = abs ( triangle_area_2d ( t3 ) );

        [ phi, phi_dx1, phi_dx2 ] = basis_mn_t3 ( t3, quad_x_num, quad_x_p );

        phys_x_p(1:2,1:quad_x_num) = reference_to_physical_t3 ( t3, quad_x_num, quad_x_p );

        [ bzero, bvec ] = b_evaluator ( n, quad_x_num, phys_x_p );

        if ( l == k )
          clk(1:quad_x_num) = bzero(1:quad_x_num);
        else
          clk(1:quad_x_num) = 0.0;
        end

        for i2 = 1 : n

          factor = 1.0;
          for i3 = 1 : n
            if ( i3 ~= i2 )
              if ( basis_y_degree(i3) ~= test_y_degree(i3) )
                factor = 0.0;
              end
            end
          end

          clk(1:quad_x_num) = clk(1:quad_x_num) ...
            + bvec(i2,1:quad_x_num) * table(basis_y_degree(i2)+1,test_y_degree(i2)+1)...
            * factor;

        end

        fvec = f_evaluator ( quad_x_num, phys_x_p );
%
%  Integrate over the finite element space X.
%
        for quad_x = 1 : quad_x_num
%
%  LOOP J:
%  All finite element basis functions J.
%  But we actually only look at those which are nonzero in this element.
%
          for basis_x = 1 : basis_x_num

            j = element_x_node(basis_x,element_x);
%
%  LOOP I:
%  Finite element test function I.
%  But we actually only look at those which are nonzero in this element.
%
            for test_x = 1 : test_x_num

              i = element_x_node(test_x,element_x);

              ik = ( k - 1 ) * node_x_num + i;
              jl = ( l - 1 ) * node_x_num + j;
%
%  If I is a boundary node, the equation associated with its test function
%  is replaced by a simple equation that enforces a zero Dirichlet condition.
%
              if ( i <= node_x1_num || ...
                        node_x1_num * ( node_x2_num - 1) + 1 <= i || ...
                        mod ( i, node_x1_num ) == 1 || ...
                        mod ( i, node_x1_num ) == 0 )

                if ( ik == jl )
                  bxy(ik,jl) = 1.0;
                  fxy(ik) = 0.0;
                end
%
%  If I is not a boundary node, the equation associated with its test function
%  generates a finite element equation.
%
              else

                bxy(ik,jl) = bxy(ik,jl) + quad_x_w(quad_x) * area_x ...
                  * clk(quad_x) ...
                  * ( phi_dx1(test_x,quad_x) * phi_dx1(basis_x,quad_x) ...
                    + phi_dx2(test_x,quad_x) * phi_dx2(basis_x,quad_x) );
%
%  The only nonzero result for the right hand side FXY occurs when K = 1,
%  since this is the single test function in Y which is the product of N
%  copies of the constant Legendre polynomial.
%
%  (This is true as long as there is no Y dependence in F(), and
%  assuming we are using Legendre polynomials.)
%
                if ( k == 1 )
                  psi0_integral = 1 / sqrt ( 2^n );
                  fxy(ik) = fxy(ik) + quad_x_w(quad_x) * area_x ...
                    * fvec(quad_x) * phi(test_x,quad_x) * psi0_integral;
                end

              end

            end
          end
        end

      end

      if ( test_y_more == FALSE )
        break
      end

    end

    if ( basis_y_more == FALSE )
      break
    end

  end

  return
end
function [ bzero, bvec ] = b_evaluator ( n, nx, x )

%*****************************************************************************80
%
%% B_EVALUATOR evaluates KL expansion coefficients for the diffusion coefficient.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 May 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of factors in the probability space.
%
%    Input, integer NX, the number of points.
%
%    Input, real X(1:2,1:NX), the point coordinates.
%
%    Output, real BZERO(1:NX), the zero-th order coefficient evaluated at X.
%
%    Output, real BVEC(1:N,1:NX), the coefficients of orders 1 to N,
%    evaluated at X.
%
  bzero(1:nx) = 1.0;
  bvec = zeros ( n, nx );
  for i = 1 : n
    bvec(i,1:nx) = sin ( i * x(1,1:nx) );
  end

  return
end
function [ phi, dphidx, dphidy ] = basis_mn_t3 ( t, n, p )

%*****************************************************************************80
%
%% BASIS_MN_T3: all bases functions at N points for a T3 element.
%
%  Discussion:
%
%    The routine is given the coordinates of the vertices of a triangle.
%    It works directly with these coordinates, and does not refer to a
%    reference element.
%
%    The sides of the triangle DO NOT have to lie along a coordinate
%    axis.
%
%    The routine evaluates the basis functions associated with each vertex,
%    and their derivatives with respect to X and Y.
%
%  Physical Element T3:
%
%            3
%           / \
%          /   \
%         /     \
%        /       \
%       1---------2
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real T(2,3), the vertices of the triangle.  It is common to list
%    these points in counter clockwise order.
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real P(2,N), the coordinates of the evaluation points.
%
%    Output, real PHI(3,N), the basis functions at the evaluation points.
%
%    Output, real DPHIDX(3,N), DPHIDY(3,N), the basis derivatives
%    at the evaluation points.
%
%  Local parameters:
%
%    Local, real AREA, is (twice) the area of the triangle.
%
  area = t(1,1) * ( t(2,2) - t(2,3) ) ...
       + t(1,2) * ( t(2,3) - t(2,1) ) ...
       + t(1,3) * ( t(2,1) - t(2,2) );

  phi(1,1:n) =     (  ( t(1,3) - t(1,2) ) * ( p(2,1:n) - t(2,2) )     ...
                    - ( t(2,3) - t(2,2) ) * ( p(1,1:n) - t(1,2) ) );
  dphidx(1,1:n) =   - ( t(2,3) - t(2,2) );
  dphidy(1,1:n) =     ( t(1,3) - t(1,2) );

  phi(2,1:n) =     (  ( t(1,1) - t(1,3) ) * ( p(2,1:n) - t(2,3) )     ...
                    - ( t(2,1) - t(2,3) ) * ( p(1,1:n) - t(1,3) ) );
  dphidx(2,1:n) =   - ( t(2,1) - t(2,3) );
  dphidy(2,1:n) =     ( t(1,1) - t(1,3) );

  phi(3,1:n) =     (  ( t(1,2) - t(1,1) ) * ( p(2,1:n) - t(2,1) )     ...
                    - ( t(2,2) - t(2,1) ) * ( p(1,1:n) - t(1,1) ) );
  dphidx(3,1:n) =   - ( t(2,2) - t(2,1) );
  dphidy(3,1:n) =     ( t(1,2) - t(1,1) );
%
%  Normalize.
%
  phi(1:3,1:n)    = phi(1:3,1:n) / area;
  dphidx(1:3,1:n) = dphidx(1:3,1:n) / area;
  dphidy(1:3,1:n) = dphidy(1:3,1:n) / area;

  return
end
function [ a, more, h, t ] = comp_next ( n, k, a, more, h, t )

%*****************************************************************************80
%
%% COMP_NEXT computes the compositions of the integer N into K parts.
%
%  Discussion:
%
%    A composition of the integer N into K parts is an ordered sequence
%    of K nonnegative integers which sum to N.  The compositions (1,2,1)
%    and (1,1,2) are considered to be distinct.
%
%    The routine computes one composition on each call until there are no more.
%    For instance, one composition of 6 into 3 parts is
%    3+2+1, another would be 6+0+0.
%
%    On the first call to this routine, set MORE = FALSE.  The routine
%    will compute the first element in the sequence of compositions, and
%    return it, as well as setting MORE = TRUE.  If more compositions
%    are desired, call again, and again.  Each time, the routine will
%    return with a new composition.
%
%    However, when the LAST composition in the sequence is computed
%    and returned, the routine will reset MORE to FALSE, signaling that
%    the end of the sequence has been reached.
%
%    This routine originally used a SAVE statement to maintain the
%    variables H and T.  I have decided (based on wasting an
%    entire morning trying to track down a problem) that it is safer
%    to pass these variables as arguments, even though the user should
%    never alter them.  This allows this routine to safely shuffle
%    between several ongoing calculations.
%
%  Example:
%
%    The 28 compositions of 6 into three parts are:
%
%      6 0 0,
%      5 1 0,
%      5 0 1,
%      4 2 0,
%      4 1 1,
%      4 0 2,
%      3 3 0,
%      3 2 1,
%      3 1 2,
%      3 0 3,
%      2 4 0,
%      2 3 1,
%      2 2 2,
%      2 1 3,
%      2 0 4,
%      1 5 0,
%      1 4 1,
%      1 3 2,
%      1 2 3,
%      1 1 4,
%      1 0 5,
%      0 6 0,
%      0 5 1,
%      0 4 2,
%      0 3 3,
%      0 2 4,
%      0 1 5,
%      0 0 6.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 July 2007
%
%  Author:
%
%    Original FORTRAN77 version by Albert Nijenhuis and Herbert Wilf
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Albert Nijenhuis, Herbert Wilf,
%    Combinatorial Algorithms for Computers and Calculators,
%    Second Edition,
%    Academic Press, 1978,
%    ISBN: 0-12-519260-6,
%    LC: QA164.N54.
%
%  Parameters:
%
%    Input, integer N, the integer whose compositions are desired.
%
%    Input, integer K, the number of parts in the composition.
%
%    Input, integer A(K), the previous composition.  On the first call,
%    with MORE = FALSE, set A = [].  Thereafter, A should be the
%    value of A output from the previous call.
%
%    Input, logical MORE.  The input value of MORE on the first
%    call should be FALSE, which tells the program to initialize.
%    On subsequent calls, MORE should be TRUE, or simply the
%    output value of MORE from the previous call.
%
%    Input, integer H, T, two internal parameters needed for the
%    computation.  The user may need to initialize these before the
%    very first call, but these initial values are not important.
%    The user should not alter these parameters once the computation
%    begins.
%
%    Output, integer A(K), the next composition.
%
%    Output, logical MORE, will be TRUE unless the composition
%    that is being returned is the final one in the sequence.
%
%    Output, integer H, T, the updated values of the two internal
%    parameters.
%
  if ( ~more )

    t = n;
    h = 0;
    a(1) = n;
    a(2:k) = 0;

  else

    if ( 1 < t )
      h = 0;
    end

    h = h + 1;
    t = a(h);
    a(h) = 0;
    a(1) = t - 1;
    a(h+1) = a(h+1) + 1;

  end

  more = ( a(k) ~= n );

  return
end
function fvec = f_evaluator ( n, x )

%*****************************************************************************80
%
%% F_EVALUATOR evaluates the finite element right hand side function F.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 May 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of point.
%
%    Input, real X(2,N), the point coordinates.
%
%    Output, real FVEC(N), the value of F at the points.
%
  fvec(1:n) = sin ( pi * x(1,1:n) ) .* sin ( pi * x(2,1:n) );

  return
end
function node_xy = grid_nodes_01 ( x_num, y_num )

%*****************************************************************************80
%
%% GRID_NODES_01 returns an equally spaced grid of nodes in the unit square.
%
%  Example:
%
%    X_NUM = 5
%    Y_NUM = 3
%
%    NODE_XY =
%    ( 0, 0.25, 0.5, 0.75, 1, 0,   0.25, 0.5, 0.75, 1,   0, 0.25, 0.5, 0.75, 1;
%      0, 0,    0,   0,    0, 0.5, 0.5,  0.5, 0.5,  0.5, 1, 1.0,  1.0, 1.0,  1 )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 May 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer X_NUM, Y_NUM, the number of nodes in the X and Y directions.
%
%    Output, real NODE_XY(2,X_NUM*Y_NUM), the coordinates of the nodes.
%
  node_num = x_num * y_num;

  node_xy(1:2,1:node_num) = 0.0;

  for i = 1 : x_num
    node_xy(1,i:x_num:i+(y_num-1)*x_num) = ( i - 1 ) / ( x_num - 1 );
  end

  for j = 1 : y_num
    node_xy(2,1+(j-1)*x_num:j*x_num) = ( j - 1 ) / ( y_num - 1 );
  end

  return
end
function [ element_node ] = grid_t3_element ( nelemx, nelemy )

%*****************************************************************************80
%
%% GRID_T3_ELEMENT produces a grid of pairs of 3 node triangles.
%
%  Example:
%
%    Input:
%
%      NELEMX = 3, NELEMY = 2
%
%    Output:
%
%      ELEMENT_NODE =
%         1,  2,  5;
%         6,  5,  2;
%         2,  3,  6;
%         7,  6,  3;
%         3,  4,  7;
%         8,  7,  4;
%         5,  6,  9;
%        10,  9,  6;
%         6,  7, 10;
%        11, 10,  7;
%         7,  8, 11;
%        12, 11,  8.
%
%  Grid:
%
%    9---10---11---12
%    |\ 8 |\10 |\12 |
%    | \  | \  | \  |
%    |  \ |  \ |  \ |
%    |  7\|  9\| 11\|
%    5----6----7----8
%    |\ 2 |\ 4 |\ 6 |
%    | \  | \  | \  |
%    |  \ |  \ |  \ |
%    |  1\|  3\|  5\|
%    1----2----3----4
%
%  Reference Element T3:
%
%    |
%    1  3
%    |  |\
%    |  | \
%    S  |  \
%    |  |   \
%    |  |    \
%    0  1-----2
%    |
%    +--0--R--1-->
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NELEMX, NELEMY, the number of elements along the
%    X and Y directions.  The number of elements generated will be
%    2 * NELEMX * NELEMY.
%
%    Output, integer ELEMENT_NODE(3,2*NELEMX*NELEMY), the nodes that form
%    each element.
%

%
%  Node labeling:
%
%    NW--NE
%     |\ |
%     | \|
%    SW--SE
%
  element = 0;
  element_node = zeros ( 3, 2 * nelemx * nelemy );
  for j = 1 : nelemy
    for i = 1 : nelemx

      sw = i     + ( j - 1 ) * ( nelemx + 1 );
      se = i + 1 + ( j - 1 ) * ( nelemx + 1 );
      nw = i     +   j       * ( nelemx + 1 );
      ne = i + 1 +   j       * ( nelemx + 1 );

      element = element + 1;

      element_node(1,element) = sw;
      element_node(2,element) = se;
      element_node(3,element) = nw;

      element = element + 1;

      element_node(1,element) = ne;
      element_node(2,element) = nw;
      element_node(3,element) = se;

    end
  end

  return
end
function value = i4_choose ( n, k )

%*****************************************************************************80
%
%% I4_CHOOSE computes the binomial coefficient C(N,K).
%
%  Discussion:
%
%    The value is calculated in such a way as to avoid overflow and
%    roundoff.  The calculation is done in integer arithmetic.
%
%    The formula used is:
%
%      C(N,K) = N! / ( K! * (N-K)! )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 June 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    ML Wolfson, HV Wright,
%    Algorithm 160:
%    Combinatorial of M Things Taken N at a Time,
%    Communications of the ACM,
%    Volume 6, Number 4, April 1963, page 161.
%
%  Parameters:
%
%    Input, integer N, K, are the values of N and K.
%
%    Output, integer VALUE, the number of combinations of N
%    things taken K at a time.
%
  mn = min ( k, n - k );

  if ( mn < 0 )

    value = 0;

  elseif ( mn == 0 )

    value = 1;

  else

    mx = max ( k, n - k );
    value = mx + 1;

    for i = 2 : mn
      value = ( value * ( mx + i ) ) / i;
    end

  end

  return
end
function table = legendre_linear_product ( p, e )

%*****************************************************************************80
%
%% LEGENDRE_LINEAR_PRODUCT computes a linearly weighted Legendre product table.
%
%  Discussion:
%
%    Let L(i)(X) represent the Legendre polynomial of degree i.
%
%    For polynomial chaos applications, it is of interest to know the
%    value of the integrals of products of X with every possible pair
%    of basis functions.  That is, we'd like to form
%
%      Tij = Integral ( -1 <= X <= +1 ) X^E * L(i)(X) * L(j)(X) dx
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 May 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer P, the maximum degree of the polyonomial factors.
%    0 <= P.
%
%    Input, integer E, the exponent of X in the integrand.
%    0 <= E.
%
%    Output, real TABLE(P+1,P+1), the table of integrals.  TABLE(I,J)
%    represents the weighted integral of X^E * L(i+1)(X) * L(j+1)(X).
%
  table(1:p+1,1:p+1) = 0.0;

  order = p + 1 + floor ( ( e + 1 ) / 2 );
  [ x_table, w_table ] = legendre_quadrature_rule ( order );

  for k = 1 : order

    x = x_table(k);
    l_table = legendre_polynomial ( p, x );
%
%  The following formula is an outer product in L_TABLE.
%
    if ( e == 0 )
      table(1:p+1,1:p+1) = table(1:p+1,1:p+1) ...
        + w_table(k) *       l_table(1:p+1)' * l_table(1:p+1);
    else
      table(1:p+1,1:p+1) = table(1:p+1,1:p+1) ...
        + w_table(k) * x^e * l_table(1:p+1)' * l_table(1:p+1);
    end

  end

  return
end
function l = legendre_polynomial ( p, x )

%*****************************************************************************80
%
%% LEGENDRE_POLYNOMIAL evaluates the Legendre polynomials L(0:P)(X).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 May 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Daniel Zwillinger, editor,
%    CRC Standard Mathematical Tables and Formulae,
%    30th Edition,
%    CRC Press, 1996.
%
%  Parameters:
%
%    Input, integer P, the highest evaluation degree.
%
%    Input, real X, the evaluation point.
%
%    Output, real L(1:P+1), the Legendre polynomials of order 0 through P at X.
%
  if ( p < 0 )
    l = [];
    return
  end
%
%  Allocate space.
%
  l(1:p+1) = 0.0;
%
%  Apply recursion.
%
  l(1) = 1.0;

  if ( 1 <= p )

    l(2) = x;

    for i = 2 : p

      l(i+1) = ( ( 2 * i - 1 ) * x * l(i)   ...
               - (     i - 1 ) *     l(i-1) ) ...
               / (     i     );

    end

  end
%
%  Normalize.
%
  l(1:p+1) = l(1:p+1) .* sqrt ( ( 1 : 2 : 2*p+1 ) / 2 );

  return
end
function [ xtab, weight ] = legendre_quadrature_rule ( order )

%*****************************************************************************80
%
%% LEGENDRE_QUADRATURE_RULE computes a Gauss-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 <= NORDER ) WEIGHT(I) * F ( XTAB(I) )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 May 2008
%
%  Author:
%
%    FORTRAN77 original 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 XTAB(ORDER), the abscissas of the rule.
%
%    Output, real WEIGHT(ORDER), the weights of the rule.
%    The weights are positive, symmetric, and should sum to 2.
%
  xtab = zeros ( order, 1 );
  weight = zeros ( order, 1 );

  if ( order < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'LEGENDRE_RULE - Fatal error!\n' );
    fprintf ( 1, '  Illegal value of ORDER = %d\n', order );
    error ( 'LEGENDRE_RULE - Fatal error!' );
  end

  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;

    xtab(mp1mi) = xtemp;

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

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

  end

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

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

  return
end
function phy = reference_to_physical_t3 ( t, n, ref )

%*****************************************************************************80
%
%% REFERENCE_TO_PHYSICAL_T3 maps a reference point to a physical point.
%
%  Discussion:
%
%    Given the vertices of an order 3 physical triangle and a point
%    (XSI,ETA) in the reference triangle, the routine computes the value
%    of the corresponding image point (X,Y) in physical space.
%
%    Note that this routine may also be appropriate for an order 6
%    triangle, if the mapping between reference and physical space
%    is linear.  This implies, in particular, that the sides of the
%    image triangle are straight and that the "midside" nodes in the
%    physical triangle are halfway along the sides of
%    the physical triangle.
%
%    The T3 reference element is suggested by the following diagram:
%
%    |
%    1  3
%    |  |\
%    |  | \
%    S  |  \
%    |  |   \
%    |  |    \
%    0  1-----2
%    |
%    +--0--R--1-->
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 June 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real T(2,3), the coordinates of the vertices.  The vertices are assumed
%    to be the images of (0,0), (1,0) and (0,1) respectively.
%
%    Input, integer N, the number of points to transform.
%
%    Input, real REF(2,N), the coordinates of points in the reference space.
%
%    Output, real PHY(2,N), the coordinates of the corresponding points in the
%    physical space.
%
  phy = zeros ( 2, n );

  for i = 1 : 2

    phy(i,1:n) = t(i,1) * ( 1.0 - ref(1,1:n) - ref(2,1:n) ) ...
               + t(i,2) *        ref(1,1:n)                ...
               + t(i,3) *                     ref(2,1:n);
  end

  return
end
function [ a, more, h, t, n2, more2 ] = subcomp_next ( n, k, a, more, h, t, ...
  n2, more2 )

%*****************************************************************************80
%
%% SUBCOMP_NEXT computes the next subcomposition of N into K parts.
%
%  Discussion:
%
%    A composition of the integer N into K parts is an ordered sequence
%    of K nonnegative integers which sum to a value of N.
%
%    A subcomposition of the integer N into K parts is a composition
%    of M into K parts, where 0 <= M <= N.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 May 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the integer whose subcompositions are desired.
%
%    Input, integer K, the number of parts in the subcomposition.
%
%    Input, integer A(K), the parts of the subcomposition.
%
%    Input, logical MORE, set to FALSE by the user to start the computation.
%
%    Input, integer H, T, N2, MORE2, internal parameters needed for the
%    computation.  The user may need to initialize these before the
%    very first call, but these initial values are not important.
%    The user should not alter these parameters once the computation
%    begins.
%
%    Output, integer A(K), the parts of the subcomposition.
%
%    Output, logical MORE, set to FALSE by the routine to terminate
%    the computation.
%
%    Output, integer H, T, N2, MORE2, the updated values of the two internal
%    parameters.
%

%
%  The first computation.
%
  if ( ~more )

    n2 = 0;
    a(1:k) = 0;
    more2 = 0;
    h = 0;
    t = 0;

    more = 1;
%
%  Do the next element at the current value of N.
%
  elseif ( more2 )

    [ a, more2, h, t ] = comp_next ( n2, k, a, more2, h, t );

  else

    more2 = 0;
    n2 = n2 + 1;

    [ a, more2, h, t ] = comp_next ( n2, k, a, more2, h, t );

  end
%
%  Termination occurs if MORE2 = FALSE and N2 = N.
%
  if ( ~more2 && n2 == n )
    more = 0;
  end

  return
end
function area = triangle_area_2d ( t )

%*****************************************************************************80
%
%% TRIANGLE_AREA_2D computes the area of a triangle in 2D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 January 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real T(2,3), the triangle vertices.
%
%    Output, real AREA, the absolute area of the triangle.
%
  area = 0.5 * abs ( ...
      t(1,1) * ( t(2,2) - t(2,3) ) ...
    + t(1,2) * ( t(2,3) - t(2,1) ) ...
    + t(1,3) * ( t(2,1) - t(2,2) ) );

  return
end
function [ x, w ] = triangle_rule_13 ( )

%*****************************************************************************80
%
%% TRIANGLE_RULE_13 sets a 13 point quadrature rule for triangles.
%
%  Discussion:
%
%    The Integration region is points (X,Y) such that
%
%      0 <= X,
%      0 <= Y, and
%      X + Y <= 1.
%
%    Graph:
%
%      ^
%    1 | *
%      | |\
%    Y | | \
%      | |  \
%    0 | *---*
%      +------->
%        0 X 1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 May 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Gilbert Strang, George Fix,
%    An Analysis of the Finite Element Method,
%    Prentice Hall, 1973, page 184,
%    ISBN: 096140888X,
%    LC: TA335.S77.
%
%  Parameters:
%
%    Output, real X(2,13), the abscissas.
%
%    Output, real W(13), the weights.
%
  a = 0.479308067841923;
  b = 0.260345966079038;
  c = 0.869739794195568;
  d = 0.065130102902216;
  e = 0.638444188569809;
  f = 0.312865496004875;
  g = 0.048690315425316;
  h = 1.0 / 3.0;
  t = 0.175615257433204;
  u = 0.053347235608839;
  v = 0.077113760890257;
  w = -0.149570044467670;

  x(1:2,1:13) = [ a, b, b, c, d, d, e, e, f, f, g, g, h;
                  b, a, b, d, c, d, f, g, e, g, e, f, h ];

  w(1:13) = [ t, t, t, u, u, u, v, v, v, v, v, v, w ];

  return
end


function legendre_set_test ( )

%*****************************************************************************80
%
%% LEGENDRE_SET_TEST tests LEGENDRE_SET.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 November 2014
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'LEGENDRE_SET_TEST\n' );
  fprintf ( 1, '  LEGENDRE_SET returns points and weights of \n' );
  fprintf ( 1, '  Gauss-Legendre quadrature rules.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   N               1             X^4           Runge\n' );
  fprintf ( 1, '\n' );

  for n = 1 : 10
    [ x, w ] = legendre_set ( n );
    e1 = sum ( w(1:n,1) );
    e2 = w(1:n,1)' * x(1:n,1).^4;
    e3 = w(1:n,1)' * ( 1.0 ./ ( 1.0 + 25.0 * x(1:n,1).^2 ) );
    fprintf ( 1, '  %2d  %14.6g  %14.6g  %14.6g\n', n, e1, e2, e3 );
  end

  return
end


function cordic_test001 ( )

%*****************************************************************************80
%
%% TEST001 demonstrates the use of COSSIN_CORDIC.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 June 2007
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST001:\n' );
  fprintf ( 1, '  COSSIN_CORDIC computes the cosine and sine of an angle\n' );
  fprintf ( 1, '  using the CORDIC algorithm.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      A    N           Cos(A)     Cos(A)      Difference\n' );
  fprintf ( 1, '                     Tabulated   CORDIC\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, a, c1 ] = cos_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '\n' );

    for n = 0 : 5 : 25

      v = cossin_cordic ( a, n );
      c2 = v(1);
      d = c1 - c2;

      fprintf ( 1, '  %12f  %4d  %16.8f  %16.8f  %9e\n', a, n, c1, c2, d );

    end

  end

  return
end


function [ a, b ] = p23_lim ( dim_num )

%*****************************************************************************80
%
%% P23_LIM returns the integration limits for problem 23.
%
%  Discussion:
%
%    Because the integration region is the interior of the unit simplex,
%    the integration limits simply specify the limits of a box containing
%    the integration region.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 March 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the dimension of the argument.
%
%    Output, real A(DIM_NUM), B(DIM_NUM), the lower and upper
%    limits of integration.
%
  a(1:dim_num) = 0.0;
  b(1:dim_num) = 1.0;

  return
end


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

%-----------------------------------------------------------------------
% This script demonstrates the use of function LINPROG on the basis of
% the C-VaR portfolio optimization problen given in:
% Uryasev, Rockafellar: Optimization of Conditional Value-at-Risk(1999)

clear
close all
clc
warning 'off'

tic

% load matrix data
% of annualized linear stock returns
load 'LinRetMat'

% M: number of samples
% N: number of assets
[M,N] = size(data);
% sample mean
mu = mean(data)';

% confidence level
beta = 0.99;

% number portfolios
NumPorts = 50;
% target returns
Rstar = min(mu):(max(mu)-min(mu))/(NumPorts-1):max(mu);
% objective function vector
f = objfCVaR(beta, M, N);
% constraints matrices
[A, b, Aeq, beq, lb, ub] = constCVaR(data, mu, Rstar(1));

CVaR    = zeros(NumPorts,1);
weights = zeros(N,NumPorts);

for i = 1:NumPorts
    beq(1) = -Rstar(i);
    [optimvar, CVaR(i)] = linprog(f, A, b, Aeq, beq, lb, ub);
    weights(:,i) = optimvar(1:N);
end

toc
warning 'on'

% Create figure
figure1 = figure('Color',[1 1 1]);
colormap('gray');

% plot efficient frontier
plot(CVaR,Rstar,'k','LineWidth',1.5)
legend('Mean-CVaR Frontier','Location','NorthWest')
set(gca,'xlim',[0 CVaR(end)],'ylim',[0 Rstar(end)])
title('Mean-CVaR Frontier','FontSize',17)
xlabel('$\beta$-CVaR','Interpreter','latex','FontName','Helvetia','FontSize',16)
ylabel('$\mu$','Interpreter','latex','FontName','Helvetia','FontSize',16,'Rotation',0)
ticks = get(gca,'YTick');
set(gca,'YTickLabel',[num2str(ticks'*100),repmat('%',length(ticks),1)])
ticks = get(gca,'XTick');
set(gca,'XTickLabel',[num2str(ticks'*100),repmat('%',length(ticks),1)])


function cg_rc_test02 ( )

%*****************************************************************************80
%
%% CG_RC_TEST02 tests CG_RC with the Wathen matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 January 2013
%
%  Author:
%
%    John Burkardt
%
  n = 79;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CG_RC_TEST02\n' );
  fprintf ( 1, '  Use CG_RC to solve a linear system\n' );
  fprintf ( 1, '  involving the Wathen matrix.\n' );

  nx = 5;
  ny = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  NX = %d\n', nx );
  fprintf ( 1, '  NY = %d\n', ny );
  fprintf ( 1, '  N  = %d\n', n );

  a = wathen ( nx, ny, n );

  seed = 123456789;
  [ x_exact, seed ] = r8vec_uniform_01 ( n, seed );

  size ( x_exact )
  size ( a )
  b(1:n) = a(1:n,1:n) * x_exact(1:n);

  x = zeros ( n, 1 );
%
%  Parameters we need for the stopping test.
%
  it = 0;
  it_max = 30;
  tol = 1.0E-05;
  bnrm2 = norm ( b(1:n) );
%
%  Set parameters for CG_RC.
%
  r = zeros ( n, 1 );
  z = zeros ( n, 1 );
  p = zeros ( n, 1 );
  q = zeros ( n, 1 );
  job = 1;
%
%  Repeatedly call CG_RC, and on return, do what JOB tells you.
%
  while ( 1 )

    [ x, r, z, p, q, job ] = cg_rc ( n, b, x, r, z, p, q, job );
%
%  Compute q = A * p.
%
    if ( job == 1 )

      q = a * p;
%
%  Solve M * z = r;
%
    elseif ( job == 2 )

      for i = 1 : n
        z(i) = r(i) / a(i,i);
      end
%
%  Compute r = r - A * x.
%
    elseif ( job == 3 )

      r = r - a * x;
%
%  Stopping test.
%
    elseif ( job == 4 )

      rnrm2 = norm ( r );

      if ( bnrm2 == 0.0 )
        if ( rnrm2 <= tol )
          break
        end
      else
        if ( rnrm2 <= tol * bnrm2 )
          break
        end
      end

      it = it + 1;

      if ( it_max <= it )
        fprintf ( 1, '\n' );
        fprintf ( 1, '  Iteration limit exceeded.\n' );
        fprintf ( 1, '  Terminating early.\n' );
        break
      end

    end

    job = 2;

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of iterations was %d\n', it );
  fprintf ( 1, '  Estimated error is %g\n', rnrm2 );
  err = max ( abs ( x_exact(1:n) - x(1:n) ) );
  fprintf ( 1, '  Loo error is %g\n', err );

  fprintf ( 1, '\n' );
  fprintf ( 1, '     I      X(I)         X_EXACT(I)        B(I)\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %4d  %14f  %14f  %14f\n', ...
      i, x(i), x_exact(i), b(i) );
  end

  return
  end


function suborder = lyness_suborder ( rule, suborder_num )

%*****************************************************************************80
%
%% LYNESS_SUBORDER returns the suborders for a Lyness rule.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 September 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    James Lyness, Dennis Jespersen,
%    Moderate Degree Symmetric Quadrature Rules for the Triangle,
%    Journal of the Institute of Mathematics and its Applications,
%    Volume 15, Number 1, February 1975, pages 19-32.
%
%  Parameters:
%
%    Input, integer RULE, the index of the rule.
%
%    Input, integer SUBORDER_NUM, the number of suborders
%    of the rule.
%
%    Output, integer SUBORDER(SUBORDER_NUM), the suborders
%    of the rule.
%
  if ( rule == 0 )
    suborder = [ 1 ]';
  elseif ( rule == 1 )
    suborder = [ 3 ]';
  elseif ( rule == 2 )
    suborder = [ 1, 3 ]';
  elseif ( rule == 3 )
    suborder = [ 1, 3 ]';
  elseif ( rule == 4 )
    suborder = [ 1, 3, 3 ]';
  elseif ( rule == 5 )
    suborder = [ 3, 3  ]';
  elseif ( rule == 6 )
    suborder = [ 1, 3, 6  ]';
  elseif ( rule == 7 )
    suborder = [ 3, 3, 3 ]';
  elseif ( rule == 8 )
    suborder = [ 1, 3, 3 ]';
  elseif ( rule == 9 )
    suborder = [ 1, 3, 3, 3 ]';
  elseif ( rule == 10 )
    suborder = [ 3, 3, 6 ]';
  elseif ( rule == 11 )
    suborder = [ 1, 3, 3, 3, 6 ]';
  elseif ( rule == 12 )
    suborder = [ 1, 3, 3, 6 ]';
  elseif ( rule == 13 )
    suborder = [ 1, 3, 3, 6 ]';
  elseif ( rule == 14 )
    suborder = [ 1, 3, 3, 3, 6 ]';
  elseif ( rule == 15 )
    suborder = [ 1, 3, 3, 3, 6 ]';
  elseif ( rule == 16 )
    suborder = [ 3, 3, 3, 3, 3, 6 ]';
  elseif ( rule == 17 )
    suborder = [ 1, 3, 3, 3, 6 ]';
  elseif ( rule == 18 )
    suborder = [ 1, 3, 3, 3, 3, 6 ]';
  elseif ( rule == 19 )
    suborder = [ 1, 3, 3, 3, 3, 3, 6 ]';
  elseif ( rule == 20 )
    suborder = [ 3, 3, 3, 3, 3, 6, 6 ]';
  elseif ( rule == 21 )
    suborder = [ 1, 3, 3, 3, 3, 3, 6, 6 ]';
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, 'LYNESS_SUBORDER - Fatal error!\n' );
    fprintf ( 1, '  Illegal RULE = %d\n', rule );
    error ( 'LYNESS_SUBORDER - Fatal error!' );
  end

  return
end


function hammersley_test01 ( )

%*****************************************************************************80
%
%% TEST01 tests I4_TO_HAMMERSLEY_SEQUENCE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 May 2008
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST01\n' );
  fprintf ( 1, '  I4_TO_HAMMERSLEY_SEQUENCE computes N elements of\n' );
  fprintf ( 1, '  a Hammersley sequence on a single call.\n' );
  fprintf ( 1, '  All arguments are specified explicitly.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  In this example, we compute the first 10 elements\n' );
  fprintf ( 1, '  of a "classical" Hammersley sequence, and then\n' );
  fprintf ( 1, '  the "last" 10 elements.\n' );

  nmax = 1000;

  dim_num = 4;
  n = 10;
  step = 1;
  seed(1:dim_num) = 0;
  leap(1:dim_num) = 1;
  base(1) = -nmax;
  for i = 2 : dim_num
    base(i) = prime ( i - 1 );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  DIM_NUM = %12d\n', dim_num );
  fprintf ( 1, '  N =    %12d\n', n );
  fprintf ( 1, '  STEP = %12d\n', step );
  i4vec_transpose_print ( dim_num, seed, '  SEED = ' );
  i4vec_transpose_print ( dim_num, leap, '  LEAP = ' );
  i4vec_transpose_print ( dim_num, base, '  BASE = ' );

  r = i4_to_hammersley_sequence ( dim_num, n, step, seed, leap, base );

  fprintf ( 1, '\n' );
  fprintf ( 1, '    STEP   Hammersley\n' );
  fprintf ( 1, '\n' );
  for j = 1 : n
    fprintf ( 1, '  %6d  ', step+j-1 );
    for i = 1 : dim_num
      fprintf ( 1, '%12f  ', r(i,j) );
    end
    fprintf ( 1, '\n' );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  We can jump ahead in the sequence by changing STEP.\n' );

  step = nmax - n + 1;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  STEP = %12d\n', step );

  r = i4_to_hammersley_sequence ( dim_num, n, step, seed, leap, base );

  fprintf ( 1, '\n' );
  fprintf ( 1, '    STEP   Hammersley\n' );
  fprintf ( 1, '\n' );
  for j = 1 : n
    fprintf ( 1, '  %6d  ', step+j-1 );
    for i = 1 : dim_num
      fprintf ( 1, '%12f  ', r(i,j) );
    end
    fprintf ( 1, '\n' );
  end

  return
end


% GP_COV_SE - Squared exponential covariance function for Gaussian
% processes.
%
% [K, DK_LOGTHETA, DK_X2] = GP_COV_SE(X1, X2, LOGTHETA)

% Last modified 2010-10-06
% Copyright (c) Jaakko Luttinen (jaakko.luttinen@tkk.fi)

function [K, dK_logtheta, dK_x2] = gp_cov_se(x1, x2, logtheta)

% 2-norm distances
if ~isempty(x2)
  % Distances for covariances
  n1 = cols(x1);
  n2 = cols(x2);
  D = zeros(n1,n2);
  for i=1:n1
    for j=1:n2
      D(i,j) = norm(x1(:,i) - x2(:,j));
    end
  end
else
  % Distances for variances
  n = rows(x1);
  D = zeros(n,1);
end

% Covariance matrix
D2 = D.^2;
K = exp(logtheta(1)) * exp(-0.5*exp(-2*logtheta(2))*D2);

% Gradient for hyperparameters
if nargout >= 2
  dK_logtheta = zeros([size(D), 2]);
  dK_logtheta(:,:,1) = K;
  dK_logtheta(:,:,2) = K .* (-0.5*D2*exp(-2*logtheta(2))) .* (-2);
end

% Gradients for inputs x2
if nargout >= 3
  if isempty(x2)
    error('Can''t calculate gradient: x2 not given');
  end
  d = rows(x2); % dimensionality of inputs
  m = cols(x1); % number of other inputs
  n = cols(x2); % number of inputs
  dK_x2 = zeros([d,m,n]);
  for i=1:m
    for j=1:n
      dK_x2(:,i,j) = K(i,j) * (-0.5*exp(-2*logtheta(2))) * 2*(x2(:,j)-x1(:,i));
    end
  end
end

function [K, dK] = gpK_sqexp(D, p1, p2)

% [K, dK] = gpK_sqexp(D, p1, p2)
%
% Squared exponential covariance function for GP.
%
% p1 is the scale
% p2 is the length
%
K = p1^2*exp(-0.5*D.^2/(p2^2));

if nargout >= 2
  dK = zeros([size(D), 2]);
  dK(:,:,1) = K .* 2 / p1;
  dK(:,:,2) = K .* (-0.5*D.^2) .* (-2*p2^(-3));
end


function [Fs spec] = PTSpec2d(Y, F, psd)
%% [Fs spec] = PTSpec2d(Y, F, psd)
%   computes standard fft on input data Y. F is sample frequency in Hz.

% ----------------------------------------------------------------------------------
% "THE BEER-WARE LICENSE" (Revision 42):
% <brian.white@queensu.ca> wrote this file. As long as you retain this notice you
% can do whatever you want with this stuff. If we meet some day, and you think
% this stuff is worth it, you can buy me a beer in return. -Brian White
% ----------------------------------------------------------------------------------

if psd
    % N = length(Y);
    % [psdx,Fs] = periodogram(Y,[], N-1, F*1000,'psd'); % power psd
    % [spec,Fs] = pspectrum(Y, F*1000, 'FrequencyResolution', 10);

    N = length(Y);
    Fs = ((F*1000)*(0:(N/2))/N);
    Y = Y.*hann(N)';
    Y = fft(Y);
    psdx = abs(Y).^2 / (F*1000*N); % (1/(F*N)) * abs(Y).^2  is exactly same as  abs(Y).^2 / (F*N), to
    psdx(2:end-1) = 2*psdx(2:end-1);
    psdx = psdx(1:N/2+1);
%     % scale to dB
     spec = 10 * log10(psdx)';
else
    N = length(Y);
    Fs = ((F*1000)*(0:(N/2))/N);
    Y = Y.*hann(N)';
    Y = fft(Y);
    spec = abs(Y) / N; % (1/(F*N)) * abs(Y).^2  is exactly same as  abs(Y).^2 / (F*N), to
    spec = spec(1:N/2+1)';
end

end


function [A,B] = kmo(X)
%KMO Kaiser-Meyer-Olkin Measure of Sampling Adequacy.
% Factor analysis can be used as a guide to how coherently a set of variables
% relate to a hypothesized underlying dimension that they are all being used
% to measure. External validity analysis assesses whether the scale that has
% been constructed performs as theoretically expected in correlation with
% other variables to which it is expected to be related.
% There are some assumptions about the characteristics of factors that are
% extracted and defined that are unobserved common dimensions that may be
% listed to account for the correlations among observed variables. Sampling
% adequacy predicts if data are likely to factor well, based on correlation
% and partial correlation. Is used to assess which variables to drop from the
% model because they are too multicollinear.
% It has been suggested that inv(R) should be a near-diagonal matrix in order
% to successfully fit a factor analysis model.  To assess how close inv(R)
% is to a diagonal matrix, Kaiser (1970) proposed a measure of sampling
% adequacy, now called KMO (Kaiser-Meyer-Olkin) index. The common part, called
% the image of a variable, is defined as that part which is predictable by
% regressing each variable on all other variables.
% The anti-image is the specific part of the variable that cannot be predicted.
% Examining the anti-image of the correlation matrix. That is the negative of the
% partial correlations, partialling out all other variables.
% There is a KMO statistic for each individual variable and their sum is
% the overall statistic. If it is not > 0.6 drop the indicator variables with
% the lowest individual statistic value until the overall one rises above 0.6:
% factors which is meritorious. The diagonal elements on the Anti-image
% correlation matrix are the KMO individual statistics for each variable. A KMO
% index <= 0.5 indicates the correlation matrix is not suitable for factor
% analysis.
%
% Syntax: function kmo(X)
%
%     Input:
%          X - Input matrix can be a data matrix (size n-data x p-variables)
%     Output(s):
%            - Kaiser-Meyer-Olkin Index.
%            - Degree of Common Variance Report (shared by a set of variables
%              and thus assesses the degree to which they measure a common
%              underlying factor).
%        optional(s):
%            - Anti-image Covariance Matrix.
%            - Anti-image Correlation Matrix
%
%  Example: From the example given on the web page
%  http://www.ncl.ac.uk/iss/statistics/docs/factoranalysis.html
%  We are interested to calculate the Kaiser-Meyer-Olkin measure of sampling
%  adequacy in order to see if proceeds a satisfactory factor analysis to
%  investigate the reasons why customers buy a product such as a particular
%  brand of soft drink (e.g. coca cola). Several variables were identified
%  which influence customer to buy coca cola. Some of the variables identified
%  as being influential include availability of product (X1), cost of product
%  (X2), experience with product (X3), popularity of product (X4), prestige
%  attached to product (X5), quality of product (X6), quantity of product (X7),
%  and respectability of product (X8). From this, you designed a questionnaire
%  to solicit customers' view on a seven point scale, where 1 = not important
%  and 7 = very important. The results from your questionnaire are show on the
%  table below. Only the first twelve respondents (cases) are used in this
%  example.
%
%  Table 1: Customer survey
%  --------------------------------------------------
%     X1    X2    X3    X4    X5    X6    X7    X8
%  --------------------------------------------------
%      4     1     4     5     2     3     6     7
%      4     2     7     6     6     3     3     4
%      6     4     3     4     2     5     7     7
%      5     3     5     4     3     4     6     7
%      5     2     4     5     2     5     5     6
%      6     3     3     5     4     4     7     7
%      6     2     4     4     4     3     4     5
%      4     1     3     4     3     3     5     6
%      5     3     4     3     4     3     6     6
%      5     4     3     4     4     4     6     7
%      6     2     4     4     4     3     7     5
%      5     2     3     3     3     3     7     6
%  --------------------------------------------------
%
% Data matrix must be:
%  X=[4 1 4 5 2 3 6 7;4 2 7 6 6 3 3 4;6 4 3 4 2 5 7 7;5 3 5 4 3 4 6 7;
%  5 2 4 5 2 5 5 6;6 3 3 5 4 4 7 7;6 2 4 4 4 3 4 5;4 1 3 4 3 3 5 6;
%  5 3 4 3 4 3 6 6;5 4 3 4 4 4 6 7;6 2 4 4 4 3 7 5;5 2 3 3 3 3 7 6];
%
%  Calling on Matlab the function:
%            kmo(X)
%
%  Answer is:
%
%  Kaiser-Meyer-Olkin Measure of Sampling Adequacy: 0.4172
%  The KMO test yields a degree of common variance unacceptable (Don't Factor).
%
%
%  Created by A. Trujillo-Ortiz, R. Hernandez-Walls, A. Castro-Perez,
%             K. Barba-Rojo and A. Otero-Limon
%             Facultad de Ciencias Marinas
%             Universidad Autonoma de Baja California
%             Apdo. Postal 453
%             Ensenada, Baja California
%             Mexico.
%             atrujo@uabc.mx
%
%  Copyright. October 10, 2006.
%
% To cite this file, this would be an appropriate format:
% Trujillo-Ortiz, A., R. Hernandez-Walls, A. Castro-Perez, K. Barba-Rojo
%   and A. Otero-Limon (2006). kmo:Kaiser-Meyer-Olkin Measure of Sampling
%   Adequacy. A MATLAB file. [WWW document]. URL http://www.mathworks.com/
%   matlabcentral/fileexchange/loadFile.do?objectId=12736
%
%  References:
%  Rencher, A. C. (2002), Methods of Multivariate Analysis. 2nd. ed.
%            New-Jersey:John Wiley & Sons. Chapter 13 (pp. 408-450).
%

error(nargchk(1,1,nargin));
msg = nargoutchk(1, 2, nargout);

X = corrcoef(X);

iX = inv(X);
S2 = diag(diag((iX.^-1)));
AIS = S2*iX*S2; %anti-image covariance matrix
IS = X+AIS-2*S2; %image covariance matrix
Dai = diag(diag(sqrt(AIS)));
IR = inv(Dai)*IS*inv(Dai); %image correlation matrix
AIR = inv(Dai)*AIS*inv(Dai); %anti-image correlation matrix
a = sum((AIR - diag(diag(AIR))).^2);
AA = sum(a);
b = sum((X - eye(size(X))).^2);
BB = sum(b);
MSA = b./(b+a); %measures of sampling adequacy
AIR = AIR-eye(size(AIR))+diag(MSA);
%Examine the anti-image of the correlation matrix. That is the negative of the partial correlations,
%partialling out all other variables.
N = BB;
D = AA+BB;
kmo = N/D;

disp(' ')
fprintf('Kaiser-Meyer-Olkin Measure of Sampling Adequacy: %3.4f\n', kmo);
if (kmo >= 0.00 && kmo < 0.50);
    disp('The KMO test yields a degree of common variance unacceptable (Don''t Factor).')
elseif (kmo >= 0.50 && kmo < 0.60);
    disp('The KMO test yields a degree of common variance miserable.')
elseif (kmo >= 0.60 && kmo < 0.70);
    disp('The KMO test yields a degree of common variance mediocre.')
elseif (kmo >= 0.70 && kmo < 0.80);
    disp('The KMO test yields a degree of common variance middling.')
elseif (kmo >= 0.80 && kmo < 0.90);
    disp('The KMO test yields a degree of common variance meritorious.')
else (kmo >= 0.90 && kmo <= 1.00);
    disp('The KMO test yields a degree of common variance marvelous.')
end

if nargout == 1;
    disp(' ')
    disp('A = Anti-image covariance matrix.');
    A = AIS;
elseif nargout > 1;
    disp(' ')
    disp('A = Anti-image covariance matrix.');
    A = AIS;
    disp('B = Anti-image correlation matrix.');
    B = AIR;
end

return,


% INVWISHRND - Inverse Wishart Distribution - Random Matrix Value
% Copyright (c) 1998, Harvard University. Full copyright in the file Copyright
%
%   [ IW ] = invwishrnd(S,d)
%
% S = p x p symmetric, postitive definite "scale" matrix
% d = "degrees of freedom" parameter (integer)
%   = "precision" parameter (d may be non-integer)
%
% IW = random matrix from the inverse Wishart distribution
%
% Note:
%   Different sources use different parameterizations.
%   This routine uses that of Press and Shigemasu (1989):
%   density (IW) is proportional to
%     exp[-.5*trace(S*inv(IW))] / [det(IW) ^ (d/2)].
%
%   With this density definition:
%   mean of IW = S/(d-2p-2) for d>2p+2,
%   mode of IW = S/d.
%
% See also: INVWISHIRND, WISHRND

function [IW] = invwishrnd(S,d)

[p,p2] = size(S) ;

W = wishrnd(inv(S),d-p-1) ;

IW = inv(W) ;


function y = FourierShift2D(x, delta)
%
% y = FourierShift(x, [delta_x delta_y])
%
% Shifts x by delta cyclically. Uses the fourier shift theorem.
%
% Real inputs should give real outputs.
%
% By Tim Hutt, 26/03/2009
% Small fix thanks to Brian Krause, 11/02/2010

% The size of the matrix.
[N, M] = size(x);

% FFT of our possibly padded input signal.
X = fft2(x);

% The mathsy bit. The floors take care of odd-length signals.
x_shift = exp(-1i * 2 * pi * delta(1) * [0:floor(N/2)-1 floor(-N/2):-1]' / N);
y_shift = exp(-1i * 2 * pi * delta(2) * [0:floor(M/2)-1 floor(-M/2):-1] / M);

% Force conjugate symmetry. Otherwise this frequency component has no
% corresponding negative frequency to cancel out its imaginary part.
if mod(N, 2) == 0
	x_shift(N/2+1) = real(x_shift(N/2+1));
end
if mod(M, 2) == 0
	y_shift(M/2+1) = real(y_shift(M/2+1));
end

Y = X .* (x_shift * y_shift);

% Invert the FFT.
y = ifft2(Y);

% There should be no imaginary component (for real input
% signals) but due to numerical effects some remnants remain.
if isreal(x)
    y = real(y);
end

end


function [lp,dlp] = priorGaussMulti(mu,s2,x)

% Multivariate Gaussian hyperparameter prior distribution.
% Compute log-likelihood and its derivative or draw a random sample.
% The prior distribution is parameterized as:
%
%   p(x) = exp(-r2/2) / sqrt(det(2*pi*s2)) where r2(x) = (x-mu)'*inv(s2)*(x-mu),
%
% mu(Dx1) is the mean parameter, s2(Dx1) or s2(DxD) is the variance parameter
% and x(DxN) contains query hyperparameters for prior evaluation.
%
% For more help on design of priors, try "help priorDistributions".
%
% Copyright (c) by Roman Garnett and Hannes Nickisch, 2014-09-12.
%
% See also PRIORDISTRIBUTIONS.M, PRIORGAUSS.M.

if nargin<2, error('mu and s2 parameters need to be provided'), end
if ndims(mu)~=2 || size(mu,2)~=1, error('mu needs to be (Dx1)'), end
D = size(mu,1);
s2_ok = ndims(s2)==2 && all(size(s2)==[D,1] | size(s2)==[D,D]);
if ~s2_ok, error('s2 needs to be (DxD) or (Dx1)'), end
if size(s2,2)==D                                        % full multivariate case
  s = chol(s2)'; lds = sum(log(diag(s)));                 % lds = log(det(s2))/2
else                                                       % diagonal covariance
  s = sqrt(s2);  lds = sum(log(s));
end
if nargin<3                                             % return a random sample
  lp = randn(D,1);                                                 % unit sample
  if size(s,2)==D, lp = s*lp+mu; else lp = s.*lp+mu; end % affine transformation
  return
end
if D==1 && size(x,1)>1                            % both mu/s2 scalar => inflate
  D = size(x,1); mu = mu*ones(D,1); s = s*ones(D,1); lds = D*lds;
end
if ~(ndims(x)==2 && size(x,1)==D), error('x needs to be (Dxn)'), end

oN = ones(1,size(x,2));
if size(s,2)==D
  xs = s\(x-mu*oN);       dlp = -s'\xs;
else
  xs = (x-mu*oN)./(s*oN); dlp = -xs./(s*oN);
end
lp = -sum(xs.^2,1)/2 - D*log(2*pi)/2 - lds;


function [Results,u_star2,t]=Arm_1DOF_LinearStateTransition_Fun(StartAngle,Stable,I,l_c)
% this suggests an initial solution for the pendulium problem
%Stable 1 hanging pendulium, -1 inverted
%StartAngle given in degrees

%system parameters
m=1; %mass of plumb, kg
g=9.806; %gravity, m/sec^2
% I and l_c are now set by input

% simumation parameters
X_0=[StartAngle(1),0].'*pi/180; %intial value
X_F=[0,0].'*pi/180; %final value
t0=0;
tf=1;
T=linspace(t0,tf,1000);
T_2=[T, T(end)+(0.0001:0.01:0.1)];

% I*theta_ddot = -m*g*l_c*sin(theta)+u
% regular pendulium
% linearizing about theta=0 for now
A = [0 1; -Stable*m*g*l_c/I, 0];
B=[0; 1/I];

% W_c_fun=@(t) expm(A*t(n))*B*(B.')*expm(A.'*t(n));
% Written as subroutine below
temp11= quad(@(timein) W_c_fun(timein,A,B,1,1),t0,tf);
temp21= quad(@(timein) W_c_fun(timein,A,B,2,1),t0,tf);
temp12= quad(@(timein) W_c_fun(timein,A,B,1,2),t0,tf);
temp22= quad(@(timein) W_c_fun(timein,A,B,2,2),t0,tf);
W_c= [temp11, temp12; temp21, temp22];

u_pre=-B.';
u_post=(W_c\(expm(tf*A)*X_0-X_F));
u_star=zeros(size(T_2));
for alice=1:length(T)
u_star(alice)=u_pre*expm(A.'*(tf-T(alice)))*u_post;
end

Dynamics=@(t,x) [x(2); -Stable*(m*g*l_c/I)*sin(x(1))+...
    (sign(interp1(T_2,u_star,t,'pchip'))*min([abs(interp1(T_2,u_star,t,'pchip')),7]))/I];
[t,Results]=ode45(Dynamics,T_2,X_0);
% Apply saturation
u_star2=sign(interp1(T_2,u_star,t,'pchip')).*...
    min(abs(interp1(T_2,u_star,t,'pchip')),7);
end

function temp = W_c_fun(t,A,B,WhichRow,WhichCol)
temp=zeros([1,length(t)]);
for n=1:length(t)
    temp1=expm(A*t(n))*B*(B.')*expm(A.'*t(n));
    temp(n)=temp1(WhichRow,WhichCol);
end
end


function varargout = sumk(varargin)
% SUMK  Returns sum of k largest (eigen-)values.
%
% s = SUMK(X,k,w)
%
% For a vector X, SUMK returns the sum of the k largest elements.
%
% For a symmetric matrix X, SUMK returns the sum of the k largest eigen-values.
%
% A third argument can be used to generalize to weighted sum.
% The weights have to be non-negative and non-increasing.
%
% See also SUMABSK

if nargin == 1
    error('sumk needs at least two arguments');
end

switch class(varargin{1})

    case 'double' % What is the numerical value of this argument (needed for displays etc)

        X = varargin{1};
        [n,m] = size(X);
        if (min(n,m) > 1 & ~issymmetric(X))
            error('sumk can only be applied on vectors and symmetric matrices');
        else
            k = min(length(X),varargin{2});
            w = ones(k,1);
            if nargin == 3
                w = varargin{3};
                if length(w)==1
                    w = ones(k,1)*w;
                end
            end
            if min(n,m)==1
                sorted = sort(X,'descend');
            else
                sorted = sort(eig(X),'descend');
            end
            sorted = sorted(:);
            w = w(:);
            varargout{1} = sum(sorted(1:k).*w(1:k));
        end

    case 'sdpvar' % Overloaded operator for SDPVAR objects. Pass on args and save them.
        X = varargin{1};
        [n,m] = size(X);
        if nargin < 3
            w = 1;
        else
            w = varargin{3};
            if length(w)>1
                if any(diff(w)>0) || any(w<0)
                    error('The weights have to be non-negative non-increasing')
                end
            end
        end
        if (min(n,m) > 1 & ~issymmetric(X))
            error('sumk can only be applied on vectors and symmetric matrices');
        else
            varargout{1} = yalmip('define',mfilename,varargin{:});
        end

    case 'char' % YALMIP send 'model' when it wants the epigraph or hypograph
        if isequal(varargin{1},'graph')
            t = varargin{2}; % Second arg is the extended operator variable
            X = varargin{3}; % Third arg and above are the args user used when defining t.
            k = min(varargin{4},length(X));
            if nargin == 5
                w = varargin{5};
            else
                w = 1;
            end
            [Model,Properties] = sumk_generator(X,k,t,w);
            varargout{1} = Model;
            varargout{2} = Properties;
            varargout{3} = X;
        else
        end
    otherwise
end


function Ch4Ex1_main()
%% SOS-based Policy Iteration for a car suspension systems
% The function is tested in MATLAB R2014b
% Copyright 2015 Yu Jiang
% Contact Yu Jiang (yu.jiang@nyu.edu)

% System requirements:
% - MATLAB (Manually Tested in MATLAB R2014b)
% - MATLAB Symbolic Toolbox
% - SDPT3-4.0
% - SISOTOOLS (free to download at http://www.cds.caltech.edu/sostools/)

% You can download tools.zip and run setuptools.m in the folder.

syms x1 x2 x3 x4 real
mb = 300;    % kg
mw = 60;     % kg
bs = 1000;   % N/m/s
ks = 16000 ; % N/m
kt = 190000; % N/m
kn = 0.1*ks;

% State matrices
A = [ 0 1 0 0;
    [-ks -bs ks bs]/mb ; ...
    0 0 0 1;
    [ks bs -ks-kt -bs]/mw];
B = [ 0 0; 0 10000/mb ; 0 0 ; [kt -10000]/mw];
B = B(:,2);

% LQR feedback gains for the linearized system
% Klqr = lqr(A,B,eye(4),1);
% f(x)
f = A*[x1;x2;x3;x4] + [0;-kn*(x1-x3)^3/mb;0;kn*(x1-x3)^3/mw];

% Polynomial Weighting functions
q0 = 100*x1^2+x2^2+x3^2+x4^2;
rx = 1;%+(x1^2+x2^2+x3^2+x4^2);
vars = [x1;x2;x3;x4];

% Initialize the SOSp
prog = sosprogram(vars);

% The Lyapunov function V(x)
[prog,V] = sospolyvar(prog,monomials([x1;x2;x3;x4],2:4),'wscoeff');

% Objective of the SOSp
myObj = int(int(int(int(V,-.5,.5),-10,10),-.5,.5),-10,10);

% Add Inequality constraint to assure V is positive definite
prog = sosineq(prog,V-0.0001*(x1^2+x2^2+x3^2+x4^2));

% Add Inequality constraint to assure stability and performance
expr = -[diff(V,x1) diff(V,x2) diff(V,x3) diff(V,x4)]*rx*f-rx*q0;
prog = sosineq(prog,expr);

% Solve the SOSp
prog = sossolve(prog);

% Obtain the Initial Lyapunov function
V0 = sosgetsol(prog,V);
V_old = V0;

% Initializing the old contol policy
u_prev = zeros(size(x1));

% Iteration
for i=1:10
    clear prog V
    %------------------------------ SOSp Start ----------------------------
    prog = sosprogram(vars);
    [prog,V] = sospolyvar(prog,monomials([x1;x2;x3;x4],2:4),'wscoeff');
    prog = sosineq(prog,V_old - V);
    prog = sosineq(prog, V);
    u = -1/2*B'*[diff(V_old,x1) diff(V_old,x2) diff(V_old,x3) diff(V_old,x4)].';
    qfcn =rx*q0 + u'*u;
    expr = -[diff(V,x1) diff(V,x2) diff(V,x3) diff(V,x4)]*(rx*f+B*u)-qfcn;
    prog = sosineq(prog,expr);
    prog = sossetobj(prog, myObj);
    prog = sossolve(prog);
    %------------------------------ SOSp End ------------------------------
    V_ = sosgetsol(prog,V);
    V_old = V_;
end

% Save the improved value function
Vnew = V_;

%% Post-processing results
x0 = [0,0,0,0];  % Iniial Condition
tIntv = [0 3];
[t1,y1] = ode23s(@(t,x) LocalSuspSys(t,x,u), [0 3], x0);
[t,y] = ode23s(@(t,x) LocalSuspSys(t,x,0), tIntv, x0);

%% Plot Results
figure(1)
subplot(411);
plot(t1,y1(:,1),t,y(:,1), 'r:','linewidth',2);
xlabel('time (sec)', 'FontSize',12)
ylabel('x_1','FontSize',12)
hl = legend('Improved performance', 'Uncontrolled performance');
set(hl, 'FontSize', 12');
subplot(412);
plot(t1,y1(:,2),t,y(:,2),'r:','linewidth',2);
xlabel('time (sec)', 'FontSize',12)
ylabel('x_2','FontSize',12)
hl = legend('Improved performance', 'Uncontrolled performance');
set(hl, 'FontSize', 12');
subplot(413);
plot(t1,y1(:,3),t,y(:,3), 'r:','linewidth',2);
xlabel('time (sec)', 'FontSize',12)
ylabel('x_3','FontSize',12)
hl = legend('Improved performance', 'Uncontrolled performance');
set(hl, 'FontSize', 12');
subplot(414);
plot(t1,y1(:,4),t,y(:,4),'r:','linewidth',2);
xlabel('time (sec)', 'FontSize',12)
ylabel('x_4','FontSize',12)
hl = legend('Improved performance', 'Uncontrolled performance');
set(hl, 'FontSize', 12');

figure(2)
xx1 = -.4:.04:.4;
xx2 = -5:0.5:5;
vn = zeros(length(xx1),length(xx2));
v1 = zeros(length(xx1),length(xx2));
un=vn;
ulqr = un;
kn=vn;
k1=v1;
x3=0;
x4=0;
for i=1:length(xx1)
    x1 = xx1(i);
    for j=1:length(xx2)
        x2 = xx2(j);
        vn(i,j)=eval(Vnew);
        v1(i,j)=eval(V0);
    end
end
surf(xx1,xx2,vn')
hold on
surf(xx1,xx2,v1')
hold off
xlabel('x_1', 'FontSize', 12)
ylabel('x_2', 'FontSize', 12)
view(gca,[-30.5 28]);
% Create textarrow
annotation(gcf,'textarrow',[0.210714285714286 0.174535137214669],...
    [0.895238095238095 0.631440045897884],'TextEdgeColor','none','FontSize',12,...
    'String',{'V_0(x1,x2,0,0)'});

% Create textarrow
annotation(gcf,'textarrow',[0.139285714285714 0.186735060271868],...
    [0.183333333333333 0.386454388984516],'TextEdgeColor','none','FontSize',12,...
    'String',{'V_{10}(x1,x2,0,0)'});
% export_fig Ex3_cost -pdf -transparent

end

%% LocalSuspSys
% Dynamics of the nonlinear suspension system
function dx = LocalSuspSys(t,x,u)
mb = 300;    % kg
mw = 60;     % kg
bs = 1000;   % N/m/s
ks = 16000 ; % N/m
kt = 190000; % N/m

[x1,x2,x3,x4] = deal(x(1),x(2),x(3),x(4));

% State matrices
A = [ 0 1 0 0;
    [-ks -bs ks bs]/mb ; ...
    0 0 0 1;
    [ks bs -ks-kt -bs]/mw];
B = [ 0; 10000/mb ; 0 ; -10000/mw];
B1 = [ 0; 0 ; 0 ; kt/mw];

if ~isdouble(u)
    u = eval(u);
end

if t <= 0.001
    r = 10;
else
    r = 0;
end

dx = A*x + B*u + B1*r;
end


function x=t2x(T,str)

% x=t2x(T,str);
%
% Converts transformation matrix T between B and A coordinate
% frames into a generalized position vector x, which contains
% position and orientation vectors of B with respect to A.
% Orientation can be expressed with quaternions, euler angles
% (xyz or zxz convention), unit vector and rotation angle.
% Also both orientation and position can be expressed with
% Denavitt-Hartemberg parameters.
%
% ---------------------------------------------------------------------------
%
% The transformation matrix T between B and A coordinate
% frames is a 4 by 4 matrix such that:
% T(1:3,1:3) = Orientation matrix between B and A = unit vectors
%              of x,y,z axes of B expressed in the A coordinates.
% T(1:3,4)   = Origin of B expressed in A coordinates.
% T(4,1:3)   = zeros(1,3)
% T(4,4)     = 1
%
% ---------------------------------------------------------------------------
%
% The generalized position vector x contains the origin of B
% expressed in the A coordinates in the first four entries,
% and orientation of B with respect to A in the last four entries.
% In more detail, its shape depends on the value of str as
% specified below :
%
% ---------------------------------------------------------------------------
%
% str='van' : UNIT VECTOR AND ROTATION ANGLE
%
%           [ Ox ]     origin of the B coordinate frame
%  x(1:4) = [ Oy ]     with respect to A.
%           [ Oz ]
%           [  1 ]
%
%           [ Vx ]     Vx,Vy,Vz = unit vector respect to A,
%  x(5:8) = [ Vy ]     which B is rotated about.
%           [ Vz ]
%           [ Th ]     Th = angle which B is rotated (-pi,pi].
% ---------------------------------------------------------------------------
%
% str='qua' : UNIT QUATERNION
%
%           [ Ox ]     origin of the B coordinate frame
%  x(1:4) = [ Oy ]     with respect to A.
%           [ Oz ]
%           [  1 ]
%
%           [ q1 ]     q1,q2,q3 = V*sin(Th/2)
%  x(5:8) = [ q2 ]     q0 = cos(Th/2) where :
%           [ q3 ]     V = unit vector respect to A, which B is
%           [ q0 ]     rotated about, Th = angle which B is rotated (-pi,pi].
% ---------------------------------------------------------------------------
%
% str='erp' : EULER-RODRIGUEZ PARAMETERS
%
%           [ Ox ]     origin of the B coordinate frame
%  x(1:4) = [ Oy ]     with respect to A.
%           [ Oz ]
%           [  1 ]
%
%           [ r1 ]     r1,r2,r3 = V*tan(Th/2), where :
%  x(5:8) = [ r2 ]     V = unit vector with respect to A, which B is
%           [ r3 ]     rotated about.
%           [  0 ]     Th = angle which B is rotated (-pi,pi) (<> pi).
% ---------------------------------------------------------------------------
%
% str='rpy' : ROLL, PITCH, YAW ANGLES (euler x-y-z convention)
%
%           [ Ox ]     origin of the B coordinate frame
%  x(1:4) = [ Oy ]     with respect to A.
%           [ Oz ]
%           [  1 ]
%
%            [ r ]     r = roll angle  ( fi    (-pi,pi], about x,          )
%  x(5:8) =  [ p ]     p = pitch angle ( theta (-pi,pi], about y, <> +-pi/2)
%            [ y ]     y = yaw angle   ( psi   (-pi,pi], about z,          )
%            [ 0 ]
% ---------------------------------------------------------------------------
%
% str='rpm' : ROTATION, PRECESSION, MUTATION ANGLES (euler z-x-z convention)
%
%           [ Ox ]     origin of the B coordinate frame
%  x(1:4) = [ Oy ]     with respect to A.
%           [ Oz ]
%           [  1 ]
%
%            [ r ]     r = rotation angle   ( (-pi,pi] ,about z           )
%  x(5:8) =  [ p ]     p = precession angle ( (-pi,pi] ,about x , <> 0,pi )
%            [ y ]     y = mutation angle   ( (-pi,pi] ,about z           )
%            [ 0 ]
% ---------------------------------------------------------------------------
%
% str='dht' : DENAVITT-HARTEMBERG PARAMETERS
%
%            [ b ]                [ a ]     this four-parameter
%  x(1:4) =  [ d ] ,   x(5:8) =   [ t ] ,   description does not involve
%            [ 0 ]                [ 0 ]     a loss of information if and
%            [ 0 ]                [ 0 ]     only if T has this shape:
%
%           [    ct   -st     0     b ]     where :
%  T =      [ ca*st ca*ct   -sa -d*sa ]
%           [ sa*st sa*ct    ca  d*ca ]     sa = sin(a), ca = cos(a)
%           [     0     0     0     1 ]     st = sin(t), ct = cos(t)
% ---------------------------------------------------------------------------
%
% Example (see also x2t):
% x=[rand(3,1);1;rand(3,1);0];x-t2x(x2t(x,'rpm'),'rpm')
%
%
% Giampiero Campa 1/11/96
%

% ---------------------------------------------------------------------------
% UNIT VECTOR AND ROTATION ANGLE

if [ str=='van' size(T)==[4 4] ],

O=T(1:3,4);
R=T(1:3,1:3);
d=round(.5*(trace(R)-1)*1e12)/1e12;

if d==1,
   v=[0 0 1]';
   th=0;

elseif d==-1,
    v0=sum(R'+eye(3,3))';

    if v0 == 0
	 v0=R(:,2)+R(:,3)+[0 1 1]';
    end;

    if v0 == 0
	 v0=R(:,3)+[0 0 1]';
    end;

    v=v0/norm(v0);
    th=pi;

else

   sg=(vp([1 0 0]',R(:,1))+vp([0 1 0]',R(:,2))+vp([0 0 1]',R(:,3)));

    if norm(sg) < 1e-12
        disp(' ');
        disp('T2x warning: det(R)<>1, unit vector assumed to be [0 0 1]''.');
        disp(' ');
        sg=[0 0 1]';
    end

   v=sg/norm(sg);
   th=atan2(norm(sg)/2,d);

end

x=[O;1;v;th];

% ---------------------------------------------------------------------------
% UNIT QUATERNION

elseif [ str=='qua' size(T)==[4 4] ],

O=T(1:3,4);
R=T(1:3,1:3);
d=round(.5*(trace(R)-1)*1e12)/1e12;

if d==1,
   v=[0 0 1]';
   th=0;

elseif d==-1,
    v0=sum(R'+eye(3,3))';

    if v0 == 0
	v0=R(:,2)+R(:,3)+[0 1 1]';
    end;

    if v0 == 0
	v0=R(:,3)+[0 0 1]';
    end;

    v=v0/norm(v0);
    th=pi;

else

   sg=(vp([1 0 0]',R(:,1))+vp([0 1 0]',R(:,2))+vp([0 0 1]',R(:,3)));

    if norm(sg) < 1e-12
        disp(' ');
        disp('T2x warning: det(R)<>1, unit vector assumed to be [0 0 1]''.');
        disp(' ');
        sg=[0 0 1]';
    end

   v=sg/norm(sg);
   th=atan2(norm(sg)/2,d);

end

q=v*sin(th/2);
q0=cos(th/2);

x=[O;1;q;q0];

% ---------------------------------------------------------------------------
% EULER-RODRIGUEZ PARAMETERS

elseif [ str=='erp' size(T)==[4 4] ],

O=T(1:3,4);
R=T(1:3,1:3);
d=round(.5*(trace(R)-1)*1e12)/1e12;

if d==1,
   v=[0 0 1]';
   th=0;

elseif d==-1,
    v0=sum(R'+eye(3,3))';

    if v0 == 0
	v0=R(:,2)+R(:,3)+[0 1 1]';
    end;

    if v0 == 0
	v0=R(:,3)+[0 0 1]';
    end;

    v=v0/norm(v0);
    th=pi;

else

   sg=(vp([1 0 0]',R(:,1))+vp([0 1 0]',R(:,2))+vp([0 0 1]',R(:,3)));

    if norm(sg) < 1e-12
        disp(' ');
        disp('T2x warning: det(R)<>1, unit vector assumed to be [0 0 1]''.');
        disp(' ');
        sg=[0 0 1]';
    end

   v=sg/norm(sg);
   th=atan2(norm(sg)/2,d);

end

p=v*tan(th/2);
x=[O;1;p;0];

% ---------------------------------------------------------------------------
% ROLL, PITCH, YAW ANGLES (euler x-y-z convention)

elseif [ str=='rpy' size(T)==[4 4] ],

O=T(1:3,4);
R=T(1:3,1:3);
d=round([0 0 1]*R(:,1)*1e12)/1e12;

if d==1,
   y=atan2([0 1 0]*R(:,2),[1 0 0]*R(:,2));
   p=-pi/2;
   r=-pi/2;

elseif d==-1
   y=atan2([0 1 0]*R(:,2),[1 0 0]*R(:,2));
   p=pi/2;
   r=pi/2;

else
   sg=vp([0 0 1]',R(:,1));
   j2=sg/sqrt(sg'*sg);
   k2=vp(R(:,1),j2);

   r=atan2(k2'*R(:,2),j2'*R(:,2));
   p=atan2(-[0 0 1]*R(:,1),[0 0 1]*k2);
   y=atan2(-[1 0 0]*j2,[0 1 0]*j2);
end

y1=y+(1-sign(y)-sign(y)^2)*pi;
p1=p+(1-sign(p)-sign(p)^2)*pi;
r1=r+(1-sign(r)-sign(r)^2)*pi;

% takes smaller values of angles

if norm([y1 p1 r1]) < norm([y p r])
    x=[O;1;r1;-p1;y1;0];
else
    x=[O;1;r;p;y;0];
end

% ---------------------------------------------------------------------------
% ROTATION, PRECESSION, MUTATION ANGLES (euler z-x-z convention)

elseif [ str=='rpm' size(T)==[4 4] ],

O=T(1:3,4);
R=T(1:3,1:3);
d=round([0 0 1]*R(:,3)*1e12)/1e12;

if d==1,
   m=0;
   p=0;
   r=atan2([0 1 0]*R(:,1),[1 0 0]*R(:,1));

elseif d==-1
   m=0;
   p=pi;
   r=atan2([0 1 0]*R(:,1),[1 0 0]*R(:,1));

else
   sg=vp([0 0 1]',R(:,3));
   i2=sg/norm(sg);
   j2=vp(R(:,3),i2);

   m=atan2(j2'*R(:,1),i2'*R(:,1));
   p=atan2([0 0 1]*j2,[0 0 1]*R(:,3));
   r=atan2([0 1 0]*i2,[1 0 0]*i2);

end

r1=r+(1-sign(r)-sign(r)^2)*pi;
p1=p;
m1=m+(1-sign(m)-sign(m)^2)*pi;

% takes the smaller values of angles

if norm([r1 p1 m1]) < norm([r p m])
    x=[O;1;r1;-p1;m1;0];
else
    x=[O;1;r;p;m;0];
end

% ---------------------------------------------------------------------------
% DENAVITT-HARTEMBERG PARAMETERS

elseif [ str=='dht' size(T)==[4 4] ],

% b calculation
b=T(1,4);

% d calculation
if norm(T(3,3)) > norm(T(2,3))
    d=T(3,4)/T(3,3);
else
    d=T(2,4)/T(2,3);
end

% alfa and theta computation by mean of euler-zxz angles
R=T(1:3,1:3);
dl=round([0 0 1]*R(:,3)*1e12)/1e12;

if dl==1,
   th=0;
   alfa=0;
   n=atan2([0 1 0]*R(:,1),[1 0 0]*R(:,1));
elseif dl==-1
   th=0;
   alfa=pi;
   n=atan2([0 1 0]*R(:,1),[1 0 0]*R(:,1));
else
   sg=vp([0 0 1]',R(:,3));
   i2=sg/norm(sg);
   j2=vp(R(:,3),i2);

   th=atan2(j2'*R(:,1),i2'*R(:,1));
   alfa=atan2([0 0 1]*j2,[0 0 1]*R(:,3));
   n=atan2([0 1 0]*i2,[1 0 0]*i2);
end

th1=th+(1-sign(th)-sign(th)^2)*pi;
alfa1=alfa;
n1=n+(1-sign(n)-sign(n)^2)*pi;

% takes the smaller values of angles

if norm([th1 alfa1 n1]) < norm([th alfa n])
   x=[b;d;0;0;-alfa1;th1;0;0];
else
   x=[b;d;0;0;alfa;th;0;0];
end

% ---------------------------------------------------------------------------
% OTHER STRING

else

disp('   ');
disp('   x=T2x(T,str)');
disp('   where T is a 4 by 4 matrix (see help for details)');
disp('   and str can be : ''van'',''qua'',''erp'',''rpy'',''rpm'',''dht''. ');
disp('   ');

end

function z=vp(x,y)

% z=vp(x,y); z = 3d cross product of x and y
% vp(x) is the 3d cross product matrix : vp(x)*y=vp(x,y).
%
% by Giampiero Campa.

z=[  0    -x(3)   x(2);
    x(3)    0    -x(1);
   -x(2)   x(1)    0   ];

if nargin>1, z=z*y; end


function prob_test1341 ( )

%*****************************************************************************80
%
%% TEST01341 checks RIBESL against BESSEL_IX_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST1341:\n' );
  fprintf ( 1, '  RIBESL computes values of Bessel functions\n' );
  fprintf ( 1, '  of NONINTEGER order.\n' );
  fprintf ( 1, '  BESSEL_IX_VALUES returns selected values of the\n' );
  fprintf ( 1, '  Bessel function In for NONINTEGER order.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, ...
    '      ALPHA         X             FX                        FX2\n' );
  fprintf ( 1, ...
    '                                  (table)                   (RIBESL)\n' );
  fprintf ( 1, '\n' );
  n_data = 0;

  while ( 1 )

    [ n_data, alpha, x, fx ] = bessel_ix_values ( n_data );

    if ( n_data == 0 );
      break
    end

    ize = 1;
    nb = floor ( alpha ) + 1;
    alpha_frac = alpha - floor ( alpha );

    [ b, ncalc ] = ribesl ( x, alpha_frac, nb, ize );
    fx2 = b(nb);

    fprintf ( 1, '  %12f  %12f  %24.16e  %24.16e\n', alpha, x, fx, fx2 );

  end

  return
end


%  Script file: doy.m
%
%  Purpose:
%    This program calculates the day of year corresponding
%    to a specified date.  It illustrates the use switch
%    and for constructs.
%
%  Record of revisions:
%      Date       Programmer          Description of change
%      ====       ==========          =====================
%    01/27/07    S. J. Chapman        Original code
%
% Define variables:
%   day          -- Day (dd)
%   day_of_year  -- Day of year
%   ii           -- Loop index
%   leap_day     -- Extra day for leap year
%   month        -- Month (mm)
%   year         -- Year (yyyy)

% Get day, month, and year to convert
disp('This program calculates the day of year given the ');
disp('specified date.');
month = input('Enter specified month (1-12): ');
day   = input('Enter specified day(1-31):    ');
year  = input('Enter specified year(yyyy):   ');

% Check for leap year, and add extra day if necessary
if mod(year,400) == 0
   leap_day = 1;          % Years divisible by 400 are leap years
elseif mod(year,100) == 0
   leap_day = 0;          % Other centuries are not leap years
elseif mod(year,4) == 0
   leap_day = 1;          % Otherwise every 4th year is a leap year
else
   leap_day = 0;          % Other years are not leap years
end

% Calculate day of year by adding current day to the
% days in previous months.
day_of_year = day;
for ii = 1:month-1

   % Add days in months from January to last month
   switch (ii)
   case {1,3,5,7,8,10,12},
      day_of_year = day_of_year + 31;
   case {4,6,9,11},
      day_of_year = day_of_year + 30;
   case 2,
      day_of_year = day_of_year + 28 + leap_day;
   end

end

% Tell user
fprintf('The date %2d/%2d/%4d is day of year %d.\n', ...
         month, day, year, day_of_year);


function centroid = polyhedronCentroid(vertices, faces) %#ok<INUSD>
%POLYHEDRONCENTROID Compute the centroid of a 3D convex polyhedron.
%
%   CENTRO = polyhedronCentroid(V, F)
%   Computes the centroid (center of mass) of the polyhedron defined by
%   vertices V and faces F.
%   The polyhedron is assumed to be convex.
%
%   Example
%     % Creates a polyhedron centered on origin, and add an arbitrary
%     % translation
%     [v, f] = createDodecahedron;
%     v2 = bsxfun(@plus, v, [3 4 5]);
%     % computes the centroid, that should equal the translation vector
%     centroid = polyhedronCentroid(v2, f)
%     centroid =
%         3.0000    4.0000    5.0000
%
%
%   See also
%   meshes3d, meshVolume, meshSurfaceArea, polyhedronMeanBreadth
%

% ------
% Author: David Legland
% E-mail: david.legland@nantes.inra.fr
% Created: 2012-04-05, using Matlab 7.9.0.529 (R2009b)
% Copyright 2012-2022 INRA - Cepia Software Platform

% compute set of elementary tetrahedra
DT = delaunayTriangulation(vertices);
T = DT.ConnectivityList;

% number of tetrahedra
nT  = size(T, 1);

% initialize result
centroid = zeros(1, 3);
vt = 0;

% Compute the centroid and the volume of each tetrahedron
for i = 1:nT
    % coordinates of tetrahedron vertices
    tetra = vertices(T(i, :), :);

    % centroid is the average of vertices.
    centi = mean(tetra);

    % compute volume of tetrahedron
    vol = det(tetra(1:3,:) - tetra([4 4 4],:)) / 6;

    % add weighted centroid of current tetraedron
    centroid = centroid + centi * vol;

    % compute the sum of tetraedra volumes
    vt = vt + vol;
end

% compute by sum of tetrahedron volumes
centroid = centroid / vt;