function triangulate ( prefix, animate )

%*****************************************************************************80
%
%% TRIANGULATE triangulates a polygon using the ear slicing algorithm.
%
%  Discussion:
%
%    The polygon is described by a file which lists the coordinates of the
%    vertices in consecutive order during a counterclockwise traversal.
%
%    Note that no two successive vertices can be equal, nor can the first
%    and last vertices be equal.
%
%    Information about the polygon is stored as a structure we will call
%    a "vertex array".
%
%    There is one entry in the array for each of the N vertices in the
%    polygon.  The I-th entry is a structure with the following fields:
%
%      vertex(i).index  the index of the node.
%      vertex(i).next   the index of the next node.
%      vertex(i).prev   the index of the previous node.
%      vertex(i).x      the X coordinate.
%      vertex(i).y      the Y coordinate.
%      vertex(i).ear    1 if the vertex represents an "ear" that can
%                       be sliced off.
%
%  Usage:
%
%    triangulate ( 'prefix', 'animate' )
%
%    where
%
%    * 'prefix' is the filename prefix
%    * 'animate' is 'y' if you want to see an animation of the triangulation.
%
%    reads
%
%    * 'prefix'_nodes.txt, the node coordinate file,
%
%    creates
%
%    * 'prefix'_diagonals.txt, the internal diagonals file.
%    * 'prefix'_elements.txt, the triangle file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 January 2013
%
%  Author:
%
%    Original C version by Joseph ORourke.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Joseph ORourke,
%    Computational Geometry in C,
%    Cambridge, 1998,
%    ISBN: 0521649765,
%    LC: QA448.D38.
%
%  Parameters:
%
%    Input, string PREFIX, the filename prefix.
%
%    Input, string ANIMATE, is 'y' if the user wants to see the triangulation
%    process animated.
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TRIANGULATE\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Read a file listing the coordinates of the vertices\n' );
  fprintf ( 1, '  of a polygon, and determine a triangulation.\n' );
