function fem1d_adaptive ( )

%*****************************************************************************80
%
%% MAIN is the main program for FEM1D_ADAPTIVE.
%
%  Discussion:
%
%    FEM1D_ADAPTIVE solves a 1D problem using an adaptive finite element method.
%
%    The equation to be treated is:
%
%      -d/dx ( P(x) * dU(x)/dx ) + Q(x) * U(x)  =  F(x)
%
%    by the finite-element method using piecewise linear basis
%    functions.
%
%    An adaptive method is used to try to reduce the maximum
%    error by refining the mesh in certain places.
%
%    Here U is an unknown scalar function of X defined on the
%    interval [XL,XR], and P, Q and F are given functions of X.
%
%    The values of U at XL and XR are also specified.
%
%    The interval [XL,XR] is "meshed" with N+1 points,
%
%      XN(0) = XL, XN(1) = XL+H, XN(2) = XL+2*H, ..., XN(N) = XR.
%
%    This creates N subintervals, with interval I having endpoints
%    XN(I-1) and XN(I).
%
%    The algorithm tries to guarantee a certain amount
%    of accuracy by examining the current solution, estimating the error
%    in each subinterval, and, if necessary, subdividing one or more
%    subintervals and repeating the calculation.
%
%    We can think of the adaptive part of the algorithm as a refined
%    problem.  The program re-solves the problem on the pair of
%    intervals J and J+1, which extend from node J-1 to node J+1.
%    The values of U that were just computed at nodes J-1 and J+1
%    will be used as the boundary values for this refined problem.
%    The intervals J and J+1 will each be evenly divided into NY
%    smaller subintervals.  This boundary value problem is solved,
%    and the derivatives of the original and refined solutions are
%    then compared to get an estimate of the error.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    real ADIAG(NU).
%    ADIAG(I) is the "diagonal" coefficient of the I-th
%    equation in the linear system.  That is, ADIAG(I) is
%    the coefficient of the I-th unknown in the I-th equation.
%
%    real ALEFT(NU).
%    ALEFT(I) is the "left hand" coefficient of the I-th
%    equation in the linear system.  That is, ALEFT(I) is the
%    coefficient of the (I-1)-th unknown in the I-th equation.
%    There is no value in ALEFT(1), since the first equation
%    does not refer to a "0-th" unknown.
%
%    real ARITE(NU).
%    ARITE(I) is the "right hand" coefficient of the I-th
%    equation in the linear system.  ARITE(I) is the coefficient
%    of the (I+1)-th unknown in the I-th equation.  There is
%    no value in ARITE(NU) because the NU-th equation does not
%    refer to an "NU+1"-th unknown.
%
%    real ETA(N).
%    ETA(I) is the error estimate for interval I.  It is computed
%    as the sum of two quantities, one associated with the left
%    and one with the right node of the interval.
%
%    real F(NU).
%    ASSEMBLE stores into F the right hand side of the linear
%    equations.
%    SOLVE replaces those values of F by the solution of the
%    linear equations.
%
%    real FY(M).
%    FY is the right hand side of the linear system of the refined
%    problem.
%
%    real H(N)
%    H(I) is the length of subinterval I.  This code uses
%    equal spacing for all the subintervals.
%
%    real HY(M).
%    HY(I) is the length of subinterval I in the refined problem.
%
%    integer IBC.
%    IBC declares what the boundary conditions are.
%    1, at the left endpoint, U has the value UL,
%       at the right endpoint, U' has the value UR.
%    2, at the left endpoint, U' has the value UL,
%       at the right endpoint, U has the value UR.
%    3, at the left endpoint, U has the value UL,
%       and at the right endpoint, U has the value UR.
%    4, at the left endpoint, U' has the value UL,
%       at the right endpoint U' has the value UR.
%
%    integer IBCY.
%    IBCY declares the boundary conditions for the refined problem
%    which should always be that the value of U is specified at
%    both the left and right endpoints.  This corresponds to a
%    value of IBCY = 3.
%
%    integer INDX(0:N).
%    For a node I, INDX(I) is the index of the unknown
%    associated with node I.
%    If INDX(I) is equal to -1, then no unknown is associated
%    with the node, because a boundary condition fixing the
%    value of U has been applied at the node instead.
%    Unknowns are numbered beginning with 1.
%    If IBC is 2 or 4, then there is an unknown value of U
%    at node 0, which will be unknown number 1.  Otherwise,
%    unknown number 1 will be associated with node 1.
%    If IBC is 1 or 4, then there is an unknown value of U
%    at node N, which will be unknown N or N+1,
%    depending on whether there was an unknown at node 0.
%
%    integer INDY(0:M).
%    INDY(I) records the index of the unknown associated with
%    node I for the refined problem.
%
%    integer JADD(N).
%    JADD(I) is 1 if the error estimates show that interval I
%    should be subdivided.
%
%    integer KOUNT, the number of adaptive steps that have been taken.
%
%    integer M.
%    M is the number of subintervals used in the refined problem.
%    M is equal to NY for computations centered at node 0 or node N,
%    and otherwise, M is equal to 2*NY.
%
%    integer N
%    The number of subintervals into which the interval
%    [XL,XR] is broken.
%
%    integer NL.
%    The number of basis functions used in a single
%    subinterval.  (NL-1) is the degree of the polynomials
%    used.  For this code, NL is fixed at 2, meaning that
%    piecewise linear functions are used as the basis.
%
%    integer NMAX, the maximum number of unknowns that can be handled.
%
%    integer NODE(NL,N).
%    For each subinterval I:
%    NODE(1,I) is the number of the left node, and
%    NODE(2,I) is the number of the right node.
%
%    integer NODEY(NL,M).
%    NODEY performs the same function for the refined problem that
%    NODE performs for the full problem, recording the node numbers
%    associated with a particular subinterval.
%
%    integer NQUAD
%    The number of quadrature points used in a subinterval.
%
%    integer NU.
%    NU is the number of unknowns in the linear system.
%    Depending on the value of IBC, there will be N-1,
%    N, or N+1 unknown values, which are the coefficients
%    of basis functions.
%
%    integer NUY.
%    The number of unknowns in the refined problem.
%
%    integer NY.
%    NY is the number of subintervals into which a given interval
%    will be subdivided, before solving the refined probelm.
%
%    integer PROBLEM, chooses the problem to be solved.
%    The user must choose this value by setting it in routine GETPRB.
%    * 1, u = x, p = 1, q = 0, f = 0, ibc = 3, ul = 0, ur = 1.
%    The program should find the solution exactly, and the
%    adaptive code should find that there is no reason to
%    subdivide any interval.
%    * 2, u = x*x, p = 1, q = 0, f = -2, ibc = 3, ul = 0, ur = 1.
%    This problem should find the solution exactly, and
%    the adaptive code should again find there is nothing
%    to do.
%    *3, u = sin(pi*x/2), p = 1, q = 0, ibc = 3, f = 0.25*pi*pi*sin(pi*x/2),
%    ul = 0, ur = 1.
%    *4, u = cos(pi*x/2), p = 1, q = 0, ibc = 3, f = 0.25*pi*pi*cos(pi*x/2),
%    ul = 1, ur = 0.
%    *5: u = x**(beta+2)/((beta+2)*(beta+1)), p = 1, q = 1, ibc = 3,
%    f = -x**beta + (x**(beta+2))/((beta+2)*(beta+1)),
%    ul = 0, ur = 1/((beta+2)*(beta+1))
%    (beta must be greater than -2, and not equal to -1)
%    *6: u = atan((x-0.5)/alpha), p = 1, q = 0, ibc = 3,
%    f =  2*alpha*(x-0.5) / (alpha^2 + (x-0.5)^2) ^2,
%    ul = u(0), ur = u(1)
%
%    integer STATUS, reports status of subdivision.
%    0, a new subdivision was carried out.
%    1, no more subdivisions are needed.
%    -1, no more subdivisions can be carried out.
%
%    real TOL.
%    A tolerance that is used to determine whether the estimated
%    error in an interval is so large that it should be subdivided
%    and the problem solved again.
%
%    real UL.
%    If IBC is 1 or 3, UL is the value that U is required
%    to have at X = XL.
%    If IBC is 2 or 4, UL is the value that U' is required
%    to have at X = XL.
%
%    real UR.
%    If IBC is 2 or 3, UR is the value that U is required
%    to have at X = XR.
%    If IBC is 1 or 4, UR is the value that U' is required
%    to have at X = XR.
%
%    real WQUAD(NQUAD).
%    WQUAD(I) is the weight associated with the I-th point
%    of an NQUAD point Gaussian quadrature rule.
%
%    real XL.
%    XL is the left endpoint of the interval over which the
%    differential equation is being solved.
%
%    real XN(0:N).
%    XN(I) is the location of the I-th node.  XN(0) is XL,
%    and XN(N) is XR.
%
%    real XQUAD(NQUAD,NMAX), the I-th quadrature point
%    in interval J.
%
%    real XQUADY(NQUAD,NMAY ), the I-th quadrature point
%    in subinterval J of the refined problem.
%
%    real XR.
%    XR is the right endpoint of the interval over which the
%    differential equation is being solved.
%
%    Workspace, double precision XT(0:NMAX), used to compute a new
%    set of nodes.
%
%    real YN(0:M).
%    YN(I) is the location of the I-th node in the refined
%    problem.
%
  nl = 2;
  nmax = 30;
  nquad = 2;

  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FEM1D_ADAPTIVE\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'Solve the two-point boundary value problem:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  -d/dx ( P(x) * dU(x)/dx ) + Q(x) * U(x)  =  F(x)\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'on the interval [0,1], specifying the value\n' );
  fprintf ( 1, 'of U at each endpoint.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'The number of basis functions per element is %d\n', nl );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'The number of quadrature points per element is %d\n', nquad );

  problem = get_problem ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Problem index = %d\n', problem );
  fprintf ( 1, '\n' );

  if ( problem == 1 )

    fprintf ( 1, '\n' );
    fprintf ( 1, '  "Linear" problem:\n' );
    fprintf ( 1, '  (No refinement needed)\n' );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  U(X) =  X\n' );
    fprintf ( 1, '  P(X) =  1.0\n' );
    fprintf ( 1, '  Q(X) =  0.0\n' );
    fprintf ( 1, '  F(X) =  0.0\n' );
    fprintf ( 1, '  IBC  =  3\n' );
    fprintf ( 1, '  UL   =  0.0\n' );
    fprintf ( 1, '  UR   =  1.0\n' );

  elseif ( problem == 2 )

    fprintf ( 1, '\n' );
    fprintf ( 1, '  "Quadratic" problem:\n' );
    fprintf ( 1, '  (No refinement needed)\n' );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  U(X) =  X*X\n' );
    fprintf ( 1, '  P(X) =  1.0\n' );
    fprintf ( 1, '  Q(X) =  0.0\n' );
    fprintf ( 1, '  F(X) = -2.0\n' );
    fprintf ( 1, '  IBC  =  3\n' );
    fprintf ( 1, '  UL   =  0.0\n' );
    fprintf ( 1, '  UR   =  1.0\n' );

  elseif ( problem == 3 )

    fprintf ( 1, '\n' );
    fprintf ( 1, '  "SINE" problem:\n' );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  U(X) =  SIN(PI*X/2)\n' );
    fprintf ( 1, '  P(X) =  1.0\n' );
    fprintf ( 1, '  Q(X) =  0.0\n' );
    fprintf ( 1, '  F(X) =  PI*PI*SIN(PI*X/2)/4\n' );
    fprintf ( 1, '  IBC  =  3\n' );
    fprintf ( 1, '  UL   =  0.0\n' );
    fprintf ( 1, '  UR   =  1.0\n' );

  elseif ( problem == 4 )

    fprintf ( 1, '\n' );
    fprintf ( 1, '  "COSINE" problem:\n' );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  U(X) =  COS(PI*X/2)\n' );
    fprintf ( 1, '  P(X) =  1.0\n' );
    fprintf ( 1, '  Q(X) =  0.0\n' );
    fprintf ( 1, '  F(X) =  PI*PI*COS(PI*X/2)/4\n' );
    fprintf ( 1, '  IBC  =  3\n' );
    fprintf ( 1, '  UL   =  0.0\n' );
    fprintf ( 1, '  UR   =  1.0\n' );

  elseif ( problem == 5 )

    beta = get_beta ( );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  "RHEINBOLDT" problem:\n' );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  U(X) =  X**(B+2)/((B+2)*(B+1))\n' );
    fprintf ( 1, '  P(X) =  1.0\n' );
    fprintf ( 1, '  Q(X) =  1.0\n' );
    fprintf ( 1, '  F(X) =  -X**B+(X**B+2))/((B+2)*(B+1))\n' );
    fprintf ( 1, '  IBC  =  3\n' );
    fprintf ( 1, '  UL   =  0.0\n' );
    fprintf ( 1, '  UR   =  1/((B+2)*(B+1))\n' );
    fprintf ( 1, '  B    = %f\n', beta );

  elseif ( problem == 6 )

    alpha = get_alpha ( );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  "ARCTAN" problem:\n' );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  U(X) =  ATAN((X-0.5)/A)\n' );
    fprintf ( 1, '  P(X) =  1.0\n' );
    fprintf ( 1, '  Q(X) =  0.0\n' );
    fprintf ( 1, '  F(X) =  2*A*(X-0.5)/(A^2+(X-0.5)^2)^2\n' );
    fprintf ( 1, '  IBC  =  3\n' );
    fprintf ( 1, '  UL   =  ATAN(-0.5/A)\n' );
    fprintf ( 1, '  UR   =  ATAN( 0.5/A)\n' );
    fprintf ( 1, '  A    = %f\n', alpha );

  end
