function fem1d ( )

%*****************************************************************************80
%
%% MAIN is the main program for FEM1D.
%
%  Discussion:
%
%    FEM1D solves a one dimensional ODE using the finite element method.
%
%    The differential equation has the form:
%
%      -d/dx ( p(x) du/dx ) + q(x) * u = f(x)
%
%    The finite-element method uses piecewise linear basis functions.
%
%    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 or U' at XL and XR are also specified.
%
%    The interval [XL,XR] is "meshed" with NSUB+1 points,
%
%    XN(0) = XL, XN(1)=XL+H, XN(2)=XL+2*H, ..., XN(NSUB)=XR.
%
%    This creates NSUB subintervals, with interval number 1
%    having endpoints XN(0) and XN(1), and so on up to interval
%    NSUB, which has endpoints XN(NSUB-1) and XN(NSUB).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 October 2008
%
%  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 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 H(N), the length of the subintervals.
%
%    integer 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 INDX(1:N+1).
%    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 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 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 NQUAD.
%    The number of quadrature points used in a subinterval.
%    This code uses NQUAD = 1.
%
%    integer NSUB, the number of subintervals into which the interval
%    [XL,XR] is broken.
%
%    integer NU, 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.
%
%    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 XL, the left endpoint of the interval over which the
%    differential equation is being solved.
%
%    real XN(1:N+1).
%    XN(I) is the location of the I-th node.  XN(1) is XL,
%    and XN(N+1) is XR.
%
%    real XQUAD(N)
%    XQUAD(I) is the location of the single quadrature point
%    in interval I.
%
%    real XR, the right endpoint of the interval over which the
%    differential equation is being solved.
%
  n = 100;
  nl = 2;

  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FEM1D\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Solve the two-point boundary value problem:\n' );
  fprintf ( 1, '    -d/dx (p(x) du/dx) + q(x)*u  =  f(x)\n' );
  fprintf ( 1, '  on an interval [xl,xr], with the values of\n' );
  fprintf ( 1, '  u or u'' specified at xl and xr.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The interval is broken into %d subintervals.\n', n );
  fprintf ( 1, '  The number of basis functions per element is %d\n', nl );
%
%  Initialize variables that define the problem.
%
  [ ibc, nquad, ul, ur, xl, xr ] = init ( );
%
%  Compute the quantities that describe the geometry of the problem.
%
  [ h, indx, node, nu, xn, xquad ] = geometry ( ibc, nl, n, xl, xr );
%
%  Assemble the matrix.
%
  [ adiag, aleft, arite, f ] = assemble ( h, indx, nl, node, ...
    nu, nquad, n, ul, ur, xn, xquad );
%
%  Print out the linear system.
%
  system_print ( adiag, aleft, arite, f, nu );
%
%  Solve the linear system.
%
  u = solve ( adiag, aleft, arite, f, nu );
%
%  Print the current solution.
%
  output ( u, ibc, indx, n, nu, ul, ur, xn );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FEM1D:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

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

%*****************************************************************************80
%
%% ASSEMBLE assembles the matrix and right hand side of the linear system.
%
%  Discussion:
%
%    Note that a 1 point quadrature rule, which is sometimes used to
%    assemble the matrix and right hand side, is just barely accurate
%    enough for simple problems.  If you want better results, you
%    should use a quadrature rule that is more accurate.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 April 2007
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, real H(N), the length of the subintervals.
%
%    Input, integer INDX(1:N+1).
%    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 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 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 NQUAD, the number of quadrature points in a subinterval.
%    This code uses NQUAD = 1.
%
%    Input, integer N, the number of subintervals into which the interval
%    [XL,XR] is broken.
%
%    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(1:N+1), the location of the I-th node.  XN(1) is XL,
%    and XN(N+1) is XR.
%
%    Input, real XQUAD(N), the location of the single quadrature point
%    in interval I.
%
%    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), the right hand side of the linear
%    equations.
%
  f(1:nu) = 0.0;
  adiag(1:nu) = 0.0;
  aleft(1:nu) = 0.0;
  arite(1:nu) = 0.0;
%
%  For element IE...
%
  for ie = 1 : n

    he = h(ie);
    xleft = xn(node(1,ie)+1);
    xrite = xn(node(2,ie)+1);
%
%  For quadrature point IQ...
%
    for iq = 1 : nquad

      xqe = xquad(ie);
%
%  For basis function IL...
%
      for il = 1 : nl

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

        if ( 0 < iu )

          [ phii, phiix ] = phi ( il, xqe, xleft, xrite );

          f(iu) = f(iu) + he * ff ( xqe ) * phii;
%
%  Handle boundary conditions.
%
          if ( ig == 0 )

            x = xn(1);
            f(iu) = f(iu) - pp ( x ) * ul;

          elseif ( ig == n )

            x = xn(n+1);
            f(iu) = f(iu) + pp ( x ) * ur;

          end
%
%  For basis function JL...
%
          for jl = 1 : nl

            jg = node(jl,ie);
            ju = indx(jg+1);

            [ phij, phijx ] = phi ( jl, xqe, xleft, xrite );

            aij = he * ( pp ( xqe ) * phiix * phijx ...
                  + qq ( xqe ) * phii * phij );

            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 returns the right hand side of the differential equation.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of F(X).
%
  value = 0.0;

  return
end
function [ h, indx, node, nu, xn, xquad ] = geometry ( ibc, nl, nsub, ...
  xl, xr )

%*****************************************************************************80
%
%% GEOMETRY sets up the geometry for the interval [XL,XR].
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 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 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 NSUB.
%    The number of subintervals into which the interval
%    [XL,XR] is broken.
%
%    Input, real XL.
%    XL is the left endpoint of the interval over which the
%    differential equation is being solved.
%
%    Input, real XR.
%    XR is the right endpoint of the interval over which the
%    differential equation is being solved.
%
%    Output, real H(NSUB), the length of the subintervals.
%
%    Output, integer INDX(1:NSUB+1).
%    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,NSUB).
%    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 NSUB-1,
%    NSUB, or NSUB+1 unknown values, which are the coefficients
%    of basis functions.
%
%    Output, real XN(1:NSUB+1).
%    XN(I) is the location of the I-th node.  XN(1) is XL,
%    and XN(N+1) is XR.
%
%    Output, real XQUAD(NSUB)
%    XQUAD(I) is the location of the single quadrature point
%    in interval I.
%

%
%  Set the value of XN, the locations of the nodes.
%
  xn = zeros ( nsub + 1, 1 );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Node      Location\n' );
  fprintf ( 1, '\n' );
  for i = 0 : nsub
    xn(i+1) =  ( ( nsub - i ) * xl   ...
               + (        i ) * xr ) ...
               / ( nsub     );
  end
  r8vec_print_some ( nsub + 1, xn, 1, 10, '  First 10 nodes:' );
%
%  Set the lengths of each subinterval.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'Subint    Length\n' );
  fprintf ( 1, '\n' );
  h = zeros ( nsub, 1 );
  for i = 1 : nsub
    h(i) = xn(i+1) - xn(i);
  end
  r8vec_print_some ( nsub, h, 1, 10, '  First 10 interval widths:' );
%
%  Set the quadrature points, each of which is the midpoint of its subinterval.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'Subint    Quadrature point\n' );
  fprintf ( 1, '\n' );
  xquad = zeros ( nsub );
  for i = 1 : nsub
    xquad(i) = 0.5 * ( xn(i) + xn(i+1) );
  end
  r8vec_print_some ( nsub, xquad, 1, 10, '  First 10 quadrature points:' );
%
%  Set the value of NODE, which records, for each interval,
%  the node numbers at the left and right.
%
  node = zeros ( 2, nsub );
  for i = 1 : nsub
    node(1,i) = i - 1;
    node(2,i) = i;
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  First 10 pairs of nodes defining intervals:\n' );
  fprintf ( 1, 'Subint  Left Node  Right Node\n' );
  fprintf ( 1, '\n' );
  for i = 1 : min ( nsub, 10 )
    fprintf ( 1, '  %6d  %6d  %6d\n', i, node(1,i), node(2,i) );
  end
%
%  Starting with node 0, see if an unknown is associated with
%  the node.  If so, give it an index.
%
  nu = 0;
%
%  Handle first node.
%
  i = 0;
  if ( ibc == 1 || ibc == 3 )
    indx(i+1) = -1;
  else
    nu = nu + 1;
    indx(i+1) = nu;
  end
%
%  Handle nodes 1 through nsub-1
%
  for i = 1 : nsub-1
    nu = nu + 1;
    indx(i+1) = nu;
  end
%
%  Handle the last node.
%
  i = nsub;
  if ( ibc == 2 || ibc == 3 )
    indx(i+1) = -1;
  else
    nu = nu + 1;
    indx(i+1) = nu;
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  First 10 unknown indices\n' );
  fprintf ( 1, '    Node  Unknown\n' );
  fprintf ( 1, '\n' );
  for i = 0 : min ( nsub, 9 )
    fprintf ( 1, '  %6d  %6d\n', i, indx(i+1) );
  end

  return
end
function [ ibc, nquad, ul, ur, xl, xr ] = init  ( )

%*****************************************************************************80
%
%% INIT initializes variables that define the problem.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    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, integer NQUAD.
%    The number of quadrature points used in a subinterval.
%    This code uses NQUAD = 1.
%
%    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 XR.
%    XR is the right endpoint of the interval over which the
%    differential equation is being solved.
%
  ibc = 1;
  nquad = 1;
  ul = 0.0;
  ur = 1.0;
  xl = 0.0;
  xr = 1.0;
%
%  Print out the values that have been set.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'The equation is to be solved for\n' );
  fprintf ( 1, 'X greater than XL = %f\n', xl );
  fprintf ( 1, ' and less than XR = %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

  fprintf ( 1, '\n' );
  fprintf ( 1, 'Number of quadrature points per element is %d\n', nquad );

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

%*****************************************************************************80
%
%% OUTPUT prints out the computed solution at the nodes.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, real F(NU), 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(1:N+1).
%    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 NSUB.
%    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(1:N+1).
%    XN(I) is the location of the I-th node.  XN(1) is XL,
%    and XN(N+1) is XR.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'First 10 entries of computed solution:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    Node        X(I)          U(I)\n' );
  fprintf ( 1, '\n' );

  for i = 0 : min ( nsub, 9 )

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

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

  end

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

%*****************************************************************************80
%
%% PHI evaluates a linear basis function and its derivative.
%
%  Discussion:
%
%    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:
%
%    09 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, integer IL, the index of the basis function.
%    1, the function which is 1 at XLEFT and 0 at XRITE.
%    2, the function which is 0 at XLEFT and 1 at XRITE.
%
%    Input, real X, the evaluation point.
%
%    Input, real XLEFT, XRITE, the left and right
%    endpoints of the interval.
%
%    Output, real PHII, PHIIX, the value of the
%    basis function and its derivative at X.
%
  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 returns the value of the coefficient function P(X).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of P(X).
%
  value = 1.0;

  return
end
function value = qq ( x )

%*****************************************************************************80
%
%% QQ returns the value of the coefficient function Q(X).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of Q(X).
%
  value = 0.0;

  return
end
function r8vec_print_some ( n, a, i_lo, i_hi, title )