%
%  If the user did not supply the filename prefix, request it.
%
  if ( nargin < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TRIANGULATE:\n' );
    prefix = input ( '  Enter the filename prefix (in ''quotes''!).\n' );
  end
%
%  If the user did not supply the animation option, assume it is NO.
%
  if ( nargin < 2 )
    animate = 'n';
  end
%
%  Create filenames.
%
  node_filename     = strcat ( prefix, '_nodes.txt' );
  diagonal_filename = strcat ( prefix, '_diagonals.txt' );
  triangle_filename = strcat ( prefix, '_elements.txt' );
%
%  Read the node coordinates.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Processing the vertex coordinate file "%s".\n', node_filename )
  direction = +1;
  vertex = read_vertices ( node_filename, direction );

  vertex = ear_init ( vertex );

  area = area_poly2 ( vertex );
%
%  If area is zero, refuse to go on.
%
  if ( area == 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TRIANGULATE - Fatal area!\n' );
    fprintf ( 1, '  The area is computed to be exactly zero.\n' );
    return
  end
%
%  If area is NEGATIVE, reverse the ordering.
%
  if ( area < 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Polygon area is negative!\n' );
    fprintf ( 1, '  Will try reversing vertex ordering.\n' );
    direction = -1;
    vertex = read_vertices ( node_filename, direction );
    vertex = ear_init ( vertex );
    area = area_poly2 ( vertex );
  end

  n = length ( vertex );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of vertices is %d\n', n );

  print_vertices ( vertex );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Area of polygon is %g\n', area );
%
%  Refuse to triangulate a region in which two successive vertices are equal.
%
  for i = 1 : n
    j = vertex(i).next;
    if ( vertex(i).x == vertex(j).x && vertex(i).y == vertex(j).y )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'TRIANGULATE - Fatal error!\n' );
      fprintf ( 1, '  Two neighbor vertices are equal.\n' );
      fprintf ( 1, '  Vertex(%d) = Vertex(%d)\n', i, j );
      error ( 'TRIANGULATE - Fatal error!' );
    end
  end
%
%  Determine a reasonable scale for the plot.
%
  xmin = vertex(1).x;
  xmax = vertex(1).x;
  ymin = vertex(1).y;
  ymax = vertex(1).y;
  for i = 2 : n
    xmin = min ( xmin, vertex(i).x );
    xmax = max ( xmax, vertex(i).x );
    ymin = min ( ymin, vertex(i).y );
    ymax = max ( ymax, vertex(i).y );
  end

  dx = 0.0125 * max ( xmax - xmin, ymax - ymin );
%
%  Draw the (non-triangulated) polygon.
%
%  You might think a single PLOT command would suffice to display
%  the vertices.  However, the proper way of extracting a vector of
%  values from a structure eludes me.
%
  figure ( 1 )

  clf

  grid on

  hold on
%
%  Make the plot a little bigger than the data.
%
  plot ( xmin - 2 * dx, ymin - 2 * dx, 'w.' );
  plot ( xmax + 2 * dx, ymin + 2 * dx, 'w.' );

  for i = 1 : n
    plot ( vertex(i).x, vertex(i).y, 'k.', 'MarkerSize', 25 )
  end

  for i = 1 : n
    text ( vertex(i).x + dx, vertex(i).y + dx, num2str ( i ) );
  end

  for i = 1 : n
    line ( [ vertex(i).x, vertex(vertex(i).next).x ], ...
           [ vertex(i).y, vertex(vertex(i).next).y ], ...
           'LineWidth', 3, 'Color', 'r' )
  end

  xlabel ( '<-- X -->' )
  ylabel ( '<-- Y -->' )
  title ( prefix );
  axis equal
%
%  Triangulate the polygon.
%
  triangles = polygon_triangulate ( vertex );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Internal diagonals for triangulation:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n - 3
    fprintf ( 1, '  %2d  %2d\n', triangles(i,1:2) );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Triangles:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n - 2
    fprintf ( 1, '  %2d  %2d  %2d\n', triangles(i,1:3) );
  end
%
%  Draw the triangulated polygon.
%
  figure ( 2 )

  clf

  grid on

  hold on
%
%  Make the plot a little bigger than the data.
%
  plot ( xmin - 2 * dx, ymin - 2 * dx, 'w.' );
  plot ( xmax + 2 * dx, ymin + 2 * dx, 'w.' );

  for i = 1 : n
    plot ( vertex(i).x, vertex(i).y, 'k.', 'MarkerSize', 25 )
  end

  for i = 1 : n
    text ( vertex(i).x + dx, vertex(i).y + dx, num2str ( i ) );
  end

  for i = 1 : n
    line ( [ vertex(i).x, vertex(vertex(i).next).x ], ...
           [ vertex(i).y, vertex(vertex(i).next).y ], 'LineWidth', 2, 'Color', 'r' )
  end

  for i = 1 : n - 3
    i1 = triangles(i,1);
    i2 = triangles(i,2);
    line ( [ vertex(i1).x, vertex(i2).x ], ...
           [ vertex(i1).y, vertex(i2).y ], 'LineWidth', 3, 'Color', 'b' )
  end

  xlabel ( '<-- X -->' )
  ylabel ( '<-- Y -->' )
  title ( sprintf ( 'Triangulation of "%s"', prefix ) );
  axis equal
%
%  Optionally, animate the triangulation.
%
  if ( animate == 'y' )

    fprintf ( 1, '\n' );
    fprintf ( 1, '  ANIMATION OPTION SELECTED!\n' );
    fprintf ( 1, '  Press return to see each diagonal added.\n' );

    figure ( 3 )

    clf

    grid on

    hold on
%
%  Make the plot a little bigger than the data.
%
    plot ( xmin - 2 * dx, ymin - 2 * dx, 'w.' );
    plot ( xmax + 2 * dx, ymin + 2 * dx, 'w.' );

    for i = 1 : n
      plot ( vertex(i).x, vertex(i).y, 'k.', 'MarkerSize', 25 )
    end

    for i = 1 : n
      text ( vertex(i).x + dx, vertex(i).y + dx, num2str ( i ) );
    end

    for i = 1 : n
      line ( [ vertex(i).x, vertex(vertex(i).next).x ], ...
             [ vertex(i).y, vertex(vertex(i).next).y ], 'LineWidth', 2, 'Color', 'r' )
    end

    for i = 1 : n - 3
      i1 = triangles(i,1);
      i2 = triangles(i,2);
      i3 = triangles(i,3);
      axis equal
      pause

      patch ( [ vertex(i1).x, vertex(i2).x, vertex(i3).x ], ...
              [ vertex(i1).y, vertex(i2).y, vertex(i3).y ], 'g' );

      line ( [ vertex(i1).x, vertex(i2).x ], ...
             [ vertex(i1).y, vertex(i2).y ], 'LineWidth', 3, 'Color', 'b' )

      axis equal

    end

    pause
    i = n - 2;
    i1 = triangles(i,1);
    i2 = triangles(i,2);
    i3 = triangles(i,3);
    line ( [ vertex(i1).x, vertex(i2).x ], ...
           [ vertex(i1).y, vertex(i2).y ], 'LineWidth', 3, 'Color', 'b' )

    patch ( [ vertex(i1).x, vertex(i2).x, vertex(i3).x ], ...
            [ vertex(i1).y, vertex(i2).y, vertex(i3).y ], 'g' );

    xlabel ( '<-- X -->' )
    ylabel ( '<-- Y -->' )
    title ( sprintf ( 'Triangulation of "%s"', prefix ) );
    axis equal

  end
%
%  Write the diagonals to a file.
%
  i4mat_write ( diagonal_filename, 2, n-3, triangles(1:n-3,1:2)' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Diagonals written to "%s".\n', diagonal_filename );
%
%  Write the triangles to a file.
%
  i4mat_write ( triangle_filename, 3, n-2, triangles(1:n-2,1:3)' );

  fprintf ( 1, '  Triangles written to "%s".\n', triangle_filename );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TRIANGULATE\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end
function area = area_poly2 ( vertex )

%*****************************************************************************80
%
%% AREA_POLY2 computes the area of a polygon.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 February 2011
%
%  Author:
%
%    Original C version by Joseph ORourke.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Joseph ORourke,
%    Computational Geometry in C,
%    Cambridge, 1998,
%    ISBN: 0521649765,
%    LC: QA448.D38.
%
%  Parameters:
%
%    Input, vertex array VERTEX(*).
%
%    Output, real AREA, the area of the polygon.
%
  area = 0.0;

  first = 1;

  i = first;

  ip1 = vertex(i).next;
  if ( ip1 == first )
    return
  end

  ip2 = vertex(ip1).next;
  if ( ip2 == first )
    return
  end

  while ( 1 )

    area = area + triangle_area ( vertex(i), vertex(ip1), vertex(ip2) );

    i = ip1;
    ip1 = ip2;
    ip2 = vertex(ip1).next;

    if ( ip2 == first )
      break
    end

  end

  return
end
function value = between ( va, vb, vc )

%*****************************************************************************80
%
%% BETWEEN is TRUE if vertex VC is between vertices VA and VB.
%
%  Discussion:
%
%    The points must be (numerically) collinear.
%
%    Given that condition, we take the greater of VA.X - VB.X and VA.Y - VB.Y
%    as a "scale" and check where VC's value lies.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 February 2011
%
%  Author:
%
%    Original C version by Joseph ORourke.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Joseph ORourke,
%    Computational Geometry in C,
%    Cambridge, 1998,
%    ISBN: 0521649765,
%    LC: QA448.D38.
%
%  Parameters:
%
%    Input, real VA, VB, VC, the vertices.
%
%    Output, integer VALUE, is 1 if VC is between VA and VB,
%    and 0 otherwise.
%
  if ( ~collinear ( va, vb, vc ) )
    value = 0;
    return
  end

  if ( abs ( va.y - vb.y ) < abs ( va.x - vb.x ) )
    xmax = max ( va.x, vb.x );
    xmin = min ( va.x, vb.x );
    value = ( xmin <= vc.x && vc.x <= xmax );
  else
    ymax = max ( va.y, vb.y );
    ymin = min ( va.y, vb.y );
    value = ( ymin <= vc.y && vc.y <= ymax );
  end

  return
end
function value = collinear ( va, vb, vc )

%*****************************************************************************80
%
%% COLLINEAR returns a measure of collinearity for three points.
%
%  Discussion:
%
%    In order to deal with collinear points whose coordinates are not
%    numerically exact, we compare the area of the largest square
%    that can be created by the line segment between two of the points
%    to (twice) the area of the triangle formed by the points.
%
%    If the points are collinear, their triangle has zero area.
%    If the points are close to collinear, then the area of this triangle
%    will be small relative to the square of the longest segment.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 February 2011
%
%  Author:
%
%    Original C version by Joseph ORourke.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Joseph ORourke,
%    Computational Geometry in C,
%    Cambridge, 1998,
%    ISBN: 0521649765,
%    LC: QA448.D38.
%
%  Parameters:
%
%    Input, real VA, VB, VC, the vertices.
%
%    Output, integer VALUE, is 1 if the points are judged to be collinear,
%    and 0 otherwise.
%
  area = 0.5 * ( ( vb.x - va.x ) * ( vc.y - va.y ) ...
               - ( vc.x - va.x ) * ( vb.y - va.y ) );
%
%  MATLAB's MAX function has decided to be unfriendly, and I am
%  too annoyed to try to figure out how to placate it when all
%  I want to say is M = max ( A, B, C )!
%
  area_max =                 ( va.x - vb.x )^2 + ( va.y - vb.y )^2;
  area_max = max ( area_max, ( vb.x - vc.x )^2 + ( vb.y - vc.y )^2 );
  area_max = max ( area_max, ( vc.x - va.x )^2 + ( vc.y - va.y )^2 );

  if ( area_max <= eps )
    value = 1;
  elseif ( 2.0 * abs ( area ) <= eps * area_max )
    value = 1;
  else
    value = 0;
  end

  return
end
function value = diagonal ( im1, ip1, vertex )

%*****************************************************************************80
%
%% DIAGONAL: VERTEX(IM1):VERTEX(IP1) is a proper internal diagonal.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 May 2014
%
%  Author:
%
%    Original C version by Joseph ORourke.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Joseph ORourke,
%    Computational Geometry in C,
%    Cambridge, 1998,
%    ISBN: 0521649765,
%    LC: QA448.D38.
%
%  Parameters:
%
%    Input, integer IM1, IP1, the indices of two vertices.
%
%    Input, vertex array VERTEX(*).
%
%    Output, integer VALUE, the value of the test.
%
  value = in_cone ( im1, ip1, vertex ) && ...
          in_cone ( ip1, im1, vertex ) && ...
          diagonalie ( im1, ip1, vertex );

  return
end
function value = diagonalie ( im1, ip1, vertex )

%*****************************************************************************80
%
%% DIAGONALIE: VERTEX(IM1):VERTEX(IP1) is a proper diagonal.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 February 2011
%
%  Author:
%
%    Original C version by Joseph ORourke.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Joseph ORourke,
%    Computational Geometry in C,
%    Cambridge, 1998,
%    ISBN: 0521649765,
%    LC: QA448.D38.
%
%  Parameters:
%
%    Input, integer IM1, IP1, the indices of two vertices.
%
%    Input, vertex array VERTEX(*).
%
%    Output, int VALUE, the value of the test.
%
  first = im1;

  j = vertex(first).index;
  jp1 = vertex(first).next;

  value = 1;
%
%  For each edge VERTEX(J):VERTEX(JP1) of the polygon:

  while ( 1 )
%
%  Skip any edge that includes vertex IM1 or IP1.
%
    if ( j == im1 || j == ip1 || jp1 == im1 || jp1 == ip1 )

    else
      if ( intersect ( vertex(im1), vertex(ip1), vertex(j), vertex(jp1) ) )
        value = 0;
        return
      end
    end

    j = jp1;
    jp1 = vertex(j).next;

    if ( j == vertex(first).index )
      break
    end

  end

  return
end
function vertex = ear_init ( vertex )

%*****************************************************************************80
%
%% EAR_INIT initializes the "earity" of each vertex.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 February 2011
%
%  Author:
%
%    Original C version by Joseph ORourke.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Joseph ORourke,
%    Computational Geometry in C,
%    Cambridge, 1998,
%    ISBN: 0521649765,
%    LC: QA448.D38.
%
%  Parameters:
%
%    Input/output, vertex array VERTEX(*).  On output, the "ear" field
%    of the vertex array has been initialized.
%
  im1 = vertex(1).prev;
  i   = vertex(1).index;
  ip1 = vertex(1).next;

  while ( 1 )

    vertex(i).ear = diagonal ( im1, ip1, vertex );

    im1 = i;
    i   = ip1;
    ip1 = vertex(i).next;

    if ( i == vertex(1).index )
      break
    end

  end

  return
end
function value = i4_modp ( i, j )

%*****************************************************************************80
%
%% I4_MODP returns the nonnegative remainder of I4 division.
%
%  Discussion:
%
%    If
%      NREM = I4_MODP ( I, J )
%      NMULT = ( I - NREM ) / J
%    then
%      I = J * NMULT + NREM
%    where NREM is always nonnegative.
%
%    The MOD function computes a result with the same sign as the
%    quantity being divided.  Thus, suppose you had an angle A,
%    and you wanted to ensure that it was between 0 and 360.
%    Then mod(A,360) would do, if A was positive, but if A
%    was negative, your result would be between -360 and 0.
%
%    On the other hand, I4_MODP(A,360) is between 0 and 360, always.
%
%  Example:
%
%        I     J     MOD  I4_MODP    Factorization
%
%      107    50       7       7    107 =  2 *  50 + 7
%      107   -50       7       7    107 = -2 * -50 + 7
%     -107    50      -7      43   -107 = -3 *  50 + 43
%     -107   -50      -7      43   -107 =  3 * -50 + 43
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 March 1999
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer I, the number to be divided.
%
%    Input, integer J, the number that divides I.
%
%    Output, integer VALUE, the nonnegative remainder when I is
%    divided by J.
%
  if ( j == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4_MODP - Fatal error!\n' );
    fprintf ( 1, '  Illegal divisor J = %d\n', j );
    error ( 'I4_MODP - Fatal error!' );
  end

  value = mod ( i, j );

  if ( value < 0 )
    value = value + abs ( j );
  end

  return
end
function value = i4_wrap ( ival, ilo, ihi )

%*****************************************************************************80
%
%% I4_WRAP forces an integer to lie between given limits by wrapping.
%
%  Example:
%
%    ILO = 4, IHI = 8
%
%    I   Value
%
%    -2     8
%    -1     4
%     0     5
%     1     6
%     2     7
%     3     8
%     4     4
%     5     5
%     6     6
%     7     7
%     8     8
%     9     4
%    10     5
%    11     6
%    12     7
%    13     8
%    14     4
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 October 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer IVAL, an integer value.
%
%    Input, integer ILO, IHI, the desired bounds for the integer value.
%
%    Output, integer I4_WRAP, a "wrapped" version of IVAL.
%
  jlo = min ( ilo, ihi );
  jhi = max ( ilo, ihi );

  wide = jhi - jlo + 1;

  if ( wide == 1 )
    value = jlo;
  else
    value = jlo + i4_modp ( ival - jlo, wide );
  end

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

%*****************************************************************************80
%
%% I4MAT_WRITE writes an I4MAT file.
%
%  Discussion:
%
%    An I4MAT is an array of I4's.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 June 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string OUTPUT_FILENAME, the output filename.
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of points.
%
%    Input, integer TABLE(M,N), the points.
%

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

  if ( output_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4MAT_WRITE - Error!\n' );
    fprintf ( 1, '  Could not open the output file.\n' );
    error ( 'I4MAT_WRITE - Error!' );
  end
%
%  Write the data.
%
  for j = 1 : n
    for i = 1 : m
      fprintf ( output_unit, '  %12d', round ( table(i,j) ) );
    end
    fprintf ( output_unit, '\n' );
  end
%
%  Close the file.
%
  fclose ( output_unit );

  return
end
function value = in_cone ( im1, ip1, vertex )

%*****************************************************************************80
%
%% IN_CONE is TRUE if the diagonal VERTEX(IM1):VERTEX(IP1) is strictly internal.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 February 2011
%
%  Author:
%
%    Original C version by Joseph ORourke.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Joseph ORourke,
%    Computational Geometry in C,
%    Cambridge, 1998,
%    ISBN: 0521649765,
%    LC: QA448.D38.
%
%  Parameters:
%
%    Input, integer IM1, IP1, the indices of two vertices.
%
%    Input, vertex array VERTEX(*).
%
%    Output, int VALUE, the value of the test.
%
  im2 = vertex(im1).prev;
  i = vertex(im1).next;

  if ( left_on ( vertex(im1), vertex(i), vertex(im2) ) )

    value = left ( vertex(im1), vertex(ip1), vertex(im2) ) && ...
            left ( vertex(ip1), vertex(im1), vertex(i) );

  else

    value = ~ ( left_on ( vertex(im1), vertex(ip1), vertex(i) ) && ...
                left_on ( vertex(ip1), vertex(im1), vertex(im2) ) );

  end

  return
end
function value = intersect ( va, vb, vc, vd )

%*****************************************************************************80
%
%% INTERSECT is true if lines VA:VB and VC:VD intersect.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 February 2011
%
%  Author:
%
%    Original C version by Joseph ORourke.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Joseph ORourke,
%    Computational Geometry in C,
%    Cambridge, 1998,
%    ISBN: 0521649765,
%    LC: QA448.D38.
%
%  Parameters:
%
%    Input, vertex VA, VB, VC, VD, four vertices.
%
%    Output, integer VALUE, the value of the test.
%
  if ( intersect_prop ( va, vb, vc, vd ) )
    value = 1;
  elseif ( between ( va, vb, vc ) || ...
           between ( va, vb, vd ) || ...
           between ( vc, vd, va ) || ...
           between ( vc, vd, vb ) )
    value = 1;
  else
    value = 0;
  end

  return
end
function value = intersect_prop ( va, vb, vc, vd )

%*****************************************************************************80
%
%% INTERSECT_PROP is TRUE if lines VA:VB and VC:VD have a proper intersection.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 February 2011
%
%  Author:
%
%    Original C version by Joseph ORourke.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Joseph ORourke,
%    Computational Geometry in C,
%    Cambridge, 1998,
%    ISBN: 0521649765,
%    LC: QA448.D38.
%
%  Parameters:
%
%    Input, vertex VA, VB, VC, VD, four vertices.
%
%    Output, integer VALUE, the result of the test.
%
  if ( collinear ( va, vb, vc ) || ...
       collinear ( va, vb, vd ) || ...
       collinear ( vc, vd, va ) || ...
       collinear ( vc, vd, vb ) )
    value = 0;
  else
    value = xor ( left ( va, vb, vc ), left ( va, vb, vd ) ) && ...
            xor ( left ( vc, vd, va ), left ( vc, vd, vb ) );
  end

  return
end
function value = left ( va, vb, vc )

%*****************************************************************************80
%
%% LEFT is true if VC is to the left of the line VA:VB.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 February 2011
%
%  Author:
%
%    Original C version by Joseph ORourke.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Joseph ORourke,
%    Computational Geometry in C,
%    Cambridge, 1998,
%    ISBN: 0521649765,
%    LC: QA448.D38.
%
%  Parameters:
%
%    Input, vertex VA, VB, VC, three vertices.
%
%    Output, integer VALUE, the result of the test.
%
  value = ( 0 < triangle_area ( va, vb, vc ) );

  return
end
function value = left_on ( va, vb, vc )

%*****************************************************************************80
%
%% LEFT_ON is TRUE if VC is to the left of, or on, the line VA:VB.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    Original C version by Joseph ORourke.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Joseph ORourke,
%    Computational Geometry in C,
%    Cambridge, 1998,
%    ISBN: 0521649765,
%    LC: QA448.D38.
%
%  Parameters:
%
%    Input, vertex VA, VB, VC, three vertices.
%
%    Output, integer VALUE, the result of the test.
%
  value = ( 0 <= triangle_area ( va, vb, vc ) );

  return
end
function triangles = polygon_triangulate ( vertex )

%*****************************************************************************80
%
%% POLYGON_TRIANGULATE determines a triangulation of a polygon.
%
%  Discussion:
%
%    There are N-3 triangles in the triangulation.
%
%    For the first N-2 triangles, the first edge listed is always an
%    internal diagonal.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 February 2011
%
%  Author:
%
%    Original C version by Joseph ORourke.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Joseph ORourke,
%    Computational Geometry in C,
%    Cambridge, 1998,
%    ISBN: 0521649765,
%    LC: QA448.D38.
%
%  Parameters:
%
%    Input, vertex array VERTEX(N).
%
%    Output, integer TRIANGLES(N-2,3), the triangles of the triangulation.
%
  n = length ( vertex );
  first = 1;

  triangles = zeros ( n - 2, 3 );
  triangle_num = 0;

  i2 = first;

  while ( triangle_num < n - 3 )
%
%  If I2 is an ear, gather information necessary to carry out
%  the slicing operation and subsequent "healing".
%
    if ( vertex(i2).ear )

      i3 = vertex(i2).next;
      i4 = vertex(i3).next;
      i1 = vertex(i2).prev;
      i0 = vertex(i1).prev;
%
%  Make vertex I2 disappear.
%
      vertex(i1).next = i3;
      vertex(i2).index = 0;
      vertex(i3).prev = i1;
%
%  Update the earity of I1 and I3, because I2 disappeared.
%
      vertex(i1).ear = diagonal ( i0, i3, vertex );
      vertex(i3).ear = diagonal ( i1, i4, vertex );
%
%  Add the diagonal [I3, I1, I2] to the list.
%
      triangle_num = triangle_num + 1;
      triangles(triangle_num,1) = i3;
      triangles(triangle_num,2) = i1;
      triangles(triangle_num,3) = i2;

    end
%
%  Try the next vertex.
%
    i2 = vertex(i2).next;

  end
%
%  The last triangle is formed from the three remaining vertices.
%
  i3 = vertex(i2).next;
  i1 = vertex(i2).prev;
  triangle_num = triangle_num + 1;
  triangles(triangle_num,1) = i3;
  triangles(triangle_num,2) = i1;
  triangles(triangle_num,3) = i2;

  return
end
function print_vertices ( vertex )

%*****************************************************************************80
%
%% PRINT_VERTICES prints the vertex information.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 February 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, vertex array VERTEX(*).
%
  n = length ( vertex );

  fprintf ( 1, '\n' );
  fprintf ( 1, '   I IM1 IP1      X           Y       Earity\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n

    if ( vertex(i).ear == -1 )
      earity = 'Unknown';
    elseif ( vertex(i).ear == 0 )
      earity = 'No';
    else
      earity = 'Yes';
    end

    fprintf ( 1, '  %2d  %2d  %2d  %10f  %10f  %s\n', ...
      vertex(i).index, vertex(i).prev, vertex(i).next, ...
      vertex(i).x, vertex(i).y, earity );

  end

  return
end
function vertex = read_vertices ( filename, direction )

%*****************************************************************************80
%
%% READ_VERTICES reads vertex coordinates and initializes the vertex structure.
%
%  Discussion:
%
%    If the polygon was found to have negative area on the first pass,
%    we try again, reversing the vertex ordering, and hoping the user simply
%    listed the vertices in clockwise order.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 January 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string FILENAME, the name of the file to be read.
%
%    Input, integer DIRECTION, the ordering for the vertices.
%    +1, first vertex is 1, and count up.
%    -1, first vertex is N, and count down.
%
%    Output, vertex array VERTEX(*).
%
  xy = load ( filename );

  [ n, dim ] = size ( xy );

  if ( direction == -1 )
    xy(1:n,1) = xy(n:-1:1,1);
    xy(1:n,2) = xy(n:-1:1,2);
  end

  for i = 1 : n
    vertex(i).index = i;
    vertex(i).prev = i4_wrap ( i - 1, 1, n );
    vertex(i).next = i4_wrap ( i + 1, 1, n );
    vertex(i).x = xy(i,1);
    vertex(i).y = xy(i,2);
    vertex(i).ear = -1;
  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 area = triangle_area ( v1, v2, v3 )

%*****************************************************************************80
%
%% TRIANGLE_AREA computes the signed area of a triangle.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 February 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, vertex V1, V2, V3, three vertices.
%
%    Output, real AREA, the signed area of the triangle.
%
  area = 0.5 * ( ( v2.x - v1.x ) * ( v3.y - v1.y ) ...
               - ( v3.x - v1.x ) * ( v2.y - v1.y ) );

  return
end


function res = fft2c(x)

S = size(x);
fctr = S(1)*S(2);

x = reshape(x,S(1),S(2),prod(S(3:end)));

res = zeros(size(x));
for n=1:size(x,3)
	res(:,:,n) = 1/sqrt(fctr)*fftshift(fft2(ifftshift(x(:,:,n))));
end

res = reshape(res,S);


% created: Zoya Bylinskii, Aug 2014
% Code for computing degrees of visual angle

% This code has been written to make it easier to report visual angle in
% papers, and to ensure a standardized calculation is used.

% This code can take variable input (e.g. horizontal, vertical, or diagonal
% screen measurements in any units), and will calculate any number of
% degrees of visual angle in pixels, as well as degrees of visual angle
% spanned by the computer screen or an image.

% The code is provided on: http://saliency.mit.edu/

% Resources used: http://osdoc.cogsci.nl/miscellaneous/visual-angle/,
%                 http://en.wikipedia.org/wiki/Visual_angle
% Thank you to Lavanya Sharan and Melissa Vo for helpful discussion.

function computeDegVisAngle_gui()

% ------ INPUT ARGUMENTS: ------
% necessary:
% 'distance'         distance to screen with units (e.g. 56 cm)

% provide at least one of:
% 'height'           height of screen with units
% 'width'            width of screen with units
% 'diagonal'         diagonal of screen with units

% provide at least one of:
% 'hres'             horizontal resolution of screen (provide this if also provided 'height')
% 'vres'             vertical resolution of screen (provide this if also provided 'width')
% provide both if only provided 'diagonal'

% optional:
%'outputUnit'        output units to measure screen, distance in (default: cm)
%'numDegs'           number of degres of visual angle (in pixels) to calculate (default numDegs=1)
%'imageWidth'        width of image in pixels
%'imageHeight'       height of image in pixels

% NOTE: if you provide the screen diagonal and resolution, then the screen
% height and width will be estimated under the assumption that the aspect
% ratio of the screen dimensions matches that of the set resolution.
% If these estimates look wrong, either:
% (1) you measured the monitor dimensions, instead of the screen dimensions
% (2) the screen resolution aspect ratio does not match the screen
%     dimensions
% (3) explicitely enter the screen height and width instead of the diagonal

% Common mistake: remember that the number of pixels per unit size of screen
% or image is a linear function, but number of pixels per degree of visual
% angle is NOT.

% Remember: when writing a paper, include enough information for visual
% angle to be calculated - providing screen resolution without screen
% dimensions or vice versa is insufficient! Also, remember to measure the
% screen itself, rather than the whole monitor.

% units required for distance, height, width, and diagonal values
ui=inputdlg({'distance (with units)','height (with units)',...
    'width (with units)','diagonal (with units)',...
    'horizontal resolution','vertical resolution',...
    'output units (optional)','number of degrees (optional)',...
    'image width in pixels (optional)','image height in pixels (optional)'},...
    'Visual Angle Calculator',1,...
    {'','','','','','','cm','1','',''});

dist = ui{1};
height = ui{2};
width = ui{3};
diagonal = ui{4};
hres = str2num(ui{5});
vres = str2num(ui{6});
outputUnit = ui{7};
numDegs = str2num(ui{8});
imageWidth = str2num(ui{9});
imageHeight = str2num(ui{10});

computeDegVisAngle_helper(dist,height,width,diagonal,hres,vres,outputUnit,numDegs,imageWidth,imageHeight);


function [ x, seed ] = maxwell_sample ( a, seed )

%*****************************************************************************80
%
%% MAXWELL_SAMPLE samples the Maxwell PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, the parameter of the PDF.
%    0 < A.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real X, a sample of the PDF.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  a2 = 3.0;
  [ x, seed ] = chi_square_sample ( a2, seed );

  x = a * sqrt ( x );

  return
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Note, in order to run this script, you will need
% the package sparsify v0.5 by Thomas Blumensath
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Signal space
N = 1024;
% Number of measurements
M = 100;
% Sparsity level
K = 8;

% solver instance
% solver = spx.pursuit.single.MatchingPursuit(Phi);
% Solve the sparse recovery problem
% result = solver.solve(y);
max_residual_norm = 1e-6;
options.max_residual_norm = max_residual_norm;
tstart = tic;
T = 1000;
t1 = 0;
t2 = 0;
for i=1:T
    if mod(i, 10) == 0
        fprintf('.');
    end
    % Construct the signal generator.
    gen  = spx.data.synthetic.SparseSignalGenerator(N, K);
    % Generate bi-uniform signals
    x = gen.biUniform(1, 2);
    % Sensing matrix
    Phi = spx.dict.simple.gaussian_mtx(M, N);
    % Measurement vectors
    y = Phi * x;
    tic;
    if 0
        result = spx.pursuit.single.mp(Phi, y,options);
    else
        [s, err_mse, iter_time] = greed_mp(y, Phi, N, ...
            'stopCrit', 'mse', 'stopTol', max_residual_norm^2, ...
            'maxIter', 4 * N);
    end
    % result.iterations
    t1 = t1 + toc;
    % norm(result.r) / norm(y)
    tic;
    result = spx.fast.mp(Phi, y,options);
    t2 = t2 + toc;
end
fprintf('\n');
fprintf('Time taken by MATLAB implementation: %0.3f\n', t1);
fprintf('Time taken by C++ implementation: %0.3f\n', t2);
fprintf('Gain: %0.2f\n', t1/t2);


function err = test_performance
%TEST_PERFORMANCE compare performance of factorization/solve methods.
% Returns the largest relative residual seen.
%
% Example
%   err = test_performance
%
% See also test_all, test_factorize, factorize, inverse, mldivide

% Copyright 2009, Timothy A. Davis, University of Florida

fprintf ('\nPerformance comparisons of 5 methods:\n') ;
fprintf ('    backslash: A\\b, or L\\b (and related) for solve times.\n') ;
fprintf ('    linsolve:  a built-in MATLAB function\n') ;
fprintf ('    factorize1:  the simple factorization object (uses LU)\n') ;
fprintf ('    factorize:  the full-featured factorization object\n') ;
fprintf ('    inv: x=inv(A)*b, the explicit inverse (ack!)\n') ;
fprintf ('Run times are in seconds.\n') ;
fprintf ('Times relative to best times are in parentheses.\n') ;

% linsolve options:
A_is_posdef.POSDEF = true ;     % for x = A\b where A is sym. pos. definite
A_is_posdef.SYM = true ;
lsolve.LT = true ;              % for x = L\b where L is lower triangular
usolve.UT = true ;              % for x = U\b where U is upper triangular
ltsolve.LT = true ;             % for x = L'b where L is lower triangular
ltsolve.TRANSA = true ;

nn = [50 100 500 1000] ;        % matrix sizes to test
ns = length (nn) ;
tmax = 1 ;                      % minimum testing time
err = 0 ;                       % largest relative residual seen

for posdef = 0:1

    if (posdef)
        fprintf ('\n------------------ For positive definite matrices:\n') ;
    else
        fprintf ('\n------------------ For unsymmetric matrices:\n') ;
    end

    Tfactor = zeros (ns,5) ;
    Tsolve  = zeros (ns,5) ;

    %-----------------------------------------------------------------------
    % compare factorizations times (plus a single solve)
    %-----------------------------------------------------------------------

    fprintf ('\nCompare factorization times:\n') ;
    for i = 1:ns
        n = nn (i) ;

        fprintf ('n %4d ', n) ;

        A = rand (n) ;
        if (posdef)
            A = A'*A + eye (n) ;
        end
        anorm = norm (A,1) ;
        b = rand (n,1) ;

        % method 1: backslash
        t = 0 ;
        k = 0 ;
        tic
        while (t < tmax)
            x = A\b ;
            k = k + 1 ;
            t = toc ;
        end
        Tfactor (i,1) = t / k ;
        err = max (err, norm (A*x-b,1) / (anorm + norm (x,1))) ;
        clear x

        if (posdef)

            % method 2: linsolve (pos. definite case)
            t = 0 ;
            k = 0 ;
            tic
            while (t < tmax)
                x = linsolve (A,b, A_is_posdef) ;
                k = k + 1 ;
                t = toc ;
            end
            Tfactor (i,2) = t / k ;
            err = max (err, norm (A*x-b,1) / (anorm + norm (x,1))) ;
            clear x

        else

            % method 2: linsolve (unsymmetric case)
            t = 0 ;
            k = 0 ;
            tic
            while (t < tmax)
                x = linsolve (A,b) ;
                k = k + 1 ;
                t = toc ;
            end
            Tfactor (i,2) = t / k ;
            err = max (err, norm (A*x-b,1) / (anorm + norm (x,1))) ;
            clear x

        end

        % method 3: factorize1 method
        t = 0 ;
        k = 0 ;
        tic
        while (t < tmax)
            F = factorize1 (A) ;
            x = F\b ;
            k = k + 1 ;
            t = toc ;
        end
        Tfactor (i,3) = t / k ;
        err = max (err, norm (A*x-b,1) / (anorm + norm (x,1))) ;
        clear x F

        % method 4: factorize method
        t = 0 ;
        k = 0 ;
        tic
        while (t < tmax)
            F = factorize (A, posdef) ;
            x = F\b ;
            k = k + 1 ;
            t = toc ;
        end
        Tfactor (i,4) = t / k ;
        err = max (err, norm (A*x-b,1) / (anorm + norm (x,1))) ;
        clear x F

        % method 5: inv method (ack!)
        t = 0 ;
        k = 0 ;
        tic
        while (t < tmax)
            S = inv (A) ;
            x = S*b ;               %#ok
            k = k + 1 ;
            t = toc ;
        end
        Tfactor (i,5) = t / k ;
        err = max (err, norm (A*x-b,1) / (anorm + norm (x,1))) ;
        clear x S

        tbest = min (Tfactor (i,:)) ;
        fprintf ('tbest %10.6f : backslash (%5.2f) ', tbest, ...
            Tfactor (i,1) / tbest);
        fprintf ('linsolve (%5.2f) ', Tfactor (i,2) / tbest) ;
        fprintf ('factorize1 (%5.2f) ', Tfactor (i,3) / tbest) ;
        fprintf ('factorize (%5.2f) ', Tfactor (i,4) / tbest) ;
        fprintf ('inv (%5.2f)\n',  Tfactor (i,5) / tbest) ;

    end

    %-----------------------------------------------------------------------
    % compare solve times
    %-----------------------------------------------------------------------

    fprintf ('\nCompare solve times:\n') ;
    for i = 1:ns
        n = nn (i) ;

        fprintf ('n %4d ', n) ;

        A = rand (n) ;
        if (posdef)
            A = A'*A + eye (n) ;
        end
        b = rand (n,1) ;
        anorm = norm (A,1) ;

        if (posdef)
            L = chol (A, 'lower') ;
        else
            [L,U,p] = lu (A,'vector') ;
        end

        S = inv (A) ;
        F = factorize  (A) ;
        F1 = factorize1 (A) ;

        if (posdef)

            % method 1: backslash (pos. definite case)
            t = 0 ;
            k = 0 ;
            tic
            while (t < tmax)
                x = L' \ (L\b) ;
                k = k + 1 ;
                t = toc ;
            end
            Tsolve (i,1) = t / k ;
            err = max (err, norm (A*x-b,1) / (anorm + norm (x,1))) ;
            clear x

            % method 2: linsolve (pos. definite case)
            t = 0 ;
            k = 0 ;
            tic
            while (t < tmax)
                x = linsolve (L, linsolve (L, b, lsolve), ltsolve) ;
                k = k + 1 ;
                t = toc ;
            end
            Tsolve (i,2) = t / k ;
            err = max (err, norm (A*x-b,1) / (anorm + norm (x,1))) ;
            clear x

        else

            % method 1: backslash (unsymmetric case)
            t = 0 ;
            k = 0 ;
            tic
            while (t < tmax)
                x = U \ (L \ (b (p))) ;
                k = k + 1 ;
                t = toc ;
            end
            Tsolve (i,1) = t / k ;
            err = max (err, norm (A*x-b,1) / (anorm + norm (x,1))) ;
            clear x

            % method 2: linsolve (unsymmetric case)
            t = 0 ;
            k = 0 ;
            tic
            while (t < tmax)
                x = linsolve (U, linsolve (L, b (p), lsolve), usolve) ;
                k = k + 1 ;
                t = toc ;
            end
            Tsolve (i,2) = t / k ;
            err = max (err, norm (A*x-b,1) / (anorm + norm (x,1))) ;
            clear x

        end

        % method 3: factorize1 object
        t = 0 ;
        k = 0 ;
        tic
        while (t < tmax)
            x = F1\b ;
            k = k + 1 ;
            t = toc ;
        end
        Tsolve (i,3) = t / k ;
        err = max (err, norm (A*x-b,1) / (anorm + norm (x,1))) ;
        clear x

        % method 4: factorize object
        t = 0 ;
        k = 0 ;
        tic
        while (t < tmax)
            x = F\b ;
            k = k + 1 ;
            t = toc ;
        end
        Tsolve (i,4) = t / k ;
        err = max (err, norm (A*x-b,1) / (anorm + norm (x,1))) ;
        clear x

        % method 5: "inv" method (ack!)
        t = 0 ;
        k = 0 ;
        tic
        while (t < tmax)
            x = S*b ;           %#ok
            k = k + 1 ;
            t = toc ;
        end
        Tsolve (i,5) = t / k ;
        err = max (err, norm (A*x-b,1) / (anorm + norm (x,1))) ;
        clear x

        tbest = min (Tsolve (i,:)) ;
        fprintf ('tbest %10.6f : backslash (%5.2f) ', tbest, ...
            Tsolve (i,1) / tbest);
        fprintf ('linsolve (%5.2f) ', Tsolve (i,2) / tbest) ;
        fprintf ('factorize1 (%5.2f) ', Tsolve (i,3) / tbest) ;
        fprintf ('factorize (%5.2f) ', Tsolve (i,4) / tbest) ;
        fprintf ('inv (%5.2f)\n',  Tsolve (i,5) / tbest) ;

        clear F F1 S L U p

    end

    % determine break-even values for inv
    fprintf ('\nBreak-even values K for inv vs the other methods\n') ;
    fprintf ('(# of solves must exceed K for inv(A)*b to be faster):\n') ;
    for i = 1:ns
        n = nn (i) ;
        s = zeros (4,1) ;
        for t = 1:4
            if (Tfactor (i,5) > Tfactor (i,t) && ...
                Tsolve (i,5) > Tsolve (i,t))
                % inv is always slower, for any K
                s (t) = inf ;
            elseif (Tfactor (i,5) < Tfactor (i,t) && ...
                Tsolve (i,5) < Tsolve (i,t))
                % inv is always faster, for any K
                s (t) = 1 ;
            else
                s (t) = (Tfactor (i,5) - Tfactor (i,t)) / ...
                         (Tsolve (i,t) - Tsolve (i,5));
            end
        end

        fprintf ('n %4d  # solves vs backslash %8.1f', n, max (1, s (1))) ;
        fprintf (' vs linsolve: %8.1f  ', max (1, s (2))) ;
        fprintf (' vs factorize1: %8.1f  ', max (1, s (3))) ;
        fprintf (' vs factorize: %8.1f\n', max (1, s (4))) ;
    end

end

%---------------------------------------------------------------------------
% compute the Schur complement, S = A-B*inv(D)*C
%---------------------------------------------------------------------------

fprintf ('\nSchur complement, S=A-B*inv(D)*C or A-B(D\\C),\n') ;
fprintf ('where A, B, C, and D are square and unsymmetric.\n') ;
fprintf ('"inverse" means S=A-B*inverse(D)*C, which does not actually\n') ;
fprintf ('use the inverse, but uses the factorization object instead.\n') ;

Tschur = zeros (ns,6) ;

for i = 1:ns
    n = nn (i) ;
    fprintf ('n %4d ', n) ;

    A = rand (n) ;
    B = rand (n) ;
    C = rand (n) ;
    D = rand (n) ;

    % method 1: backslash
    t = 0 ;
    k = 0 ;
    tic
    while (t < tmax)
        S = A - B*(D\C) ;                                               %#ok
        k = k + 1 ;
        t = toc ;
    end
    Tschur (i,1) = t / k ;
    clear S

    % method 2: linsolve
    t = 0 ;
    k = 0 ;
    tic
    while (t < tmax)
        S = A - B * linsolve (D,C) ;                                    %#ok
        k = k + 1 ;
        t = toc ;
    end
    Tschur (i,2) = t / k ;
    clear S

    % method 3: factorize1 object
    t = 0 ;
    k = 0 ;
    tic
    while (t < tmax)
        S = A - B*(factorize1(D)\C) ;                                   %#ok
        k = k + 1 ;
        t = toc ;
    end
    Tschur (i,3) = t / k ;
    clear F S

    % method 4: factorize object
    t = 0 ;
    k = 0 ;
    tic
    while (t < tmax)
        S = A - B*(factorize(D)\C) ;                                    %#ok
        k = k + 1 ;
        t = toc ;
    end
    Tschur (i,4) = t / k ;
    clear F S

    % method 5:
    t = 0 ;
    k = 0 ;
    tic
    while (t < tmax)
        S = A - B*inverse(D)*C ;                                        %#ok
        k = k + 1 ;
        t = toc ;
    end
    Tschur (i,5) = t / k ;
    clear S

    % method 6: "inv" method, using the explicit inverse (ack!)
    t = 0 ;
    k = 0 ;
    tic
    while (t < tmax)
        S = A - B*inv(D)*C ;                                            %#ok
        k = k + 1 ;
        t = toc ;
    end
    Tschur (i,6) = t / k ;
    clear S

    tbest = min (Tschur (i,:)) ;
    fprintf ('tbest %10.6f : backslash (%5.2f) ', tbest, ...
        Tschur (i,1) / tbest);
    fprintf ('linsolve (%5.2f) ', Tschur (i,2) / tbest) ;
    fprintf ('factorize1 (%5.2f) ', Tschur (i,3) / tbest) ;
    fprintf ('factorize (%5.2f) ', Tschur (i,4) / tbest) ;
    fprintf ('inverse (%5.2f) ', Tschur (i,5) / tbest) ;
    fprintf ('inv (%5.2f)\n',  Tschur (i,6) / tbest) ;

end


function y = log_normcdf( x, approx )

%LOG_NORMCDF   Logarithm of the cumulative normal distribution.
%   Y = LOG_NORMCDF(X) is the logarithm of the CDF of the normal
%   distribution at the point X.
%
%                                1    / x
%       LOG_NORMCDF(X) = LOG( ------- |   exp(-t^2/2) dt )
%                             sqrt(2) / -Inf
%
%   For numeric X, LOG_NORMCDF(X) is computed using the equivalent
%   expression LOG(0.5*ERFC(-X*SQRT(0.5))). When X is a CVX variable, a
%   a piecewise quadratic *approximation* is employed instead. This
%   approximation gives good results when -4 <= x <= 4, and will be
%   improved in future releases of CVX.
%
%   For array values of X, the LOG_NORMCDF returns an array of identical
%   size with the calculation applied independently to each element.
%
%   X must be real.
%
%   Disciplined convex programming information:
%       LOG_NORMCDF is concave and nondecreasing in X. Therefore, when used
%       in CVX specifications, X must be concave.

error(nargchk(1,2,nargin));
if ~isreal( x ),
    error( 'Argument must be real.' );
end
if nargin > 1,
    % For debugging purposes only
    y = cvx_constant(log_normcdf(cvx(x)));
else
    y = log(0.5*erfc(-x*sqrt(0.5)));
end


% collection of m-files to extend the statistics toolbox with the
% NIG distribution
%
% INVGRND       random number generator for the inverse Gaussian
%               distribution
% NIGCDF        cumulative distribution function for NIG distribution
% NIGINV        inverse cdf, computes the quantiles of the NIG distribution
% NIGPAR        parameters for NIG distribution given mean, standard
%               deviation, skewness and kurtosis
% NIGPDF        probability density function for NIG distribution
% NIGRND        random number generator for the normal inverse Gaussian
%               distribution
% NIGSTAT       mean, standard deviation, skewness and kurtosis for a
%               given parameter set


function out = DN_cv(x,k)
% DN_cv     Coefficient of variation
%
% Coefficient of variation of order k is sigma^k / mu^k (for sigma, standard
% deviation and mu, mean) of a data vector, x
%
%---INPUTS:
%
% x, the input data vector
% k, the order of coefficient of variation (k = 1 is default)

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function 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.
%
% This program 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
% this program. If not, see <http://www.gnu.org/licenses/>.
% ------------------------------------------------------------------------------

%-------------------------------------------------------------------------------
% Check inputs
%-------------------------------------------------------------------------------
if nargin < 2 || isempty(k)
    k = 1; % Do standard CV by default
end

if (rem(k,1) ~= 0) || (k < 0)
    warning('k should probably be a positive integer');
    % Carry on with just this warning, though
end

% Compute the coefficient of variation (of order k) of the data

out = (std(x))^k / (mean(x))^k;

end


% Copyright (c) 2012-2016 Ali Akbar Eftekhari
% See the license file
% Coupled nonlinear PDE's
% Buckley Leverett equation
% dependent variables: pressure and water saturation
% Prepared for educational purposes by ** AAE **
% Written by Ali A. Eftekhari
% Last checked: June 2021
% This code works fine (two phase flow + Pc) and shows the effect of Pc
% in the simulation of a heterogeneous 2D field.
clc
%% define the geometry
Nx = 100; % number of cells in x direction
Ny = 30; % number of cells in y direction
W = 300; % [m] length of the domain in x direction
H = 30; % [m] length of the domain in y direction
% m = createMesh1D(Nx, W);
m = createMesh2D(Nx, Ny, W, H); % creates a 2D mesh
%% define the physical parametrs
krw0 = 1.0;
kro0 = 0.76;
nw = 2.4;
no = 2.0;
sor=0.12;
swc=0.09;
sws=@(sw)((sw>swc).*(sw<1-sor).*(sw-swc)/(1-sor-swc)+(sw>=1-sor).*ones(size(sw)));
kro=@(sw)((sw>=swc).*kro0.*(1-sws(sw)).^no+(sw<swc).*(1+(kro0-1)/swc*sw));
krw=@(sw)((sw<=1-sor).*krw0.*sws(sw).^nw+(sw>1-sor).*(-(1-krw0)/sor.*(1.0-sw)+1.0));
dkrwdsw=@(sw)((sw<=1-sor).*nw.*krw0.*(1/(1-sor-swc)).*sws(sw).^(nw-1)+(sw>1-sor)*((1-krw0)/sor));
dkrodsw=@(sw)((sw>=swc).*(-kro0*no*(1-sws(sw)).^(no-1))/(-swc-sor+1)+(sw<swc).*((kro0-1)/swc));
p0 = 100e5; % [bar] pressure
pin = 150e5; % [bar] injection pressure at the left boundary
u_in= 1.0/(24*3600); % [m/s] equal to 1 m/day
sw0 = swc+0.1; % initial water saturation
sw_in = 1;
mu_oil = 2e-3; % [Pa.s] oil viscosity
mu_water = 1e-3; % [Pa.s] water viscosity
% reservoir
k0 = 2e-12; % [m^2] average reservoir permeability
phi0 = 0.2; % average porosity
teta_ow=deg2rad(30);
gama_ow=0.03; % N/m
labda=10.0;
eps1=1e-6;
clx=0.2;
cly=0.2;
V_dp=0.7; % Dykstra-Parsons coef.
perm_val= field2d(Nx,Ny,k0,V_dp,clx,cly);
k=createCellVariable(m, perm_val);
phi=createCellVariable(m, phi0);
pce=gama_ow*cos(teta_ow)*(phi./k).^0.5;
pce.value(isnan(pce.value)) = 1.0;
pce_face=gama_ow*cos(teta_ow)*(arithmeticMean(phi)./geometricMean(k)).^0.5;
pc=@(sw)(pce.*(sws(sw)+eps).^(-1/labda)); % it can also be defined for each block
dpc=@(sw)((-1/labda)*(1/(1-sor-swc)).*pce_face.*(sws(sw)+eps).^(-1/labda-1));
dpcdk=@(sw)(0.5*gama_ow*cos(teta_ow)*(geometricMean(k)./arithmeticMean(phi)).^0.5.*(sws(sw)+eps).^(-1/labda));
grad_phik=gradientTerm(phi./k);
% sw_plot=linspace(0,1, 10000);
% plot(sw_plot, pc(sw_plot))
lw = geometricMean(k)/mu_water;
lo = geometricMean(k)/mu_oil;
Lw = @(sw)(krw(sw));
Lo = @(sw)(k/mu_oil*kro(sw));
dLwdsw = @(sw)(k/mu_water*dkrwdsw(sw));
dLodsw = @(sw)(k/mu_oil*dkrodsw(sw));
%% Define the boundaries
BCp = createBC(m); % Neumann BC for pressure
BCs = createBC(m); % Neumann BC for saturation
% left boundary pressure gradient
BCp.left.a(:)=(krw(sw_in)*lw.xvalue(1,:)+kro(sw_in)*lo.xvalue(1,:)); BCp.left.b(:)=0; BCp.left.c(:)=-u_in;
% change the right boandary to constant pressure (Dirichlet)
% BCp.left.a(:)=0; BCp.left.b(:)=1; BCp.left.c(:)=pin;
BCp.right.a(:)=0; BCp.right.b(:)=1; BCp.right.c(:)=p0;
% BCs.right.a(:)=0; BCs.right.b(:)=1; BCs.right.c(:)=0.8; % end effect
% change the left boundary to constant saturation (Dirichlet)
BCs.left.a(:)=0; BCs.left.b(:)=1; BCs.left.c(:)=1;
%% define the time step and solver properties
% dt = 1000; % [s] time step
dt=(W/Nx)/u_in/20; % [s]
t_end = 1000*dt; % [s] final time
eps_p = 1e-5; % pressure accuracy
eps_sw = 1e-5; % saturation accuracy
%% define the variables
sw_old = createCellVariable(m, sw0, BCs);
p_old = createCellVariable(m, p0, BCp);
sw = sw_old;
p = p_old;
uw = -gradientTerm(p_old); % an estimation of the water velocity
%% start the main loop
% generate intial pressure profile (necessary to initialize the fully
% implicit solver)

t = 0;
while (t<t_end)
    error_p = 1e5;
    error_sw = 1e5;
    % Implicit loop
    while ((error_p>eps_p) || (error_sw>eps_sw))
        % calculate parameters
        pgrad = gradientTerm(p);
%         pcgrad=gradientTerm(pc(sw));
        sw_face = upwindMean(sw, -pgrad); % average value of water saturation
        sw_grad=gradientTerm(sw);
        sw_ave=arithmeticMean(sw);
        pcgrad=dpc(sw_ave).*sw_grad+dpcdk(sw_ave).*grad_phik;
        labdao = lo.*funceval(kro, sw_face);
        labdaw = lw.*funceval(krw, sw_face);
        dlabdaodsw = lo.*funceval(dkrodsw, sw_face);
        dlabdawdsw = lw.*funceval(dkrwdsw, sw_face);
        labda = labdao+labdaw;
        dlabdadsw = dlabdaodsw+dlabdawdsw;
        % compute [Jacobian] matrices
        Mdiffp1 = diffusionTerm(-labda);
        Mdiffp2 = diffusionTerm(-labdaw);
%         Mconvsw1 = convectionUpwindTerm(-dlabdadsw.*pgrad); % without capillary
        Mconvsw1 = convectionUpwindTerm(-dlabdawdsw.*pgrad-dlabdaodsw.*(pgrad+pcgrad)); % with capillary
        Mconvsw2 = convectionUpwindTerm(-dlabdawdsw.*pgrad);
        [Mtranssw2, RHStrans2] = transientTerm(sw_old, dt, phi);
        % Compute RHS values
        RHSpc1=divergenceTerm(labdao.*pcgrad);
        RHS1 = divergenceTerm((-dlabdawdsw.*pgrad-dlabdaodsw.*(pgrad+pcgrad)).*sw_face); % with capillary
        RHS2 = divergenceTerm(-dlabdawdsw.*sw_face.*pgrad);
        % include boundary conditions
        [Mbcp, RHSbcp] = boundaryCondition(BCp);
        [Mbcsw, RHSbcsw] = boundaryCondition(BCs);
        % Couple the equations; BC goes into the block on the main diagonal
        M = [Mdiffp1+Mbcp Mconvsw1; Mdiffp2 Mconvsw2+Mtranssw2+Mbcsw];
        RHS = [RHS1+RHSpc1+RHSbcp; RHS2+RHStrans2+RHSbcsw];
        % solve the linear system of equations
        x = M\RHS;
        % x = agmg(M, RHS, [], 1e-10, 500, [], [p.value(:); sw.value(:)]);
        % separate the variables from the solution
        p_new = reshapeCell(m,full(x(1:(Nx+2)*(Ny+2))));
        sw_new = reshapeCell(m,full(x((Nx+2)*(Ny+2)+1:end)));
        % calculate error values
        error_p = max(abs((p_new(:)-p.value(:))./p_new(:)));
        error_sw = max(abs(sw_new(:)-sw.value(:)));
        % assign new values of p and sw
        p.value = p_new;
        sw.value = sw_new;
    end
    t=t+dt;
    p_old = p;
    sw_old = sw;
    figure(1);visualizeCells(sw); drawnow;
end


function [] = testUnpackTheta()

visibleDim = 5;  % width/height of input
visibleSize = visibleDim^2;

hiddenDimL1 = 3;  % width/height of hidden layer 1
hiddenSizeL1 = hiddenDimL1^2;
hiddenViewDimL1 = 3;  % width/height of local receptive area for hidden layer 1
hiddenViewSizeL1 = hiddenViewDimL1^2;  % local receptive area size for hidden layer 1
hiddenViewStepL1 = 1;  % step for receptive area moving across input
assert (((hiddenDimL1-1) * hiddenViewStepL1 + hiddenViewDimL1) == visibleDim);

hiddenDimL2 = 2;  % width/height of hidden layer 2
hiddenSizeL2 = hiddenDimL2^2;

hiddenViewDimL2 = 2;  % width/height of local receptive area for hidden layer 2
hiddenViewSizeL2 = hiddenViewDimL2^2;  % local receptive area size for hidden layer 2
hiddenViewStepL2 = 1;  % step for receptive area moving across hidden layer 1
assert (((hiddenDimL2-1) * hiddenViewStepL2 + hiddenViewDimL2) == hiddenDimL1);

[W1, b1] = initializeOneLayerParams(hiddenSizeL1, hiddenViewSizeL1);
[W2, b2] = initializeOneLayerParams(hiddenSizeL2, hiddenViewSizeL2);
[W3, b3] = initializeOneLayerParams(1, hiddenSizeL2);

theta = [W1(:);b1(:);W2(:);b2(:);W3(:);b3(:)];
[tW1, tb1, tW2, tb2, tW3, tb3] = unpackTheta(theta, hiddenSizeL1, hiddenViewSizeL1, hiddenSizeL2, hiddenViewSizeL2);

assert(all(W1(:) == tW1(:)));
assert(all(b1(:) == tb1(:)));
assert(all(W2(:) == tW2(:)));
assert(all(b2(:) == tb2(:)));
assert(all(W3(:) == tW3(:)));
assert(all(b3(:) == tb3(:)));

% Real sizes

visibleDim = 96;  % width/height of input
visibleSize = visibleDim^2;

hiddenDimL1 = 18;  % width/height of hidden layer 1
hiddenSizeL1 = hiddenDimL1^2;
hiddenViewDimL1 = 11;  % width/height of local receptive area for hidden layer 1
hiddenViewSizeL1 = hiddenViewDimL1^2;  % local receptive area size for hidden layer 1
hiddenViewStepL1 = 5;  % step for receptive area moving across input
assert(all(((hiddenDimL1-1) * hiddenViewStepL1 + hiddenViewDimL1) == visibleDim));

hiddenDimL2 = 8;  % width/height of hidden layer 2
hiddenSizeL2 = hiddenDimL2^2;

hiddenViewDimL2 = 11;  % width/height of local receptive area for hidden layer 2
hiddenViewSizeL2 = hiddenViewDimL2^2;  % local receptive area size for hidden layer 2
hiddenViewStepL2 = 1;  % step for receptive area moving across hidden layer 1
assert(all(((hiddenDimL2-1) * hiddenViewStepL2 + hiddenViewDimL2) == hiddenDimL1));

[W1, b1] = initializeOneLayerParams(hiddenSizeL1, hiddenViewSizeL1);
[W2, b2] = initializeOneLayerParams(hiddenSizeL2, hiddenViewSizeL2);
[W3, b3] = initializeOneLayerParams(1, hiddenSizeL2);

theta = [W1(:);b1(:);W2(:);b2(:);W3(:);b3(:)];
[tW1, tb1, tW2, tb2, tW3, tb3] = unpackTheta(theta, hiddenSizeL1, hiddenViewSizeL1, hiddenSizeL2, hiddenViewSizeL2);

assert(all(W1(:) == tW1(:)));
assert(all(b1(:) == tb1(:)));
assert(all(W2(:) == tW2(:)));
assert(all(b2(:) == tb2(:)));
assert(all(W3(:) == tW3(:)));
assert(all(b3(:) == tb3(:)));

end


function [fi,ps] = VF(r,T,p)
% VF Conversion of relative humidity to volume fraction of water vapor
%
% This program transfers relative humidity, absolute temperature and
% atmospheric pressure to volume fraction of water vapor and saturation
% vapour pressure using Goff-Gratch equation over plane surfaces of pure
% ice. Relation is based on integration of Clausius-Clapeyron equation and
% on experimental data. Stated range of equation validity is -100 to 0 °C.
%
% [fi,ps] = VF(r,T,p)
%
% Inputs:
%   r is the relative humidity (%). r may be a vector or a scalar
%     variable in the interval [0,100].
%
%   T means the absolute temperature (degrees K). T must have
%     the same number of elements as r. A warning is generated
%     if any elements fall outside the range [173,273].
%
%   p is the atmospheric pressure (kPa). p must have the same number
%     of elements as r. All elements must be non-negative.
%
% Results:
%   fi is the volume fraction of water vapor (%).
%
%   ps is the saturation vapour pressure (kPa).
%
% Example: At an rh of 35 %, Temp = 265 °K, atmospheric pressure = 98.9 kPa
%  [fi,ps] = VF(0.35,265,98.9)
%
%   fi =
%       0.00108
%
%   ps =
%       0.305
%
% For citation ensue following format: Malec L. (2007). VF: Conversion
% of relative humidity to volume fraction. A MATLAB file. [WWW document].
% URL http://www.mathworks.com/matlabcentral/fileexchange/loadFile.do?
% objectId=14165
%
% List R. J.: Smithsonian Meteorological Tables. Smithsonian Institution,
% Washington 1984.
%
% Copyright. February 26, 2007.

if any(r(:)<0) || any(r(:)>100)
  error 'Relative humidity(r) must be percent'
end
if any(T(:)<=0)
  error 'Temperature(T) must be positive (°K)'
elseif any(T(:)<173) || any(T(:)>273)
  warning 'Temperature(T) fell outside the working range of this relation'
end
if any(p(:)<0)
  error 'Pressure(p) must be positive (kPa)'
end

a1 = -9.09718;
a2 = -3.56654;
a3 = 0.876793;

% Induction of ice point and saturation vapour pressure.
To = 273.16;
eo = 6.1071;

% Saturation vapour pressure and water fraction computation.
ps = 10.^(a1 * (To ./ T - 1) + a2 * log10(To ./ T) + a3 * (1 - T / To) ...
    + log10(eo));
ps = ps / 10;
fi = r .* ps ./ p;


function [px, py, gx, gy] = position_7_link(z,P)
%[px,py,gx,gy] = POSITION_7_LINK(Z,P)
%
%FUNCTION:  This function computes the cartesian positions
%    of the CoM and tip of each link
%INPUTS:
%
%
%OUTPUTS:
%    px = [nLink X nTime] position of the end of each link
%    py = [nLink X nTime] position of the end of each link
%    gx = [nLink X nTime] position of the CoM of each link
%    gy = [nLink X nTime] position of the CoM of each link
%
%NOTES:
%    This file was automatically generated by writePosition.m

g  = P.g ; %gravity
m1 = P.m(1); % Link 1 mass
m2 = P.m(2); % Link 2 mass
m3 = P.m(3); % Link 3 mass
m4 = P.m(4); % Link 4 mass
m5 = P.m(5); % Link 5 mass
m6 = P.m(6); % Link 6 mass
m7 = P.m(7); % Link 7 mass
l1 = P.l(1); % Link 1 length
l2 = P.l(2); % Link 2 length
l3 = P.l(3); % Link 3 length
l4 = P.l(4); % Link 4 length
l5 = P.l(5); % Link 5 length
l6 = P.l(6); % Link 6 length
l7 = P.l(7); % Link 7 length
I1 = P.I(1); % Link 1 moment of inertia about its center of mass
I2 = P.I(2); % Link 2 moment of inertia about its center of mass
I3 = P.I(3); % Link 3 moment of inertia about its center of mass
I4 = P.I(4); % Link 4 moment of inertia about its center of mass
I5 = P.I(5); % Link 5 moment of inertia about its center of mass
I6 = P.I(6); % Link 6 moment of inertia about its center of mass
I7 = P.I(7); % Link 7 moment of inertia about its center of mass
d1 = P.d(1); % Link 1 distance between center of mass and parent joint
d2 = P.d(2); % Link 2 distance between center of mass and parent joint
d3 = P.d(3); % Link 3 distance between center of mass and parent joint
d4 = P.d(4); % Link 4 distance between center of mass and parent joint
d5 = P.d(5); % Link 5 distance between center of mass and parent joint
d6 = P.d(6); % Link 6 distance between center of mass and parent joint
d7 = P.d(7); % Link 7 distance between center of mass and parent joint

th1 = z(1,:);
th2 = z(2,:);
th3 = z(3,:);
th4 = z(4,:);
th5 = z(5,:);
th6 = z(6,:);
th7 = z(7,:);

nTime = length(th1);
px = zeros(7,nTime);
py = zeros(7,nTime);
gx = zeros(7,nTime);
gy = zeros(7,nTime);

px(1,:) = l1.*cos(th1);
py(1,:) = l1.*sin(th1);
gx(1,:) = d1.*cos(th1);
gy(1,:) = d1.*sin(th1);

px(2,:) = l1.*cos(th1) + l2.*cos(th2);
py(2,:) = l1.*sin(th1) + l2.*sin(th2);
gx(2,:) = d2.*cos(th2) + l1.*cos(th1);
gy(2,:) = d2.*sin(th2) + l1.*sin(th1);

px(3,:) = l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3);
py(3,:) = l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3);
gx(3,:) = d3.*cos(th3) + l1.*cos(th1) + l2.*cos(th2);
gy(3,:) = d3.*sin(th3) + l1.*sin(th1) + l2.*sin(th2);

px(4,:) = l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3) + l4.*cos(th4);
py(4,:) = l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3) + l4.*sin(th4);
gx(4,:) = d4.*cos(th4) + l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3);
gy(4,:) = d4.*sin(th4) + l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3);