%
%  Start out with just 4 subintervals.
%
  n = 4;
%
%  Initialize values that define the problem.
%
  [ ibc, tol, ul, ur, xl, xn, xr ] = init ( n );
%
%  Start the iteration counter off at 0.
%
  kount = 0;
%
%  Begin the next iteration.
%
  while ( 1 )

    kount = kount + 1;

    fprintf ( 1, '\n' );
    fprintf ( 1, 'Begin new iteration with %d nodes.\n', n );
    fprintf ( 1, '\n' );
%
%  Solve the regular problem.
%
    [ u, h, nu ] = solvex ( ibc, kount, n, nl, nmax, ul, ur, xn );
%
%  Solve N subproblems to get the error estimators.
%
    eta = solvey ( u, h, n, nu, ul, ur, xn );
%
%  Examine the error estimators, and see how many intervals should
%  be subdivided.
%
    [ n, xn, status ] = subdiv ( eta, kount, n, nmax, tol, xn );

    if ( status ~= 0 )
      break
    end

  end
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FEM1D_ADAPTIVE:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end
function [ adiag, aleft, arite, f ] = assemble ( h, n, indx, node, nu, nl, ...
  nquad, nmax, ul, ur, wquad, xn, xquad )

%*****************************************************************************80
%
%% ASSEMBLE assembles the global matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, real H(N)
%    H(I) is the length of subinterval I.  This code uses
%    equal spacing for all the subintervals.
%
%    Input, integer N
%    The number of subintervals into which the interval
%    [XL,XR] is broken.
%
%    Input, integer INDX(0:N).
%    For a node I, INDX(I) is the index of the unknown
%    associated with node I.
%    If INDX(I) is equal to -1, then no unknown is associated
%    with the node, because a boundary condition fixing the
%    value of U has been applied at the node instead.
%    Unknowns are numbered beginning with 1.
%    If IBC is 2 or 4, then there is an unknown value of U
%    at node 0, which will be unknown number 1.  Otherwise,
%    unknown number 1 will be associated with node 1.
%    If IBC is 1 or 4, then there is an unknown value of U
%    at node N, which will be unknown N or N+1,
%    depending on whether there was an unknown at node 0.
%
%    Input, integer NODE(NL,N).
%    For each subinterval I:
%    NODE(1,I) is the number of the left node, and
%    NODE(2,I) is the number of the right node.
%
%    Input, integer NU.
%    NU is the number of unknowns in the linear system.
%    Depending on the value of IBC, there will be N-1,
%    N, or N+1 unknown values, which are the coefficients
%    of basis functions.
%
%    Input, integer NL.
%    The number of basis functions used in a single
%    subinterval.  (NL-1) is the degree of the polynomials
%    used.  For this code, NL is fixed at 2, meaning that
%    piecewise linear functions are used as the basis.
%
%    Input, integer NQUAD
%    The number of quadrature points used in a subinterval.
%
%    Input, integer NMAX, the maximum number of unknowns that can be handled.
%
%    Input, real UL.
%    If IBC is 1 or 3, UL is the value that U is required
%    to have at X = XL.
%    If IBC is 2 or 4, UL is the value that U' is required
%    to have at X = XL.
%
%    Input, real UR.
%    If IBC is 2 or 3, UR is the value that U is required
%    to have at X = XR.
%    If IBC is 1 or 4, UR is the value that U' is required
%    to have at X = XR.
%
%    Input, real WQUAD(NQUAD).
%    WQUAD(I) is the weight associated with the I-th point
%    of an NQUAD point Gaussian quadrature rule.
%
%    Input, real XN(0:N).
%    XN(I) is the location of the I-th node.  XN(0) is XL,
%    and XN(N) is XR.
%
%    Input, real XQUAD(NQUAD,NMAX), the I-th quadrature point
%    in interval J.
%
%    Output, real ADIAG(NU).
%    ADIAG(I) is the "diagonal" coefficient of the I-th
%    equation in the linear system.  That is, ADIAG(I) is
%    the coefficient of the I-th unknown in the I-th equation.
%
%    Output, real ALEFT(NU).
%    ALEFT(I) is the "left hand" coefficient of the I-th
%    equation in the linear system.  That is, ALEFT(I) is the
%    coefficient of the (I-1)-th unknown in the I-th equation.
%    There is no value in ALEFT(1), since the first equation
%    does not refer to a "0-th" unknown.
%
%    Output, real ARITE(NU).
%    ARITE(I) is the "right hand" coefficient of the I-th
%    equation in the linear system.  ARITE(I) is the coefficient
%    of the (I+1)-th unknown in the I-th equation.  There is
%    no value in ARITE(NU) because the NU-th equation does not
%    refer to an "NU+1"-th unknown.
%
%    Output, real F(NU).
%    ASSEMBLE stores into F the right hand side of the linear
%    equations.
%    SOLVE replaces those values of F by the solution of the
%    linear equations.
%

%
%  Zero out the entries.
%
  f(1:nu) = 0.0;
  aleft(1:nu) = 0.0;
  arite(1:nu) = 0.0;
  adiag(1:nu) = 0.0;
%
%  For each interval,
%
  for ie = 1 : n

    he = h(ie);
    xleft = xn(node(1,ie)+1);
    xrite = xn(node(2,ie)+1);
%
%  For each quadrature point in the interval,
%
    for iq = 1 : nquad

      xquade = xquad(iq,ie);
      wquade = wquad(iq);
%
%  Pick a basis function which defines the equation,
%
      for il = 1 : nl

        ig = node(il,ie);
        iu = indx(ig+1);

        if ( 0 < iu )

          [ phii, phiix ] = phi ( il, xquade, xleft, xrite );
          f(iu) = f(iu) + he * wquade * ff ( xquade ) * phii;
%
%  Take care of boundary conditions specifying the value of U'.
%
          if ( ig == 0 )
            x = 0.0;
            f(iu) = f(iu) - pp ( x ) * ul;
          elseif ( ig == n )
            x = 1.0;
            f(iu) = f(iu) + pp ( x ) * ur;
          end
%
%  Pick a basis function which defines the coefficient
%  being computed.
%
          for jl = 1 : nl

            jg = node(jl,ie);
            ju = indx(jg+1);
            [ phij, phijx ] = phi ( jl, xquade, xleft, xrite );

            aij = he * wquade * ...
               ( pp ( xquade ) * phiix * phijx ...
               + qq ( xquade ) * phii * phij );
%
%  Decide where the coefficient is to be added.
%
            if ( ju <= 0 )

              if ( jg == 0 )
                f(iu) = f(iu) - aij * ul;
              elseif ( jg == n )
                f(iu) = f(iu) - aij * ur;
              end

            elseif ( iu == ju )
              adiag(iu) = adiag(iu) + aij;
            elseif ( ju < iu )
              aleft(iu) = aleft(iu) + aij;
            else
              arite(iu) = arite(iu) + aij;
            end

          end
        end
      end
    end
  end

  return
end
function value = ff ( x )

%*****************************************************************************80
%
%% FF evaluates the function F in the differential equation.
%
%  Discussion:
%
%    This is the function F(X) that appears on the right hand
%    side of the equation:
%
%      -d/dx ( P(x) * dU(x)/dx ) + Q(x) * U(x)  =  F(x)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of F(X).
%

%
%  Find out which problem we're working on.
%
  problem = get_problem ( );

  if ( problem == 1 )

    value = 0.0;

  elseif ( problem == 2 )

    value = -2.0 * x;

  elseif ( problem == 3 )

    value = 0.25 * pi^2 * sin ( 0.5 * pi * x );

  elseif ( problem == 4 )

    value = 0.25 * pi^2 * cos ( 0.5 * pi * x );

  elseif ( problem == 5 )

    beta = get_beta ( );

    value = - ( x^beta ) + ( x^( beta + 2.0 ) ) ...
      / ( ( beta + 2.0 ) * ( beta + 1.0 ) );

  elseif ( problem == 6 )

    alpha = get_alpha ( );

    value = 2.0 * alpha * ( x - 0.5 ) ...
      / ( alpha^2 + ( x - 0.5 )^2 )^2;

  end

  return
end
function [ h, indx, node, nu, wquad, xquad ] = geometry ( ibc, n, nl, nmax, ...
  nquad, xn )

%*****************************************************************************80
%
%% GEOMETRY sets up some of the geometric information for the problem.
%
%  Discussion:
%
%    Note, however, that the location of the nodes
%    is done outside of this routine, and, in fact, before this
%    routine is called.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, integer IBC.
%    IBC declares what the boundary conditions are.
%    1, at the left endpoint, U has the value UL,
%       at the right endpoint, U' has the value UR.
%    2, at the left endpoint, U' has the value UL,
%       at the right endpoint, U has the value UR.
%    3, at the left endpoint, U has the value UL,
%       and at the right endpoint, U has the value UR.
%    4, at the left endpoint, U' has the value UL,
%       at the right endpoint U' has the value UR.
%
%    Input, integer N
%    The number of subintervals into which the interval
%    [XL,XR] is broken.
%
%    Input, integer NL.
%    The number of basis functions used in a single
%    subinterval.  (NL-1) is the degree of the polynomials
%    used.  For this code, NL is fixed at 2, meaning that
%    piecewise linear functions are used as the basis.
%
%    Input, integer NMAX, the maximum number of unknowns that can be handled.
%
%    Input, integer NQUAD
%    The number of quadrature points used in a subinterval.
%
%    Input, real XN(0:N).
%    XN(I) is the location of the I-th node.  XN(0) is XL,
%    and XN(N) is XR.
%
%    Output, real H(N)
%    H(I) is the length of subinterval I.  This code uses
%    equal spacing for all the subintervals.
%
%    Output, integer INDX(0:N).
%    For a node I, INDX(I) is the index of the unknown
%    associated with node I.
%    If INDX(I) is equal to -1, then no unknown is associated
%    with the node, because a boundary condition fixing the
%    value of U has been applied at the node instead.
%    Unknowns are numbered beginning with 1.
%    If IBC is 2 or 4, then there is an unknown value of U
%    at node 0, which will be unknown number 1.  Otherwise,
%    unknown number 1 will be associated with node 1.
%    If IBC is 1 or 4, then there is an unknown value of U
%    at node N, which will be unknown N or N+1,
%    depending on whether there was an unknown at node 0.
%
%    Output, integer NODE(NL,N).
%    For each subinterval I:
%    NODE(1,I) is the number of the left node, and
%    NODE(2,I) is the number of the right node.
%
%    Output, integer NU.
%    NU is the number of unknowns in the linear system.
%    Depending on the value of IBC, there will be N-1,
%    N, or N+1 unknown values, which are the coefficients
%    of basis functions.
%
%    Output, real WQUAD(NQUAD).
%    WQUAD(I) is the weight associated with the I-th point
%    of an NQUAD point Gaussian quadrature rule.
%
%    Output, real XQUAD(NQUAD,NMAX), the I-th quadrature point
%    in interval J.
%

%
%  Store in NODE the fact that interval I has node I-1
%  as its left endpoint, and node I as its right endpoint.
%
  for i = 1 : n
    node(1,i) = i-1;
    node(2,i) = i;
  end
%
%  For every node that is associated with an unknown, we
%  record the number of the unknown in INDX.
%
  nu = 0;

  for i = 0 : n

    if ( i == 0 & ( ibc == 1 | ibc == 3 ) )
      indx(i+1) = -1;
    elseif ( i == n & ( ibc == 2 | ibc == 3 ) )
      indx(i+1) = -1;
    else
      nu = nu + 1;
      indx(i+1) = nu;
    end

  end
%
%  We compute the width of each interval.
%
  for i = 1 : n
    igl = node(1,i);
    igr = node(2,i);
    h(i) = xn(igr+1) - xn(igl+1);
  end