%*****************************************************************************80
%
%% R8VEC_PRINT_SOME prints "some" of an R8VEC.
%
%  Discussion:
%
%    An R8VEC is a vector of R8 values.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 September 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the dimension of the vector.
%
%    Input, real A(N), the vector to be printed.
%
%    Input, integer MAX_PRINT, the maximum number of lines to print.
%
%    Input, string TITLE, a title.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '%s\n', title );
  fprintf ( 1, '\n' );

  for i = max ( 1, i_lo ) : min ( n, i_hi )
    fprintf ( 1, '  %8d: %12f\n', i, a(i) );
  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:
%
%    09 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.
%    Note that for the first equation, there is no ALEFT
%    coefficient, and for the last, there is no ARITE.
%    So there is no need to store a value in ALEFT(1), nor
%    in ARITE(NU).
%
%    Input, real F(NU),  the right hand side of the linear
%    system to be solved.
%
%    Input, integer NU.
%    NU is the number of equations to be solved.
%
%    Output, real U(NU), the solution of the linear system.
%
  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 = zeros ( 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

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

%*****************************************************************************80
%
%% SYSTEM_PRINT prints out the tridiagonal linear system to be solved.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 October 2008
%
%  Author:
%
%    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.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'First 10 rows of tridiagonal linear system:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'Equation   ALEFT         ADIAG         ARITE         RHS\n' );
  fprintf ( 1, '\n' );

  for i = 1 : min ( nu, 10 )

    fprintf ( 1, '%3d', i );

    if ( i == 1 )
      fprintf ( 1, '              ' );
    else
      fprintf ( 1, '  %12f', aleft(i) );
    end

    fprintf ( 1, '  %12f', adiag(i) );

    if ( i < nu )
      fprintf ( 1, '  %12f', arite(i) );
    else
      fprintf ( 1, '              ' );
    end

    fprintf ( 1, '  %12f\n', f(i) );

  end

  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 [edges,didSucceed]=graph4VertexDeg(d)
%%GRAPH4VERTEXDEG Given a list of the degrees of vertices in a
%           non-directional graph, get a set of edges representing a graph
%           that satisfies the degree profile, or indicate if no such graph
%           can exist. Note that a set of edges for a degree profile is not
%           unique. This just returns one set.
%
%INPUTS: d An nX1 or 1Xn vector of the degrees of the n vertices. These are
%          integer values >=0. They represent the number of edges touching
%          the nodes.
%
%OUTPUTS: edges A 2XnumEdge set of edges that satisfies the degree profile.
%               An empty matrix is returned if all edges have zero degree
%               or if no set ofvedges exists that can result in the given
%               degree profile. An edge [i;j] goes between vertex i and
%               vertex j and adds to the degree count for both vertices.
%    didSucceed This is true if an edges exists that can satisfy d.
%               Otherwise, this is false and edges is an empty matrix.
%
%This function implements Algorithm H in Chapter 7 of [1].
%
%EXAMPLE:
% [edges,didSucceed]=graph4VertexDeg([2;1;6;3;2;1;1])
%The algorithm in the example will succeed and the edge set is
%edges=[7,6,2,5,5,1,1,4;
%       3,3,3,4,3,4,3,3];
%
%REFERENCES:
%[1] D. E. Knuth, The Art of Computer Programming. Vol. 4A: Combinatorial
%    Algorithms, Part I, Boston: Addison-Wesley, 2011.
%
%October 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

[d,idx]=sort([d(:);0],'descend');

%Allocate space
maxEdges=ceil(sum(d)/2);
edges=zeros(2,maxEdges);
c=zeros(d(1),1);

numEdges=0;

%Step H1
k=d(1);
j=0;
while(k>0)
    j=j+1;
    while(k>d(j+1))
        c(k)=j;
        k=k-1;
    end
end
%If all of the d's are zero.
if(j==0)
    edges=[];
    didSucceed=true;
    return;
end

while(1)
    %Step H2, find n.
    n=c(1);
    if(n==0)
        %Shrink to fit and restore the original indexation.
        edges=idx(edges(:,1:numEdges));
        %Restore the original indexation.
        didSucceed=true;
        return;
    elseif(d(1)>=n)
        edges=[];
        didSucceed=false;
        return;
    end

    %Step H3
    i=1;
    t=d(1);
    r=c(t);
    j=d(n);

    %Step H4, generate a new edge.
    while(1)
        c(j)=c(j)-1;
        m=c(t);
        numEdges=numEdges+1;
        edges(:,numEdges)=[n;m];
        d(m)=d(m)-1;
        c(t)=m-1;
        j=j-1;
        if(j==0)
            break;
        elseif(m==i)
            i=r+1;
            t=d(i);
            r=c(t);
        end
    end
end

end

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


function mean = half_normal_mean ( a, b )

%*****************************************************************************80
%
%% HALF_NORMAL_MEAN returns the mean of the Half Normal PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, the parameters of the PDF.
%    0.0 < B.
%
%    Output, real MEAN, the mean of the PDF.
%
  mean = a + b * sqrt ( 2.0 / pi );

  return
end


function [d2f, err] = tapas_riddersdiff2(f, x, varargin)
% Differentiates the function f *twice* at point x according to Ridders' method:
%
% Ridders, CJF. (1982). Accurate computation of F'(x) and F'(x) F''(x). Advances in Engineering
%     Software, 4(2), 75-6.
%
% INPUT:
%    f             Function handle of a scalar real function of one real variable
%    x             Point at which to differentiate f
%
% OUTPUT:
%    d2f           Second derivative of f at x
%    err           Error estimate
%
% OPTIONS:
%    Optionally, the third argument of the function can be a structure containing further
%    settings for Ridder's method.
%
%    varargin{1}.init_h      Initial finite difference (default: 1)
%    varargin{1}.div         Divisor used to reduce h on each step (default: 1.2)
%    varargin{1}.min_steps   Minimum number of steps in h (default: 3)
%    varargin{1}.max_steps   Maximum number of steps in h (default: 100)
%    varargin{1}.tf          Terminate if last step worse than preceding by a factor of tf
%                            (default: 2)
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2012-2013 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is released under the terms of the GNU General Public Licence (GPL), version 3. You can
% redistribute it and/or modify it under the terms of the GPL (either version 3 or, at your option,
% any later version). For further details, see the file COPYING or <http://www.gnu.org/licenses/>.

    % Defaults
    init_h     = 1;
    div        = 1.2;
    min_steps  = 3;
    max_steps  = 100;
    tf         = 2;
    d2f        = NaN;
    err        = realmax;

    % Overrides
    if nargin > 2
        options = varargin{1};

        if isfield(options,'init_h')
            init_h = options.init_h;
        end

        if isfield(options,'div')
            div = options.div;
        end

        if isfield(options,'min_steps')
            min_steps = options.min_steps;
        end

        if isfield(options,'max_steps')
            max_steps = options.max_steps;
        end

        if isfield(options,'tf')
            tf = options.tf;
        end
    end

    % Initialize matrix of polynomial interpolation values
    P = NaN(max_steps);

    % Initialize finite difference step
    h = init_h;

    % Approximate 2nd derivative at initial step
    P(1,1) = (f(x+h)-2*f(x)+f(x-h))/h^2;

    % Loop through rows of P (i.e., steps of h)
    for i = 2:max_steps

        % New step size
        h = h/div;

        % Approximate 2nd derivative at this step
        P(i,1) = (f(x+h)-2*f(x)+f(x-h))/h^2;

        % Use square of div for extrapolation because errors increase
        % quadratically with h (here, of course, they decrease quadratically
        % because we're reducing h...)
        divsq = div^2;
        t = divsq;

        % Fill the current row using Richardson extrapolation
        for j = 2:i

            % Richardson
            P(i,j) = (t*P(i,j-1)-P(i-1,j-1))/(t-1);

            % Increment extrapolation factor
            t = t*divsq;

            % Error on this trial is defined as the maximum absolute difference
            % to the extrapolation parents
            currerr = max(abs(P(i,j)-P(i,j-1)),abs(P(i,j)-P(i-1,j-1)));

            if currerr < err
                err = currerr;
                d2f = P(i,j);
            end
        end

        % Stop if errors start increasing (to be expected for very small
        % values of h)
        if i > min_steps && abs(P(i,i)-P(i-1,i-1)) > tf*err
            return
        end
    end
end


function [t,X] = smdplot(t,X)
% See Article 2.6
% [t,X]=smdplot(t,X)
% This function plots the response and animates the
% motion of a damped linear harmonic oscillator
% characterized by the differential equation
% m*x''+c*x'+k*x=f1*cos(w*t)+f2*sin(w*t)
% with initial conditions x(0)=x0, x'(0)=v0.
% The animation depicts forced motion of a block
% attached to a wall by a spring. The block
% slides on a horizontal plane which provides
% viscous damping.

% Default data case used when no input arguments
% are given
if nargin==0
  m=1; c=.3; k=1; f1=1.5; f2=0; w=2; x0=0; v0=2;
  %tmax=30; nt=250; t=linspace(0,tmax,nt);
  tmax=25; nt=200; t=linspace(0,tmax,nt);
  X=smdsolve(m,c,k,f1,f2,w,x0,v0,t);
end

% Plot the displacement versus time
plot(t,X), xlabel('time'), ylabel('displacement')
title(...
'FORCED RESPONSE OF A DAMPED HARMONIC OSCILLATOR')
grid on, shg, pause(3)

% Add a block and a spring to the displacement
xmx=max(abs(X)); X=X/1.1/xmx;
xb=[0,0,1,1,0,0]/2; yb=[0,-1,-1,1,1,0]/2;

% Make an arrow tip
d=.08; h=.05;
xtip=[0,-d,-d,0]; ytip=[0,0,0,h,-h,0];

% Add a spring and a block to the response
[xs,ys]=spring; nm=length(X); ns=length(xs);
nb=length(xb); x=zeros(nm,ns+nb);y=[ys,yb];
for j=1:nm, x(j,:)=[-1+(1+X(j))*xs,X(j)+xb];end
r=[min(x(:)),max(x(:))+1,-2,2];
rx=r([1 1 2]); ry=[.5,-.5,-.5]; close;

% Plot the motion
for j=1:nm
   % Compute and scale the applied force
   f=f1*cos(w*t(j))+f2*sin(w*t(j));
   f=.3*f; fa=abs(f); sf=sign(f);
   xj=x(j,:); xmaxj=max(xj);
   if sf>0
      xforc=xmaxj+[0,fa,fa+xtip];
   else
      xforc=xmaxj+[fa,0,-xtip];
   end

   % Plot the spring, block, and force
   plot(xj,y,rx,ry,'k',xforc,ytip,'r')
   title('FORCED MOTION WITH DAMPING')
   axis(r), axis('off'), drawnow
   figure(gcf), pause(.05)
end

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

function [x,y] = spring(len,ht)
% This function generates a set of points
% defining a spring

if nargin==0, len=1; ht=.125; end
x=[0,.5,linspace(1,11,10),11.5,12];
y=[ones(1,5);-ones(1,5)];
y=[0;0;y(:);0;0]'; y=ht/2/max(y)*y;
x=len/max(x)*x;

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

function [x,v]=smdsolve(m,c,k,f1,f2,w,x0,v0,t)
% [x,v]=smdsolve(m,c,k,f1,f2,w,x0,v0,t)
% This function solves the differential equation
% m*x''(t)+c*x'(t)+k*x(t)=f1*cos(w*t)+f2*sin(w*t)
% with x(0)=x0 and x'(0)=v0
%
% m,c,k  - mass, damping and stiffness coefficients
% f1,f2  - magnitudes of cosine and sine terms in
%          the forcing function
% w      - frequency of the forcing function
% t      - vector of times to evaluate the solution
% x,v    - computed position and velocity vectors

ccrit=2*sqrt(m*k); wn=sqrt(k/m);

% If the system is undamped and resonance will
% occur, add a little damping
if c==0 & w==wn; c=ccrit/1e6; end;

% If damping is critical, modify the damping
% very slightly to avoid repeated roots
zeta=c/ccrit; if zeta==1, zeta=zeta+1e-6; end

% Forced response solution
a=(f1-i*f2)/(k-m*w^2+i*c*w);
X0=real(a); V0=real(i*w*a);
X=real(a*exp(i*w*t)); V=real(i*w*a*exp(i*w*t));

% Homogeneous solution
r=sqrt(zeta^2-1); s1=wn*(-zeta+r); s2=wn*(-zeta-r);
p=[1,1;s1,s2]\[x0-X0;v0-V0];

% Total solution satisfying the initial conditions
x=X+real(p(1)*exp(s1*t)+p(2)*exp(s2*t));
v=V+real(p(1)*s1*exp(s1*t)+p(2)*s2*exp(s2*t));


function c = t_project_coefficients ( n, f )

%*****************************************************************************80
%
%% T_PROJECT_COEFFICIENTS: function projected onto T(0:n,x).
%
%  Discussion:
%
%    It is assumed that the interval of definition is -1 <= x <= +1.
%
%    Over this interval, f(x) will be well approximated by
%
%      f(x) approx sum ( 0 <= i <= n ) c(i) * T(i,x)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 March 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the highest order polynomial to compute.
%
%    Input, function handle F, of the form
%      function v = f ( x )
%
%    Output, real C(N+1,1), the projection coefficients of f(x) onto
%    T(0,x) through T(n,x).
%
  for k = 1 : n + 1
    y = cos ( pi * ( k - 0.5 ) / ( n + 1 ) );
    d(k) = f ( y );
  end

  fac = 2.0 / ( n + 1 );
  for j = 1 : ( n + 1 )
    sum = 0.0;
    for k = 1 : ( n + 1 )
      sum = sum + d(k) * cos ( ( pi * ( j - 1 ) ) * ( ( k - 0.5 ) / ( n + 1 ) ) );
    end
    c(j) = fac * sum;
  end

  c(1) = c(1) / 2.0;

  return
end


function [ok,tp,tq] = linenear(pa,pb,pc,pd)
%LINENEAR calc. the nearest points on line segments embedded
%in d-dimensions. Line paramters are bounded on [-1,+1].

%-----------------------------------------------------------
%   Darren Engwirda : 2017 --
%   Email           : de2363@columbia.edu
%   Last updated    : 10/10/2017
%-----------------------------------------------------------

    m1 = (pa+pb) * +.5 ;
    D1 = (pb-pa) * +.5 ;

    m2 = (pc+pd) * +.5 ;
    D2 = (pd-pc) * +.5 ;

    r1 = sum(m2.*D1,2) ...
       - sum(m1.*D1,2) ;
    r2 = sum(m1.*D2,2) ...
       - sum(m2.*D2,2) ;

    A1 = sum(D1.*D1,2) ;
    A2 =-sum(D1.*D2,2) ;
    A3 =-sum(D1.*D2,2) ;
    A4 = sum(D2.*D2,2) ;

    dd = A1.*A4 - A2.*A3 ;

    tp = A4.*r1 - A2.*r2 ;
    tq =-A3.*r1 + A1.*r2 ;

    rt = max(abs([A1,A2,A3,A4]),[],2);
    rt = rt * eps ^ .8 ;

    ok = abs(dd) > +rt ;

    tp(~ok) = +0. ;
    tq(~ok) = +0. ;

    tp(ok) = tp(ok) ./ dd(ok) ;
    tq(ok) = tq(ok) ./ dd(ok) ;

end


% generating channels with fliter method and spectrum method
% evaluate autocorrelation functions
%%
clear all
clc
close all

% doppler
fDTs=0.01;
fD=100;
% sampling time
Ts=fDTs/fD;

Ns=1E5;

%% spectrum method
h1=flat_spec(Ns,fD,fDTs);

% autocorrelation
Acn=xcorr(h1,'biased');
l=length(Acn);
% truncate
w=ceil(4/fDTs);
TAc=Acn(ceil(l/2)-w:ceil(l/2)+w);

lt=length(TAc);
t=-Ts*floor(lt/2):Ts:Ts*floor(lt/2);
% theoritical value of autocorrelation
Ac_th=besselj(0,2*pi*fD*t);

figure;
plot(t,real(Ac_th),t,real(TAc),'r');
title('Autocorrelation');
xlabel('time');
legend('theory','spec. method');
grid on

%% filter method
h1=flat_filter(Ns,fD,fDTs);

Acn=xcorr(h1,'biased');
l=length(Acn);
w=ceil(4/fDTs);
TAc=Acn(ceil(l/2)-w:ceil(l/2)+w);

l=length(TAc);
t=-Ts*floor(l/2):Ts:Ts*floor(l/2);
% theory
Ac_th=besselj(0,2*pi*fD*t);

figure;
plot(t,real(Ac_th),t,real(TAc),'r');
title('Autocorrelation');
xlabel('time');
legend('theory','filter method');
grid on


function [ pts ] = genRansacTestPoints( ptNum,outlrRatio,inlrStd,inlrCoef )
%GENRANSACTESTPOINTS Generate the points used by RANSAC function
%   PTS = GENRANSACTESTPOINTS(PTNUM,OUTLRRATIO,INLRSTD,INLRCOEF) PTS is
%   2*PTNUM, including PTNUM points, among which ROUND(OUTLRRATIO*PTNUM)
%   are outliers, others are inliers.
%		The inliers are around the line: y = INLRCOEF(1)*x + INLRCOEF(2),
%   INLRSTD is the standard deviation of, the dist between inliers and the
%	line. The outliers

outlrNum = round(outlrRatio*ptNum);
inlrNum = ptNum-outlrNum;

k = inlrCoef(1);
b = inlrCoef(2);
X = (rand(1,inlrNum)-.5)*ptNum; % X is in [-ptNum/2,ptNum/2]
Y = k*X+b;

% add noise for inliers
dist = randn(1,inlrNum)*inlrStd;
theta = atan(k);
X = X+dist*(-sin(theta));
Y = Y+dist*cos(theta);
inlrs = [X;Y];

outlrs = (rand(2,outlrNum)-.5)*ptNum;
% outlrs = (rand(2,outlrNum)-[ones(1,outlrNum)*.5;ones(1,outlrNum)*.1])*ptNum;
pts = [inlrs,outlrs];

end


function p = predictOneVsAll(all_theta, X)
%PREDICT Predict the label for a trained one-vs-all classifier. The labels
%are in the range 1..K, where K = size(all_theta, 1).
%  p = PREDICTONEVSALL(all_theta, X) will return a vector of predictions
%  for each example in the matrix X. Note that X contains the examples in
%  rows. all_theta is a matrix where the i-th row is a trained logistic
%  regression theta vector for the i-th class. You should set p to a vector
%  of values from 1..K (e.g., p = [1; 3; 1; 2] predicts classes 1, 3, 1, 2
%  for 4 examples)

m = size(X, 1);
num_labels = size(all_theta, 1);

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

% Add ones to the X data matrix
X = [ones(m, 1) X];

% ====================== YOUR CODE HERE ======================
% Instructions: Complete the following code to make predictions using
%               your learned logistic regression parameters (one-vs-all).
%               You should set p to a vector of predictions (from 1 to
%               num_labels).
%
% Hint: This code can be done all vectorized using the max function.
%       In particular, the max function can also return the index of the
%       max element, for more information see 'help max'. If your examples
%       are in rows, then, you can use max(A, [], 2) to obtain the max
%       for each row.
%

sig = sigmoid(X * all_theta');
[maxSig, maxSig_2] = max(sig');
p = maxSig_2';

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

end


function cosh_values_test ( )

%*****************************************************************************80
%
%% COSH_VALUES_TEST demonstrates the use of COSH_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 June 2007
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'COSH_VALUES_TEST:\n' );
  fprintf ( 1, '  COSH_VALUES stores values of \n' );
  fprintf ( 1, '  the hyperbolic cosine function.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      X           FX\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, fx ] = cosh_values ( n_data );

    if ( n_data == 0 )
      break
    end

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

  end

  return
end


function [ c, discrepancy ] = partition_brute ( n, w )

%*****************************************************************************80
%
%% PARTITION_BRUTE approaches the partition problem using brute force.
%
%  Discussion:
%
%    We are given a set of N integers W.
%
%    We seek to partition W into subsets W0 and W1, such that the subsets
%    have equal sums.
%
%    The "discrepancy" is the absolute value of the difference between the
%    two sums, and will be zero if we have solved the problem.
%
%    For a given set of integers, there may be zero, one, or many solutions.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 May 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the size of the set.
%
%    Input, integer W(N), the integers.
%
%    Output, integer C(N), indicates the proposed solution.
%    C(I) is 0 for items in set W0 and 1 for items in set W1.
%
%    Output, integer DISCREPANCY, the discrepancy.
%
  w = w(:);
  w_sum = sum ( w(1:n) );
  discrepancy = w_sum;

  d = [];
  rank = -1;

  while ( 1 )

    [ d, rank ] = subset_next ( n, d, rank );

    if ( rank == -1 )
      break
    end

    d_discrepancy = abs ( w_sum - 2 * d' * w );

    if ( d_discrepancy < discrepancy )
      discrepancy = d_discrepancy;
      c(1:n) = d(1:n);
    end

    if ( discrepancy == 0 )
      break
    end

  end

  return
end


% demo for image registration

close all
clear variables

% 1: create original 2D image
[X,Y] = meshgrid(-2:.2:2, -2:.2:2);
Z = X .* exp(-X.^2 - Y.^2);
inG.Y = Z;
[nx,ny] = size(inG.Y);

% 2: dis-align original image with rotation & translation
phi = [2;-2;-1];
sigma = 1e6;
[gx] = g_rigid2D([],phi,[],inG);
y = gx +  (1/sqrt(sigma))*randn(size(gx));

% 3: display original and disaligned images
hf = figure;
subplot(2,2,1),imagesc(inG.Y),colormap(bone),colorbar,axis equal, axis tight,title('original image')
subplot(2,2,2),imagesc(reshape(y,nx,ny)),colormap(bone),colorbar,axis equal, axis tight,title('disaligned image (target)')

% 4: undo dis-alignement:
%   - data consist of original image
%   - alignement parameters are not known
g_fname = @g_rigid2D;
dim.n = 0;
dim.n_theta = 0;
dim.n_phi = 3;
options.inG = inG;
[posterior,out] = VBA_NLStateSpaceModel(...
    y,...
    [],...
    [],...
    g_fname,...
    dim,...
    options);
hg = out.suffStat.gx;

% 5: display realigned and residuals images
figure(hf)
subplot(2,2,3),imagesc(reshape(hg,nx,ny)),colormap(bone),colorbar,axis equal, axis tight,title('realigned image')
subplot(2,2,4),imagesc(reshape(hg-y,nx,ny)),colormap(bone),colorbar,axis equal, axis tight,title('realigned - disaligned')

displayResults(posterior,out,y,[],[],[],phi,[],sigma);


function [week,sec_of_week] = gps_time(julday)
% GPS_TIME   Conversion of Julian Day number to GPS week and
%	                Seconds of Week as reckoned from midnight between
%                  Saturday  and Sunday

% Written by Kai Borre
% January 7, 2016

    a = floor(julday+.5);
    b = a+1537;
    c = floor((b-122.1)/365.25);
    e = floor(365.25*c);
    f = floor((b-e)/30.6001);
    d = b-e-floor(30.6001*f)+rem(julday+.5,1);
    day_of_week = rem(floor(julday+.5),7);
    week = floor((julday-2444244.5)/7);
    % We add +1 as the GPS week starts at Saturday midnight
    sec_of_week = (rem(d,1)+day_of_week+1)*86400;
%%%%%%% end gps_time.m	%%%%%%%%%%%%%


% KM_DEMO_AKCCA Kernel-Based Identification of Hammerstein Systems. This
% demo generates some Hammerstein system input and output data and performs
% identification of the system using KIHAM.
%
% Author: Steven Van Vaerenbergh (steven *at* gtas.dicom.unican.es), 2013.
%
% The algorithm in this file is based on the following publication:
% S. Van Vaerenbergh and L. A. Azpicueta-Ruiz, "Kernel-Based Identification
% of Hammerstein Systems for Nonlinear Acoustic Echo-Cancellation", 2014
% IEEE International Conference on Acoustics, Speech, and Signal Processing
% (ICASSP), Florence, Italy, May 2014.
%
% This file is part of the Kernel Methods Toolbox for MATLAB.
% https://github.com/steven2358/kmbox

%% STATE

close all; clear
% rs = sum(100*clock);
rs = 1; % seed for random generator
rng('default')
rng(rs)

%% PARAMETERS

% system
f = @(x) tanh(x);
L = 256; % linear channel length
N = 1024; % number of samples
SNR = 50;

% kiham
kerneltype = 'gauss';
kernelpar = .5;
M = 25; % dictionary size
ca = .001;
ch = 1;
it_max = 50; % maximum number of iterations
it_stop = 1E-6; % stop if change in cost is smaller than this

%% GENERATE DATA

H = [1 randn(1,L-1)]';

x = 5*(2*rand(N,1)-1);
% x = randn(N,1);
z = f(x);
z_mem = km_memfill(z,L);
y_ref = z_mem*H;

sig = y_ref;
sigvar = var(sig);
noisevar = 10^(-SNR/10)*sigvar;
noise = sqrt(noisevar)*randn(size(sig));
y = y_ref + noise;

%% PROGRAM
tic

xdict = linspace(min(x),max(x),M)';

[alpha,h] = km_kiham(x,y,xdict,L,kerneltype,kernelpar,ca,ch,it_max,it_stop);

fprintf('Training: elapsed time is %.2f seconds.\n',toc)
%% OUTPUT

x_test = linspace(min(x),max(x),100)';
z_test_true = f(x_test);
K_test = km_kernel(x_test,xdict,kerneltype,kernelpar);
z_test_est = K_test*alpha;
scale = norm(h)/norm(H)*sign(h(1))*sign(H(1)); % internal ambiguity
z_test_est = z_test_est*scale;

figure; hold all
plot(x_test,z_test_true,'.');
% grid on
% legend('true nonlinearity')
plot(x_test,z_test_est,'.','Color',[0 .5 0]);
legend({'true nonlinearity','estimated nonlinearity'},'Location','SE')
grid on

figure; hold all
stem(H);
stem(h/scale)
legend('true linear channel','estimated linear channel')

fprintf('Test MSE = %.4f dB\n',10*log10(mean((z_test_true-z_test_est).^2)))


function [e0,e1,theta] = det2MM(im,sigmaI,sigmaO)
% function [e0,e1,theta] = det2MM(im,sigmaI,sigmaO)
%
% Compute the eigenspectrum of the spatially averaged second moment
% matrix.
%
% INPUT
%	im	Image.
%	sigmaI	Inner scale (sigma for image derivatives).
%	sigmaO	Outer scale (sigma for spatial averaging).
%
% OUTPUT
%	e0,e1	Smaller,larger eigenvalues.
%	theta	Orientation of fist eigenvector + pi/2
%		(i.e. orientation of possible edge).
%
% David R. Martin <dmartin@eecs.berkeley.edu>
% March 2003

if ndims(im)>2, im = rgb2gray(im); end;
idiag = norm(size(im));

if nargin<2, sigmaI=2; end
if nargin<3, sigmaO=sigmaI; end % Scott Konishi says this is close
                                % to optimal.
sigmaI = max(0.5,sigmaI);
sigmaO = max(0.5,sigmaO);

% compute x and y image derivatives at inner scale
fb = cell(2,1);
fb{1} = oeFilter(sigmaI,3,pi/2,1);
fb{2} = fb{1}';
fim = fbRun(fb,im);
dx = fim{1};
dy = fim{2};

% compute smoothed squared image derivatives at outer scale
f = oeFilter(sigmaO,3);
dx2 = applyFilter(f,dx.^2);
dy2 = applyFilter(f,dy.^2);
dxy = applyFilter(f,dx.*dy);

% compute eigenvalues of the spatially averaged 2nd moment matrix
% and the orientations of the eigenvectors
k = sqrt( (dx2-dy2).^2 + 4.*dxy.^2 );
eig0 = (dx2 + dy2 - k) / 2;
eig1 = (dx2 + dy2 + k) / 2;
t0 = atan2( dx2-eig0, -dxy );
t1 = atan2( dx2-eig1, -dxy );

% order eigenvalues by their absolute value, so e0<=e1, and pick
% out the orientation corresponding to the largest eigenvalue
x = (abs(eig1) > abs(eig0));
e0 = abs(eig0.*x + eig1.*~x);
e1 = abs(eig1.*x + eig0.*~x);
theta = t1.*x + t0.*~x;
theta = mod(theta+pi/2,pi);

% check postconditions
if any(e0>e1), error('e0>e1'); end


function [di, dj] = gradient_2d(M, mask)
% [di dj] = gradient_2d(M)
% [di dj] = gradient_2d(M, mask)
%
% Approximate the gradient of a two-dimensional function represented by M(i,j), using
% a 3x3 plane fitting algorithm.
%
%                         PARAMETERS
%
% M Matrix of function values, M(i,j) = f(i,j), the spacing is assumed to be uniform.
%
% mask (Optional) Matrix of the same size of M, with zero values in positions of M to be ignored by the
%                 gradient computations.
%
%                        RETURN VALUES
%
% di First component of the gradient: di(i,j) is the approximated [d/di f](i,j)
%
% dj Second component of the gradient: dj(i,j) is the approximated [d/dj f](i,j)
%
%                        LIMITATIONS
%
% To avoid ill-conditioned solutions, the mask is ignored near the "edges" of M,
% so this may result in artifacts in some cases. Leaving a "safety" border
% on the edges should be more than enough to prevent the issue.
%
% Author: Luca Vezzaro (elvezzaro@gmail.com)

	nr = size(M, 1);
	nc = size(M, 2);

	if nargin < 2
		mask = ones(nr, nc);
	end

	v = zeros(3,1);

	% The used algorithm is simple: the evaluation point is set
	% as the origin, and a plane is fitted to the 8 nearest positions,
	% centered on the origin. On the boundary, there are less values,
	% but the method is the same.

	% First the "corners"

	p(1,:) = [1 0]; v(1) = M(2,1) - M(1,1);
	p(2,:) = [0 1]; v(2) = M(1,2) - M(1,1);
	p(3,:) = [1 1]; v(3) = M(2,2) - M(1,1);

	n = p \ v;

	di(1,1) = n(1);
	dj(1,1) = n(2);

	p(1,:) = [0 -1]; v(1) =  M(1,nc-1) - M(1,nc);
	p(2,:) = [1 -1]; v(2) =  M(2,nc-1) - M(1,nc);
	p(3,:) = [1 0];  v(3) =  M(2,nc)   - M(1,nc);

	n = p \ v;

	di(1,nc) = n(1);
	dj(1,nc) = n(2);

	p(1,:) = [-1 0]; v(1) = M(nr-1,1) - M(nr,1);
	p(2,:) = [-1 1]; v(2) = M(nr-1,2) - M(nr,1);
	p(3,:) = [0 1];  v(3) = M(nr,2)   - M(nr,1);

	n = p \ v;

	di(nr,1) = n(1);
	dj(nr,1) = n(2);

	p(1,:) = [-1 -1]; v(1) = M(nr-1,nc-1) - M(nr,nc);
	p(2,:) = [0 -1];  v(2) = M(nr,nc-1)   - M(nr,nc);
	p(3,:) = [-1 0];  v(3) = M(nr-1,nc)   - M(nr,nc);

	n = p \ v;

	di(nr,nc) = n(1);
	dj(nr,nc) = n(2);

	% Then, the boundaries

	for j=2:nc-1
		p(1,:) = [0 -1]; v(1) = M(1,j-1) - M(1,j);
		p(2,:) = [1 -1]; v(2) = M(2,j-1) - M(1,j);
		p(3,:) = [1 0];  v(3) = M(2,j)   - M(1,j);
		p(4,:) = [0 1];  v(4) = M(1,j+1) - M(1,j);
		p(5,:) = [1 1];  v(5) = M(2,j+1) - M(1,j);

		n = p \ v;

		di(1,j) = n(1);
		dj(1,j) = n(2);

		p(1,:) = [-1 -1]; v(1) = M(nr-1,j-1) - M(nr,j);
		p(2,:) = [0 -1];  v(2) = M(nr,j-1)   - M(nr,j);
		p(3,:) = [-1 0];  v(3) = M(nr-1,j)   - M(nr,j);
		p(4,:) = [-1 1];  v(4) = M(nr-1,j+1) - M(nr,j);
		p(5,:) = [0 1];   v(5) = M(nr,j+1)   - M(nr,j);

		n = p \ v;

		di(nr,j) = n(1);
		dj(nr,j) = n(2);
	end

	for i=2:nr-1
		p(1,:) = [-1 0]; v(1) = M(i-1,1) - M(i,1);
		p(2,:) = [1 0];  v(2) = M(i+1,1) - M(i,1);
		p(3,:) = [-1 1]; v(3) = M(i-1,2) - M(i,1);
		p(4,:) = [0 1];  v(4) = M(i,2)   - M(i,1);
		p(5,:) = [1 1];  v(5) = M(i+1,2) - M(i,1);

		n = p \ v;

		di(i,1) = n(1);
		dj(i,1) = n(2);

		p(1,:) = [-1 -1]; v(1) = M(i-1,nc-1) - M(i,nc);
		p(2,:) = [0 -1];  v(2) = M(i,nc-1)   - M(i,nc);
		p(3,:) = [1 -1];  v(3) = M(i+1,nc-1) - M(i,nc);
		p(4,:) = [-1 0];  v(4) = M(i-1,nc)   - M(i,nc);
		p(5,:) = [1 0];   v(5) = M(i+1,nc)   - M(i,nc);

		n = p \ v;

		di(i,nc) = n(1);
		dj(i,nc) = n(2);
	end

	% And finally the remaining function values
	for i=2:nr-1
		for j=2:nc-1
			if mask(i,j) ~= 0
				k = 1;

				if mask(i-1,j-1) ~= 0
					p(k,:) = [-1 -1]; v(k) = M(i-1,j-1) - M(i,j);
					k = k + 1;
				end

				if mask(i-1,j) ~= 0
					p(k,:) = [-1 0];  v(k) = M(i-1,j)   - M(i,j);
					k = k + 1;
				end

				if mask(i-1,j+1) ~= 0
					p(k,:) = [-1 1];  v(k) = M(i-1,j+1) - M(i,j);
					k = k + 1;
				end

				if mask(i,j-1) ~= 0
					p(k,:) = [0 -1];  v(k) = M(i,j-1)   - M(i,j);
					k = k + 1;
				end

				if mask(i,j+1) ~= 0
					p(k,:) = [0 1];   v(k) = M(i,j+1)   - M(i,j);
					k = k + 1;
				end

				if mask(i+1,j-1) ~= 0
					p(k,:) = [1 -1];  v(k) = M(i+1,j-1) - M(i,j);
					k = k + 1;
				end

				if mask(i+1,j) ~= 0
					p(k,:) = [1 0];   v(k) = M(i+1,j)   - M(i,j);
					k = k + 1;
				end

				if mask(i+1,j+1) ~= 0
					p(k,:) = [1 1];   v(k) = M(i+1,j+1) - M(i,j);
					k = k + 1;
				end

				if k > 2
					n = p(1:k-1,:) \ v(1:k-1);

					di(i,j) = n(1);
					dj(i,j) = n(2);
				else
					di(i,j) = 0;
					dj(i,j) = 0;
				end
			else
				di(i,j) = 0;
				dj(i,j) = 0;
			end
		end
	end


function ell = equivalentEllipsoid(points)
% Equivalent ellipsoid of a set of 3D points.
%
%   ELL = equivalentEllipsoid(PTS)
%   Compute the equivalent ellipsoid of the set of points PTS. The result
%   is an ellipsoid defined by:
%   ELL = [XC YC ZC  A B C  PHI THETA PSI]
%   where [XC YC ZY] is the center, [A B C] are the lengths of the
%   semi-axes (in decreasing order), and [PHI THETA PSI] are Euler angles
%   representing  the ellipsoid orientation, in degrees.
%
%   Example
%     pts = randn(300, 3);
%     pts = transformPoint3d(pts, createScaling3d([6 4 2]));
%     pts = transformPoint3d(pts, createRotationOx(pi/6));
%     pts = transformPoint3d(pts, createRotationOy(pi/4));
%     pts = transformPoint3d(pts, createRotationOz(pi/3));
%     pts = transformPoint3d(pts, createTranslation3d([5 4 3]));
%     elli = equivalentEllipsoid(pts);
%     figure; drawPoint3d(pts); axis equal;
%     hold on; drawEllipsoid(elli, ...
%         'drawEllipses', true, 'EllipseColor', 'b', 'EllipseWidth', 3);
%
%   See also
%     spheres, drawEllipsoid, equivalentEllipse, principalAxes
%     rotation3dToEulerAngles

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2011-03-12,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2011 INRA - Cepia Software Platform.

% number of points
n = size(points, 1);

% compute centroid
center = mean(points);

% compute the covariance matrix
covPts = cov(points)/n;

% perform a principal component analysis with 2 variables,
% to extract equivalent axes
[U, S] = svd(covPts);

% extract length of each semi axis
radii = sqrt(5) * sqrt(diag(S)*n)';

% sort axes from greater to lower
[radii, ind] = sort(radii, 'descend');

% format U to ensure first axis points to positive x direction
U = U(ind, :);
if U(1,1) < 0
    U = -U;
    % keep matrix determinant positive
    U(:,3) = -U(:,3);
end

% convert axes rotation matrix to Euler angles
angles = rotation3dToEulerAngles(U);

% concatenate result to form an ellipsoid object
ell = [center, radii, angles];


function [ w, x ] = gm_rule_set ( rule, dim_num, point_num )

%*****************************************************************************80
%
%% GM_RULE_SET sets a Grundmann-Moeller rule.
%
%  Discussion:
%
%    This is a revised version of the calculation which seeks to compute
%    the value of the weight in a cautious way that avoids intermediate
%    overflow.  Thanks to John Peterson for pointing out the problem on
%    26 June 2008.
%
%    This rule returns weights and abscissas of a Grundmann-Moeller
%    quadrature rule for the DIM_NUM-dimensional unit simplex.
%
%    The dimension POINT_NUM can be determined by calling GM_RULE_SIZE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 June 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Axel Grundmann, Michael Moeller,
%    Invariant Integration Formulas for the N-Simplex
%    by Combinatorial Methods,
%    SIAM Journal on Numerical Analysis,
%    Volume 15, Number 2, April 1978, pages 282-290.
%
%  Parameters:
%
%    Input, integer RULE, the index of the rule.
%    0 <= RULE.
%
%    Input, integer DIM_NUM, the spatial dimension.
%    1 <= DIM_NUM.
%
%    Input, integer POINT_NUM, the number of points in the rule.
%
%    Output, real W(POINT_NUM), the weights.
%
%    Output, real X(DIM_NUM,POINT_NUM), the abscissas.
%
  s = rule;
  d = 2 * s + 1;
  k = 0;
  n = dim_num;
  one_pm = 1;

  for i = 0 : s

    weight =  one_pm;

    for j = 1 : max ( n, d, d + n - i )

      if ( j <= n )
        weight = weight *  ( j );
      end
      if ( j <= d )
        weight = weight * ( d + n - 2 * i );
      end
      if ( j <= 2 * s )
        weight = weight / 2.0;
      end
      if ( j <= i )
        weight = weight / j;
      end
      if ( j <= d + n - i )
        weight = weight / j;
      end

    end

    one_pm = - one_pm;

    beta_sum = s - i;
    more = 0;
    beta = [];
    h = 0;
    t = 0;

    while ( 1 )

      [ beta, more, h, t ] = comp_next ( beta_sum, dim_num + 1, ...
        beta, more, h, t );

      k = k + 1;

      w(k) = weight;

      x(1:dim_num,k) = ( 2 * beta(2:dim_num+1)' + 1 ) / ( d + n - 2 * i );

      if ( ~more )
        break
      end

    end

  end

  return
end


function [c1,c2]= binaryfit(phi,U,epsilon)
% compute c1 c2 for optimal binary fitting
% input:
%    U: input image
%    phi: level set function
%    epsilon: parameter for computing smooth Heaviside and dirac function
% output:
%    c1: a constant to fit the image U in the region phi>0
%    c2: a constant to fit the image U in the region phi<0

H = Heaviside(phi,epsilon); % compute the Heaveside function values

a = H .* U;
numer_1 = sum(a(:));
denom_1 = sum(H(:));
c1 = numer_1 / denom_1;

b = (1-H) .* U;
numer_2 = sum(b(:));
c = 1-H;
denom_2 = sum(c(:));
c2 = numer_2 / denom_2;


function [center, U, obj_fcn] = fcm(data, cluster_n, options)
%FCM Data set clustering using fuzzy c-means clustering.
%
%   [CENTER, U, OBJ_FCN] = FCM(DATA, N_CLUSTER) finds N_CLUSTER number of
%   clusters in the data set DATA. DATA is size M-by-N, where M is the number of
%   data points and N is the number of coordinates for each data point. The
%   coordinates for each cluster center are returned in the rows of the matrix
%   CENTER. The membership function matrix U contains the grade of membership of
%   each DATA point in each cluster. The values 0 and 1 indicate no membership
%   and full membership respectively. Grades between 0 and 1 indicate that the
%   data point has partial membership in a cluster. At each iteration, an
%   objective function is minimized to find the best location for the clusters
%   and its values are returned in OBJ_FCN.
%
%   [CENTER, ...] = FCM(DATA,N_CLUSTER,OPTIONS) specifies a vector of options
%   for the clustering process:
%       OPTIONS(1): exponent for the matrix U             (default: 2.0)
%       OPTIONS(2): maximum number of iterations          (default: 100)
%       OPTIONS(3): minimum amount of improvement         (default: 1e-5)
%       OPTIONS(4): info display during iteration         (default: 1)
%   The clustering process stops when the maximum number of iterations
%   is reached, or when the objective function improvement between two
%   consecutive iterations is less than the minimum amount of improvement
%   specified. Use NaN to select the default value.
%
%   Example
%       data = rand(100,2);
%       [center,U,obj_fcn] = fcm(data,2);
%       plot(data(:,1), data(:,2),'o');
%       hold on;
%       maxU = max(U);
%       % Find the data points with highest grade of membership in cluster 1
%       index1 = find(U(1,:) == maxU);
%       % Find the data points with highest grade of membership in cluster 2
%       index2 = find(U(2,:) == maxU);
%       line(data(index1,1),data(index1,2),'marker','*','color','g');
%       line(data(index2,1),data(index2,2),'marker','*','color','r');
%       % Plot the cluster centers
%       plot([center([1 2],1)],[center([1 2],2)],'*','color','k')
%       hold off;
%
%   See also FCMDEMO, INITFCM, IRISFCM, DISTFCM, STEPFCM.

%   Roger Jang, 12-13-94, N. Hickey 04-16-01
%   Copyright 1994-2018 The MathWorks, Inc.

if nargin ~= 2 && nargin ~= 3
	error(message("fuzzy:general:errFLT_incorrectNumInputArguments"))
end

data_n = size(data, 1);

% Change the following to set default options
default_options = [2;	% exponent for the partition matrix U
		100;	% max. number of iteration
		1e-5;	% min. amount of improvement
		1];	% info display during iteration

if nargin == 2
	options = default_options;
else
	% If "options" is not fully specified, pad it with default values.
	if length(options) < 4
		tmp = default_options;
		tmp(1:length(options)) = options;
		options = tmp;
	end
	% If some entries of "options" are nan's, replace them with defaults.
	nan_index = find(isnan(options)==1);
	options(nan_index) = default_options(nan_index);
	if options(1) <= 1
		error(message("fuzzy:general:errFcm_expMustBeGtOne"))
	end
end

expo = options(1);		% Exponent for U
max_iter = options(2);		% Max. iteration
min_impro = options(3);		% Min. improvement
display = options(4);		% Display info or not

obj_fcn = zeros(max_iter, 1);	% Array for objective function

U = initfcm(cluster_n, data_n);			% Initial fuzzy partition
% Main loop
for i = 1:max_iter
	[U, center, obj_fcn(i)] = stepfcm(data, U, cluster_n, expo);
	if display
		fprintf('Iteration count = %d, obj. fcn = %f\n', i, obj_fcn(i));
	end
	% check termination condition
	if i > 1
		if abs(obj_fcn(i) - obj_fcn(i-1)) < min_impro, break; end
	end
end

iter_n = i;	% Actual number of iterations
obj_fcn(iter_n+1:max_iter) = [];


function [Zica, W, T, mu] = kICA(Z,r)
%
% Syntax:       Zica = kICA(Z,r);
%               [Zica, W, T, mu] = kICA(Z,r);
%
% Inputs:       Z is an d x n matrix containing n samples of d-dimensional
%               data
%
%               r is the number of independent components to compute
%
% Outputs:      Zica is an r x n matrix containing the r independent
%               components - scaled to variance 1 - of the input samples
%
%               W and T are the ICA transformation matrices such that
%               Zr = T \ W' * Zica + repmat(mu,1,n);
%               is the r-dimensional ICA approximation of Z
%
%               mu is the d x 1 sample mean of Z
%
% Description:  Performs independent component analysis (ICA) on the input
%               data using the max-kurtosis ICA algorithm
%
% Reference:    http://www.cs.nyu.edu/~roweis/kica.html
%
% Author:       Brian Moore
%               brimoor@umich.edu
%
% Date:         November 12, 2016
%

% Center and whiten data
mu = mean(Z,2);
T = sqrtm(inv(cov(Z')));
Zcw = T * bsxfun(@minus,Z,mu);

% Max-kurtosis ICA
[W, ~, ~] = svd(bsxfun(@times,sum(Zcw.^2,1),Zcw) * Zcw');
Zica = W(1:r,:) * Zcw;


function inside = triangle_contains_point_2d ( t, p )

%*****************************************************************************80
%
%% TRIANGLE_CONTAINS_POINT_2D finds if a point is inside a triangle in 2D.
%
%  Discussion:
%
%    The routine assumes that the vertices are given in counter-clockwise
%    order.
%
%    The routine determines if a point P is "to the right of" each of the lines
%    that bound the triangle.  It does this by computing the cross product
%    of vectors from a vertex to its next vertex, and to P.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real T(2,3), the triangle vertices.
%    The vertices should be given in counter clockwise order.
%
%    Input, real P(2), the point to be checked.
%
%    Output, logical INSIDE, is TRUE if the point is inside
%    the triangle or on its boundary.
%
  dim_num = 2;

  inside = 0;

  if ( 0.0 < ( p(1)   - t(1,1) ) * ( t(2,2) - t(2,1) ) ...
           - ( t(1,2) - t(1,1) ) * ( p(2)   - t(2,1) ) );
    return
  end

  if ( 0.0 < ( p(1)   - t(1,2) ) * ( t(2,3) - t(2,2) ) ...
           - ( t(1,3) - t(1,2) ) * ( p(2)   - t(2,2) ) );
    return
  end

  if ( 0.0 < ( p(1)   - t(1,3) ) * ( t(2,1) - t(2,3) ) ...
           - ( t(1,1) - t(1,3) ) * ( p(2)   - t(2,3) ) );
    return
  end

  inside = 1;

  return
end


 function cx = cheby_u_poly ( m, n, x )

%*****************************************************************************80
%
%% CHEBY_U_POLY evaluates Chebyshev polynomials U(n,x).
%
%  Differential equation:
%
%    (1-X*X) Y'' - 3 X Y' + N (N+2) Y = 0
%
%  First terms:
%
%    U(0,X) =   1
%    U(1,X) =   2 X
%    U(2,X) =   4 X^2 -   1
%    U(3,X) =   8 X^3 -   4 X
%    U(4,X) =  16 X^4 -  12 X^2 +  1
%    U(5,X) =  32 X^5 -  32 X^3 +  6 X
%    U(6,X) =  64 X^6 -  80 X^4 + 24 X^2 - 1
%    U(7,X) = 128 X^7 - 192 X^5 + 80 X^3 - 8X
%
%  Recursion:
%
%    U(0,X) = 1,
%    U(1,X) = 2 * X,
%    U(N,X) = 2 * X * U(N-1,X) - U(N-2,X)
%
%  Norm:
%
%    Integral ( -1 <= X <= 1 ) ( 1 - X^2 ) * U(N,X)^2 dX = PI/2
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 January 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the number of evaluation points.
%
%    Input, integer N, the highest polynomial to compute.
%
%    Input, real X(M,1), the evaluation points.
%
%    Output, real V(M,N+1), the values of the Chebyshev polynomials
%    0 through N at X(1:M).
%
  if ( n < 0 )
    v = [];
    return
  end

  v = zeros ( m, n + 1 );

  v(1:m,1) = 1.0;

  if ( n < 1 )
    return
  end

  x = x(:);

  v(1:m,2) = 2.0 * x(1:m,1);

  for j = 2 : n
    v(1:m,j+1) = 2.0 * x(1:m,1) .* v(1:m,j) - v(1:m,j-1);
  end

  return
end


function chx = bbp_pi ( d )

%*****************************************************************************80
%
%% BBP_PI implements the Bailey-Borwein-Plouffe algorithm.
%
%  Discussion:
%
%    The BBP algorithm can be used to compute a a few hex digits of pi starting
%    at any position.
%
%    In particular, bbp_pi ( d ) is a char string of hex digits d+1 through
%    d+13 in the hexadecimal expansion of pi.
%
%    This function does not require extended precision arithmetic or
%    symbolic computation.
%
%    The results are usually accurate to about 11 or 12 of the 13 digits.
%
%    This program is derived from a C program by David H. Bailey dated
%    2006-09-08, http://www.experimentalmath.info/bbp-codes/piqpr8.c
%
%    For many other references: Google "BBP Pi".
%
%  Licensing:
%
%    Copyright (c) 2011, The MathWorks, Inc.
%    All rights reserved.
%
%    Redistribution and use in source and binary forms, with or without
%    modification, are permitted provided that the following conditions are
%    met:
%
%        * Redistributions of source code must retain the above copyright
%          notice, this list of conditions and the following disclaimer.
%        * Redistributions in binary form must reproduce the above copyright
%          notice, this list of conditions and the following disclaimer in
%          the documentation and/or other materials provided with the distribution
%        * Neither the name of the The MathWorks, Inc. nor the names
%          of its contributors may be used to endorse or promote products derived
%          from this software without specific prior written permission.
%
%    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
%    AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
%    IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
%    ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
%    LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
%    CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
%    SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
%    INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
%    CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
%    ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
%    POSSIBILITY OF SUCH DAMAGE.
%
%  Modified:
%
%    27 February 2012
%
%  Author:
%
%    Cleve Moler
%
%  Reference:
%
%    Cleve Moler,
%    Cleve's Corner, "Computing Pi",
%    http://www.mathworks.com/company/newsletters/news_notes/2011/
%
%  Parameters:
%
%    Input, integer D, the number of decimal digits desired.
%
%    Output, symbolic P, the value of pi to D digits.
%
   s1 = series ( 1, d );
   s2 = series ( 4, d );
   s3 = series ( 5, d );
   s4 = series ( 6, d );
   P = 4. * s1 - 2. * s2 - s3 - s4;
   P = P - floor ( P ) + 1.;
   chx = hexchar ( P, 13 );

  return
end
function s = series ( m, d )

%*****************************************************************************80
%
%% SERIES
%
% s = series(m,d) = sum_k 16^(d-k)/(8*k+m)
% using the modular powering technique.
%
  s = 0;
  k = 0;
  t = Inf;

  while k < 13 || t > eps
    ak = 8 * k + m;
    if k < d
      t = powmod(16, d - k, ak) / ak;
     else
       t = 16^(d - k) / ak;
     end
     s = mod(s + t, 1);
     k = k + 1;
   end

  return
end
function r = powmod ( b, p, a )

%*****************************************************************************80
%
%% POWMOD computes mod(b^p,a) without computing b^p.
%
  persistent twop

  if isempty ( twop )
    twop = 2.^(0:25)';
  end

  if a == 1
    r = 0;
    return
  end

  n = find ( p <= twop, 1, 'first' );

  pt = twop ( n );
  r = 1;
  for j = 1 : n
    if p >= pt
      r = mod ( b * r, a );
      p = p - pt;
    end
    pt = 0.5 * pt;
    if pt >= 1
      r = mod ( r * r, a );
    end
  end

  return
end
function chx = hexchar ( s, n )

%*****************************************************************************80
%
%% HEXCHAR returns hex digits.
%
% chx(s,n) = string of the first n hex digits of s.
%
  hx = '0123456789ABCDEF';

  s = abs ( s );

  for j = 1 : n
    s = 16. * mod ( s, 1 );
    chx(j) = hx ( floor ( s ) + 1 );
  end

  return
end


function [nInput,inputMin,inputMax, nTarget, targetMin, targetMax] = preNorm(input, target, inputMax)
% Preprocesses data so that minimum is -1 and maximum is 1.
%
%% Syntax
%   [nInput,inputMin,inputMax] = preNorm(input);
%   [nInput,inputMin,inputMax,nTarget,targetMin,targetMax] = preNorm(input, target);
%   [nInput] = preNorm(input,inputMin,inputMax);
%
%
%% Description
% Function normalizes inputs so that they fall in the interval [-1,1].
% Algorithm:  nInput = 2*(input-inputMin)/(inputMax-inputMin) - 1; If the
% data needs to be normalzed using previously computed minimums and
% maximums, we use: nInput = preNorm(input,inputMin,inputMax);
%
% Input:
% * input           ... the n x D input matrix
% * target/inputMin ... the target vector/if there are 3 input arguments it
%                       will be considered as a vector of minimums
% * inputMax        ... maximums
%
% Output:
% * nInput      ... the n x D normalized input matrix
% * inputMin    ... the row vector containing minimums for each dimension
% * inputMax    ... the row vector containing maximums for each dimension
% * nTarget     ... the normalized target vector
% * targetMin   ... the target minimum
% * targetMax   ... the target maximum
%
% See also:
% postNorm, postNormVar

%% Signature
% * Written by Tomaz Sustar, January 2012

if nargin > 3
  error('Wrong number of arguments.');
end

if nargin==3 % normalize targets if given
  inputMin = target;
  [nInput,inputMin,inputMax] = normalize(input,inputMin,inputMax); %normalize input
else
  [nInput,inputMin,inputMax] = normalize(input); %normalize input
end

if nargin==2 % normalize targets if given
  [nTarget, targetMin, targetMax] = normalize(target);
end

function [nInput,inputMin,inputMax] = normalize(input, inputMin, inputMax)
[n, D] = size(input); % n - number of mesurements, D - input space dimenson

if nargin ~= 3
  inputMin = min(input); % row vector of minimiums
  inputMax = max(input); % row vector of maximums
end

isequal = inputMin==inputMax;
notequal = ~isequal;
if sum(isequal) ~= 0
  warning('Some maximums and minimums are equal. Those inputs will not be transformed.');
  inputMin0 = inputMin.*notequal - 1*isequal; % where equal set minimums to -1
  inputMax0 = inputMax.*notequal + 1*isequal; % and maximums to +1 so the data will not be transformed
else
  inputMin0 = inputMin;
  inputMax0 = inputMax;
end

nInput = 2*(input-repmat(inputMin0,n,1))./repmat((inputMax0-inputMin0),n,1) - 1; % normalize


function [H,mu,SR]=standardEpanechnikovKernelBW(param1,N)
%%STANDARDEPANECHNIKOVKERNELBW Estimate the (univariate or multivariate)
%           kernel bandwidth when approximating a set of samples whose
%           underlying density is (or is approximated as) Gaussian with an
%           Epanechnikov kernel. The Epanechnikov kernel has certain
%           desirable optimality properties.
%
%INPUTS: param1 If inputType=0, then this is:
%              xi A numDimXN set of samples of the distribution from which
%                 a Gaussian kernel estimator should interpolate a PDF. For
%                 multidimensional estimation, there must be >=numDim
%                 samples. Specifically, the sample covariance matrix must
%                 be positive definite.
%              If inputType=1, then this is:
%              SR A root covariance matrix such that SR*SR' equals the
%                 (exact or approximated) covariance matrix of the PDF
%                 being approximated.
%            N If this parameter is provided, then param1 is assumed to be
%              SR and N is the number of samples that are being smoothed.
%              Otherwise, if this is omitted or an empty matrix is passed,
%              param1 is xi.
%
%OUTPUTS: H The numDimXnumDim kernel bandwidth.
%         mu If xi is given, then this is the sample mean. Otherwise, this
%            is an empty matrix.
%         SR The root covariance matrix from the input or computed from xi.
%
%The formulae for the optimal bandwidth of an Epanechnikov kernel given
%samples from an underlying Gaussian distribution is given in Equation 2.3
%of [1]. The function EpanechnikovKernel implements the Epanechnikov
%kernel.
%
%REFERENCES:
%[1] C. Musso, N. Oudjane, and F. LeGland, "Improving regularised particle
%    filters," in Sequential Monte Carlo Methods in Practice, A. Doucet, J.
%    F. G. de Freitas, and N. J. Gordon, Eds., Ch. 12, New York:
%    Springer-Verlag, 2001.
%
%March 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<2)
    N=[];
end

nx=size(param1,1);%Dimensionality.

if(isempty(N))%xi is given
    xi=param1;
    N=size(xi,2);
    [mu,S]=calcMixtureMoments(xi);
    SR=cholSemiDef(S,'lower',1);%Not fully triangular. but S=SR*SR'
else%SR is given and N must be given.
    mu=[];
    SR=param1;
end

cn=hypersphereVolume(1,nx);
A=((8/cn)*(nx+4)*2*sqrt(pi)^nx)^(1/(nx+4));
hOpt=A*N^(-1/(nx+4));

H=hOpt*SR*eye(nx);

end

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


function value = lrline ( xu, yu, xv1, yv1, xv2, yv2, dv )

%*****************************************************************************80
%
%% LRLINE determines if a point is left of, right or, or on a directed line.
%
%  Discussion:
%
%    The directed line is parallel to, and at a signed distance DV from
%    a directed base line from (XV1,YV1) to (XV2,YV2).
%
%  Modified:
%
%    07 February 2005
%
%  Author:
%
%    Original FORTRAN77 version by Barry Joe.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Barry Joe,
%    GEOMPACK - a software package for the generation of meshes
%    using geometric algorithms,
%    Advances in Engineering Software,
%    Volume 13, pages 325-331, 1991.
%
%  Parameters:
%
%    Input, real XU, YU, the coordinates of the point whose
%    position relative to the directed line is to be determined.
%
%    Input, real XV1, YV1, XV2, YV2, the coordinates of two points
%    that determine the directed base line.
%
%    Input, real DV, the signed distance of the directed line
%    from the directed base line through the points (XV1,YV1) and (XV2,YV2).
%    DV is positive for a line to the left of the base line.
%
%    Output, integer VALUE, the result:
%    +1, the point is to the right of the directed line;
%     0, the point is on the directed line;
%    -1, the point is to the left of the directed line.
%
  tol = 100.0 * r8_epsilon ( );

  dx = xv2 - xv1;
  dy = yv2 - yv1;
  dxu = xu - xv1;
  dyu = yu - yv1;

  tolabs = tol * max ( abs ( dx ), ...
                 max ( abs ( dy ), ...
                 max ( abs ( dxu ), ...
                 max ( abs ( dyu ), abs ( dv ) ) ) ) );

  t = dy * dxu - dx * dyu + dv * sqrt ( dx * dx + dy * dy );

  if ( tolabs < t )
    value = 1;
  elseif ( -tolabs <= t )
    value = 0;
  else
    value = -1;
  end

  return
end


function qr = quatrot(w);
% QUATROT - returns the quaternion representing the rotation given by
%           the 3D vector w (rotation of ||w|| around the direction given by w)
%
%   qr = quatrot(w);
%
%  ON - 3/99
%

th = norm(w);

qr = [cos(th/2) sin(th/2)*w(:)'/th];


% sgwt_demo1 : SGWT for swiss roll data set
%
% This demo builds the SGWT for the swiss roll synthetic data set. It
% computes a set of scales adapted to the computed upper bound on the
% spectrum of the graph Laplacian, and displays the scaling function and
% the scaled wavelet kernels, as well as the corresponding frame bounds. It
% then computes the wavelets centered on a single vertex, and displays
% them. This essentally reproduces figure 3 from
% Hammond,Vangergheynst, Gribonval 2010.

% This file is part of the SGWT toolbox (Spectral Graph Wavelet Transform toolbox)
% Copyright (C) 2010, David K. Hammond.
%
% The SGWT toolbox is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% The SGWT toolbox is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with the SGWT toolbox.  If not, see <http://www.gnu.org/licenses/>.

% Create swiss roll point cloud
function sgwt_demo1
close all
fprintf('Welcome to SGWT demo #1\n');

npoints=500;
fprintf('Creating Swiss Roll point cloud with %g points\n',npoints);
dataparams=struct('n',npoints,'dataset',-1','noise',0,'state',0);
r=create_synthetic_dataset(dataparams);
x=rescale_center(r.x);

fprintf('Computing edge weights and graph Laplacian\n');
% Compute Weighted graph adjacency matrix, and graph Laplacian
d=distanz(x);
s=.1;
A=exp(-d.^2/(2*s^2));
L=full(sgwt_laplacian(A));

fprintf('Measuring largest eigenvalue, lmax = ');

%% Design filters for transform
Nscales=4;
lmax=sgwt_rough_lmax(L);
fprintf('%g\n',lmax);

fprintf('Designing transform in spectral domain\n');

designtype='abspline3';
%designtype='mexican_hat';
%designtype='meyer';
%designtype='simple_tf';

[g,t]=sgwt_filter_design(lmax,Nscales,'designtype',designtype);

arange=[0 lmax];
%% Display filter design in spectral domain
figure
sgwt_view_design(g,t,arange);
ylim([0 3])
set(gcf,'position',[0 780,600,300])
%% Chebyshev polynomial approximation
m=50; % order of polynomial approximation
fprintf('Computing Chebyshev polynomials of order %g for fast transform \n',m);
for k=1:numel(g)
  c{k}=sgwt_cheby_coeff(g{k},m,m+1,arange);
end

%% compute transform of delta at one vertex
jcenter=32; % vertex to center wavelets to be shown
fprintf('Computing forward transform of delta at vertex %g\n',jcenter);
N=size(L,1);
d=sgwt_delta(N,jcenter);
% forward transform, using chebyshev approximation
wpall=sgwt_cheby_op(d,L,c,arange);

fprintf('Displaying wavelets\n');
if (exist('OCTAVE_VERSION','builtin')~=0)
    % settings for octave
    msize=5;
    plotchar='s';
else
    % settings for native matlab
    msize=100;
    plotchar='.';
end

cp=[-1.4,-16.9,3.4]; % camera position
%% Visualize result

% show original point
ws=300;
figure;
xp=0; yp=ws+100;
set(gcf,'position',[xp,yp,ws-10,ws+10]);
scatter3(x(1,:),x(2,:),x(3,:),msize,d,plotchar);
set(gcf,'Colormap',[.5 .5 .5;1 0 0]);
clean_axes(cp);
title(sprintf('Vertex %g',jcenter));

% show wavelets
for n=1:Nscales+1
    wp=wpall{n};
    figure
    xp=mod(n,3)*(ws+10);
    yp=(1-floor((n)/3))*(ws+100);
    set(gcf,'position',[xp,yp,ws-10,ws+10]);
    scatter3(x(1,:),x(2,:),x(3,:),msize,wp,plotchar);
    colormap jet
    caxis([-1 1]*max(abs(wp)));
    clean_axes(cp);

    hcb=colorbar('location','north');
    cxt=get(hcb,'Xtick');
    cxt=[cxt(1),0,cxt(end)];
    set(hcb,'Xtick',cxt);
    cpos=get(hcb,'Position');
    cpos(4)=.02; % make colorbar thinner
    set(hcb,'Position',cpos);
    set(hcb,'Position',[.25 .91 .6 .02]);

    if n==1
      title('Scaling function');
    else
      title(sprintf('Wavelet at scale j=%g, t_j = %0.2f',n-1,t((n-1))));

    end
end

function clean_axes(cp)
xlim([-1 1]);ylim([-1 1]);zlim([-1 1]);
set(gca,'Xtick',[-1 0 1]);
set(gca,'Ytick',[-1 0 1]);
set(gca,'Ztick',[-1 0 1]);
axis square
set(gca,'CameraPosition',cp);

% rescale_center
% center input data at origin, then rescale so that all coordinates
% are between -1 and 1
%
% x should be dxN
function r=rescale_center(x)
N=size(x,2);
d=size(x,1);
x=x-repmat(mean(x,2),[1,N]);
c=max(abs(x(:)));
r=x/c;


%  Figure 6.58      Feedback Control of Dynamic Systems, 5e
%                   Franklin, Powell, Emami
%

clear all
close all

num=9;
den=conv([1 0.5],[1 1]);
den=conv(den,[1 2]);
w=logspace(-1,1,500);
[mag,phas]=bode(num,den,w);
[OLGM,OLPM,OLWcg,OLWcp]=margin(mag,phas,w)

%Lead compensator, first guess
numl=[1 1];
denl=0.333*[1 3];
numc=conv(num,numl);
denc=conv(den,denl);
[magc,phasc]=bode(numc,denc,w);
[D1GM,D1PM,D1Wcg,D1Wcp]=margin(magc,phasc,w)
dencl=denc+[0 0 0 numc];
t=0:.1:20;
y=step(numc,dencl,t);
%Design Iteration, next guess
numl=[1 1.5];
denl=0.1*[1 15];
numcc=conv(num,numl);
dencc=conv(den,denl);
[magcc,phascc]=bode(numcc,dencc,w);
[D2GM,D2PM,D2Wcg,D2Wcp]=margin(magcc,phascc,w)
subplot(2,1,1)
loglog(w,mag,'-',w,magc,'--',w,magcc,'-.',w,ones(500,1),'-');
grid;
%xlabel('w (rad/sec)');
ylabel('Magnitude');
title('Fig. 6.58 Bode Plot for lead-compensation design (a) magnitude');
subplot(2,1,2)
semilogx(w,phas,'-',w,phasc,'--',w,phascc,'-.',w,-180*ones(500,1));
grid;
xlabel('\omega (rad/sec)');
ylabel('Phase (deg)');
title('Fig. 6.58 (b) phase');


function [U, L, mu, mse] = pca(X, m)
% Principal component analysis
% Input:
%   X: d x n data matrix
%   m: target dimension
% Output:
%   U: d x m Projection matrix
%   L: m x 1 Eigen values
%   mu: d x 1 mean
%   mse: mean square error
% Written by Mo Chen (sth4nth@gmail.com).
n = size(X,2);
mu = mean(X,2);
Xo = bsxfun(@minus,X,mu);
S = Xo*Xo'/n;                   % 12.3
[U,L] = eig(S);                         % 12.5
[L,idx] = sort(diag(L),'descend');
mse = sum(L)-sum(L(1:m));
U = U(:,idx(1:m));
L = L(1:m);


function d = vgg_H_sampson_distance_sqr(H,X1,X2)

% d = vgg_H_sampson_distance_sqr(H,X1,X2)
%
% Returns an approximation of the squared geometric distance from the
% 4d joint-space point [X1;X2] to the H manifold. See Torr CVIU 97.

if (size(X1) ~= size(X2))
  error('Point sets not same size!');
end

p1 = X1 ./ repmat(X1(3,:),3,1);
p2 = X2 ./ repmat(X2(3,:),3,1);

alg = vgg_H_algebraic_distance(H,p1,p2);

N = size(X1,2);

h = reshape(H',9,1);

G1 = [ H(1,1) - p2(1,:) * H(3,1) ; ...
  H(1,2) - p2(1,:) * H(3,2) ; ...
  -p1(1,:) * H(3,1) - p1(2,:) * H(3,2) - H(3,3) ; ...
  zeros(1,N) ];

G2 = [ H(2,1) - p2(2,:) * H(3,1) ; ...
  H(2,2) - p2(2,:) * H(3,2) ; ...
  zeros(1,N) ; ...
  -p1(1,:) * H(3,1) - p1(2,:) * H(3,2) - H(3,3) ];

magG1 = sqrt(sum(G1 .* G1));
magG2 = sqrt(sum(G2 .* G2));
magG1G2 = sum(G1 .*  G2);

alpha = acos( magG1G2 ./ (magG1 .* magG2) );

D1 = alg(1,:) ./ magG1;
D2 = alg(2,:) ./ magG2;

d = (D1.*D1 + D2.*D2 - 2 * D1 .* D2 .* cos(alpha)) ./ sin(alpha);


function prob_test120 ( )

%*****************************************************************************80
%
%% TEST120 tests PARETO_MEAN, PARETO_SAMPLE, PARETO_VARIANCE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  nsample = 1000;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST120\n' );
  fprintf ( 1, '  For the Pareto PDF:\n' );
  fprintf ( 1, '  PARETO_MEAN computes the mean;\n' );
  fprintf ( 1, '  PARETO_SAMPLE samples;\n' );
  fprintf ( 1, '  PARETO_VARIANCE computes the variance.\n' );

  a = 2.0;
  b = 3.0;

  check = pareto_check ( a, b );

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

  mean = pareto_mean ( a, b );
  variance = pareto_variance ( a, b );

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

  for i = 1 : nsample
    [ x(i), seed ] = pareto_sample ( a, b, seed );
  end

  mean = r8vec_mean ( nsample, x );
  variance = r8vec_variance ( nsample, x );
  xmax = max ( x(1:nsample) );
  xmin = min ( x(1:nsample) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Sample size =     %6d\n', nsample );
  fprintf ( 1, '  Sample mean =     %14f\n', mean );
  fprintf ( 1, '  Sample variance = %14f\n', variance );
  fprintf ( 1, '  Sample maximum =  %14f\n', xmax );
  fprintf ( 1, '  Sample minimum =  %14f\n', xmin );

  return
end


function ps = normpdf2( xs, m, C )
% Normal prob. density function (pdf) with arbitrary covariance matrix.
%
% Evaluate the multi-variate density with mean vector m and covariance
% matrix C for the input vector xs.  Assumes that the N datapoints are d
% dimensional.  Then m is dx1 or 1xd, C is dxd, and xs is dxN or NxD where
% N is the number of samples to be evaluated.
%
% USAGE
%  ps = normpdf2( xs, m, C )
%
% INPUTS
%  xs  - points to evaluated (Nxd or dxN)
%  m   - mean vector (dx1 or 1xd)
%  C   - Covariance matrix (dxd)
%
% OUTPUTS
%  ps  - probability density at each x (Nx1)
%
% EXAMPLE
%  ps = normpdf2( randn(10,2), [0 0], eye(2) )
%
% See also NORMPDF
%
% Piotr's Image&Video Toolbox      Version 2.30
% Copyright 2012 Piotr Dollar.  [pdollar-at-caltech.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the Simplified BSD License [see external/bsd.txt]

% get dimensions of data
d=length(m);
if( size(xs,1)~=d ); xs=xs'; end
N=size(xs,2);

if( d==1 ) % fast special case
  ps = 1/sqrt(2*pi*C) * exp(-(xs-m).*(xs-m)/(2*C))';

elseif( rcond(C)<eps ) % if matrix is badly conditioned
  warning('normpdf2: Covariance matrix close to singular.'); %#ok<WNTAG>
  ps = zeros(N,1);

else % get probabilities
  xs = (xs-m(:)*ones(1,N))';
  denom = (2*pi)^(d/2)*sqrt(abs(det(C)));
  mahal = sum( (xs/C).*xs, 2 );
  numer = exp(-0.5*mahal);
  ps = numer/denom;
end


clc;

%This code runs a simulation of a single spring pendulum, running all
%dynamics and integration in C++ code. Matlab is only used for plotting and
%compiling the code (to a mex function).

%First, compile the most recent version of the file
Recompile_Code = true;
if Recompile_Code
    mex Cpp_Integrator.cpp;
else
    disp('Warning - Did not recompile C++ files!')
    disp(' ')
end

% Initial state for the system
% [x,y,dx,dy]';
x0 = [  0;
        -0.4;
        0.6;
        0];   %Must be a column vector
Tspan = [0,20];
N = 1e5;

Params = zeros(1,6);  %Must be a row vector!
Params(1) = 0.8;    %(kg)   Mass
Params(2) = 89;    %(N/m)  Spring Constant
Params(3) = 0;    %(Ns/m)  Damping
Params(4) = 9.81;   %(m/s^2)  Gravity
Params(5) = 0.2;    %(m) Rest length of spring
Params(6) = 0.5;    %(Nm)    Forcing Amplitude
Params(7) = 20;      %(Hz)   Forcing frequency (Hz)

tic
[t, X] = Cpp_Integrator(Tspan,x0,N,Params);
disp(' ')
disp(['C++ Time: (Normal) ' num2str(toc)]);

x = X(1,:);
y = X(2,:);
dx = X(3,:);
dy = X(4,:);

figure(1)
plot(t,X)
title('All States')

figure(2)
plot(x,y)
title('Position Trace')


function normqqplot(y,class)

%NORMQQPLOT produces a Quantile-Quantile plot in which the vector y is plotted against
% the quantiles of a standard normal distribution.
%
% Required input arguments:
%       y : row or column vector
%
% Optional input arguments:
%    class : a string used for the y-label and the title(default: ' ')
%
% This function is part of LIBRA: the Matlab library for Robust Analysis,
% available at:
%              http://wis.kuleuven.be/stat/robust.html
%
%Written by Nele Smets
% Last update: 12/03/2004

set(gcf,'Name', 'Normal QQ-plot', 'NumberTitle', 'off');
if nargin==1
    class=' ';
end
n=length(y);
for i=1:n
    normalquantile(i)=norminv((i-1/3)/(n+1/3),0,1);
end
y=sort(y);
plot(normalquantile,y,'bo')
xlabel('Quantiles of the standard normal distribution');
ylabel(['Standardized ',class,' residual']);
title(class);
box on


% Optimal doping profile optimization
% Boyd, Kim, Vandenberghe, and Hassibi, "A tutorial on geometric programming"
% Joshi, Boyd, and Dutton, "Optimal doping profiles via geometric programming"
% Written for CVX by Almir Mutapcic 02/08/06
% (a figure is generated)
%
% Determines the optimal doping profile that minimizes base transit
% time in a (homojunction) bipolar junction transistor.
% This problem can be posed as a GP:
%
%   minimize   tau_B
%       s.t.   Nmin <= v <= Nmax
%              y_(i+1) + v_i^const1 <= y_i
%              w_(i+1) + v_i^const2 <= w_i, etc...
%
% where variables are v_i, y_i, and w_i.

% discretization size
M = 50;
% M = 1000; % takes a few minutes to process constraints

% problem constants
g1 = 0.42;
g2 = 0.69;
Nmax = 5*10^18;
Nmin = 5*10^16;
Nref = 10^17;
Dn0 = 20.72;
ni0 = 1.4*(10^10);
WB = 10^(-5);
C =  WB^2/((M^2)*(Nref^g1)*Dn0);

% exponent powers
pwi = g2 -1;
pwj = 1+g1-g2;

% optimization variables
cvx_begin gp
  variables v(M) y(M) w(M)

  % objective function is the base transmit time
  tau_B = C*w(1);

  minimize( tau_B )
  subject to
    % problem constraints
    v >= Nmin;
    v <= Nmax;

    for i = 1:M-1
      if( mod(i,100) == 0 ), fprintf(1,'progress counter: %d\n',i), end;
      y(i+1) + v(i)^pwj <= y(i);
      w(i+1) + y(i)*v(i)^pwi <= w(i);
    end

    y(M) == v(M)^pwj;
    w(M) == y(M)*v(M)^pwi;
cvx_end

% plot the basic optimal doping profile
figure, clf
nbw = 0:1/M:1-1/M;
semilogy(nbw,v,'LineWidth',2);
axis([0 1 1e16 1e19]);
xlabel('base');
ylabel('doping');
text(0,Nmin,'Nmin ', 'HorizontalAlignment','right');
text(0,Nmax,'Nmax ', 'HorizontalAlignment','right');
disp('Optimal doping profile is plotted.')


%% Bivariate Kernel Density Estimation Demonstration
% Several examples show how to use the gkdeb2 function.

%% Distribution with unbounded support
% Distribution with four peaks.
x = [randn(100,1), (randn(100,1)-10)*2;
      randn(100,1)+10, randn(100,1);
      randn(100,1)+10, (randn(100,1)-10)*2;
      randn(100,2)];
gkde2(x);

%% Distribution with upper and lower bounds: uniform distribution
clear
x=rand(10000,2);
% PDF with bounded support
p.xylim=[0 0 1 1];
gkde2(x,p);
% Compare with unbounded PDF estimate
figure;
gkde2(x);

%% Distribution with lower bound only: exponential distribution
clear
x=-log(rand(1000,2));
% PDF with bounded support
p.xylim=[0 0 Inf Inf];
gkde2(x,p);
% Compare with unbounded PDF estimate
figure
gkde2(x);

%% Distribution with lower bound only: log-normal distribution
clear
x=exp(randn(1000,2));
% PDF with bounded support
p.xylim=[0 0 Inf Inf];
gkde2(x,p);
% Compare with unbounded PDF estimate
figure
gkde2(x);

%% Distribution with lower bound only: chi-square distribution
clear
x=randn(1000,2).^2;
% PDF with bounded support
p.xylim=[0 0 Inf Inf];
p.alpha=0.95;
gkde2(x,p);
% Compare with unbounded PDF estimate
figure
p1.alpha=0.95;
gkde2(x,p1);

%%  Distribution with lower bound only: Rayleigh distribution
clear
x=sqrt(randn(2,1000).^2 + randn(2,1000).^2);
% PDF with bounded support
p.xylim=[0 0 Inf Inf];
p.alpha=0.95;
gkde2(x,p);
% Compare with unbounded PDF estimate
figure
p1.alpha=0.95;
gkde2(x,p1);


function [dx, dxGrad] = dynamics(x,u,p)
% [dx, dxGrad] = dynamics(x,u,p)
%
% Computes the dynamics (and gradients) for the simple pendulum
%

q = x(1,:);
dq = x(2,:);

    k = p.k;    c = p.c;
    ddq = -c*dq - k*sin(q) + u;
    dx = [dq;ddq];

if nargout == 2   % Analytic gradients
    nTime = length(u);

    dqGrad = zeros(1,4,nTime); %4 = [time + angle + rate + torque];
    dqGrad(1,3,:) = 1; %gradient dq wrt dq

    ddqGrad = zeros(1,4,nTime);  %4 = [time + angle + rate + torque];
    ddqGrad(1,2,:) = -k*cos(q);   %gradient ddq wrt q
    ddqGrad(1,3,:) = -c;  %gradient ddq wrt dq
    ddqGrad(1,4,:) = 1;  %gradient ddq wrt u

    dxGrad = cat(1, dqGrad, ddqGrad);

end

end