px(5,:) = l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3) + l4.*cos(th4) + l5.*cos(th5);
py(5,:) = l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3) + l4.*sin(th4) + l5.*sin(th5);
gx(5,:) = d5.*cos(th5) + l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3) + l4.*cos(th4);
gy(5,:) = d5.*sin(th5) + l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3) + l4.*sin(th4);

px(6,:) = l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3) + l4.*cos(th4) + l5.*cos(th5) + l6.*cos(th6);
py(6,:) = l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3) + l4.*sin(th4) + l5.*sin(th5) + l6.*sin(th6);
gx(6,:) = d6.*cos(th6) + l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3) + l4.*cos(th4) + l5.*cos(th5);
gy(6,:) = d6.*sin(th6) + l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3) + l4.*sin(th4) + l5.*sin(th5);

px(7,:) = l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3) + l4.*cos(th4) + l5.*cos(th5) + l6.*cos(th6) + l7.*cos(th7);
py(7,:) = l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3) + l4.*sin(th4) + l5.*sin(th5) + l6.*sin(th6) + l7.*sin(th7);
gx(7,:) = d7.*cos(th7) + l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3) + l4.*cos(th4) + l5.*cos(th5) + l6.*cos(th6);
gy(7,:) = d7.*sin(th7) + l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3) + l4.*sin(th4) + l5.*sin(th5) + l6.*sin(th6);