%
%  We compute the location of the quadrature points in each
%  interval.
%
  for i = 1 : n

    xl = xn(node(1,i)+1);
    xr = xn(node(2,i)+1);

    if ( nquad == 1 )
      xquad(1,i) = 0.5 * ( xl + xr );
    elseif ( nquad == 2 )
      alfa = -0.577350;
      xquad(1,i) = ( ( 1.0 - alfa ) * xl   ...
                   + ( 1.0 + alfa ) * xr ) ...
                   /   2.0;
      alfa = +0.577350;
      xquad(2,i) = ( ( 1.0 - alfa ) * xl   ...
                   + ( 1.0 + alfa ) * xr ) ...
                   /   2.0;
    elseif ( nquad == 3 )
      alfa = -0.774597;
      xquad(1,i) = ( ( 1.0 - alfa ) * xl   ...
                   + ( 1.0 + alfa ) * xr ) ...
                   /   2.0;
      xquad(2,i) = 0.5 * ( xl + xr );
      alfa = +0.774597;
      xquad(3,i) = ( ( 1.0 - alfa ) * xl   ...
                   + ( 1.0 + alfa ) * xr ) ...
                   /   2.0;
    end

  end
%
%  Store the weights for the quadrature rule.
%
  if ( nquad == 1 )
    wquad(1) = 1.0;
  elseif ( nquad == 2 )
    wquad(1) = 0.5;
    wquad(2) = 0.5;
  elseif ( nquad == 3 )
    wquad(1) = 4.0 / 9.0;
    wquad(2) = 5.0 / 18.0;
    wquad(3) = 4.0 / 9.0;
  end

  return
end
function alpha = get_alpha ( )

%*****************************************************************************80
%
%% GET_ALPHA returns the value of ALPHA, for use by problem 6.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Output, real ALPHA, the value of ALPHA.
%
  alpha = 0.01;

  return
end
function beta = get_beta ( )

%*****************************************************************************80
%
%% GET_BETA returns the value of BETA, for use by problem 5.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 February 2014
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Output, real BETA, the value of BETA.
%
  beta = -0.9;

  return
end
function problem = get_problem ( )

%*****************************************************************************80
%
%% GETPRB returns the value of the current problem number.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Output, integer PROBLEM, the index of the problem.
%
  problem = 6;

  return
end
function [ ibc, tol, ul, ur, xl, xn, xr ] = init ( n )

%*****************************************************************************80
%
%% INIT initializes some parameters that define the problem.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, integer N
%    The number of subintervals into which the interval
%    [XL,XR] is broken.
%
%    Output, integer IBC.
%    IBC declares what the boundary conditions are.
%    1, at the left endpoint, U has the value UL,
%       at the right endpoint, U' has the value UR.
%    2, at the left endpoint, U' has the value UL,
%       at the right endpoint, U has the value UR.
%    3, at the left endpoint, U has the value UL,
%       and at the right endpoint, U has the value UR.
%    4, at the left endpoint, U' has the value UL,
%       at the right endpoint U' has the value UR.
%
%    Output, real TOL.
%    A tolerance that is used to determine whether the estimated
%    error in an interval is so large that it should be subdivided
%    and the problem solved again.
%
%    Output, real UL.
%    If IBC is 1 or 3, UL is the value that U is required
%    to have at X = XL.
%    If IBC is 2 or 4, UL is the value that U' is required
%    to have at X = XL.
%
%    Output, real UR.
%    If IBC is 2 or 3, UR is the value that U is required
%    to have at X = XR.
%    If IBC is 1 or 4, UR is the value that U' is required
%    to have at X = XR.
%
%    Output, real XL.
%    XL is the left endpoint of the interval over which the
%    differential equation is being solved.
%
%    Output, real XN(0:N).
%    XN(I) is the location of the I-th node.  XN(0) is XL,
%    and XN(N) is XR.
%
%    Output, real XR.
%    XR is the right endpoint of the interval over which the
%    differential equation is being solved.
%
  tol = 0.01;
%
%  Find out which problem we're working on.
%
  problem = get_problem ( );
%
%  Set the boundary conditions for the problem, and
%  print out its title.
%
  if ( problem == 1 )

    ibc = 3;
    ul = 0.0;
    ur = 1.0;
    xl = 0.0;
    xr = 1.0;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'Exact solution is U = X\n' );

  elseif ( problem == 2 )

    ibc = 3;
    ul = 0.0;
    ur = 1.0;
    xl = 0.0;
    xr = 1.0;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'Exact solution is U = X*X\n' );

  elseif ( problem == 3 )

    ibc = 3;
    ul = 0.0;
    ur = 1.0;
    xl = 0.0;
    xr = 1.0;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'Exact solution is U = SIN(PI*X/2)\n' );

  elseif ( problem == 4 )

    ibc = 3;
    ul = 1.0;
    ur = 0.0;
    xl = 0.0;
    xr = 1.0;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'Exact solution is U = COS(PI*X/2)\n' );

  elseif ( problem == 5 )

    ibc = 3;
    beta = get_beta ( 'DUMMY' );
    ul = 0.0;
    ur = 1.0 / ( ( beta + 2.0 ) * ( beta + 1.0 ) );
    xl = 0.0;
    xr = 1.0;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'Rheinboldt problem\n' );

  elseif ( problem == 6 )

    ibc = 3;
    alpha = get_alpha ( );
    xl = 0.0;
    xr = 1.0;
    ul = uexact ( xl );
    ur = uexact ( xr );
    fprintf ( 1, '\n' );
    fprintf ( 1, 'Arctangent problem\n' );

  end
%
%  The nodes are defined here, and not in the geometry routine.
%  This is because each new iteration chooses the location
%  of the new nodes in a special way.
%
  for i = 0 : n
    xn(i+1) = ( ( n - i ) * xl   ...
              + (     i ) * xr ) ...
              / ( n     );
  end

  fprintf ( 1, 'The equation is to be solved for\n' );
  fprintf ( 1, 'X greater than %f\n', xl );
  fprintf ( 1, ' and less than %f\n', xr );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'The boundary conditions are:\n' );
  fprintf ( 1, '\n' );

  if ( ibc == 1 | ibc == 3 )
    fprintf ( 1, '  At X = XL, U = %f\n', ul );
  else
    fprintf ( 1, '  At X = XL, U'' = %f\n', ul );
  end

  if ( ibc == 2 | ibc == 3 )
    fprintf ( 1, '  At X = XR, U = %f\n', ur );
  else
    fprintf ( 1, '  At X = XR, U'' %f\n= ', ur );
  end

  return
end
function output ( f, ibc, indx, n, nu, ul, ur, xn )

%*****************************************************************************80
%
%% OUTPUT prints out the computed solution.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, real F(NU).
%    ASSEMBLE stores into F the right hand side of the linear
%    equations.
%    SOLVE replaces those values of F by the solution of the
%    linear equations.
%
%    Input, integer IBC.
%    IBC declares what the boundary conditions are.
%    1, at the left endpoint, U has the value UL,
%       at the right endpoint, U' has the value UR.
%    2, at the left endpoint, U' has the value UL,
%       at the right endpoint, U has the value UR.
%    3, at the left endpoint, U has the value UL,
%       and at the right endpoint, U has the value UR.
%    4, at the left endpoint, U' has the value UL,
%       at the right endpoint U' has the value UR.
%
%    Input, integer INDX(0:N).
%    For a node I, INDX(I) is the index of the unknown
%    associated with node I.
%    If INDX(I) is equal to -1, then no unknown is associated
%    with the node, because a boundary condition fixing the
%    value of U has been applied at the node instead.
%    Unknowns are numbered beginning with 1.
%    If IBC is 2 or 4, then there is an unknown value of U
%    at node 0, which will be unknown number 1.  Otherwise,
%    unknown number 1 will be associated with node 1.
%    If IBC is 1 or 4, then there is an unknown value of U
%    at node N, which will be unknown N or N+1,
%    depending on whether there was an unknown at node 0.
%
%    Input, integer N
%    The number of subintervals into which the interval
%    [XL,XR] is broken.
%
%    Input, integer NU.
%    NU is the number of unknowns in the linear system.
%    Depending on the value of IBC, there will be N-1,
%    N, or N+1 unknown values, which are the coefficients
%    of basis functions.
%
%    Input, real UL.
%    If IBC is 1 or 3, UL is the value that U is required
%    to have at X = XL.
%    If IBC is 2 or 4, UL is the value that U' is required
%    to have at X = XL.
%
%    Input, real UR.
%    If IBC is 2 or 3, UR is the value that U is required
%    to have at X = XR.
%    If IBC is 1 or 4, UR is the value that U' is required
%    to have at X = XR.
%
%    Input, real XN(0:N).
%    XN(I) is the location of the I-th node.  XN(0) is XL,
%    and XN(N) is XR.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'Node    X(I)        U(X(I))        Uexact        Error\n' );
  fprintf ( 1, '\n' );

  for i = 0 : n

    if ( i == 0 )

      if ( ibc == 1 | ibc == 3 )
        u = ul;
      else
        u = f(indx(i+1));
      end

    elseif ( i == n )

      if ( ibc == 2 | ibc == 3 )
        u = ur;
      else
        u = f(indx(i+1));
      end

    else

      u = f(indx(i+1));

    end

    uex = uexact ( xn(i+1) );
    error = u - uex;

    fprintf ( 1, '  %4d  %12f  %12f  %12f  %12f\n', i, xn(i+1), u, uex, error );

  end

  return
end
function [ phii, phiix ] = phi ( il, x, xleft, xrite )

%*****************************************************************************80
%
%% PHI evaluates a linear basis function and its derivative.
%
%  Discussion:
%
%    The functions are evaluated at a point X in an interval.  In any
%    interval, there are just two basis functions.  The first
%    basis function is a line which is 1 at the left endpoint
%    and 0 at the right.  The second basis function is 0 at
%    the left endpoint and 1 at the right.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, integer IL, the local index of the basis function.
%
%    Input, real X, the evaluation point.
%
%    Input, real XLEFT, XRITE, the endpoints of the interval.
%
%    Output, real PHII, PHIIX, the value of the basis function
%    and its derivative.
%
  if ( xleft <= x & x <= xrite )

    if ( il == 1 )
      phii = ( xrite - x ) / ( xrite - xleft );
      phiix = -1.0 / ( xrite - xleft );
    else
      phii = ( x - xleft ) / ( xrite - xleft );
      phiix = 1.0 / ( xrite - xleft );
    end
%
%  If X is outside of the interval, then the basis function
%  is always zero.
%
  else
    phii = 0.0;
    phiix = 0.0;
  end

  return
end
function value = pp ( x )

%*****************************************************************************80
%
%% PP evaluates the function P in the differential equation.
%
%  Discussion:
%
%    The function P(X) occurs in the differential equation:
%
%      -d/dx ( P(x) * dU(x)/dx ) + Q(x) * U(x)  =  F(x)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of P(X).
%

%
%  Find out which problem we're working on.
%
  problem = get_problem ( );

  if ( problem == 1 )
    value = 1.0;
  elseif ( problem == 2 )
    value = 1.0;
  elseif ( problem == 3 )
    value = 1.0;
  elseif ( problem == 4 )
    value = 1.0;
  elseif ( problem == 5 )
    value = 1.0;
  elseif ( problem == 6 )
    value = 1.0;
  end

  return
end
function prsys ( adiag, aleft, arite, f, nu )

%*****************************************************************************80
%
%% PRSYS prints out the tridiagonal linear system.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, real ADIAG(NU).
%    ADIAG(I) is the "diagonal" coefficient of the I-th
%    equation in the linear system.  That is, ADIAG(I) is
%    the coefficient of the I-th unknown in the I-th equation.
%
%    Input, real ALEFT(NU).
%    ALEFT(I) is the "left hand" coefficient of the I-th
%    equation in the linear system.  That is, ALEFT(I) is the
%    coefficient of the (I-1)-th unknown in the I-th equation.
%    There is no value in ALEFT(1), since the first equation
%    does not refer to a "0-th" unknown.
%
%    Input, real ARITE(NU).
%    ARITE(I) is the "right hand" coefficient of the I-th
%    equation in the linear system.  ARITE(I) is the coefficient
%    of the (I+1)-th unknown in the I-th equation.  There is
%    no value in ARITE(NU) because the NU-th equation does not
%    refer to an "NU+1"-th unknown.
%
%    Input, real F(NU).
%    ASSEMBLE stores into F the right hand side of the linear
%    equations.
%    SOLVE replaces those values of F by the solution of the
%    linear equations.
%
%    integer NU.
%    NU is the number of unknowns in the linear system.
%    Depending on the value of IBC, there will be N-1,
%    N, or N+1 unknown values, which are the coefficients
%    of basis functions.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'Printout of tridiagonal linear system:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'Equation  A-Left  A-Diag  A-Rite  RHS\n' );
  fprintf ( 1, '\n' );

  for i = 1 : nu
    if ( i == 1 )
      fprintf ( 1, '%3d                %12f  %12f  %12f\n', ...
        i, adiag(i), arite(i), f(i) );
    elseif ( i < nu )
      fprintf ( 1, '%3d  %12f  %12f  %12f  %12f\n', ...
        i, aleft(i), adiag(i), arite(i), f(i) );
    else
      fprintf ( 1, '%3d  %12f  %12f                %12f\n', ...
        i, aleft(i), adiag(i), f(i) );
    end
  end

  return
end
function value = qq ( x )