end


function [cst X A B C] = test05(m, n)
% function [cst X A B C] = test05(m, n)
% All intputs are optional.
%
% Test case for the Euclidean space (classical nonlinear optimization).
% We generate three matrices A, B and C and try to solve the Sylvester
% equation AX + XB = C by minimizing the residual ||AX+XB-C||^2.
%

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Dec. 30, 2012.
% Contributors:
% Change log:

    if ~exist('m', 'var') || isempty(m)
        m = 3;
    end
    if ~exist('n', 'var') || isempty(n)
        n = 10;
    end

    % Create the problem structure
    problem.M = euclideanfactory(m, n);

    % Define the problem data
    A = randn(m);
    B = randn(n);
    C = randn(m, n);

    % Define the problem cost function
    problem.cost = @(X) .5*norm(A*X+X*B-C, 'fro')^2;
    problem.grad = @(X) A'*(A*X+X*B-C) + (A*X+X*B-C)*B';
    problem.hess = @(X, H) A'*(A*H+H*B) + (A*H+H*B)*B';

    % Check differnetials consistency.
    checkgradient(problem);
    checkhessian(problem);

    % Solve
    [X cst] = trustregions(problem);

end


function ndx = subv2ind(siz, subv)
% SUBV2IND Like the built-in sub2ind, but the subscripts are given as row vectors.
% ind = subv2ind(siz,subv)
%
% siz can be a row or column vector of size d.
% subv should be a collection of N row vectors of size d.
% ind will be of size N * 1.
%
% Example:
% subv = [1 1 1;
%         2 1 1;
%         ...
%         2 2 2];
% subv2ind([2 2 2], subv) returns [1 2 ... 8]'
% i.e., the leftmost digit toggles fastest.
%
% See also IND2SUBV.

if isempty(subv)
  ndx = [];
  return;
end

if isempty(siz)
  ndx = 1;
  return;
end

[ncases ndims] = size(subv);

%if length(siz) ~= ndims
%  error('length of subscript vector and sizes must be equal');
%end

if all(siz==2)
  %rbits = subv(:,end:-1:1)-1; % read from right to left, convert to 0s/1s
  %ndx = bitv2dec(rbits)+1;
  twos = pow2(0:ndims-1);
  ndx = ((subv-1) * twos(:)) + 1;
  %ndx = sum((subv-1) .* twos(ones(ncases,1), :), 2) + 1; % equivalent to matrix * vector
  %ndx = sum((subv-1) .* repmat(twos, ncases, 1), 2) + 1; % much slower than ones
  %ndx = ndx(:)';
else
  %siz = siz(:)';
  cp = [1 cumprod(siz(1:end-1))]';
  %ndx = ones(ncases, 1);
  %for i = 1:ndims
  %  ndx = ndx + (subv(:,i)-1)*cp(i);
  %end
  ndx = (subv-1)*cp + 1;
end

%%%%%%%%%%%

function d = bitv2dec(bits)
% BITV2DEC Convert a bit vector to a decimal integer
% d = butv2dec(bits)
%
% This is just like the built-in bin2dec, except the argument is a vector, not a string.
% If bits is an array, each row will be converted.