%*****************************************************************************80
%
%% QQ evaluates the function Q in the differential equation.
%
%  Discussion:
%
%    The function Q(X) occurs in the differential equation:
%
%      -d/dx ( P(x) * dU(x)/dx ) + Q(x) * U(x)  =  F(x)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of Q(X).
%

%
%  Find out which problem we're working on.
%
  problem = get_problem ( );

  if ( problem == 1 )
    value = 0.0;
  elseif ( problem == 2 )
    value = 0.0;
  elseif ( problem == 3 )
    value = 0.0;
  elseif ( problem == 4 )
    value = 0.0;
  elseif ( problem == 5 )
    value = 1.0;
  elseif ( problem == 6 )
    value = 0.0;
  end

  return
end
function u = solve ( adiag, aleft, arite, f, nu )

%*****************************************************************************80
%
%% SOLVE solves a tridiagonal matrix system of the form A*x = b.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, real ADIAG(NU), ALEFT(NU), ARITE(NU).
%    the diagonal, left and right entries of the equations.
%
%    Input, real F(NU), the right hand side of the linear system.
%
%    Input, INTEGER NU, the number of equations to be solved.
%
%    Output, real U(NU), the solution of the linear system.
%

%
%  Handle the special case of a single equation.
%
  if ( nu == 1 )

    u(1) = f(1) / adiag(1);
%
%  The general case, when NU is greater than 1.
%
  else

    arite(1) = arite(1) / adiag(1);
    for i = 2 : nu-1
      adiag(i) = adiag(i) - aleft(i) * arite(i-1);
      arite(i) = arite(i) / adiag(i);
    end
    adiag(nu) = adiag(nu) - aleft(nu) * arite(nu-1);

    u(1) = f(1) / adiag(1);
    for i = 2 : nu
      u(i) = ( f(i) - aleft(i) * u(i-1) ) / adiag(i);
    end

    for i = nu-1 : -1 : 1
      u(i) = u(i) - arite(i) * u(i+1);
    end

  end

  return
end
function [ u, h, nu ] = solvex ( ibc, kount, n, nl, nmax, ul, ur, xn )

%*****************************************************************************80
%
%% SOLVEX discretizes and solves a differential equation given the nodes.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, integer IBC.
%    IBC declares what the boundary conditions are.
%    1, at the left endpoint, U has the value UL,
%       at the right endpoint, U' has the value UR.
%    2, at the left endpoint, U' has the value UL,
%       at the right endpoint, U has the value UR.
%    3, at the left endpoint, U has the value UL,
%       and at the right endpoint, U has the value UR.
%    4, at the left endpoint, U' has the value UL,
%       at the right endpoint U' has the value UR.
%
%    Input, integer KOUNT, the number of adaptive steps that have been taken.
%
%    Input, integer N
%    The number of subintervals into which the interval
%    [XL,XR] is broken.
%
%    Input, integer NL.
%    The number of basis functions used in a single
%    subinterval.  (NL-1) is the degree of the polynomials
%    used.  For this code, NL is fixed at 2, meaning that
%    piecewise linear functions are used as the basis.
%
%    Input, integer NMAX, the maximum number of unknowns that can be handled.
%
%    Input, real UL.
%    If IBC is 1 or 3, UL is the value that U is required
%    to have at X = XL.
%    If IBC is 2 or 4, UL is the value that U' is required
%    to have at X = XL.
%
%    Input, real UR.
%    If IBC is 2 or 3, UR is the value that U is required
%    to have at X = XR.
%    If IBC is 1 or 4, UR is the value that U' is required
%    to have at X = XR.
%
%    Input, real XN(0:N).
%    XN(I) is the location of the I-th node.  XN(0) is XL,
%    and XN(N) is XR.
%
%    Output, real U(NU), the finite element coefficients of the
%    solution of the differential equation.
%
%    Output, real H(N)
%    H(I) is the length of subinterval I.  This code uses
%    equal spacing for all the subintervals.
%
%    Output, integer NU.
%    NU is the number of unknowns in the linear system.
%    Depending on the value of IBC, there will be N-1,
%    N, or N+1 unknown values, which are the coefficients
%    of basis functions.
%
%  Local parameters:
%
%    Local, real ADIAG(NU).
%    ADIAG(I) is the "diagonal" coefficient of the I-th
%    equation in the linear system.  That is, ADIAG(I) is
%    the coefficient of the I-th unknown in the I-th equation.
%
%    Local, real ALEFT(NU).
%    ALEFT(I) is the "left hand" coefficient of the I-th
%    equation in the linear system.  That is, ALEFT(I) is the
%    coefficient of the (I-1)-th unknown in the I-th equation.
%    There is no value in ALEFT(1), since the first equation
%    does not refer to a "0-th" unknown.
%
%    Local, real ARITE(NU).
%    ARITE(I) is the "right hand" coefficient of the I-th
%    equation in the linear system.  ARITE(I) is the coefficient
%    of the (I+1)-th unknown in the I-th equation.  There is
%    no value in ARITE(NU) because the NU-th equation does not
%    refer to an "NU+1"-th unknown.
%
%    Local, integer INDX(0:N).
%    For a node I, INDX(I) is the index of the unknown
%    associated with node I.
%    If INDX(I) is equal to -1, then no unknown is associated
%    with the node, because a boundary condition fixing the
%    value of U has been applied at the node instead.
%    Unknowns are numbered beginning with 1.
%    If IBC is 2 or 4, then there is an unknown value of U
%    at node 0, which will be unknown number 1.  Otherwise,
%    unknown number 1 will be associated with node 1.
%    If IBC is 1 or 4, then there is an unknown value of U
%    at node N, which will be unknown N or N+1,
%    depending on whether there was an unknown at node 0.
%
%    Local, integer NODE(NL,N).
%    For each subinterval I:
%    NODE(1,I) is the number of the left node, and
%    NODE(2,I) is the number of the right node.
%
%    Local, integer NQUAD
%    The number of quadrature points used in a subinterval.
%
%    Local, real WQUAD(NQUAD).
%    WQUAD(I) is the weight associated with the I-th point
%    of an NQUAD point Gaussian quadrature rule.
%
%    Local, real XQUAD(NQUAD,NMAX), the I-th quadrature point
%    in interval J.
%
  nquad = 2;
%
%  Given a set of N nodes (where N increases on each iteration),
%  compute the other geometric information.
%
  [ h, indx, node, nu, wquad, xquad ] = geometry ( ibc, n, nl, nmax, nquad, xn );
%
%  Assemble the linear system.
%
  [ adiag, aleft, arite, f ] = assemble ( h, n, indx, node, nu, nl, ...
    nquad, nmax, ul, ur, wquad, xn, xquad );
%
%  Print out the linear system, just once.
%
  if ( kount == 1 )
    prsys ( adiag, aleft, arite, f, nu );
  end
%
%  Solve the linear system.
%
  u = solve ( adiag, aleft, arite, f, nu );
%
%  Print out the solution.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'Basic solution\n' );

  output ( u, ibc, indx, n, nu, ul, ur, xn );

  return
end
function eta = solvey ( u, h, n, nu, ul, ur, xn )

%*****************************************************************************80
%
%% SOLVEY computes error estimators for a finite element solution.
%
%  Discussion:
%
%    SOLVEY accepts information about the solution of a finite element
%    problem on a grid of nodes with coordinates XN.  It then starts
%    at node 0, and for each node, computes two "error estimators",
%    one for the left, and one for the right interval associated with the
%    node.  These estimators are found by solving a finite element problem
%    over the two intervals, using the known values of the original
%    solution as boundary data, and using a mesh that is "slightly"
%    refined over the original one.
%
%    Note that the computations at the 0-th and N-th nodes only involve
%    a single interval.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, real U(NU), the finite element representation of the
%    solution of the current problem.
%
%    Input, real H(N)
%    H(I) is the length of subinterval I.  This code uses
%    equal spacing for all the subintervals.
%
%    Input, integer N
%    The number of subintervals into which the interval
%    [XL,XR] is broken.
%
%    Input, integer NU.
%    NU is the number of unknowns in the linear system.
%    Depending on the value of IBC, there will be N-1,
%    N, or N+1 unknown values, which are the coefficients
%    of basis functions.
%
%    Input, real UL.
%    If IBC is 1 or 3, UL is the value that U is required
%    to have at X = XL.
%    If IBC is 2 or 4, UL is the value that U' is required
%    to have at X = XL.
%
%    Input, real UR.
%    If IBC is 2 or 3, UR is the value that U is required
%    to have at X = XR.
%    If IBC is 1 or 4, UR is the value that U' is required
%    to have at X = XR.
%
%    Input, real XN(0:N).
%    XN(I) is the location of the I-th node.  XN(0) is XL,
%    and XN(N) is XR.
%
%    Output, real ETA(N).
%    ETA(I) is the error estimate for interval I.  It is computed
%    as the sum of two quantities, one associated with the left
%    and one with the right node of the interval.
%
  nl = 2;
  ny = 2;
  nquad = 2;
  nmay = 2 * ny;
%
%  Initialize the error estimators to zero.
%
  eta(1:n) = 0.0;
%
%  Set the boundary conditions for each subproblem to be
%  known values of U at the left and right.
%
%
%  For each node, subdivide its left and right hand intervals
%  into NY subintervals.
%
%  Set up and solve the differential equation again on this
%  smaller region.
%
%  The 0-th and N-th nodes are special cases.
%
  ibcy = 3;

  for j = 0 : n

    if ( j == 0 )
      m = ny;
      jlo = j;
      jmid = j + 1;
      jhi = j + 1;
    elseif ( j == n )
      m = ny;
      jlo = j - 1;
      jmid = j;
      jhi = j;
    else
      m = 2 * ny;
      jlo = j - 1;
      jmid = j;
      jhi = j + 1;
    end
%
%  Set the location of the nodes in the subintervals.
%
    yl = xn(jlo+1);
    ym = xn(jmid+1);
    yr = xn(jhi+1);

    for i = 0 : ny
      yn(i+1) = ( ( ny - i ) * yl   ...
                + (      i ) * ym ) ...
                / ( ny     );
    end

    for i = ny+1 : m
      yn(i+1) = ( ( m - i      ) * ym   ...
                + (     i - ny ) * yr ) ...
                / ( m -     ny );
    end
%
%  Set up the geometry of the sub-problem.
%
    [ hy, indy, nodey, nuy, wquad, xquady ] = geometry ( ibcy, m, nl, ...
      nmay, nquad, yn );
%
%  Set the boundary values for the sub-problem.
%
    if ( j <= 1 )
      uly = ul;
    else
      uly = u(j-1);
    end

    if ( n - 1 <= j )
      ury = ur;
    else
      ury = u(j+1);
    end
%
%  Assemble the matrix for the sub-problem.
%
    [ adiag, aleft, arite, fy ] =  assemble ( hy, m, indy, nodey, nuy, nl, ...
      nquad, nmay, uly, ury, wquad, yn, xquady );
%
%  Solve the system.
%
    uy = solve ( adiag, aleft, arite, fy, nuy );
%
%  Compute the weighted sum of the squares of the differences
%  of the original computed slope and the refined computed slopes.
%
%  Calculation for left interval.
%
    if ( 1 <= j )

      if ( j <= 1 )
        uleft = ul;
        urite = u(1);
      elseif ( j == n )
        uleft = u(j-1);
        urite = ur;
      else
        uleft = u(j-1);
        urite = u(j);
      end

      uprime = ( urite - uleft ) / h(j);

      total = 0.0;
      for i = 1 : ny

        yl = yn(i-1+1);
        yr = yn(i+1);

        if ( i == 1 )
          vlval = uly;
          vrval = uy(i);
        elseif ( i == m )
          vlval = uy(i-1);
          vrval = ury;
        else
          vlval = uy(i-1);
          vrval = uy(i);
        end

        vprime = ( vrval - vlval ) / hy(i);

        ulval = ( real ( ny - i + 1 ) * uleft   ...
                + real (      i - 1 ) * urite ) ...
                / real ( ny         );

        urval = ( real ( ny - i ) * uleft   ...
                + real (      i ) * urite ) ...
                / real ( ny     );
%
%  Compute the integral of
%
%    p(x)*(u'(x)-v'(x))^2 + q(x)*(u(x)-v(x))^2
%
        for k = 1 : nquad

          y  =  xquady(k,i);

          uval = ( ( yl - y      ) * urval   ...
                 + (      y - yr ) * ulval ) ...
                 / ( yl     - yr );

          vval = ( ( yl - y      ) * vrval   ...
                 + (      y - yr ) * vlval ) ...
                 / ( yl     - yr );

          total = total + 0.5 * wquad(k) * hy(i) * ...
            ( pp ( y ) * ( uprime - vprime )^2 ...
            + qq ( y ) * ( uval - vval )^2 );

        end

      end

      eta(j) = eta(j) + 0.5 * sqrt ( total );

    end