[m n] = size(bits);
twos = pow2(n-1:-1:0);
d = sum(bits .* twos(ones(m,1),:),2);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% SABR European option pricer comparison
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Descritpion: Script to Compare European Option pricers in SABR Model
% Author:      Justin Kirkby
% References:  (1) General Valuation Framework for SABR and Stochastic Local Volatility
%                   Models. SIAM J. Financial Mathematics, 2018.
%              (2) Full Fledge SABR with Markov Chains. Wimott, 2019.
% Disclaimer: this is research code, not production code. The parameter settings etc
%            should not be expected to work as is in all cases. Determining the right parameter
%            settings will depend on your usecase. This is left up to the user.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[folder, name, ext] = fileparts(which( mfilename('fullpath')));
cd(folder);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  Step 1) CHOOSE CONTRACT/GENERAL PARAMETERS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
T    = 1;         % time to matruity
r    = 0.05;      % interest rate
F_0  = 1.1;       % Futures price
K    = F_0*0.95;  % strike
call = 1;         % 1 = call option, else put

% -----------------
% Model Params
% -----------------
ModParams.beta   = .6;
ModParams.alpha  = 0.08;
ModParams.v0     = 0.2;
ModParams.rho    = 0.0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  Reference Price (Using Obloj Asymptotic Formula with correction)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
addpath('../../Approx/SABR')
addpath('../../Analytical/BlackScholes')

tic
IV = SABR_Hagan_Obloj_ImpliedVol( K, F_0, ModParams.v0, T, ModParams.alpha, ModParams.beta, ModParams.rho );
price_obloj = BSM_Greeks( 0, F_0/exp(r*T), IV, r, 0, T, K, call)
time_oblog=toc;

price_ref = price_obloj;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  Antonov Approximation (Using Asymptotic Formula)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
tic
price_ant = SABR_European_AntonovApprox(F_0,K,T,call,r,ModParams)
time_ant = toc;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  Monte Carlo (Euler)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
addpath('../../Monte_Carlo')
addpath('../../Monte_Carlo/European')
M = 400;
N_sim = 5*10^4;

tic;
Spath = Simulate_SLV_func( N_sim, M, T, F_0, 0, 0, 2, ModParams);
disc = exp(-r*T);
[price_MC, stdErr] = Price_MC_European_Strikes_func(Spath, disc, call, K );

price_MC_L = price_MC - 2*stdErr; price_MC_U = price_MC + 2*stdErr;
time_MC = toc;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  CTMC-Double Layer Method
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
addpath('../../PROJ/SABR/European_American_Barrier/')
addpath('../../PROJ/SABR/Helper_Functions/')

% -----------------
% Numerical Params
% -----------------
M    = 100;   % number of time steps
contract_type = 1;    % 1 = European, 2 = American, 3 = Down and Out Barrier
L    = 0.6*F_0;     % For barrier contract, this is the barrier

CTMCParams.m_0 = 26;
CTMCParams.N = 90;
CTMCParams.gridMult_v = 0.5;
CTMCParams.gridMult_s = 0.05;  %Grid mult param for S
CTMCParams.gamma = 6;  %Grid width param for variance grid

tic
price_ctmc = SABR_EurBarAmer_func(call, M, T, F_0, K, r, CTMCParams, ModParams, contract_type, L)
time_ctmc = toc;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% COMPARE
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('\n---------------------------------------------\n')
fprintf('Method      |    Price    |    Err   |  CPU \n')
fprintf('---------------------------------------------\n')
fprintf('Ref (Obloj) | %.8f  |          |       \n', price_ref)
fprintf('---------------------------------------------\n')
fprintf('Antonov     | %.8f  | %.2e | %.4f \n', price_ant, abs(price_ref-price_ant), time_ant)
fprintf('CTMC        | %.8f  | %.2e | %.4f \n', price_ctmc, abs(price_ref-price_ctmc), time_ctmc)
fprintf('MC-Euler    |[%.3f,%.3f]| %.2e | %.4f \n', price_MC_L, price_MC_U, abs(price_ref-price_MC), time_MC)
fprintf('---------------------------------------------\n')


function y = smoothRamp(x,alpha,order)
%
% y = smoothRamp(x,alpha,order)
%
% FUNCTION:
%This function is a smooth version of the standard ramp function, which is:
%   ramp(x) = 0 ; (x<0)
%   ramp(x) = x ; (x>=0)
% Smoothing is done using a polynomial transition, the degree of smoothness
% at the transitions is set using order.
%
% INPUTS:
%   x = a vector of matrix of real numbers
%   alpha = smoothing parameter: -alpha<x<alpha will be smoothed
%   order = the order of the smoothing (0=no smoothing)
%           order must be in the set {0,1,..,8}
%
% OUTPUTS:
%   y(x<-alpha) = 0;
%   y(x>alpha) = x;
%   y(else) = smooth_transition
%
        y=x;
        c1 = x < -alpha;
        c2 = x > alpha;