%
%  Calculation for right interval.
%
    if ( j <= n - 1 )

      if ( j == 0 )
        uleft = ul;
        urite = u(j+1);
      elseif ( n - 1 <= j )
        uleft = u(j);
        urite = ur;
      else
        uleft = u(j);
        urite = u(j+1);
      end

      uprime = ( urite - uleft ) / h(j+1);

      total = 0.0;
      for i = m+1-ny : m

        yl = yn(i);
        yr = yn(i+1);

        if ( i == 1 )
          vlval = uly;
          vrval = uy(i);
        elseif ( i == m )
          vlval = uy(i-1);
          vrval = ury;
        else
          vlval = uy(i-1);
          vrval = uy(i);
        end

        vprime = ( vrval - vlval ) / hy(i);

        ulval = ( (      m - i + 1 ) * uleft   ...
                + ( ny - m + i - 1 ) * urite ) ...
                / ( ny             );

        urval = ( (      m - i ) * uleft   ...
                + ( ny - m + i ) * urite ) ...
                / ( ny         );
%
%  Compute the integral of
%
%    p(x)*(u'(x)-v'(x))^2 + q(x)*(u(x)-v(x))^2
%
        for k = 1 : nquad

          y  =  xquady(k,i);

          uval = ( ( yl - y      ) * urval   ...
                 + (      y - yr ) * ulval ) ...
                 / ( yl     - yr );

          vval = ( ( yl - y      ) * vrval   ...
                 + (      y - yr ) * vlval ) ...
                 / ( yl     - yr );

          total = total + 0.5 * wquad(k) * hy(i) * ...
            ( pp ( y ) * ( uprime - vprime )^2 ...
            + qq ( y ) * ( uval - vval )^2 );

        end

      end

      eta(j+1) = eta(j+1) + 0.5 * sqrt ( total );

    end

  end
%
%  Print out the error estimators.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'ETA\n' );
  fprintf ( 1, '\n' );
  for j = 1 : n
    fprintf ( 1, '  %12f\n', eta(j) );
  end

  return
end
function [ n, xn, status ] = subdiv ( eta, kount, n, nmax, tol, xn )

%*****************************************************************************80
%
%% SUBDIV decides which intervals should be subdivided.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, real ETA(N).
%    ETA(I) is the error estimate for interval I.  It is computed
%    as the sum of two quantities, one associated with the left
%    and one with the right node of the interval.
%
%    Input, integer KOUNT, the number of adaptive steps that have been taken.
%
%    Input, integer N
%    The number of subintervals into which the interval
%    [XL,XR] is broken.
%
%    Input, integer NMAX, the maximum number of unknowns that can be handled.
%
%    Input, real TOL.
%    A tolerance that is used to determine whether the estimated
%    error in an interval is so large that it should be subdivided
%    and the problem solved again.
%
%    Input, real XN(0:N).
%    XN(I) is the location of the I-th node.  XN(0) is XL,
%    and XN(N) is XR.
%
%    Output, integer N, the updated number of nodes.
%
%    Output, real XN(0:N), the updated nodes.
%
%    Output, integer STATUS, reports status of subdivision.
%    0, a new subdivision was carried out.
%    1, no more subdivisions are needed.
%    -1, no more subdivisions can be carried out.
%
%  Local Parameters:
%
%    Local, integer JADD(N).
%    JADD(I) is 1 if the error estimates show that interval I
%    should be subdivided.
%
  status = 0;
%
%  Add up the ETA's, and get their average.
%
  ave = sum ( eta(1:n) ) / n;
%
%  Look for intervals whose ETA value is relatively large,
%  and note in JADD that these intervals should be subdivided.
%
  k = 0;
  temp = max ( 1.2 * ave + 0.00001, tol^2 / n );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'Tolerance = %f\n', temp );
  fprintf ( 1, '\n' );

  for j = 1 : n

    if ( temp < eta(j) )
      k = k + 1;
      jadd(j) = 1;
      fprintf ( 1, 'Subdivide interval %d\n', j );
    else
      jadd(j) = 0;
    end

  end
%
%  If no subdivisions needed, we're done.
%
  if ( k <= 0 )
    fprintf ( 1, 'Success on step %d\n', kount );
    status = 1;
    return
  end
%
%  See if we're about to go over our limit.
%
  if ( nmax < n + k )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'The iterations did not reach their goal.\n' );
    fprintf ( 1, 'The next value of N is %d\n', n + k );
    fprintf ( 1, 'which exceeds NMAX = %d\n', nmax );
    status = -1;
    return
  end
%
%  Insert new nodes where needed.
%
  k = 0;
  xt(1) = xn(1);
  for j = 1 : n

    if ( 0 < jadd(j) )
      xt(j+k+1) = 0.5 * ( xn(j+1) + xn(j) );
      k = k + 1;
    end

    xt(j+k+1) = xn(j+1);

  end
%
%  Update the value of N, and copy the new nodes into XN.
%
  n = n + k;

  xn(1:n+1) = xt(1:n+1);

  return
end
function timestamp ( )

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

  return
end
function value = uexact ( x )

%*****************************************************************************80
%
%% UEXACT returns the value of the exact solution at any point X.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 November 2006
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of the exact solution at X.
%

%
%  Find out which problem we're working on.
%
  problem = get_problem ( );

  if ( problem == 1 )
    value = x;
  elseif ( problem == 2 )
    value = x^2;
  elseif ( problem == 3 )
    value = sin ( pi * x / 2.0 );
  elseif ( problem == 4 )
    value = cos ( pi * x / 2.0 );
  elseif ( problem == 5 )
    beta = get_beta ( );
    value = ( x^( beta + 2.0 ) ) ...
      / ( ( beta + 2.0 ) * ( beta + 1.0 ) );
  elseif ( problem == 6 )
    alpha = get_alpha ( );
    value = atan ( ( x - 0.5 ) / alpha );
  else
    value = 0.0;
  end

  return
end


function [ x, w ] = rule03 ( n )

%*****************************************************************************80
%
%% RULE03 returns the rule of degree 3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 July 2014
%
%  Author:
%
%    Original FORTRAN77 version by Hong Xiao, Zydrunas Gimbutas.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Hong Xiao, Zydrunas Gimbutas,
%    A numerical algorithm for the construction of efficient quadrature
%    rules in two and higher dimensions,
%    Computers and Mathematics with Applications,
%    Volume 59, 2010, pages 663-676.
%
%  Parameters:
%
%    Input, integer N, the number of nodes.
%
%    Output, real X(2,N), the coordinates of the nodes.
%
%    Output, real W(N), the weights.
%
  xs = [ ...
    -.5773502691896256,0.5773502691896260, ...
    0.5773502691896256,-.5773502691896260 ];
  ys = [ ...
    -.5773502691896260,-.5773502691896256, ...
    0.5773502691896260,0.5773502691896256 ];
  ws = [ ...
    0.7071067811865476,0.7071067811865476, ...
    0.7071067811865476,0.7071067811865476 ];

  x(1,1:n) = xs(1:n);
  x(2,1:n) = ys(1:n);
  w(1:n) = ws(1:n);

  return
end


function rotMat = matRad_getRotationMatrix(gantryAngle,couchAngle,system)
% matRad function to return the rotation / transformation matrix
% for gantry and/or couch rotation. The Rotation matrix stands for a (1)
% counter-clockwise, (2) active rotation in the patient coordinate system
% that is performed on a (4) column vector (by premultiplying the matrix).
% Per change of one of these directions a matrix transpose of the returned
% matrix is required.
%
%
% call
%  rotMat = matRad_getRotationMatrix(gantryAngle,couchAngle,type,system)
%
% input
%   gantryAngle:    beam/gantry angle
%   couchAngle:     couch angle
%
%   system:         optional coordinate system the transformation matrix is
%                   requested for. So far, only the default option 'LPS' is
%                   supported (right handed system).
%
% output
%   rotMat:         3x3 matrix that performs an active rotation around the
%                   patient system origin via rotMat * x
%
% References
%   https://en.wikipedia.org/wiki/Rotation_matrix (2017, Mar 1)
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copyright 2015 the matRad development team.
%
% This file is part of the matRad project. It is subject to the license
% terms in the LICENSE file found in the top-level directory of this
% distribution and at https://github.com/e0404/matRad/LICENSES.txt. No part
% of the matRad project, including this file, may be copied, modified,
% propagated, or distributed except according to the terms contained in the
% LICENSE file.
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Parse arguments
%We need at least two and max 3 input arguments
narginchk(2,3);

% Coordinate System (only LPS so far)
if nargin < 3
    system = 'LPS';
end

%% Set Up requested Rotation Matrix
switch system
    case 'LPS'
        %The LPS system is right-handed, gantry rotates counter-clockwise
        %around z and couch rotates counter-clockwise around y

        %active, counter-clockwise rotation Matrix for Gantry around z
        %with pre-multiplication of the matrix (R*x)
        %Note: Gantry rotation is physically an active rotation of a beam
        %vector around the target / isocenterin the patient coordinate
        %system
        R_Gantry = [cosd(gantryAngle)  -sind(gantryAngle)  0; ...
            sind(gantryAngle)    cosd(gantryAngle)  0; ...
            0                            0           1];

        %active, counter-clockwise rotation for couch around y
        %with pre-multiplication of the matrix (R*x)
        %Note: Couch rotation is physically a passive rotation of the
        %patient system around the beam target point / isocenter
        R_Couch = [cosd(couchAngle) 0 sind(couchAngle); ...
            0 1 0; ...
            -sind(couchAngle)    0    cosd(couchAngle)];
    otherwise
        error('matRad only supports LPS system so far');
end

rotMat = R_Couch*R_Gantry;

end


function utmp = IEC_MANN(Nx,Ny,Nz,delta,UHub,HubHt,IECturbC)
%%INITIALISATION

% definition of constants
twopi=2*pi;
%constants and derived parameters from IEC
gamma = 3.9; %IEC, (B.12)
alpha = 0.2; %IEC, sect. 6.3.1.2

%set delta1 according to guidelines (chap.6)
if HubHt<=60,
    delta1=0.7*HubHt;
else
    delta1=42;
end;

%IEC, Table 1, p.22
if IECturbC == 'A',
    Iref=0.16;
elseif IECturbC == 'B',
    Iref=0.14;
elseif IECturbC == 'C',
    Iref=0.12;
else
    error('IECturbC can be equal to A,B or C;adjust the input value')
end;

%IEC, sect. 6.3.1.3
b=5.6;
sigma1=Iref*(0.75*UHub+b);
sigma2=0.7*sigma1;
sigma3=0.5*sigma1;
%derived constants
l=0.8*delta1; %IEC, (B.12)
sigmaiso=0.55*sigma1; %IEC, (B.12)

Cij2=zeros(3,3,Nx,Ny,Nz);
ikx=cat(2,-Nx/2:-1,1:Nx/2-1);
[x y z]=ndgrid(ikx,1:Ny,1:Nz);
k1=(x)*l/(Nx*delta)*twopi;
k2=(y-Ny/2)*l/(Ny*delta)*twopi;
k3=(z-Nz/2)*l/(Nz*delta)*twopi;
kabs=sqrt(k1.^2+k2.^2+k3.^2);
beta= gamma./(kabs.^(2/3).*real(pfq([1/3 17/6],4/3,-kabs.^(-2))));
k03=k3+beta.*k1;
k0abs=sqrt(k1.^2+k2.^2+k03.^2);
Ek0=1.453*k0abs.^4./(1+k0abs.^2).^(17/6);
C1=beta.*k1.^2.*( k0abs.^2 - 2*k03.^2 + beta.*k1.*k03 )./( kabs.^2.*( k1.^2 + k2.^2 ));
C2=k2.*k0abs.^2./ (exp( (3/2).*log( k1.^2 + k2.^2 ) )) .* atan2( beta.*k1.* sqrt( k1.^2 + k2.^2 ) ,( k0abs.^2 - k03.*k1.*beta));
xhsi1=C1 - k2.*C2./k1;
xhsi2=k2.*C1./k1 + C2;
CC=sigmaiso*sqrt(twopi*pi*l^3.*Ek0./(Nx*Ny*Nz*delta^3.*k0abs.^4));
Cij2(1,1,1:size(k1,1),:,:)= CC.*( k2.*xhsi1);
Cij2(1,2,1:size(k1,1),:,:)= CC.*( k3 - k1.*xhsi1 + beta.*k1);
Cij2(1,3,1:size(k1,1),:,:)= CC.*( -k2);
Cij2(2,1,1:size(k1,1),:,:)= CC.*( k2.*xhsi2 - k3 - beta.*k1);
Cij2(2,2,1:size(k1,1),:,:)= CC.*( -k1.*xhsi2);
Cij2(2,3,1:size(k1,1),:,:)= CC.*( k1);
Cij2(3,1,1:size(k1,1),:,:)= CC.*( k0abs.^2.*k2 ./ (kabs.^2));
Cij2(3,2,1:size(k1,1),:,:)= CC.*( -k0abs.^2.*k1 ./ (kabs.^2));
%Cij2(isnan(Cij2))=0;
%% FOURIER COEFFICIENTS
n=complex(normrnd(0,1,[3 1 Nx Ny Nz]),normrnd(0,1,[3 1 Nx Ny Nz]));
H = reshape(mtimesx(Cij2,n,'speed'),[],Nx,Ny,Nz);
%% TIME SERIES
u(1,:,:,:)=real(fftn(fftshift(H(1,:,:,:))));
u(2,:,:,:)=real(fftn(fftshift(H(2,:,:,:))));
u(3,:,:,:)=real(fftn(fftshift(H(3,:,:,:))));