switch order
    case 0 % No Smoothing
        y(x<0)=0;
    case 1
        y=x;
        y(c1)=0;
        p = [1/4,1/2,1/4];
        idx = ~c1&~c2;
        y(idx)=alpha*polyval(p,x(idx)/alpha);
    case 2
        y=x;
        y(c1)=0;
        p = [-1/16,0,3/8,1/2,3/16];
        idx = ~c1&~c2;
        y(idx)=alpha*polyval(p,x(idx)/alpha);
    case 3
        y=x;
        y(c1)=0;
        p = [1/32,0,-5/32,0,15/32,1/2,5/32];
        idx = ~c1&~c2;
        y(idx)=alpha*polyval(p,x(idx)/alpha);
    case 4
        y=x;
        y(c1)=0;
        p = [-5/256,0,7/64,0,-35/128,0,35/64,1/2,35/256];
        idx = ~c1&~c2;
        y(idx)=alpha*polyval(p,x(idx)/alpha);
    case 5
        y=x;
        y(c1)=0;
        p = [7/512,0,-45/512,0,63/256,0,-105/256,0,315/512,1/2,63/512];
        idx = ~c1&~c2;
        y(idx)=alpha*polyval(p,x(idx)/alpha);
    case 6
        y=x;
        y(c1)=0;
        p = [-21/2048,0,77/1024,0,-495/2048,0,231/512,0,-1155/2048,0,693/1024,1/2,231/2048];
        idx = ~c1&~c2;
        y(idx)=alpha*polyval(p,x(idx)/alpha);
    case 7
        y=x;
        y(c1)=0;
        p = [33/4096,0,-273/4096,0,1001/4096,0,-2145/4096,0,3003/4096,0,-3003/4096,0,3003/4096,1/2,429/4096];
        idx = ~c1&~c2;
        y(idx)=alpha*polyval(p,x(idx)/alpha);
    case 8
        y=x;
        y(c1)=0;
        p = [-429/65536,0,495/8192,0,-4095/16384,0,5005/8192,0,-32175/32768,0,9009/8192,0,-15015/16384,0,6435/8192,1/2,6435/65536];
        idx = ~c1&~c2;
        y(idx)=alpha*polyval(p,x(idx)/alpha);
    otherwise
        error('Order not supported');
end
end


function upsilon = lfmjpComputeUpsilonVector(gamma, sigma2, t)

% LFMJPCOMPUTEUPSILONVECTOR Upsilon vector jolt. pos. with t1, t2 limits
% FORMAT
% DESC computes a portion of the LFM kernel.
% ARG gamma : Gamma value for system.
% ARG sigma2 : length scale of latent process.
% ARG t1 : first time input (number of time points x 1).
% ARG t2 : second time input (number of time points x 1).
% derivative wrt t1, mode 1 derivative wrt t2)
% RETURN upsilon : result of this subcomponent of the kernel for the given values.
%
% COPYRIGHT : Mauricio Alvarez, 2010

% KERN

sigma = sqrt(sigma2);

upsilon = gamma^2*lfmvpComputeUpsilonVector(gamma, sigma2, t) ...
    + (4/(sqrt(pi)*sigma^3))*exp(-(t.^2)./sigma2).*(t.*(gamma + 2*t/sigma2) - 1);


function [ lines_ali ] = refitLineSegmentB( lines, vp, vpweight )
%REFITLINESEGMENTA refit direction of line segments
%   lines: original line segments
%   vp: vannishing point
%   vpweight: if set to 0, lines will not change; if set to inf, lines will
%   be forced to pass vp
if ~exist('vpweight', 'var')
    vpweight = 0.1;
end

numSample = 100;
numLine = size(lines,1);
xyz = zeros(numSample+1,3);
wei = ones(numSample+1,1); wei(numSample+1) = vpweight*numSample;
lines_ali = lines;
for i = 1:numLine
    n = lines(i,1:3);
    sid = lines(i,5)*2*pi;
    eid = lines(i,6)*2*pi;
    if eid<sid
        x = linspace(sid,eid+2*pi,numSample);
        x = rem(x,2*pi);
%         x = sid-1:(eid-1+numBins);
%         x = rem(x,numBins) + 1;
    else
        x = linspace(sid,eid,numSample);
    end
%     u = -pi + (x'-1)*uBinSize + uBinSize/2;
    u = -pi + x';
    v = computeUVN(n, u, lines(i,4));
    xyz(1:numSample,:) = uv2xyzN([u v], lines(i,4));
    xyz(numSample+1,:) = vp;
    [ ~, outputNM ] = curveFitting( xyz, wei );
    lines_ali(i,1:3) = outputNM;
end

end


function [SegLabel,NcutDiscrete,NcutEigenvectors,NcutEigenvalues,W,imageEdges]= NcutImage(I,nbSegments);
%  [SegLabel,NcutDiscrete,NcutEigenvectors,NcutEigenvalues,W,imageEdges]= NcutImage(I);
%  Input: I = brightness image
%         nbSegments = number of segmentation desired
%  Output: SegLable = label map of the segmented image
%          NcutDiscrete = Discretized Ncut vectors
%
% Timothee Cour, Stella Yu, Jianbo Shi, 2004.

if nargin <2,
   nbSegments = 10;
end

[W,imageEdges] = ICgraph(I);

[NcutDiscrete,NcutEigenvectors,NcutEigenvalues] = ncutW(W,nbSegments);

%% generate segmentation label map
[nr,nc,nb] = size(I);

SegLabel = zeros(nr,nc);
for j=1:size(NcutDiscrete,2),
    SegLabel = SegLabel + j*reshape(NcutDiscrete(:,j),nr,nc);
end


function simPrices = simulatePrices(expReturns, expCovariances, ...
    price0, allTimes)
% SIMULATEPRICES: calculate Monte Carlo prices for a set of correlated
% assets
%
% INPUTS:
% expReturn: a 1-by-nAssets vector of expected returns
% expCovariances: a nAssets-by-nAssets matrix of expected covariances
% price0: a 1-by-nAssets of initial asset prices
% allTimes: an nTimes-by-1 vector of integer times for which we need to
%   calculate the prices (ex. [30 90 180]' for 1-, 3-, and 6-month prices)
% nSims: a scalar integer for the number of Monte Carlo paths generated
%
% OUTPUT:
% simPrices: an nTimes-by-nAssets matrix of the simulated prices at the
% requested times.
%
% The variables expReturns, expCovariances, and allTimes share the same
% units-- if allTimes is in units of days, then the returns and covariances
% must be _daily_ moments.

% Calculates correlated, multivariate, normally-distributed numbers.  Uses
% Statistics Toolbox.
simReturns = mvnrnd(expReturns, expCovariances, max(allTimes));

% Convert simple returns to prices
simPrices = cumprod([price0; simReturns+1]);

% Downsample to only the times requested.  The first row is t==0 (i.e.:
% price0)
simPrices = simPrices(allTimes+1, :);


function pi = predlssvm(model,Xt,alpha,conftype)

% Construction of bias corrected 100(1-\alpha)% pointwise or
% simultaneous prediction intervals
%
% >> pi = predlssvm({X,Y,type,gam,kernel_par,kernel,preprocess}, Xt, alpha,conftype)
% >> pi = predlssvm(model, Xt, alpha, conftype)
%
% This function calculates bias corrected 100(1-\alpha)% pointwise or
% simultaneous prediction intervals. The procedure support homoscedastic
% data sets as well heteroscedastic data sets. The construction of the
% prediction intervals are based on the central limit theorem for linear
% smoothers combined with bias correction and variance estimation.
%
% 1. Using the functional interface:
%
%
% >> pi = predlssvm({X,Y,type,gam,kernel_par,kernel,preprocess})
% >> pi = predlssvm({X,Y,type,gam,kernel_par,kernel,preprocess}, alpha)
% >> pi = predlssvm({X,Y,type,gam,kernel_par,kernel,preprocess}, alpha, conftype)
%
%
%      Outputs
%        pi            : N x 2 matrix containing the lower and upper prediction intervals
%
%      Inputs
%        X             : N x d matrix with the inputs of the training data
%        Y             : N x 1 vector with the outputs of the training data
%        type          : 'function estimation' ('f') or 'classifier' ('c')
%        gam           : Regularization parameter
%        sig2          : Kernel parameter(s) (bandwidth in the case of the 'RBF_kernel')
%        kernel(*)     : Kernel type (by default 'RBF_kernel')
%        preprocess(*) : 'preprocess'(*) or 'original'
%        alpha(*)      : Significance level (by default 5%)
%        conftype(*)   : Type of prediction interval 'pointwise' or 'simultaneous' (by default 'simultaneous')
%
% 2. Using the object oriented interface:
%
%
% >> pi = predlssvm(model)
% >> pi = predlssvm(model, alpha)
% >> pi = predlssvm(model, alpha, conftype)
%
%
%      Outputs
%        pi          : N x 2 matrix containing the lower and upper confidence intervals
%
%      Inputs
%        model       : Object oriented representation of the LS-SVM model
%        alpha       : Significance level (by default 5%)
%        conftype    : Type of prediction interval 'pointwise' or 'simultaneous' (by default 'simultaneous')
%
%
%  See also:
%    trainlssvm, simlssvm, cilssvm

% Copyright (c) 2011,  KULeuven-ESAT-SCD, License & help @ http://www.esat.kuleuven.be/sista/lssvmlab

if iscell(model)
    model = initlssvm(model{:});
end

if nargin == 1
    error('Please specify a test set');
elseif nargin <= 2
    alpha = 0.05;
    conftype = 'simul';

elseif nargin <= 3
    conftype = 'simul';
end

if model.preprocess(1)=='p'
    [x,y] = postlssvm(model,model.xtrain,model.ytrain);
else
    x = model.xtrain; y = model.ytrain;
end

% train model
model = trainlssvm(model);

% smoother on test
S = smootherlssvm(model,Xt);

% bias: double smoothing with fourt order kernel RBF4
modelb = initlssvm(x,y,'f',[],[],'RBF4_kernel','o');
modelb = tunelssvm(modelb,'simplex','crossvalidatelssvm',{10,'mse'});
modelb = trainlssvm(modelb);

% output on test Yphat
Yphat = simlssvm(modelb,Xt);

% output of the smoother based on fourth order kernel on training data Yhat
Yhat = simlssvm(modelb,x);

biascorr = S*Yhat-Yphat;

%1) estimate variance nonparametrically
[sigma2,modele] = varest(model);
% smoother on training data
L = smootherlssvm(model);
sigma2t = varfun(modele,L,Xt);

%2) calculate var-cov matrix
S = S*diag(sigma2)*S';

%2b) find standardized absolute maxbias
delta = max(abs(biascorr./sqrt(diag(S))));

%3) pointwise or simultaneous
if conftype(1)=='s'
    if model.preprocess(1)=='p'
        model.xtrain = prelssvm(model,x);
    end
    z = tbform(model,alpha) + delta;
elseif conftype(1)=='p'
    z = norminv(alpha/2);
    Yphat = Yphat - biascorr;
else
    error('Wrong type of confidence interval. Please choose ''pointwise'' or ''simultaneous''');
end

V = sqrt(sigma2t+diag(S));
pi = [Yphat+z*V Yphat-z*V];