stdDv=zeros(Ny*Nz,3);
stdDv(:,1)=reshape(std(u(1,:,:,:)),Ny*Nz,1); % Standard deviation for u-component
stdDv(:,2)=reshape(std(u(2,:,:,:)),Ny*Nz,1); % Standard deviation for v-component
stdDv(:,3)=reshape(std(u(3,:,:,:)),Ny*Nz,1); % Standard deviation for w-component

% Introduction of a Corrective Factor SF in order to adjust the statistics
% as advised by the OC4 project DTUs report

sf=zeros(Ny*Nz,3);
sf(:,1)=sqrt(sigma1^2./(stdDv(:,1).^2));
sf(:,2)=sqrt((sigma2)^2./(stdDv(:,2).^2));
sf(:,3)=sqrt((sigma3)^2./(stdDv(:,3).^2));

utmp=zeros(3,Nx,Ny,Nz);
for i=1:length(sf),
    for j=1:Ny,
        for k=1:Nz,
            utmp(1,:,j,k)=sf(i,1).*u(1,:,j,k);
            utmp(2,:,j,k)=sf(i,2).*u(2,:,j,k);
            utmp(3,:,j,k)=sf(i,3).*u(3,:,j,k);
        end;
    end;
end;
for i=1:Nz,
    thisZ=HubHt-(Nz*delta)/2+1.*i*(Nz*delta)/(Nz-1);
    profile=UHub*(thisZ/HubHt)^alpha;
    utmp(1,:,:,i)=(utmp(1,:,:,i)+profile);
end;


%%% Projection onto constrained force space + Constraint Stabilization

% Course: Robotic Manipulation and Mobility
% Advisor: Dr. V. Krovi
%
% Homework Number: MIDTERM
%
% Names: Sourish Chakravarty
% 	Hrishi Lalit Shah

function [dX]= ROBO_midterm_f3(t,X)

global l1 lc1 m1 j1 tau1 l2 lc2 m2 j2 tau2 g Lambda2 omega zeta

th1=X(1);
x2=X(2);
y2=X(3);
th2=X(4);

th1d= X(5);
x2d=X(6);
y2d=X(7);
th2d= X(8);
%%%%%%%%%%%%%%%%%%%%%%% CREATING IMPORTANT MATRICES IN THE GOVERNING EQUATION
s1=sin(th1);
s2=sin(th2);
c1=cos(th1);
c2=cos(th2);

%%%% Element - 1
M1= [j1+m1*(lc1^2)];
E1= [1,-1];
G1= [m1*lc1*c1*g];
% B11 = [-l1*s1, l1*c1];
% B12 = [-m1*lc1*c1];

%%%% Element - 2
M2= [m2, 0, 0;
     0, m2, 0;
     0, 0, j2];
E2= [ 0; 0; 1];
G2= [0; m2*g; 0];
% B21= [-1, 0;
%      0, -1;
%      -lc2*s2, lc2*c2];
% B22= [0, -m2, 0]';

%%%% Constraints
C= [x2-l1*c1-lc2*c2;
    y2-l1*s1-lc2*s2];
A= [l1*s1, 1, 0, lc2*s2;
    -l1*c1, 0, 1, -lc2*c2];% Jacobian of constraint matrix
Ad = [l1*c1*th1d, 0, 0, lc2*c2*th2d;
     l1*s1*th1d, 0, 0, lc2*s2*th2d]; % Derivative of matrix A
S = [1, 0;
    -l1*s1, -lc2*s2;
    l1*c1, lc2*c2;
    0, 1]; % Null space of A or Feasible Motion
Sd = [0, 0;
    -l1*c1*th1d, -lc2*c2*th2d;
    -l1*s1*th1d, -lc2*s2*th2d;
    0, 0]; % Derivative of S matrix

%%%%%%%%%%%%%%%%%%%%%%% PROJECTION ONTO CONSTRAINED FORCE SPACE USING
%%%%%%%%%%%%%%%%%%%%%%% CONSTRAINT STABILIZATION
M=[M1, zeros(1,3);
    zeros(3,1), M2];
E=[E1;
    zeros(3,1),E2];
G= [G1;G2];
T =[tau1; tau2];
V=zeros(4,1);

Qd = [th1d, x2d, y2d, th2d]';

t1= inv([M, A';
        A, zeros(2,2)]); % since m=2;
%     size(Ad)
%     size(2*zeta*A)
%     size((Ad+2*zeta*A)*Qd)
%     size((omega^2)*C)
t2= [E*T-V-G; -(Ad+2*zeta*A)*Qd-(omega^2)*C];%%% Applying constraint stabilization
t3= t1*t2;

dX(1:4,1)= [th1d,x2d,y2d,th2d]';
dX(5:8,1)= t3(1:4);
lam = t3(5:end);
Lambda2 = [Lambda2;lam];%%% Passing instaneous values of Lagrange multipliers back to main function
return


function varargout = chebcoeffs2(varargin)
%CHEBCOEFFS2    Bivariate expansion coefficients
%   X = CHEBCOEFFS2(F) returns the matrix of bivariate coefficients such that
%       F= sum_{i=0}^{n-1} ( sum_{j=0}^{n-1} X(i+1,j+1) T_i(r) T_j(theta) ).
%   where -1 <= r <= 1 and -pi <= theta <= pi are the polar and radial
%   coordinates, respectively.
%
%   [A, D, B] = CHEBCOEFFS2( f ) returns the same coefficients keeping them in
%   low rank form, i.e., X = A * D * B'.
%
%
% See also DISKFUN/PLOTCOEFFS2, DISKFUN/COEFFS2.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

[varargout{1:nargout}] = chebcoeffs2@separableApprox(varargin{:});

end


function u = poisson(varargin)
%POISSON   Poisson solver in the unit ball with Dirichlet or Neumann
%          boundary conditions.
%   U = POISSON(F, BC, M, N, P) is the solution to the Poisson equation in
%   the unit ball with right-hand side F and Dirichlet boundary data
%   U(1,lambda,theta) = @(lambda,theta) BC(lambda, theta). It uses a
%   discretization size of M x N x P.
%
%   U = POISSON(F, BC, M) is the same as POISSON(F, BC, M, M, M).
%
%   U = POISSON(F, BC, M, N, P, 'neumann') is the solution to the Poisson
%   equation with right-hand side F and Neuamnn boundary data U(1,lambda,theta) =
%   @(lambda,theta) BC(lambda, theta). It uses a discretization size of M x N x P.
%
%   U = POISSON(F, BC, M, 'neumann') is the same as POISSON(F, BC, M, M, M, 'neumann').
%
%   Also see HELMHOLTZ.

% Copyright 2019 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Call Helmholtz command with zero frequency:
u = helmholtz(varargin{1}, 0, varargin{2:end});
end


% Exact solution of Riemann problem; plot adapted to Osher scheme

% Theory in Section 10.2 of:

% 	P. Wesseling: Principles of Computational Fluid Dynamics
% 	Springer, Heidelberg, 2000 ISBN 3-5453-0. XII, 642 pp.
% See http://ta.twi.tudelft.nl/nw/users/wesseling/cfdbook.html

% This program is called by Euler schemes

% Functions called: f

global  PRL  CRL MACHLEFT  gamma  pleft  pright  rholeft  rhoright  uleft...
	uright  tend  lambda		% lambda = dt/dx

gammab = 1/(gamma - 1); gam1 = gamma-1;

%  Assumed structure of exact solution
%
%    \         /      |con |       |s|
%     \   f   /       |tact|       |h|
% left \  a  /  state |disc| state |o| right
% state \ n /    2    |cont|   3   |c| state
%   1    \ /          |tinu|       |k|   4
%         |           |ity |       | |

PRL = pright/pleft;
cright = sqrt(gamma*pright/rhoright); cleft = sqrt(gamma*pleft/rholeft);
CRL = cright/cleft;
MACHLEFT = (uleft - uright)/cleft;

p34 = fzero('f',3);		% p34 = p3/p4

tend = n*dt			% Exact time

p3 = p34*pright; alpha = (gamma+1)/(gamma-1);
rho3 = rhoright*(1+alpha*p34)/(alpha+p34);
rho2 = rholeft*(p34*pright/pleft)^(1/gamma);
u2 = uleft-uright+(2/(gamma-1))*cleft*...
  (1-(p34*pright/pleft)^((gamma-1)/(2*gamma)));
c2 = sqrt(gamma*p3/rho2);

spos = 0.5 + ...		% Shock position
 tend*cright*sqrt((gamma-1)/(2*gamma) + (gamma+1)/(2*gamma)*p34) + tend*uright;
conpos = 0.5 + u2*tend + tend*uright;	% Position of contact discontinuity
pos1 = 0.5 + (uleft - cleft)*tend;	% Start of expansion fan
pos2 = 0.5 + (u2+uright-c2)*tend;	% End of expansion fan

xx = 0:0.002:1;			% Preallocation
pexact = zeros(size(xx)); uexact= zeros(size(xx)); rhoexact = zeros(size(xx));
machexact = zeros(size(xx));  cexact = zeros(size(xx));
for i = 1:length(xx)
  if xx(i) <= pos1
    pexact(i) = pleft;    rhoexact(i) = rholeft;
    uexact(i) = uleft;    cexact(i) = sqrt(gamma*pexact(i)/rhoexact(i));
    machexact(i) = uexact(i)/cexact(i);
  elseif xx(i) <= pos2
    pexact(i) = pleft*(1+(pos1-xx(i))/(cleft*alpha*tend))^(2*gamma/(gamma-1));
    rhoexact(i) = rholeft*(1+(pos1-xx(i))/(cleft*alpha*tend))^(2/(gamma-1));
    uexact(i) = uleft + (2/(gamma+1))*(xx(i)-pos1)/tend;
    cexact(i) = sqrt(gamma*pexact(i)/rhoexact(i));
    machexact(i) = uexact(i)/cexact(i);
  elseif xx(i) <= conpos
    pexact(i) = p3;    		rhoexact(i) = rho2;
    uexact(i) = u2+uright;    	cexact(i) = sqrt(gamma*pexact(i)/rhoexact(i));
    machexact(i) = uexact(i)/cexact(i);
  elseif xx(i) <= spos
    pexact(i) = p3;     	rhoexact(i) = rho3;
    uexact(i) = u2+uright;      cexact(i) = sqrt(gamma*pexact(i)/rhoexact(i));
    machexact(i) = uexact(i)/cexact(i);
  else
    pexact(i) = pright;    	rhoexact(i) = rhoright;
    uexact(i) = uright;    	cexact(i) = sqrt(gamma*pexact(i)/rhoexact(i));
    machexact(i) = uexact(i)/cexact(i);
  end
end
entroexact = log(pexact./rhoexact.^gamma);

subplot(2,3,1), 	plot(xx,rhoexact,'-');
subplot(2,3,2), 	plot(xx,uexact,'-');
subplot(2,3,3),		plot(xx,pexact,'-');
subplot(2,3,4),		plot(xx,machexact,'-');
subplot(2,3,5),		enthexact = (gamma/(gamma-1))*pexact./rhoexact;
			plot(xx,enthexact,'-');
subplot(2,3,6),		plot(xx,entroexact,'-');


function geometry_test0235 ( )

%*****************************************************************************80
%
%% TEST0235 tests DMS_TO_RADIANS, RADIANS_TO_DMS.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST0235\n' );
  fprintf ( 1, '  DMS_TO_RADIANS converts an angle from\n' );
  fprintf ( 1, '    degrees/minutes/seconds to radians;\n' );
  fprintf ( 1, '  RADIANS_TO_DEGREES converts an angle from radians\n' );
  fprintf ( 1, '    to degrees/minutes/seconds;\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Radians     DMS     Radians\n' );
  fprintf ( 1, '\n' );

  for i = -2 : 15
    angle_rad = pi * i / 7.0;
    [ angle_deg, angle_min, angle_sec ] = radians_to_dms ( angle_rad );
    angle_rad2 = dms_to_radians ( angle_deg, angle_min, angle_sec );
    fprintf ( 1, '  %10f  %4d  %3d  %3d  %10f\n', ...
      angle_rad, angle_deg, angle_min, angle_sec, angle_rad2 );
  end

  return
end