function V = varfun(modele,L,xt)
% estimate variance op new data, given the smooth of squared residuals
% "modele" and smoother matrix of the original smooth L
V = max(simlssvm(modele,xt),0);
% make estimate of V unbiased in homoscedastic case if regression
% estimate is unbiased
S = smootherlssvm(modele,xt);
V = V./(ones(size(xt,1),1)+S*diag(L*L'-2*L));

function [var,modele] = varest(model)

% if preprocessed data, construct original data
if model.preprocess(1)=='p'
    [x,y] = postlssvm(model,model.xtrain,model.ytrain);
else
    x = model.xtrain; y = model.ytrain;
end

model = trainlssvm(model);

Yh = simlssvm(model,x);

% Squared normalized residuals
e2 = (y-Yh).^2;

% Make variance model
if model.nb_data <= 200
    costfun = 'leaveoneoutlssvm'; costargs = {'mae'};
else
    costfun = 'crossvalidatelssvm'; costargs = {10,'mae'};
end
modele = initlssvm(x,e2,'f',[],[],'RBF_kernel');
modele = tunelssvm(modele,'simplex',costfun,costargs);
modele = trainlssvm(modele);

% variance model
var = max(simlssvm(modele,x),0);

% make estimate of Var unbiased in homoscedastic case if regression
% estimate is unbiased
L = smootherlssvm(model);
S = smootherlssvm(modele);

var = var./(ones(size(x,1),1)+S*diag(L*L'-L-L'));


% VL_DEMO_FRAME  Demonstrates vl_plotframe and vl_frame2oell

%% A standard frame is a unit circle centered at the origin with a
% radius pointing in the positive Y direction. Since images use a
% left-handed coordinate system (X axis pointing right, Y axis
% pointing downwards), the radius will point downwards.

A = eye(2) ;
T = [0;0] ;

figure(1) ; clf ;
vl_plotframe([T ; A(:)], 'b', 'linewidth', 5) ; hold on ;

% The same frame can be specified as 4 numbers: translation, scale
% (unit), and rotation (zero).
vl_plotframe([T ; 1 ; 0]) ;

axis equal ; set(gca,'ydir','reverse') ; grid on ;
vl_figaspect(1) ;
vl_demo_print(1, 'frame_standard') ;

%% A positive rotation is appears clockwise in the standard image
% reference frame.

angle = pi/4 ;
R = [cos(angle), -sin(angle) ; sin(angle) cos(angle)] ;
A = R ;

figure(2) ; clf ;
vl_plotframe([T ; A(:)], 'b', 'linewidth', 5) ; hold on ;

% The same frame can be specified as 4 numbers: translation, scale and
% rotation.
vl_plotframe([T ; 1 ; angle]) ;

axis equal ; set(gca,'ydir','reverse') ; grid on ;
vl_figaspect(1) ;
vl_demo_print(2, 'frame_rotated') ;

%% A more complex example, involving a generic affine
% frame. The frame is the result of applying a given affine
% transformation to the standard frame.

angle = pi/4 ;
R = [cos(angle), -sin(angle) ; sin(angle) cos(angle)] ;
S = [1 0 ; 0 .5] ;
A = R*S*R ;

figure(3) ; clf ;
vl_plotframe([T ; A(:)], 'b', 'linewidth', 3) ; hold on ;

axis equal ; set(gca,'ydir','reverse') ; grid on ;
vl_figaspect(1) ;
vl_demo_print(3, 'frame_affine') ;

%% An oriented ellipse can be easily transformed in an unoriented
% one by removing the orientation. The opposite transformation is
% possible as well, provided that a conventional orientation is
% assigned to it.

clear frames1 frames2 ;
randn('state',0) ;
rand('state',0) ;
for i = 1 : 5
  % sample a random ellipse
  angle1 = rand * 2*pi;
  angle2 = rand * 2*pi;
  R1 = [cos(angle1), -sin(angle1) ; sin(angle1) cos(angle1)] ;
  R2 = [cos(angle2), -sin(angle2) ; sin(angle2) cos(angle2)] ;
  A = 0.3*R1*diag(rand(2,1)+0.5)*R2 ;
  frames1(:,i) = [i ; 0 ; A(:)] ;

  % get the unoriented variant of the same ellipse
  S = A*A' ;
  frames2(:,i) = [i ; 0 ; S(1,1) ; S(1,2) ; S(2,2)] ;
end

% Recover oriented ellipses from the unoriented ones; note that
% vl_frame2oell assigns all frames an upright orientation.

frames3 = vl_frame2oell(frames2) ;

frames1(2,:) = 0 ;
frames2(2,:) = 1 ;
frames3(2,:) = 2 ;

figure(4) ; clf ;
vl_plotframe(frames1,'r') ;
vl_plotframe(frames2,'g') ;
vl_plotframe(frames3,'b') ;

axis equal tight ; set(gca,'ydir','reverse') ; grid on ;
vl_figaspect(5/3) ;
vl_demo_print(4, 'frame_frame2oell') ;


function histo_gram = r8vec_histogram ( n, a, a_lo, a_hi, histo_num )

%*****************************************************************************80
%
%% R8VEC_HISTOGRAM computes a histogram of the elements of an R8VEC.
%
%  Discussion:
%
%    Values between A_LO and A_HI will be histogrammed into the bins
%    1 through HISTO_NUM.  Values below A_LO are counted in bin 0,
%    and values greater than A_HI are counted in bin HISTO_NUM+1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 September 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of elements of A.
%
%    Input, real A(N), the array to examine.
%
%    Input, real A_LO, A_HI, the lowest and highest
%    values to be histogrammed.  These values will also define the bins.
%
%    Input, integer HISTO_NUM, the number of bins to use.
%
%    Output, integer HISTO_GRAM(0:HISTO_NUM+1), contains the number of
%    entries of A in each bin.
%
  histo_gram(1:histo_num+2) = 0;

  delta = ( a_hi - a_lo ) / ( 2 * histo_num );

  for i = 1 : n

    if ( a(i) < a_lo )

      histo_gram(1) = histo_gram(2) + 1;

    elseif ( a(i) <= a_hi )

      j = round ( ...
        ( ( a_hi -       delta - a(i)        ) * ( 1         )   ...
        + (      -       delta + a(i) - a_lo ) * ( histo_num ) ) ...
        / ( a_hi - 2.0 * delta        - a_lo ) );

      histo_gram(j+1) = histo_gram(j+1) + 1;

    elseif ( a_hi < a(i) )

      histo_gram(histo_num+2) = histo_gram(histo_num+2) + 1;

    end

  end

  return
end


function I = undistort(Idistorted, params)
fx = params.fx;
fy = params.fy;
cx = params.cx;
cy = params.cy;
k1 = params.k1;
k2 = params.k2;
p1 = params.p1;
p2 = params.p2;

Idistorted = single(Idistorted);

K = [fx 0 cx; 0 fy cy; 0 0 1];

I = zeros(size(Idistorted));
[i j] = find(~isnan(I));

% Xp = the xyz vals of points on the z plane
Xp = inv(K)*[j i ones(length(i),1)]';

% Now we calculate how those points distort i.e forward map them through the distortion
r2 = Xp(1,:).^2+Xp(2,:).^2;
x = Xp(1,:);
y = Xp(2,:);

x = x.*(1+k1*r2 + k2*r2.^2) + 2*p1.*x.*y + p2*(r2 + 2*x.^2);
y = y.*(1+k1*r2 + k2*r2.^2) + 2*p2.*x.*y + p1*(r2 + 2*y.^2);

% u and v are now the distorted cooridnates
u = reshape(fx*x + cx,size(I));
v = reshape(fy*y + cy,size(I));

% Now we perform a backward mapping in order to undistort the warped image coordinates
I = interp2(Idistorted, u, v);

end


function [ Q ] = vec2Qr( vec )
phi = -vec(1:3);
rho = -vec(4:6);

ph = norm(phi);
ph2 = ph*ph;
ph3 = ph2*ph;
ph4 = ph3*ph;
ph5 = ph4*ph;

cph = cos(ph);
sph = sin(ph);

rx = hat(rho);
px = hat(phi);

t1 = 0.5 * rx;
t2 = ((ph - sph)/ph3) * (px*rx + rx*px + px*rx*px);
m3 = ( 1 - 0.5 * ph2 - cph ) / ph4;
t3 = -m3 * (px*px*rx + rx*px*px - 3*px*rx*px);
m4 = 0.5 * ( m3 - 3*(ph - sph - ph3/6)/ph5 );
t4 = -m4 * (px*rx*px*px + px*px*rx*px);

Q = t1 + t2 + t3 + t4;

end


function M = Q2M(Q)
% Generate a rotation matrix from a quaternion xi+yj+zk+w,
% where Q = [x y z], and w = 1-x^2-y^2-z^2.
% See: http://skal.planet-d.net/demo/matrixfaq.htm
% _______________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

%
% $Id$

Q = Q(1:3); % Assume rigid body
w = sqrt(1 - sum(Q.^2));
x = Q(1); y = Q(2); z = Q(3);
if w<1e-7,
    w = 1/sqrt(x*x+y*y+z*z);
    x = x*w;
    y = y*w;
    z = z*w;
    w = 0;
end;
xx = x*x; yy = y*y; zz = z*z; ww = w*w;
xy = x*y; xz = x*z; xw = x*w;
yz = y*z; yw = y*w; zw = z*w;
M = [...
(xx-yy-zz+ww)      2*(xy-zw)      2*(xz+yw) 0
    2*(xy+zw) (-xx+yy-zz+ww)      2*(yz-xw) 0
    2*(xz-yw)      2*(yz+xw) (-xx-yy+zz+ww) 0
           0              0              0  1];
return;


function g_nm = conjCoeffs(f_nm)
%CONJCOEFFS Get the complex SH coefficients of a conjugate sph. function
%
%   f_nm:   (N+1)^2 coefficients of original spherical function f(\theta,\phi)
%
%   g_nm:   (N+1)^2 coefficients of conjugate spherical function
%               g(\theta,\phi) = (f(\theta,\phi))*
%
%   The conversion is based on the relation Y^*_{nm} = (-1)^m Y_{n(-m)}
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%   Archontis Politis, 10/10/2013
%   archontis.politis@aalto.fi
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% order
N = sqrt(length(f_nm)) - 1;

g_nm = zeros((N+1)^2, 1);
for n=0:N
    for m=-n:n
        q_g = n*(n+1)+m;
        q_f = n*(n+1)-m;
        g_nm(q_g+1) = (-1)^m * conj(f_nm(q_f+1));
    end
end

end


function u = sphs(Pf,K,t)
% Schroeder-phased harmonic sequence
% Pf = 1; % Fundamental period
% K = 10;
% u = zeros(size(time_test));
u = zeros(size(t));
for i = 1:K
    theta = 2*pi/K * sum([1:i]);
    u = u + sqrt(2/(K))*cos(2*pi * i*t/Pf + theta) ;
end


%% Orientation Dependent Functions
%
%%
% An orientation dependent function is a function that assigns to each
% rotation or orientation a numerical value. An import example of a
% rotational function is the <ODFTheory.html orientation density function
% (ODF)> that assignes to each crystal orientation the probability of its
% occurence within a specimen. Other examples are the Schmidt or the Taylor
% factor as a function of the crystal orientation.
%
%% Definition of a orientation dependent function
%
% Within MTEX a rotational function is represented by a variable of type
% <SO3Fun.SO3Fun.html |SO3Fun|>. Let us consider as an example the function
% that takes an orientation and returns it rotational angle modulo cubic
% crystal symmetry. In MTEX the rotational angle is computed by the command
% <orientation.angle.html |angle(ori)|>. In order to turn this
% correspondence into a |SO3Fun| we use the command @SO3FunHandle and pass
% the angle command as an
% <https://de.mathworks.com/help/matlab/matlab_prog/anonymous-functions.html
% anonymous function>.

% define the crystal symmetry
cs = crystalSymmetry('cubic');

% construct the SO3Fun
SO3F = SO3FunHandle(@(ori) angle(ori) ./ degree, cs)

%%
% Many more methods for defining orientation dependent functions are
% discussed <SO3FunDefinition.html here>.
%
%%
% The entire information abot the orientation dependent function is now
% stored in the variable |SO3F|. In order to determine its value for a
% specific orientation |ori| the function <SO3Fun.eval.html |eval(ori)|> is
% used

ori = orientation.rand(cs)
SO3F.eval(ori)

%% Plotting an orientation Dependent Function
%
% Orientation dependent functions are most of visualized by sections
% according to the third Euler angle $\varphi_2$
%

plotSection(SO3F)

%%
% The plot tells us for which Euler angles the the resulting rotational
% angle is large and for which Euler angles it is low. The plot of this
% "angle function" |SO3F| becomes trivial if represented in an axis angle
% sections

plotSection(SO3F,'axisAngle','upper')
mtexColorbar
mtexColorMap parula

%%
% as obviously, the function value is constant in each section.
% Many more methods for visualizing orientation dependent functions are
% discussed <SO3FunVisualization.html here>.
%
%% Computing with orientation dependent functions
%
% The power of representing an orientation dependent functions as a
% variables of type @SO3Fun is that we may apply to it a
% <SO3FunOperations.html large number of analysis tools>. In particular,
% one can add, subtract and mutiply orientation dependent functions, plot
% them in various projections or detect the local minima or maxima. In the
% case of our example function the local maxima refers to the orientations
% with maximum rotational angle in cubic symmetry. We may compute them by
% the command <SO3Fun.max.html |max|>

[value,ori] = max(SO3F,'numLocal',10)

%%
% We observe that there are exactly six symetrically not equivalent
% orientations that realize an orientation angle of about 62.77 degree and
% form the vertices of the fundamental region in orienation space

plot(ori.symmetrise,'axisAngle','filled','markerSize',15,'restrict2FundamentalRegion')

%% Representations of Rotational Functions
%
% Internally MTEX represents rotational functions in different ways:
%
% || by a harmonic series expansion || <SO3FunHarmonicRepresentation.html SO3FunHarmonic> ||
% || as superposition of radial function || <RadialODFs.html SO3FunRBF> ||
% || as superposition of fibre elements || <FibreODFs.html SO3FunCBF> ||
% || as Bingham distribution || <BinghamODFs.html SO3FunBingham> ||
% || as sum of different components || @SO3FunComposition ||
% || explicitely given by a formula || @SO3FunHandle ||
%
% All representations allow the same operations which are specified for
% the abstact class |@SO3Fun|. In particular it is possible
% to calculate with $SO(3)$ functions as with ordinary numbers, i.e., you
% can add, multiply arbitrary functions, take the mean, integrate them or
% compute gradients, see <SO3FunOperations.html Operations>.
%
%% Generalizations of Rotational Functions
%
% || rotational vector fields || <SO3FunVectorField.html SO3VectorField> ||
% || radial rotational functions || <SO3Kernels.html SO3Kernel> ||
%