function [LMSout,blms,Rsq]=LMSpol(y,p,x)
%Syntax: [LMSout,blms,Rsq]=LMSpol(y,p,x)
%_______________________________________
%
% Calculates the Least Median of Squares (LMS) polynomial regression
% parameters and output. It searches all the possible combinations
% of points and makes the intercept adjustment for every combination.
%
% LMSout is the LMS estimated values vector.
% blms is the LMS [intercept slopes] vector.
% Rsq is the R-squared.
% y is the vector of the dependent variable.
% p is the order of the polynomial.
% x is the vector of the independent variable.
%
% Reference:
% Rousseeuw PJ, Leroy AM (1987): Robust regression and outlier detection. Wiley.
%
%
% Alexandros Leontitsis
% Institute of Mathematics and Statistics
% University of Kent at Canterbury
% Canterbury
% Kent, CT2 7NF
% U.K.
%
% University e-mail: al10@ukc.ac.uk (until December 2002)
% Lifetime e-mail: leoaleq@yahoo.com
% Homepage: http://www.geocities.com/CapeCanaveral/Lab/1421
%
% Sep 3, 2001.

if nargin<1 | isempty(y)==1
   error('Not enough input arguments.');
else
   % y must be a column vector
   y=y(:);
   % n is the length of the data set
   n=length(y);
end

if nargin<2 | isempty(p)==1
   % If p is omitted give it the value of 1
   p=1;
else
   % p must be a scalar
   if sum(size(p))>2
      error('p must be a scalar.');
   end
   % p must be a non-negative integrer
   if round(p)-p~=0 | p<0
      error('p must be a non-negative integrer');
   end
end

if nargin<3 | isempty(x)==1
   % If x is omitted give it the values 1:n
   x=(1:n)';
else
   % x must be a column vector
   x=x(:);
   % x and y must have the same length
   if n~=size(x,1)
      error('x and y must have the same length.');
   end
end

if n<=p
   error('The ploynomial order is too large for the data set.');
end

% Prepare the matrix X for regression.
for i=1:p
   X(:,i)=x.^i;
end

% Perform the LMS regression
[LMSout, blms, Rsq]=LMSreg(y,X);


function f = p12_f ( n, x )

%*****************************************************************************80
%
%% P12_F evaluates the objective function for problem 12.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 March 2000
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the argument of the objective function.
%
%    Output, real F, the value of the objective function.
%
  f = 0.0;
  for i = 1 : 99
    arg = i / 100.0;
    r = abs ( ( - 50.0 * log ( arg ) )^( 2.0 / 3.0 ) + 25.0 - x(2) );

    t = exp ( - r^x(3) / x(1) ) - arg;

    f = f + t * t;

  end

  return
end


function [afixed,sqnorm,Qahat,Z] = lambda_routine1 (afloat,Qahat,ncands,fidlog)
%LAMBDA1: Integer ambiguity estimation using LAMBDA (extended version)
%
% This routine performs an integer ambiguity estimation using the
% LAMBDA-method, as developed by the Delft University of Technology,
% Mathematical Geodesy and Positioning.
%
% This version (lambda1) is intended to be used mainly for research.
% It has more options than the original code, such as output of
% intermediate results, optional number of candidates to be returned
% and so on. If you are not interested in these options, you can use
% "lambda2"
%
% Input arguments:
%    afloat: Float ambiguities (\hat{a}, must be a column!)
%    Qahat : Variance/covariance matrix of ambiguities (Q_\hat{a})
%    ncands: Number of candidates to be returned (default 2)
%    fidlog: File-id for intermediate results (default 0, no output)
%
% Output arguments:
%    afixed: Estimated integers
%    sqnorm: Distance between candidates and float ambiguity vector
%    Qzhat : Decorrelated variance/covariance matrix
%    Z     : Transformation matrix

% ----------------------------------------------------------------------
% File.....: lambda1.m
% Date.....: 19-MAY-1999
% Version..: 2.0b
% Author...: Peter Joosten
%            Mathematical Geodesy and Positioning
%            Delft University of Technology
% ----------------------------------------------------------------------

% ----------------------------------------------------------------------
% Initialise
% ----------------------------------------------------------------------

if nargin < 3; ncands = 2; end;
if nargin < 4; fidlog = 0; end;

n      = size (Qahat,1);
afixed = zeros(n,ncands);
sqnorm = zeros(1,ncands);

if fidlog ~= 0;
   if fidlog == 1; clc; end;
   fprintf (fidlog, '\n');
   fprintf (fidlog, ' -----------------------------------------------------------------------\n');
   fprintf (fidlog, ' "LAMBDA" - Least Squares Ambiguity Decorrelation Adjustment            \n');
   fprintf (fidlog, ' Rel. 2.0b d.d. 19 MAY 1999                                             \n');
   fprintf (fidlog, ' (c) Department of Mathematical Geodesy and Positioning (MGP)           \n');
   fprintf (fidlog, '     Faculty of Civil Engineering and Geosciences                       \n');
   fprintf (fidlog, '     Delft University of Technology, The Netherlands                    \n');
   fprintf (fidlog, ' -----------------------------------------------------------------------\n');
   fprintf (fidlog, '\n');
end;

% ----------------------------------------------------------------------
% Display the input-arguments
% ----------------------------------------------------------------------

writemat (fidlog,'Original float ambiguities',afloat);
writemat (fidlog,'Original variance/covariance matrix (Qahat)',Qahat);
writemat (fidlog,'Number of candidates requested',ncands);

% ----------------------------------------------------------------------
% --- Perform tests on the input, these tests are rather extensive   ---
% --- and can be switched off (by commenting-out) if necessary       ---
% --- Tests: Is the Q-matrix symmetric?                              ---
% ---        Is the Q-matrix positive-definite?                      ---
% ---        Do the Q-matrix and ambiguity-vector have identical     ---
% ---        dimensions?                                             ---
% ---        Is the ambiguity vector a column?                       ---
% ----------------------------------------------------------------------

if ~isequal(Qahat-Qahat'<1E-12,ones(size(Qahat)));
  error ('Variance/covariance matrix is not symmetric!');
end;

if sum(eig(Qahat)>0) ~= size(Qahat,1);
  error ('Variance/covariance matrix is not positive definite!');
end;

if length(afloat) ~= size(Qahat,1);
  error (['Variance/covariance matrix and vector of ambiguities do not have' ...
	  ' identical dimensions!']);
end;

if size(afloat,2) ~= 1;
  error ('Ambiguity-vector should be a column-vector');
end;

% ----------------------------------------------------------------------
% Make estimates in 'a' between -1 and +1 by subtracting an
% integer number, store the increments in 'incr' (= shift the centre
% of the ellipsoid over the grid by an integer translation)
% ----------------------------------------------------------------------

incr   = afloat - rem(afloat,1);
afloat = rem(afloat,1);
writemat (fidlog,'shifted ambiguities - increments',[afloat incr]);

% ----------------------------------------------------------------------
% Compute the Z-transformation based on L and D of Q, ambiguities
% are transformed according to \hat{z} = Z^T\hat{a}, Q is transformed
% according to Q_\hat{z} = Z^T * Q_\hat{a} * Z
% ----------------------------------------------------------------------

[Qz,Z,L,D,z] = decorrel (Qahat,afloat);

writemat (fidlog,'Decorrelated Q-matrix',Qz);
writemat (fidlog,'Transformation matrix (Z)',Z);
writemat (fidlog,'L-matrix, as used in ''search''',L);
writemat (fidlog,'D-matrix, as used in ''search''',D);
writemat (fidlog,'Transformed reduced ambiguities',z);

% ----------------------------------------------------------------------
% If the number of requested candidates is no more than "n+1", with "n"
% the dimension of Q_\hat(a), we can compute a suitable Chi^2 such that
% we have the requested number of candidates at minimum; use an eps
% to make sure the candidates are inside the ellipsoid and not exactly
% on the border.
%
% If the requested number of canditates is larger, we have to "guess"
% the volume of the search-space, it can not be guarenteed the requested
% number of canditates will actually be found.
% ----------------------------------------------------------------------

Chi2 = chistart (D,L,afloat,ncands);
writemat (fidlog,'Chi-squared, size of the search-ellipsoid',Chi2);

% ----------------------------------------------------------------------
% Find the requested number of candidates (search)
% ----------------------------------------------------------------------

[afixed,sqnorm,ierr] = lsearch (afloat,L,D,Chi2,ncands);

if ierr == 1;
   fprintf (1,'Warning: Not enough candidates were found!!\n');
   fprintf (1,'         Requested number.: %2d\n',ncands);
end;

writemat (fidlog,'Fixed transformed ''reduced'' ambiguities',afixed);
writemat (fidlog,'Corresponding squared norms',sqnorm);

% ----------------------------------------------------------------------
% Finally, perform the back-transformation and add the increments
% ----------------------------------------------------------------------

afixed = (afixed' * inv(Z))';
afixed = round(afixed + repmat(incr,1,ncands));
writemat (fidlog,'Final fixed ambiguities',afixed);

% ----------------------------------------------------------------------
% End of routine: lambda1
% ----------------------------------------------------------------------


function path = grFindMaximalLengthPath(nodes, edges, edgeWeights)
%GRFINDMAXIMALLENGTHPATH Find a path that maximizes sum of edge weights
%
%   PATH = grFindMaximalLengthPath(NODES, EDGES, EDGE_WEIGHTS);
%   Finds a greatest geodesic path in the graph. A path between two nodes
%   is a succession of adjacent edges that link the first and last nodes.
%   the length of the path is the sum of weights of edges that constitute
%   the path.
%   A geodesic path is a path that minimizes the length of the path among
%   the set of paths between the nodes.
%   A maximal length path maximizes the length of the geodesic path between
%   couples of nodes in the graph
%
%   The result PATH is the list of edge indices that constitutes the path.
%
%   PATH = grFindMaximalLengthPath(NODES, EDGES);
%   Assumes each edge has a weight equal to 1.
%
%   See Also
%   grFindGeodesicPath
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2011-05-22,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2011 INRA - Cepia Software Platform.

% ensure weights are defined
if ~exist('edgeWeights', 'var')
    edgeWeights = ones(size(edges, 1), 1);
end

% find an extremity vertex
inds = graphPeripheralVertices(nodes, edges, edgeWeights);
ind0 = inds(end);

% find a vertex opposite to the first extremity
dists = grPropagateDistance(nodes, edges, ind0, edgeWeights);
ind1 = find(dists == max(dists), 1, 'first');

% iterate on neighbors of current node: choose next neighbor with smallest
% cumulated weight, until we are back on source node
path = [];
while true
    % find neighbor with lowest cumulated distance
    neighs = grAdjacentNodes(edges, ind1);
    neighDists = dists(neighs);
    indN = find(neighDists == min(neighDists), 1);
    ind2 = neighs(indN);

    % add edge index to the path
    indE = find(sum(ismember(edges, [ind1 ind2]), 2) == 2, 1);
    path = [path indE]; %#ok<AGROW>

    % test if path is finished or not
    if ind2 == ind0
        break;
    end
    ind1 = ind2;
end


function prob_test156 ( )

%*****************************************************************************80
%
%% TEST156 tests WEIBULL_CDF, WEIBULL_CDF_INV, WEIBULL_PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST156\n' );
  fprintf ( 1, '  For the Weibull PDF:\n' );
  fprintf ( 1, '  WEIBULL_CDF evaluates the CDF;\n' );
  fprintf ( 1, '  WEIBULL_CDF_INV inverts the CDF.\n' );
  fprintf ( 1, '  WEIBULL_PDF evaluates the PDF;\n' );

  x = 3.0;

  a = 2.0;
  b = 3.0;
  c = 4.0;

  check = weibull_check ( a, b, c );

  if ( ~check );
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TEST156 - Fatal error!\n' );
    fprintf ( 1, '  The parameters are not legal.\n' );
    return
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  PDF parameter A =             %14f\n', a );
  fprintf ( 1, '  PDF parameter B =             %14f\n', b );
  fprintf ( 1, '  PDF parameter C =             %14f\n', c );

  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, '       X            PDF           CDF            CDF_INV\n' );
  fprintf ( 1, '\n' );

  for i = 1 : 10

    [ x, seed ] = weibull_sample ( a, b, c, seed );

    pdf = weibull_pdf ( x, a, b, c );

    cdf = weibull_cdf ( x, a, b, c );

    x2 = weibull_cdf_inv ( cdf, a, b, c );

    fprintf ( 1, ' %14f  %14f  %14f  %14f\n', x, pdf, cdf, x2 );

  end

  return
end


function [prob]=mhprob(val,cand,term,n,N)

% obtain the first exponential term
exp1=exp(-0.5*(exp(-cand)-exp(-val))*term);
% obtain the second exponential term
exp2=exp(-0.5*N*n*(cand-val));

% obtain the acceptance probability
prob=min(1,exp1*exp2);


function [ n_data, n, x, fx ] = w_polynomial_values ( n_data )

%*****************************************************************************80
%
%% W_POLYNOMIAL_VALUES returns values of Chebyshev polynomials W(n,x).
%
%  Discussion:
%
%    In Mathematica, the function can be evaluated by:
%
%      u = Sqrt[(x+1)/2],
%      ChebyshevU[2*n,u]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 April 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, integer N, the order of the function.
%
%    Output, real X, the point where the function is evaluated.
%
%    Output, real FX, the value of the function.
%
  n_max = 13;

  fx_vec = [ ...
     1.000000000000000E+00, ...
     2.600000000000000E+00, ...
     3.160000000000000E+00, ...
     2.456000000000000E+00, ...
     0.769600000000000E+00, ...
    -1.224640000000000E+00, ...
    -2.729024000000000E+00, ...
    -3.141798400000000E+00, ...
    -2.297853440000000E+00, ...
    -0.534767104000000E+00, ...
     1.442226073600000E+00, ...
     2.842328821760000E+00, ...
     3.105500041216000E+00 ];

  n_vec = [ ...
     0,  1,  2, ...
     3,  4,  5, ...
     6,  7,  8, ...
     9, 10, 11, ...
    12 ];

  x_vec = [ ...
     0.8E+00, ...
     0.8E+00, ...
     0.8E+00, ...
     0.8E+00, ...
     0.8E+00, ...
     0.8E+00, ...
     0.8E+00, ...
     0.8E+00, ...
     0.8E+00, ...
     0.8E+00, ...
     0.8E+00, ...
     0.8E+00, ...
     0.8E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    n = 0;
    x = 0.0;
    fx = 0.0;
  else
    n = n_vec(n_data);
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function circle_segment_test16 ( )

%*****************************************************************************80
%
%% CIRCLE_SEGMENT_TEST16 demonstrates GQCIRCSECT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 July 2013
%
%  Author:
%
%    Feifei Xu
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CIRCLE_SEGMENT_TEST17\n' );
  fprintf ( 1, '  Demonstrate GQCIRCSECT.\n' );

  close all;
  tmp = sqrt(2)/2;
%
%  Acute angle
%
  theta = -pi/4 : pi/200 : pi/4;

  for n = 9

    [x,y,w] = gqcircsect ( n, pi/4, 1 );

    figure;
    line([tmp,0,tmp],[-tmp,0,tmp]);
    hold on;
    plot(cos(theta),sin(theta));
    axis equal;
    plot(x,y,'k*');

    filename = 'test16_acute.png';
    print ( '-dpng', filename );
    fprintf ( 1, '  Created graphics image "%s".\n', filename );

  end
%
%  Obtuse angle
%
  theta = -pi/4*3 : pi/200 : pi/4*3;

  for n = 9

    [x,y,w] = gqcircsect ( n, pi/4*3, 1 );

    figure;
    line([-tmp,0,-tmp],[-tmp,0,tmp]);
    hold on;
    plot(cos(theta),sin(theta));
    axis equal;
    plot(x,y,'ko');

    filename = 'test16_obtuse.png';
    print ( '-dpng', filename );
    fprintf ( 1, '  Created graphics image "%s".\n', filename );

  end

  return
end


function bti_test03 ( )

%*****************************************************************************80
%
%% BTI_TEST03 tests BURGERS_TIME_INVISCID.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 April 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BTI_TEST03\n' );
  fprintf ( 1, '  Test BURGERS_TIME_INVISCID\n' );

  method = 3;
  nx = 81;
  nt = 200;
  t_max = 2.0;
  bc = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Method: 3, Lax Friedrichs.\n' );
  fprintf ( 1, '  Initial condition: expansion\n' );
  fprintf ( 1, '  Number of space nodes = %d\n', nx );
  fprintf ( 1, '  Number of time steps = %d\n', nt );
  fprintf ( 1, '  Final time T_MAX = %g\n', t_max );
  fprintf ( 1, '  Boundary condition = %d\n', bc );

  U = burgers_time_inviscid ( method, @ic_expansion, nx, nt, t_max, bc );

  x = linspace ( -1.0, +1.0, nx );

  figure ( 3 )

  plot ( x, U(1:50:(nt+1),:), 'Linewidth', 3 )
  grid on
  xlabel ( '<-- X -->' )
  ylabel ( '<-- U(X,T) -->' )
  title ( 'Burgers, inviscid, initial expansion, Lax Friedrichs' )

  filename = 'bti_test03.png';
  print ( '-dpng', filename )
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Saved plot as "%s"\n', filename );

  return
end


function chi = RoadInclination_Arkustangens(z_I_Ground_m,rx_I_IAji_m,ry_I_IAji_m,phiz_IK_rad)
%% Computation of Road Angles from Street Profile in Inertial Frame
% Input parameters:
% phiz_IK_rad     [rad]    Euler-Angle around z-Axis from Inertial Reference Frame I to Vehicle Fixed Reference Frame K
% rx_I_IAji_m     [m]      Track x-Position [x1 x2 x3 x4] [1x4]
% ry_I_IAji_m     [m]      Track y-Position [y1 y2 y3 y4] [1x4]
% z_I_Ground_m    [m]      Input Vector with z-Profile of Street in Inertial Coordinate System at the Axle Locations [z1 z2 z3 z4] [3x4]
% Output parameters:
% chi_rad         [rad]    Approximated Cardan Angles from Inertial Reference Frame to Road Fixed Reference Frame [chi_x chi_y Psi] [3x1]

%% Computation of Road Angles

% Distances [dxl dxr dyf dyr] between suspension hardpoints
dxl=sqrt( (rx_I_IAji_m(1)-rx_I_IAji_m(3)).^2 + (ry_I_IAji_m(1)-ry_I_IAji_m(3)).^2 );
dxr=sqrt( (rx_I_IAji_m(2)-rx_I_IAji_m(4)).^2 + (ry_I_IAji_m(2)-ry_I_IAji_m(4)).^2 );
dyf=sqrt( (rx_I_IAji_m(1)-rx_I_IAji_m(2)).^2 + (ry_I_IAji_m(1)-ry_I_IAji_m(2)).^2 );
dyr=sqrt( (rx_I_IAji_m(3)-rx_I_IAji_m(4)).^2 + (ry_I_IAji_m(3)-ry_I_IAji_m(4)).^2 );

% Inclination Angle in x-direction for negative z-road-profile
chi_y = -atan( 0.5*((z_I_Ground_m(1)+z_I_Ground_m(2))-(z_I_Ground_m(3)+z_I_Ground_m(4)))/(0.5*(dxl+dxr)) );
% Inclination Angle in y-direction for negative z-road-profile
chi_x = -atan( 0.5*((z_I_Ground_m(1)+z_I_Ground_m(3))-(z_I_Ground_m(2)+z_I_Ground_m(4)))/(0.5*(dyf+dyr)) );

%% Output Parameters
chi = [chi_x;chi_y;phiz_IK_rad];

end


function [Dlambda,volume,elemSign] = gradbasis3(node,elem)
%% GRADBASIS3 gradient of barycentric basis in 3-D.
%
% [Dlambda,volume,elemSign] = gradbasis3(node,elem) compute gradient of
% barycentric basis and volume of tetrahedron. The array volume is NT by 1
% and volume(t) is the volume of the t-th tetrahedron. Dlambda(1:NT, 1:3,
% 1:4) the first index is the label of tetrahedron, the second is the x-y-z
% coordinate, and the last one is the label of four indices of a tet. For
% example, Dlambda(t,:,1) is the gradient of lambda of the 1st index of the
% t-th tet. The elemSign array taking values 1 or -1 records the
% sign of the signed volume.
%
% See also gradbasis, gradu, gradu3
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

NT = size(elem,1);
face = uint32([elem(:,[2 4 3]);elem(:,[1 3 4]);elem(:,[1 4 2]);elem(:,[1 2 3])]);
v12 = node(face(:,2),:)-node(face(:,1),:);
v13 = node(face(:,3),:)-node(face(:,1),:);
normal = mycross(v12,v13,2);
v12 = v12(3*NT+1:4*NT,:);
v13 = v13(3*NT+1:4*NT,:);
v14 = node(elem(:,4),:)-node(elem(:,1),:);
volume = dot(mycross(v12,v13,2),v14,2)/6;
Dlambda = zeros(NT,3,4);
Dlambda(1:NT,:,1) = normal(1:NT,:)./[6*volume, 6*volume, 6*volume];
Dlambda(1:NT,:,2) = normal(NT+1:2*NT,:)./[6*volume, 6*volume, 6*volume];
Dlambda(1:NT,:,3) = normal(2*NT+1:3*NT,:)./[6*volume, 6*volume, 6*volume];
Dlambda(1:NT,:,4) = normal(3*NT+1:4*NT,:)./[6*volume, 6*volume, 6*volume];

% When the tetrahedron is not positive orientated, we reverse the sign of
% the volume. The sign of Dlambda is always right since signed volume is
% used in the computation.
idx = (volume<0);
volume(idx,:) = -volume(idx,:);
elemSign = ones(NT,1);
elemSign(idx) = -1;


clear all; close all; clc;

t=linspace(0,10,1000);
x=sin(t)+3*cos(t/3)+4*sin(t/4);

plot(t,x,'b');
grid on;
xlabel('time(s)'); ylabel('x(t)');
hold on;

Ts=0.1;
num_of_space=10/Ts;

t_DT=linspace(0,9,num_of_space);
x_DT=sin(t_DT)+3*cos(t_DT/3)+4*sin(t_DT/4);
% alpha=1;
alpha=t_DT(2)-t_DT(1);
    for ii=1:1:length(t_DT)
      if x_DT(ii)>=0
          rectangle('Position',[t_DT(ii),0,alpha,x_DT(ii)*(1/alpha)],'EdgeColor','r');
      else
          rectangle('Position',[t_DT(ii),1*x_DT(ii)*(1/alpha),alpha,abs(x_DT(ii)*(1/alpha))],'EdgeColor','r');
      end

    end

% axis([-1 11 -0.5 2.5])

% stem(t_DT,x_DT)


function varargout = gmm(varargin)
% Vl_GMM  Learn a Gaussian Mixture Model using EM
%   [MEANS, COVARIANCES, PRIORS] = VL_GMM(X, NUMCLUSTERS) fits a GMM with
%   NUMCLUSTERS components to the data X. Each column of X represent a
%   sample point. X may be either SINGLE or DOUBLE. MEANS, COVARIANCES, and
%   PRIORS are respectively the means, the diagonal covariances, and
%   the prior probabilities of the Guassian modes. MEANS and COVARIANCES
%   have the same number of rows as X and NUMCLUSTERS columns with one
%   column per mode. PRIORS is a row vector with NUMCLUSTER entries
%   summing to one.
%
%   [MEANS, COVARIANCES, PRIORS, LL] = VL_GMM(...) returns the
%   loglikelihood (LL) of the model as well.
%
%   [MEANS, COVARIANCES, PRIORS, LL, POSTERIORS] = VL_GMM(...) returns
%   the posterior probabilities POSTERIORS of the Gaussian modes given
%   each data point. The POSTERIORS matrix has NUMCLUSTERS rows and
%   NUMDATA columns.
%
%   VL_GMM() supports different initialization and optimization
%   methods. Specifically, the following options are supported:
%
%   Verbose::
%     Increase the verbosity level (may be specified multiple times).
%
%   Initialization:: RAND
%     RAND initializes the means as random data poitns and the
%     covaraince matrices as the covariance of X. CUSTOM allow
%     specifying the initial means, covariances, and prior
%     probabilities.
%
%   InitMeans:: none
%     Specify the initial means (size(X,1)-by-NUMCLUSTERS matrix).
%
%   InitPriors:: none
%     Specify the initial weights (a vector of dimension NUMCLUSTER).
%
%   InitCovariances:: none
%     Specify the initial diagonal covariance matrices
%
%   NumRepetitions:: 1
%     Number of times to restart EM. The solution with maximum
%     loglikelihood is returned.
%
%   CovarianceBound:: 10e-6
%     Set the lower bound on the diagonal covariance values.
%     The bound can be either a scalar or a vector with one
%     entry per dimension. Using null bounds is possible, but
%     may yield degenerate solutions, including NaNs.
%
%   Example::
%     VL_GMM(X, 10, 'verbose', 'MaxNumIterations', 20) learns a
%     mixture of ten Gaussians using at most twenty iterations of the
%     algorithm.
%
%   See also: GMMs, VL_KMEANS(), VL_HELP().
[varargout{1:nargout}] = vl_gmm(varargin{:});


function nnchecknumgrad(nn, x, y)
    epsilon = 1e-6;
    er = 1e-7;
    n = nn.n;
    for l = 1 : (n - 1)
        for i = 1 : size(nn.W{l}, 1)
            for j = 1 : size(nn.W{l}, 2)
                nn_m = nn; nn_p = nn;
                nn_m.W{l}(i, j) = nn.W{l}(i, j) - epsilon;
                nn_p.W{l}(i, j) = nn.W{l}(i, j) + epsilon;
				rand('state',0)
                nn_m = nnff(nn_m, x, y);
                rand('state',0)
                nn_p = nnff(nn_p, x, y);
                dW = (nn_p.L - nn_m.L) / (2 * epsilon);
                e = abs(dW - nn.dW{l}(i, j));

                assert(e < er, 'numerical gradient checking failed');
            end
        end
    end
end