function triangulation_plot ( prefix, node_show, triangle_show )

%*****************************************************************************80
%
%% TRIANGULATION_PLOT is the main program.
%
%  Discussion:
%
%    TRIANGULATION_PLOT plots a triangulated set of nodes.
%
%  Usage:
%
%    triangulation_plot prefix node_vis triangle_vis
%
%    where:
%
%    'prefix' is the common prefix for the node and triangle files:
%
%    * prefix_nodes.txt,     the node coordinates.
%    * prefix_elements.txt,  the nodes that make up each triangle.
%    * prefix.eps, the plot of the triangulation.
%
%    'node_vis' indicates the node visibility:
%
%    0: do not show the nodes;
%    1:        show the nodes;
%    2:        show the nodes, and label them.
%
%    'triangle_vis' indicates the triangle visibility:
%
%    0: do not show the triangles;
%    1:        show the triangles;
%    2:        show the triangles, and label them.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 October 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  timestamp ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TRIANGULATION_PLOT\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Read a node dataset of NODE_NUM points in 2 dimensions.\n' );
  fprintf ( 1, '  Read an associated triangulation dataset of TRIANGLE_NUM\n' );
  fprintf ( 1, '  triangles using 3, 4 or 6 nodes.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Make an EPS plot of the triangulated data.\n' );
%
%  First argument is the file prefix.
%
  if ( nargin < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TRIANGULATION_PLOT:\n' );
    prefix = input ( '  Enter the file prefix:  ' );
  end
%
%  Second argument is node visibility.
%
  if ( nargin < 2 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Options for node visibility:\n' );
    fprintf ( 1, '  0: do not show the nodes;\n' );
    fprintf ( 1, '  1:        show the nodes;\n' );
    fprintf ( 1, '  2:        show the nodes, and label them.\n' );
    node_show = input ( '  Enter the node visibility option:  ' );
  end
%
%  Third argument is triangle visibility.
%
  if ( nargin < 3 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Options for triangle visibility:\n' );
    fprintf ( 1, '  0: do not show the triangles;\n' );
    fprintf ( 1, '  1:        show the triangles;\n' );
    fprintf ( 1, '  2:        show the triangles, and label them.\n' );
    triangle_show = input ( '  Enter the triangle visibility option:  ' );
  end
%
%  Create the file names.
%
  node_filename = strcat ( prefix, '_nodes.txt' );
  element_filename = strcat ( prefix, '_elements.txt' );
  plot_filename = strcat ( prefix, '.eps' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Node file is "%s".\n', node_filename );
  fprintf ( 1, '  Element file is "%s".\n', element_filename );
  fprintf ( 1, '  Plot file is "%s".\n', plot_filename );
%
%  Read the node data.
%
  [ dim_num, node_num ] = r8mat_header_read ( node_filename );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Read the header of "%s".\n', node_filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Spatial dimension DIM_NUM = %d\n', dim_num );
  fprintf ( 1, '  Number of nodes NODE_NUM  = %d\n', node_num );

  node_xy = r8mat_data_read ( node_filename, dim_num, node_num );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Read the data in "%s".\n', node_filename );

  r8mat_transpose_print_some ( dim_num, node_num, node_xy, 1, 1, 2, 5, ...
    '  Initial portion of data read from file:' );
%
%  Read the element data.
%
  [ triangle_order, triangle_num ] = i4mat_header_read ( element_filename );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Read the header of "%s".\n', element_filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Triangle order TRIANGLE_ORDER     = %d\n', triangle_order );
  fprintf ( 1, '  Number of triangles TRIANGLE_NUM  = %d\n', triangle_num );

  triangle_node = i4mat_data_read ( element_filename, ...
    triangle_order, triangle_num );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Read the data in "%s".\n', element_filename );

  i4mat_transpose_print_some ( triangle_order, triangle_num, triangle_node, ...
    1, 1, triangle_order, 5, '  Initial portion of data read from file:' );
%
%  Detect and correct 0-based indexing.
%
  triangle_node = mesh_base_one ( node_num, triangle_order, triangle_num, ...
    triangle_node );
%
%  Create the output file.
%
  if ( triangle_order == 3 )

    triangulation_order3_plot ( plot_filename, node_num, node_xy, ...
      triangle_num, triangle_node, node_show, triangle_show );

  elseif ( triangle_order == 4 )

    triangulation_order4_plot ( plot_filename, node_num, node_xy, ...
      triangle_num, triangle_node, node_show, triangle_show );

  elseif ( triangle_order == 6 )

    triangulation_order6_plot ( plot_filename, node_num, node_xy, ...
      triangle_num, triangle_node, node_show, triangle_show );

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Created the EPS file "%s".\n', plot_filename );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TRIANGULATION_PLOT\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end
function column_num = file_column_count ( input_file_name )

%*****************************************************************************80
%
%% FILE_COLUMN_COUNT counts the columns in the first line of a file.
%
%  Discussion:
%
%    The file is assumed to be a simple text file.
%
%    Most lines of the file are presumed to consist of COLUMN_NUM words,
%    separated by spaces.  There may also be some blank lines, and some
%    comment lines, which have a "#" in column 1.
%
%    The routine tries to find the first non-comment non-blank line and
%    counts the number of words in that line.
%
%    If all lines are blanks or comments, it goes back and tries to analyze
%    a comment line.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILE_NAME, the name of the file.
%
%    Output, integer COLUMN_NUM, the number of columns in the file.
%
  FALSE = 0;
  TRUE = 1;
%
%  Open the file.
%
  input_unit = fopen ( input_file_name );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_COLUMN_COUNT - Error!\n' );
    fprintf ( 1, '  Could not open the file "%s".\n', input_file_name );
    error ( 'FILE_COLUMN_COUNT - Error!' );
  end
%
%  Read one line, but skip blank lines and comment lines.
%  Use FGETL so we drop the newline character!
%
  got_one = FALSE;

  while ( 1 )

    line = fgetl ( input_unit );

    if ( line == -1 )
      break;
    end

    if ( s_len_trim ( line ) == 0 )

    elseif ( line(1) == '#' )

    else
      got_one = TRUE;
      break;
    end

  end

  fclose ( input_unit );

  if ( got_one == FALSE )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_COLUMN_COUNT - Warning!\n' );
    fprintf ( 1, '  The file does not seem to contain any data.\n' );
    column_num = -1;
    return;
  end

  column_num = s_word_count ( line );

  return
end
function row_num = file_row_count ( input_file_name )

%*****************************************************************************80
%
%% FILE_ROW_COUNT counts the number of row records in a file.
%
%  Discussion:
%
%    Each input line is a "RECORD".
%
%    The records are divided into three groups:
%
%    * BLANK LINES (nothing but blanks)
%    * COMMENT LINES (begin with a '#')
%    * DATA RECORDS (anything else)
%
%    The value returned by the function is the number of data records.
%
%    By the way, if the MATLAB routine FGETS is used, instead of
%    FGETL, then the variable LINE will include line termination
%    characters, which means that a blank line would not actually
%    have zero characters.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 December 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILE_NAME, the name of the input file.
%
%    Output, integer ROW_NUM, the number of rows found.
%
  input_unit = fopen ( input_file_name );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_ROW_COUNT - Error!\n' );
    fprintf ( 1, '  Could not open the file "%s".\n', input_file_name );
    error ( 'FILE_ROW_COUNT - Error!' );
  end

  blank_num = 0;
  comment_num = 0;
  row_num = 0;

  record_num = 0;

  while ( 1 )

    line = fgetl ( input_unit );

    if ( line == -1 )
      break;
    end

    record_num = record_num + 1;
    record_length = s_len_trim ( line );

    if ( record_length <= 0 )
      blank_num = blank_num + 1;
    elseif ( line(1) == '#' )
      comment_num = comment_num + 1;
    else
      row_num = row_num + 1;
    end

  end

  fclose ( input_unit );

  return
end
function table = i4mat_data_read ( input_filename, m, n )

%*****************************************************************************80
%
%% I4MAT_DATA_READ reads data from an I4MAT file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 January 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Input, integer M, N, the number of rows and columns in the data.
%
%    Output, integer TABLE(M,N), the point coordinates.
%
  table = zeros ( m, n );
%
%  Build up the format string for reading M real numbers.
%
  string = ' ';

  for i = 0 : m
    string = strcat ( string, ' %d' );
  end

  input_unit = fopen ( input_filename );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4MAT_DATA_READ - Error!\n' );
    fprintf ( 1, '  Could not open the input file.\n' );
    error ( 'I4MAT_DATA_READ - Error!' );
  end

  i = 0;

  while ( i < n )

    line = fgets ( input_unit );

    if ( line == -1 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'I4MAT_DATA_READ - Error!\n' );
      fprintf ( 1, '  End of input while reading data.\n' );
      error ( 'I4MAT_DATA_READ - Error!' );
    end

    if ( line(1) == '#' )

    elseif ( s_len_trim ( line ) == 0 )

    else

      [ x, count ] = sscanf ( line, string );

      if ( count == m )
        i = i + 1;
        table(1:m,i) = x(1:m);
      end

    end

  end

  fclose ( input_unit );

  return
end
function [ m, n ] = i4mat_header_read ( input_filename )

%*****************************************************************************80
%
%% I4MAT_HEADER_READ reads the header from an I4MAT file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Output, integer M, the spatial dimension.
%
%    Output, integer N, the number of points.
%
  m = file_column_count ( input_filename );

  if ( m <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4MAT_HEADER_READ - Fatal error!\n' );
    fprintf ( 1, '  There was some kind of I/O problem while trying\n' );
    fprintf ( 1, '  to count the number of data columns in\n' );
    fprintf ( 1, '  the file %s.\n', input_filename );
  end

  n = file_row_count ( input_filename );

  if ( n <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4MAT_HEADER_READ - Fatal error!\n' );
    fprintf ( 1, '  There was some kind of I/O problem while trying\n' );
    fprintf ( 1, '  to count the number of data rows in\n' );
    fprintf ( 1, '  the file %s\n', input_filename );
  end

  return
end
function i4mat_transpose_print_some ( m, n, a, ilo, jlo, ihi, jhi, title )

%*****************************************************************************80
%
%% I4MAT_TRANSPOSE_PRINT_SOME prints some of an I4MAT, transposed.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 June 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, integer A(M,N), an M by N matrix to be printed.
%
%    Input, integer ILO, JLO, the first row and column to print.
%
%    Input, integer IHI, JHI, the last row and column to print.
%
%    Input, string TITLE, an optional title.
%
  incx = 10;

  if ( 0 < s_len_trim ( title ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, '%s\n', title );
  end

  for i2lo = max ( ilo, 1 ) : incx : min ( ihi, m )

    i2hi = i2lo + incx - 1;
    i2hi = min ( i2hi, m );
    i2hi = min ( i2hi, ihi );

    inc = i2hi + 1 - i2lo;

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Row: ' );
    for i = i2lo : i2hi
      fprintf ( 1, '%7d  ', i );
    end
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Col\n' );
    fprintf ( 1, '\n' );

    j2lo = max ( jlo, 1 );
    j2hi = min ( jhi, n );

    for j = j2lo : j2hi

      fprintf ( 1, '%5d  ', j );
      for i2 = 1 : inc
        i = i2lo - 1 + i2;
        fprintf ( 1, '%7d  ', a(i,j) );
      end
      fprintf ( 1, '\n' );

    end

  end

  return
end
function element_node = mesh_base_one ( node_num, element_order, ...
  element_num, element_node )

%*****************************************************************************80
%
%% MESH_BASE_ONE ensures that the element definition is one-based.
%
%  Discussion:
%
%    The ELEMENT_NODE array contains nodes indices that form elements.
%    The convention for node indexing might start at 0 or at 1.
%    Since a MATLAB program will naturally assume a 1-based indexing, it is
%    necessary to check a given element definition and, if it is actually
%    0-based, to convert it.
%
%    This function attempts to detect 0-based node indexing and correct it.
%
%    Thanks to Feifei Xu for pointing out that I was subtracting 1 when I
%    should have been adding 1!  29 November 2012.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 November 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, integer ELEMENT_ORDER, the order of the elements.
%
%    Input, integer ELEMENT_NUM, the number of elements.
%
%    Input/output, integer ELEMENT_NODE(ELEMENT_ORDE,ELEMENT_NUM), the element
%    definitions.
%
  node_min = min ( min ( element_node(1:element_order,1:element_num) ) );
  node_max = max ( max ( element_node(1:element_order,1:element_num) ) );

  if ( node_min == 0 && node_max == node_num - 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'MESH_BASE_ONE:\n' );
    fprintf ( 1, '  The element indexing appears to be 0-based!\n' );
    fprintf ( 1, '  This will be converted to 1-based.\n' );
    element_node(1:element_order,1:element_num) = ...
      element_node(1:element_order,1:element_num) + 1;
  elseif ( node_min == 1 && node_max == node_num )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'MESH_BASE_ONE:\n' );
    fprintf ( 1, '  The element indexing appears to be 1-based!\n' );
    fprintf ( 1, '  No conversion is necessary.\n' );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, 'MESH_BASE_ONE - Warning!\n' );
    fprintf ( 1, '  The element indexing is not of a recognized type.\n' );
    fprintf ( 1, '  NODE_MIN = %d\n', node_min );
    fprintf ( 1, '  NODE_MAX = %d\n', node_max );
    fprintf ( 1, '  NODE_NUM = %d\n', node_num );
  end

  return
end
function table = r8mat_data_read ( input_filename, m, n )

%*****************************************************************************80
%
%% R8MAT_DATA_READ reads data from an R8MAT file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 January 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Input, integer M, N, the number of rows and columns of data.
%
%    Output, real TABLE(M,N), the point coordinates.
%
  table = zeros ( m, n );
%
%  Build up the format string for reading M real numbers.
%
  string = ' ';

  for i = 0 : m
    string = strcat ( string, ' %f' );
  end

  input_unit = fopen ( input_filename );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_DATA_READ - Error!\n' );
    fprintf ( 1, '  Could not open the file.\n' );
    error ( 'R8MAT_DATA_READ - Error!' );
  end

  i = 0;

  while ( i < n )

    line = fgets ( input_unit );

    if ( line == -1 )
      break;
    end

    if ( line(1) == '#' )

    elseif ( s_len_trim ( line ) == 0 )

    else

      [ x, count ] = sscanf ( line, string );

      if ( count == m )
        i = i + 1;
        table(1:m,i) = x(1:m);
      end

    end

  end

  fclose ( input_unit );

  return
end
function [ m, n ] = r8mat_header_read ( input_filename )

%*****************************************************************************80
%
%% R8MAT_HEADER_READ reads the header from an R8MAT file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Output, integer M, the spatial dimension.
%
%    Output, integer N, the number of points.
%
  m = file_column_count ( input_filename );

  if ( m <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_HEADER_READ - Fatal error!\n' );
    fprintf ( 1, '  There was some kind of I/O problem while trying\n' );
    fprintf ( 1, '  to count the number of data columns in\n' );
    fprintf ( 1, '  the file %s.\n', input_filename );
  end

  n = file_row_count ( input_filename );

  if ( n <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_HEADER_READ - Fatal error!\n' );
    fprintf ( 1, '  There was some kind of I/O problem while trying\n' );
    fprintf ( 1, '  to count the number of data rows in\n' );
    fprintf ( 1, '  the file %s\n', input_filename );
  end

  return
end
function r8mat_transpose_print_some ( m, n, a, ilo, jlo, ihi, jhi, title )

%*****************************************************************************80
%
%% R8MAT_TRANSPOSE_PRINT_SOME prints some of an R8MAT, transposed.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, real A(M,N), an M by N matrix to be printed.
%
%    Input, integer ILO, JLO, the first row and column to print.
%
%    Input, integer IHI, JHI, the last row and column to print.
%
%    Input, string TITLE, an optional title.
%
  incx = 5;

  if ( 0 < s_len_trim ( title ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, '%s\n', title );
  end

  for i2lo = max ( ilo, 1 ) : incx : min ( ihi, m )

    i2hi = i2lo + incx - 1;
    i2hi = min ( i2hi, m );
    i2hi = min ( i2hi, ihi );

    inc = i2hi + 1 - i2lo;

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Row: ' );
    for i = i2lo : i2hi
      fprintf ( 1, '%7d       ', i );
    end
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Col\n' );

    j2lo = max ( jlo, 1 );
    j2hi = min ( jhi, n );

    for j = j2lo : j2hi

      fprintf ( 1, '%5d ', j );
      for i2 = 1 : inc
        i = i2lo - 1 + i2;
        fprintf ( 1, '%12f', a(i,j) );
      end
      fprintf ( 1, '\n' );

    end

  end

  return
end
function len = s_len_trim ( s )

%*****************************************************************************80
%
%% S_LEN_TRIM returns the length of a character string to the last nonblank.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 June 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string S, the string to be measured.
%
%    Output, integer LEN, the length of the string up to the last nonblank.
%
  len = length ( s );

  while ( 0 < len )
    if ( s(len) ~= ' ' )
      return
    end
    len = len - 1;
  end

  return
end
function word_num = s_word_count ( s )

%*****************************************************************************80
%
%% S_WORD_COUNT counts the number of "words" in a string.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string S, the string to be examined.
%
%    Output, integer WORD_NUM, the number of "words" in the string.
%    Words are presumed to be separated by one or more blanks.
%
  FALSE = 0;
  TRUE = 1;

  word_num = 0;
  s_length = length ( s );

  if ( s_length <= 0 )
    return;
  end

  blank = TRUE;

  for i = 1 : s_length

    if ( s(i) == ' ' )
      blank = TRUE;
    elseif ( blank == TRUE )
      word_num = word_num + 1;
      blank = FALSE;
    end

  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 triangulation_order3_plot ( file_name, node_num, node_xy, ...
  triangle_num, triangle_node, node_show, triangle_show )

%*****************************************************************************80
%
%% TRIANGULATION_ORDER3_PLOT plots a 3-node triangulation of a pointset.
%
%  Discussion:
%
%    The triangulation is most usually a Delaunay triangulation,
%    but this is not necessary.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 June 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string FILE_NAME, the name of the output file.
%
%    Input, integer NODE_NUM, the number of points.
%
%    Input, real NODE_XY(2,NODE_NUM), the nodes.
%
%    Input, integer TRIANGLE_NUM, the number of triangles.
%
%    Input, integer TRIANGLE_NODE(3,TRIANGLE_NUM), lists, for each triangle,
%    the indices of the points that form the vertices of the triangle.
%
%    Input, logical NODE_SHOW:
%    0, do not show the nodes.
%    1, show the nodes.
%    2, show the nodes, and label them.
%
%    Input, logical TRIANGLE_SHOW,
%    0, do not show the triangles.
%    1, show the triangles.
%    2, show the triangles, and label them.
%
  x_ps_max = 576;
  x_ps_max_clip = 594;
  x_ps_min = 36;
  x_ps_min_clip = 18;
  y_ps_max = 666;
  y_ps_max_clip = 684;
  y_ps_min = 126;
  y_ps_min_clip = 108;
%
%  We need to do some figuring here, so that we can determine
%  the range of the data, and hence the height and width
%  of the piece of paper.
%
  x_max = max ( node_xy(1,1:node_num) );
  x_min = min ( node_xy(1,1:node_num) );
  x_scale = x_max - x_min;

  x_max = x_max + 0.05 * x_scale;
  x_min = x_min - 0.05 * x_scale;
  x_scale = x_max - x_min;

  y_max = max ( node_xy(2,1:node_num) );
  y_min = min ( node_xy(2,1:node_num) );
  y_scale = y_max - y_min;

  y_max = y_max + 0.05 * y_scale;
  y_min = y_min - 0.05 * y_scale;
  y_scale = y_max - y_min;

  if ( x_scale < y_scale )

    delta = round ( ( x_ps_max - x_ps_min ) ...
      * ( y_scale - x_scale ) / ( 2.0 * y_scale ) );

    x_ps_max = x_ps_max - delta;
    x_ps_min = x_ps_min + delta;

    x_ps_max_clip = x_ps_max_clip - delta;
    x_ps_min_clip = x_ps_min_clip + delta;

  elseif ( y_scale < x_scale )

    delta = round ( ( y_ps_max - y_ps_min ) ...
      * ( x_scale - y_scale ) / ( 2.0 * x_scale ) );

    y_ps_max      = y_ps_max - delta;
    y_ps_min      = y_ps_min + delta;

    y_ps_max_clip = y_ps_max_clip - delta;
    y_ps_min_clip = y_ps_min_clip + delta;

  end
%
%  Plot the triangulation.
%
  file_unit = fopen ( file_name, 'wt' );

  if ( file_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TRIANGULATION_ORDER3_PLOT - Fatal error!\n' );
    fprintf ( 1, '  Could not open the output file.\n' );
    error ( 'TRIANGULATION_ORDER3_PLOT - Fatal error!' );
  end

  fprintf ( file_unit, '%%!PS-Adobe-3.0 EPSF-3.0\n' );
  fprintf ( file_unit, '%%%%Creator: triangulation_order3_plot.m\n' );
  fprintf ( file_unit, '%%%%Title: %s\n', file_name );
  fprintf ( file_unit, '%%%%Pages: 1\n' );
  fprintf ( file_unit, '%%%%BoundingBox:  %d  %d  %d  %d\n', ...
    x_ps_min, y_ps_min, x_ps_max, y_ps_max );
  fprintf ( file_unit, '%%%%Document-Fonts: Times-Roman\n' );
  fprintf ( file_unit, '%%%%LanguageLevel: 1\n' );
  fprintf ( file_unit, '%%%%EndComments\n' );
  fprintf ( file_unit, '%%%%BeginProlog\n' );
  fprintf ( file_unit, '/inch {72 mul} def\n' );
  fprintf ( file_unit, '%%%%EndProlog\n' );
  fprintf ( file_unit, '%%%%Page: 1 1\n' );
  fprintf ( file_unit, 'save\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%  Increase line width from default 0.\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '2 setlinewidth\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%  Set the RGB color to very light gray.\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '0.900  0.900  0.900 setrgbcolor\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%  Draw a gray border around the page.\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, 'newpath\n' );
  fprintf ( file_unit, '  %d  %d  moveto\n', x_ps_min, y_ps_min );
  fprintf ( file_unit, '  %d  %d  lineto\n', x_ps_max, y_ps_min );
  fprintf ( file_unit, '  %d  %d  lineto\n', x_ps_max, y_ps_max );
  fprintf ( file_unit, '  %d  %d  lineto\n', x_ps_min, y_ps_max );
  fprintf ( file_unit, '  %d  %d  lineto\n', x_ps_min, y_ps_min );
  fprintf ( file_unit, 'stroke\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%  Set the RGB color to black.\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '0.000  0.000  0.000 setrgbcolor\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%  Set the font and its size.\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '/Times-Roman findfont\n' );
  fprintf ( file_unit, '0.50 inch scalefont\n' );
  fprintf ( file_unit, 'setfont\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%  Print a title.\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%210  702  moveto\n' );
  fprintf ( file_unit, '%%(Triangulation)  show\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%  Define a clipping polygon.\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, 'newpath\n' );
  fprintf ( file_unit, '  %d  %d  moveto\n', x_ps_min_clip, y_ps_min_clip );
  fprintf ( file_unit, '  %d  %d  lineto\n', x_ps_max_clip, y_ps_min_clip );
  fprintf ( file_unit, '  %d  %d  lineto\n', x_ps_max_clip, y_ps_max_clip );
  fprintf ( file_unit, '  %d  %d  lineto\n', x_ps_min_clip, y_ps_max_clip );
  fprintf ( file_unit, '  %d  %d  lineto\n', x_ps_min_clip, y_ps_min_clip );
  fprintf ( file_unit, 'clip newpath\n' );
%
%  Draw the nodes.
%
  if ( node_num <= 200 )
    circle_size = 5;
  elseif ( node_num <= 500 )
    circle_size = 4;
  elseif ( node_num <= 1000 )
    circle_size = 3;
  elseif ( node_num <= 5000 )
    circle_size = 2;
  else
    circle_size = 1;
  end

  if ( 1 <= node_show )

    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '%%  Draw filled dots at the nodes.\n' );
    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '%%  Set the RGB color to blue.\n' );
    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '0.000  0.150  0.750 setrgbcolor\n' );
    fprintf ( file_unit, '%%\n' );

    for node = 1 : node_num

      x_ps = floor ( ...
        ( ( x_max - node_xy(1,node)         ) * x_ps_min ...
        + (         node_xy(1,node) - x_min ) * x_ps_max ) ...
        / ( x_max                   - x_min ) );

      y_ps = floor ( ...
        ( ( y_max - node_xy(2,node)         ) * y_ps_min   ...
        + (         node_xy(2,node) - y_min ) * y_ps_max ) ...
        / ( y_max                   - y_min ) );

      fprintf ( file_unit, ...
        '  newpath  %d  %d  %d 0 360 arc closepath fill\n', ...
        x_ps, y_ps, circle_size );

    end

  end
%
%  Label the nodes.
%
  if ( 2 <= node_show )

    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '%%  Label the nodes.\n' );
    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '%%  Set the RGB color to darker blue.\n' );
    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '0.000  0.250  0.850 setrgbcolor\n' );
    fprintf ( file_unit, '/Times-Roman findfont\n' );
    fprintf ( file_unit, '0.20 inch scalefont\n' );
    fprintf ( file_unit, 'setfont\n' );
    fprintf ( file_unit, '%%\n' );

    for node = 1 : node_num

      x_ps = floor ( ...
        ( ( x_max - node_xy(1,node)         ) * x_ps_min ...
        + (         node_xy(1,node) - x_min ) * x_ps_max ) ...
        / ( x_max                   - x_min ) );

      y_ps = floor ( ...
        ( ( y_max - node_xy(2,node)         ) * y_ps_min   ...
        + (         node_xy(2,node) - y_min ) * y_ps_max ) ...
        / ( y_max                   - y_min ) );

      fprintf ( file_unit, '  %d  %d  moveto (%d) show\n', ...
        x_ps, y_ps+5, node );

    end

  end
%
%  Draw the triangles.
%
  if ( 1 <= triangle_show )

    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '%%  Set the RGB color to red.\n' );
    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '0.900  0.200  0.100 setrgbcolor\n' );
    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '%%  Draw the triangles.\n' );
    fprintf ( file_unit, '%%\n' );

    for triangle = 1 : triangle_num

      fprintf ( file_unit, 'newpath\n' );

      for i = 1 : 4

        e = i;
        if ( e == 4 )
          e = 1;
        end

        node = triangle_node(e,triangle);

        x_ps = floor ( ...
          ( ( x_max - node_xy(1,node)         ) * x_ps_min ...
          + (         node_xy(1,node) - x_min ) * x_ps_max ) ...
          / ( x_max                   - x_min ) );

        y_ps = floor ( ...
          ( ( y_max - node_xy(2,node)         ) * y_ps_min ...
          + (         node_xy(2,node) - y_min ) * y_ps_max ) ...
          / ( y_max                   - y_min ) );

        if ( i == 1 )
          fprintf ( file_unit, '  %d  %d  moveto\n', x_ps, y_ps );
        else
          fprintf ( file_unit, '  %d  %d  lineto\n', x_ps, y_ps );
        end

      end

      fprintf ( file_unit, 'stroke\n' );

    end

  end
%
%  Label the triangles.
%
  if ( 2 <= triangle_show )

    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '%%  Label the triangles.\n' );
    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '%%  Set the RGB color to darker red.\n' );
    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '0.950  0.250  0.150 setrgbcolor\n' );
    fprintf ( file_unit, '/Times-Roman findfont\n' );
    fprintf ( file_unit, '0.20 inch scalefont\n' );
    fprintf ( file_unit, 'setfont\n' );
    fprintf ( file_unit, '%%\n' );

    for triangle = 1 : triangle_num

      ave_x = 0.0;
      ave_y = 0.0;

      for i = 1 : 3

        node = triangle_node(i,triangle);
        ave_x = ave_x + node_xy(1,node);
        ave_y = ave_y + node_xy(2,node);
      end

      ave_x = ave_x / 3.0;
      ave_y = ave_y / 3.0;

      x_ps = floor ( ...
        ( ( x_max - ave_x         ) * x_ps_min ...
        + (         ave_x - x_min ) * x_ps_max ) ...
        / ( x_max         - x_min ) );

      y_ps = floor ( ...
        ( ( y_max - ave_y         ) * y_ps_min ...
        + (         ave_y - y_min ) * y_ps_max ) ...
        / ( y_max         - y_min ) );

      fprintf ( file_unit, '  %d  %d  moveto (%d) show\n', ...
        x_ps, y_ps, triangle );

    end
  end

  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, 'restore  showpage\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%  End of page.\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%%%Trailer\n' );
  fprintf ( file_unit, '%%%%EOF\n' );

  fclose ( file_unit );

  return
end
function triangulation_order4_plot ( file_name, node_num, node_xy, ...
  triangle_num, triangle_node, node_show, triangle_show )

%*****************************************************************************80
%
%% TRIANGULATION_ORDER4_PLOT plots a 4-node triangulation of a pointset.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 June 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string FILE_NAME, the name of the output file.
%
%    Input, integer NODE_NUM, the number of points.
%
%    Input, real NODE_XY(2,NODE_NUM), the nodes.
%
%    Input, integer TRIANGLE_NUM, the number of triangles.
%
%    Input, integer TRIANGLE_NODE(4,TRIANGLE_NUM), lists, for each triangle,
%    the indices of the points that form the vertices of the triangle.
%
%    Input, logical NODE_SHOW:
%    0, do not show the nodes.
%    1, show the nodes.
%    2, show the nodes, and label them.
%
%    Input, logical TRIANGLE_SHOW,
%    0, do not show the triangles.
%    1, show the triangles.
%    2, show the triangles, and label them.
%
  x_ps_max = 576;
  x_ps_max_clip = 594;
  x_ps_min = 36;
  x_ps_min_clip = 18;
  y_ps_max = 666;
  y_ps_max_clip = 684;
  y_ps_min = 126;
  y_ps_min_clip = 108;
%
%  We need to do some figuring here, so that we can determine
%  the range of the data, and hence the height and width
%  of the piece of paper.
%
  x_max = max ( node_xy(1,1:node_num) );
  x_min = min ( node_xy(1,1:node_num) );
  x_scale = x_max - x_min;

  x_max = x_max + 0.05 * x_scale;
  x_min = x_min - 0.05 * x_scale;
  x_scale = x_max - x_min;

  y_max = max ( node_xy(2,1:node_num) );
  y_min = min ( node_xy(2,1:node_num) );
  y_scale = y_max - y_min;

  y_max = y_max + 0.05 * y_scale;
  y_min = y_min - 0.05 * y_scale;
  y_scale = y_max - y_min;

  if ( x_scale < y_scale )

    delta = round ( ( x_ps_max - x_ps_min ) ...
      * ( y_scale - x_scale ) / ( 2.0 * y_scale ) );

    x_ps_max = x_ps_max - delta;
    x_ps_min = x_ps_min + delta;

    x_ps_max_clip = x_ps_max_clip - delta;
    x_ps_min_clip = x_ps_min_clip + delta;

  elseif ( y_scale < x_scale )

    delta = round ( ( y_ps_max - y_ps_min ) ...
      * ( x_scale - y_scale ) / ( 2.0 * x_scale ) );

    y_ps_max      = y_ps_max - delta;
    y_ps_min      = y_ps_min + delta;

    y_ps_max_clip = y_ps_max_clip - delta;
    y_ps_min_clip = y_ps_min_clip + delta;

  end
%
%  Plot the triangulation.
%
  file_unit = fopen ( file_name, 'wt' );

  if ( file_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TRIANGULATION_ORDER4_PLOT - Fatal error!\n' );
    fprintf ( 1, '  Could not open the output file.\n' );
    error ( 'TRIANGULATION_ORDER4_PLOT - Fatal error!' );
  end

  fprintf ( file_unit, '%%!PS-Adobe-3.0 EPSF-3.0\n' );
  fprintf ( file_unit, '%%%%Creator: triangulation_order4_plot.m\n' );
  fprintf ( file_unit, '%%%%Title: %s\n', file_name );
  fprintf ( file_unit, '%%%%Pages: 1\n' );
  fprintf ( file_unit, '%%%%BoundingBox:  %d  %d  %d  %d\n', ...
    x_ps_min, y_ps_min, x_ps_max, y_ps_max );
  fprintf ( file_unit, '%%%%Document-Fonts: Times-Roman\n' );
  fprintf ( file_unit, '%%%%LanguageLevel: 1\n' );
  fprintf ( file_unit, '%%%%EndComments\n' );
  fprintf ( file_unit, '%%%%BeginProlog\n' );
  fprintf ( file_unit, '/inch {72 mul} def\n' );
  fprintf ( file_unit, '%%%%EndProlog\n' );
  fprintf ( file_unit, '%%%%Page: 1 1\n' );
  fprintf ( file_unit, 'save\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%  Increase line width from default 0.\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '2 setlinewidth\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%  Set the RGB color to very light gray.\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '0.900  0.900  0.900 setrgbcolor\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%  Draw a gray border around the page.\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, 'newpath\n' );
  fprintf ( file_unit, '  %d  %d  moveto\n', x_ps_min, y_ps_min );
  fprintf ( file_unit, '  %d  %d  lineto\n', x_ps_max, y_ps_min );
  fprintf ( file_unit, '  %d  %d  lineto\n', x_ps_max, y_ps_max );
  fprintf ( file_unit, '  %d  %d  lineto\n', x_ps_min, y_ps_max );
  fprintf ( file_unit, '  %d  %d  lineto\n', x_ps_min, y_ps_min );
  fprintf ( file_unit, 'stroke\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%  Set the RGB color to black.\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '0.000  0.000  0.000 setrgbcolor\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%  Set the font and its size.\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '/Times-Roman findfont\n' );
  fprintf ( file_unit, '0.50 inch scalefont\n' );
  fprintf ( file_unit, 'setfont\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%  Print a title.\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%210  702  moveto\n' );
  fprintf ( file_unit, '%%(Triangulation)  show\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%  Define a clipping polygon.\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, 'newpath\n' );
  fprintf ( file_unit, '  %d  %d  moveto\n', x_ps_min_clip, y_ps_min_clip );
  fprintf ( file_unit, '  %d  %d  lineto\n', x_ps_max_clip, y_ps_min_clip );
  fprintf ( file_unit, '  %d  %d  lineto\n', x_ps_max_clip, y_ps_max_clip );
  fprintf ( file_unit, '  %d  %d  lineto\n', x_ps_min_clip, y_ps_max_clip );
  fprintf ( file_unit, '  %d  %d  lineto\n', x_ps_min_clip, y_ps_min_clip );
  fprintf ( file_unit, 'clip newpath\n' );
%
%  Draw the nodes.
%
  if ( node_num <= 200 )
    circle_size = 5;
  elseif ( node_num <= 500 )
    circle_size = 4;
  elseif ( node_num <= 1000 )
    circle_size = 3;
  elseif ( node_num <= 5000 )
    circle_size = 2;
  else
    circle_size = 1;
  end

  if ( 1 <= node_show )

    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '%%  Draw filled dots at the nodes.\n' );
    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '%%  Set the RGB color to blue.\n' );
    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '0.000  0.150  0.750 setrgbcolor\n' );
    fprintf ( file_unit, '%%\n' );

    for node = 1 : node_num

      x_ps = floor ( ...
        ( ( x_max - node_xy(1,node)         ) * x_ps_min ...
        + (         node_xy(1,node) - x_min ) * x_ps_max ) ...
        / ( x_max                   - x_min ) );

      y_ps = floor ( ...
        ( ( y_max - node_xy(2,node)         ) * y_ps_min   ...
        + (         node_xy(2,node) - y_min ) * y_ps_max ) ...
        / ( y_max                   - y_min ) );

      fprintf ( file_unit, ...
        '  newpath  %d  %d  %d 0 360 arc closepath fill\n', ...
        x_ps, y_ps, circle_size );

    end

  end
%
%  Label the nodes.
%
  if ( 2 <= node_show )

    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '%%  Label the nodes.\n' );
    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '%%  Set the RGB color to darker blue.\n' );
    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '0.000  0.250  0.850 setrgbcolor\n' );
    fprintf ( file_unit, '/Times-Roman findfont\n' );
    fprintf ( file_unit, '0.20 inch scalefont\n' );
    fprintf ( file_unit, 'setfont\n' );
    fprintf ( file_unit, '%%\n' );

    for node = 1 : node_num

      x_ps = floor ( ...
        ( ( x_max - node_xy(1,node)         ) * x_ps_min ...
        + (         node_xy(1,node) - x_min ) * x_ps_max ) ...
        / ( x_max                   - x_min ) );

      y_ps = floor ( ...
        ( ( y_max - node_xy(2,node)         ) * y_ps_min   ...
        + (         node_xy(2,node) - y_min ) * y_ps_max ) ...
        / ( y_max                   - y_min ) );

      fprintf ( file_unit, '  %d  %d  moveto (%d) show\n', ...
        x_ps, y_ps+5, node );

    end

  end
%
%  Draw the triangles.
%
  if ( 1 <= triangle_show )

    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '%%  Set the RGB color to red.\n' );
    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '0.900  0.200  0.100 setrgbcolor\n' );
    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '%%  Draw the triangles.\n' );
    fprintf ( file_unit, '%%\n' );

    for triangle = 1 : triangle_num

      fprintf ( file_unit, 'newpath\n' );

      for i = 1 : 4

        e = i;
        if ( e == 4 )
          e = 1;
        end

        node = triangle_node(e,triangle);

        x_ps = floor ( ...
          ( ( x_max - node_xy(1,node)         ) * x_ps_min ...
          + (         node_xy(1,node) - x_min ) * x_ps_max ) ...
          / ( x_max                   - x_min ) );

        y_ps = floor ( ...
          ( ( y_max - node_xy(2,node)         ) * y_ps_min ...
          + (         node_xy(2,node) - y_min ) * y_ps_max ) ...
          / ( y_max                   - y_min ) );

        if ( i == 1 )
          fprintf ( file_unit, '  %d  %d  moveto\n', x_ps, y_ps );
        else
          fprintf ( file_unit, '  %d  %d  lineto\n', x_ps, y_ps );
        end

      end

      fprintf ( file_unit, 'stroke\n' );

    end

  end
%
%  Label the triangles.
%
  if ( 2 <= triangle_show )

    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '%%  Label the triangles.\n' );
    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '%%  Set the RGB color to darker red.\n' );
    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '0.950  0.250  0.150 setrgbcolor\n' );
    fprintf ( file_unit, '/Times-Roman findfont\n' );
    fprintf ( file_unit, '0.20 inch scalefont\n' );
    fprintf ( file_unit, 'setfont\n' );
    fprintf ( file_unit, '%%\n' );

    for triangle = 1 : triangle_num

      ave_x = 0.0;
      ave_y = 0.0;

      for i = 1 : 3

        node = triangle_node(i,triangle);
        ave_x = ave_x + node_xy(1,node);
        ave_y = ave_y + node_xy(2,node);
      end

      ave_x = ave_x / 3.0;
      ave_y = ave_y / 3.0;

      x_ps = floor ( ...
        ( ( x_max - ave_x         ) * x_ps_min ...
        + (         ave_x - x_min ) * x_ps_max ) ...
        / ( x_max         - x_min ) );

      y_ps = floor ( ...
        ( ( y_max - ave_y         ) * y_ps_min ...
        + (         ave_y - y_min ) * y_ps_max ) ...
        / ( y_max         - y_min ) );

      fprintf ( file_unit, '  %d  %d  moveto (%d) show\n', ...
        x_ps, y_ps, triangle );

    end
  end

  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, 'restore  showpage\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%  End of page.\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%%%Trailer\n' );
  fprintf ( file_unit, '%%%%EOF\n' );

  fclose ( file_unit );

  return
end
function triangulation_order6_plot ( file_name, node_num, node_xy, ...
  triangle_num, triangle_node, node_show, triangle_show )

%*****************************************************************************80
%
%% TRIANGULATION_ORDER6_PLOT plots a 6-node triangulation of a pointset.
%
%  Discussion:
%
%    The triangulation is most usually a Delaunay triangulation,
%    but this is not necessary.
%
%    In a six node triangulation, it is assumed that nodes 1, 2, and 3
%    are the vertices of the triangles, and that nodes 4, 5, and 6
%    lie between 1 and 2, 2 and 3, and 3 and 1 respectively.
%
%    This routine has been specialized to deal correctly ONLY with
%    a mesh of 6 node elements, with the property that starting
%    from local node 1 and traversing the edges of the element will
%    result in encountering local nodes 1, 4, 2, 5, 3, 6 in that order.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 June 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, character FILE_NAME(*), the name of the output file.
%
%    Input, integer NODE_NUM, the number of points.
%
%    Input, real NODE_XY(2,NODE_NUM), the nodes.
%
%    Input, integer TRIANGLE_NUM, the number of triangles.
%
%    Input, integer TRIANGLE_NODE(6,TRIANGLE_NUM), lists, for each triangle,
%    the indices of the points that form the vertices and midsides
%    of the triangle.
%
%    Input, logical NODE_SHOW:
%    0, do not show the nodes.
%    1, show the nodes.
%    2, show the nodes, and label them.
%
%    Input, logical TRIANGLE_SHOW,
%    0, do not show the triangles.
%    1, show the triangles.
%    2, show the triangles, and label them.
%
  order = [ 1, 4, 2, 5, 3, 6 ];
  x_ps_max = 576;
  x_ps_max_clip = 594;
  x_ps_min = 36;
  x_ps_min_clip = 18;
  y_ps_max = 666;
  y_ps_max_clip = 684;
  y_ps_min = 126;
  y_ps_min_clip = 108;
%
%  We need to do some figuring here, so that we can determine
%  the range of the data, and hence the height and width
%  of the piece of paper.
%
  x_max = max ( node_xy(1,1:node_num) );
  x_min = min ( node_xy(1,1:node_num) );
  x_scale = x_max - x_min;

  x_max = x_max + 0.05 * x_scale;
  x_min = x_min - 0.05 * x_scale;
  x_scale = x_max - x_min;

  y_max = max ( node_xy(2,1:node_num) );
  y_min = min ( node_xy(2,1:node_num) );
  y_scale = y_max - y_min;

  y_max = y_max + 0.05 * y_scale;
  y_min = y_min - 0.05 * y_scale;
  y_scale = y_max - y_min;

  if ( x_scale < y_scale )

    delta = round ( ( x_ps_max - x_ps_min ) ...
      * ( y_scale - x_scale ) / ( 2.0 * y_scale ) );

    x_ps_max = x_ps_max - delta;
    x_ps_min = x_ps_min + delta;

    x_ps_max_clip = x_ps_max_clip - delta;
    x_ps_min_clip = x_ps_min_clip + delta;

  elseif ( y_scale < x_scale )

    delta = round ( ( y_ps_max - y_ps_min ) ...
      * ( x_scale - y_scale ) / ( 2.0 * x_scale ) );

    y_ps_max      = y_ps_max - delta;
    y_ps_min      = y_ps_min + delta;

    y_ps_max_clip = y_ps_max_clip - delta;
    y_ps_min_clip = y_ps_min_clip + delta;

  end
%
%  Plot the triangulation.
%
  file_unit = fopen ( file_name, 'wt' );

  if ( file_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TRIANGULATION_ORDER6_PLOT - Fatal error!\n' );
    fprintf ( 1, '  Could not open the output file.\n' );
    error ( 'TRIANGULATION_ORDER6_PLOT - Fatal error!' );
  end

  fprintf ( file_unit, '%%!PS-Adobe-3.0 EPSF-3.0\n' );
  fprintf ( file_unit, '%%%%Creator: triangulation_order6_plot.m\n' );
  fprintf ( file_unit, '%%%%Title: %s\n', file_name );
  fprintf ( file_unit, '%%%%Pages: 1\n' );
  fprintf ( file_unit, '%%%%BoundingBox:  %d  %d  %d  %d\n', ...
    x_ps_min, y_ps_min, x_ps_max, y_ps_max );
  fprintf ( file_unit, '%%%%Document-Fonts: Times-Roman\n' );
  fprintf ( file_unit, '%%%%LanguageLevel: 1\n' );
  fprintf ( file_unit, '%%%%EndComments\n' );
  fprintf ( file_unit, '%%%%BeginProlog\n' );
  fprintf ( file_unit, '/inch {72 mul} def\n' );
  fprintf ( file_unit, '%%%%EndProlog\n' );
  fprintf ( file_unit, '%%%%Page: 1 1\n' );
  fprintf ( file_unit, 'save\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%  Increase line width from default 0.\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '2 setlinewidth\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%  Set the RGB color to very light gray.\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '0.900  0.900  0.900 setrgbcolor\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%  Draw a gray border around the page.\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, 'newpath\n' );
  fprintf ( file_unit, '  %d  %d  moveto\n', x_ps_min, y_ps_min );
  fprintf ( file_unit, '  %d  %d  lineto\n', x_ps_max, y_ps_min );
  fprintf ( file_unit, '  %d  %d  lineto\n', x_ps_max, y_ps_max );
  fprintf ( file_unit, '  %d  %d  lineto\n', x_ps_min, y_ps_max );
  fprintf ( file_unit, '  %d  %d  lineto\n', x_ps_min, y_ps_min );
  fprintf ( file_unit, 'stroke\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%  Set the RGB color to black.\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '0.000  0.000  0.000 setrgbcolor\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%  Set the font and its size.\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '/Times-Roman findfont\n' );
  fprintf ( file_unit, '0.50 inch scalefont\n' );
  fprintf ( file_unit, 'setfont\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%  Print a title.\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%210  702  moveto\n' );
  fprintf ( file_unit, '%%(Triangulation)  show\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%  Define a clipping polygon.\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, 'newpath\n' );
  fprintf ( file_unit, '  %d  %d  moveto\n', x_ps_min_clip, y_ps_min_clip );
  fprintf ( file_unit, '  %d  %d  lineto\n', x_ps_max_clip, y_ps_min_clip );
  fprintf ( file_unit, '  %d  %d  lineto\n', x_ps_max_clip, y_ps_max_clip );
  fprintf ( file_unit, '  %d  %d  lineto\n', x_ps_min_clip, y_ps_max_clip );
  fprintf ( file_unit, '  %d  %d  lineto\n', x_ps_min_clip, y_ps_min_clip );
  fprintf ( file_unit, 'clip newpath\n' );
%
%  Draw the nodes.
%
  if ( node_num <= 200 )
    circle_size = 5;
  elseif ( node_num <= 500 )
    circle_size = 4;
  elseif ( node_num <= 1000 )
    circle_size = 3;
  elseif ( node_num <= 5000 )
    circle_size = 2;
  else
    circle_size = 1;
  end

  if ( 1 <= node_show )

    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '%%  Draw filled dots at the nodes.\n' );
    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '%%  Set the RGB color to blue.\n' );
    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '0.000  0.150  0.750 setrgbcolor\n' );
    fprintf ( file_unit, '%%\n' );

    for node = 1 : node_num

      x_ps = floor ( ...
        ( ( x_max - node_xy(1,node)         ) * x_ps_min ...
        + (         node_xy(1,node) - x_min ) * x_ps_max ) ...
        / ( x_max                   - x_min ) );

      y_ps = floor ( ...
        ( ( y_max - node_xy(2,node)         ) * y_ps_min   ...
        + (         node_xy(2,node) - y_min ) * y_ps_max ) ...
        / ( y_max                   - y_min ) );

      fprintf ( file_unit, ...
        '  newpath  %d  %d  %d 0 360 arc closepath fill\n', ...
        x_ps, y_ps, circle_size );

    end

  end
%
%  Label the nodes.
%
  if ( 2 <= node_show )

    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '%%  Label the nodes.\n' );
    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '%%  Set the RGB color to darker blue.\n' );
    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '0.000  0.250  0.850 setrgbcolor\n' );
    fprintf ( file_unit, '/Times-Roman findfont\n' );
    fprintf ( file_unit, '0.20 inch scalefont\n' );
    fprintf ( file_unit, 'setfont\n' );
    fprintf ( file_unit, '%%\n' );

    for node = 1 : node_num

      x_ps = floor ( ...
        ( ( x_max - node_xy(1,node)         ) * x_ps_min ...
        + (         node_xy(1,node) - x_min ) * x_ps_max ) ...
        / ( x_max                   - x_min ) );

      y_ps = floor ( ...
        ( ( y_max - node_xy(2,node)         ) * y_ps_min   ...
        + (         node_xy(2,node) - y_min ) * y_ps_max ) ...
        / ( y_max                   - y_min ) );

      fprintf ( file_unit, '  %d  %d  moveto (%d) show\n', ...
        x_ps, y_ps+5, node );

    end

  end
%
%  Draw the triangles.
%
  if ( 1 <= triangle_show )

    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '%%  Set the RGB color to red.\n' );
    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '0.900  0.200  0.100 setrgbcolor\n' );
    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '%%  Draw the triangles.\n' );
    fprintf ( file_unit, '%%\n' );

    for  triangle = 1 : triangle_num

      fprintf ( file_unit, 'newpath\n' );

      node = triangle_node(order(1),triangle);

      x_ps = floor ( ...
        ( ( x_max - node_xy(1,node)         ) * x_ps_min ...
        + (         node_xy(1,node) - x_min ) * x_ps_max ) ...
        / ( x_max                   - x_min ) );

      y_ps = floor ( ...
        ( ( y_max - node_xy(2,node)         ) * y_ps_min ...
        + (         node_xy(2,node) - y_min ) * y_ps_max ) ...
        / ( y_max                   - y_min ) );

      fprintf ( file_unit, '%d  %d  moveto\n', x_ps, y_ps );

      for i = 1 : 6

        ip1 = mod ( i, 6 ) + 1;
        node = triangle_node(order(ip1),triangle);

        x_ps = floor ( ...
          ( ( x_max - node_xy(1,node)         ) * x_ps_min ...
          + (         node_xy(1,node) - x_min ) * x_ps_max ) ...
          / ( x_max                   - x_min ) );

        y_ps = floor ( ...
          ( ( y_max - node_xy(2,node)         ) * y_ps_min ...
          + (         node_xy(2,node) - y_min ) * y_ps_max ) ...
          / ( y_max                   - y_min ) );

        fprintf ( file_unit, '%d  %d  lineto\n', x_ps, y_ps );

      end

      fprintf ( file_unit, 'stroke\n' );

    end

  end
%
%  Label the triangles.
%
  if ( 2 <= triangle_show )

    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '%%  Label the triangles:\n' );
    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, '%%  Set the RGB color to darker red:\n' );
    fprintf ( file_unit, '%%\n' );
    fprintf ( file_unit, ' 0.950  0.250  0.150  setrgbcolor\n' );
    fprintf ( file_unit, '/Times-Roman findfont\n' );
    fprintf ( file_unit, '0.30 inch scalefont\n' );
    fprintf ( file_unit, 'setfont\n' );

    for triangle = 1 : triangle_num

      ave_x = 0.0;
      ave_y = 0.0;

      for i = 1 : 6
        node = triangle_node(i,triangle);
        ave_x = ave_x + node_xy(1,node);
        ave_y = ave_y + node_xy(2,node);
      end

      ave_x = ave_x / 6.0;
      ave_y = ave_y / 6.0;

      x_ps = floor ( ...
        ( ( x_max - ave_x         ) * x_ps_min ...
        + (         ave_x - x_min ) * x_ps_max ) ...
        / ( x_max         - x_min ) );

      y_ps = floor ( ...
        ( ( y_max - ave_y         ) * y_ps_min ...
        + (         ave_y - y_min ) * y_ps_max ) ...
        / ( y_max         - y_min ) );

      fprintf ( file_unit, '%d  %d  moveto\n', x_ps, y_ps );
      fprintf ( file_unit, '(%d) show\n', triangle );

    end

  end

  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, 'restore showpage\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%% End of page\n' );
  fprintf ( file_unit, '%%\n' );
  fprintf ( file_unit, '%%%%Trailer\n' );
  fprintf ( file_unit, '%%%%EOF\n' );

  fclose ( file_unit );

  return
end


function readme()
%
% I. INTRODUCTION
%     The MATLAB(R) program contained within this directory counts the total
% number of loops (cycles) in a network (graph) that consists of nodes and edges.
%     This file describes the setup/installation proceedures for the code. A
% description of the algorithm and how to use the code is contained in the
% 'DETAILS.m' file.
%
%
% II. CONTENTS
%     In the Loops/ directory, you should find the following files:
%         DETAILS.m
%         README.m
%         loops_gui.m
%         run_loops.m
%         nets/**
%         **several sample network files
%
%
% III. SETUP/INSTALLATION
%     1. You must have MATLAB software installed on your computer
%     2. Copy/move the 'Loops' folder to the MATLAB 'work' directory
%     3. Open MATLAB and add the 'Loops' directory to the Path
%         a. Go to: File -> Set Path...
%         b. Click on 'Add with subfolders...'
%         c. Select the 'Loops' directory and click 'Ok'
%         d. Click 'Save'
%         e. Click 'Close'
%     4. To run, open the 'loops_gui.m' file and press F5 or use the
%         command line: >> loops_gui
%     5. Note: this code was written using MATLAB 7R14 through R2006B. It has not
%         been tested on previous versions
%
%
% IV. HELP/REPORT BUGS
%     If you experience difficulties using this program, first make sure that
% the steps in Section III have been completed. Next, make sure your network
% satisfies all the requirements given in the 'DETAILS.m' file.
%
% Please direct questions/comments to:
% Joe Kirk
% jdkirk630@gmail.com
%
%
% V. REVISION NOTES
%     11/2005 Update:
%         1. New file 'loop_gui.m' - GUI file that replaces 'run_loops.m' and
%             displays all of the tools for the user as they are available
%         2. Added ability to save loops in .MAT format
%     10/2005 Update:
%         1. New file 'reduceNet.m' - function which allows networks to be reduced
%             (removes nodes that have only one edge, until no more remain in the net)
%         2. New file 'getStartingNode.m' - function which calculates a (nearly)
%             optimal starting node to make the ILCA more efficient (results in
%             fewer steps to complete the algorithm)
%         3. Removed file 'printNetStats.m' - the basic functionality of this
%             file was separated into two separate files ('calcNumEdges.m'
%             and 'plotHLoops.m')
%         4. New file 'calcNumEdges.m' - function which calculates the number of
%             edges in a network
%         5. New file 'plotHLoops.m' - function which plots the distribution of
%             loops of length 'h'
%         6. Modified file 'generateRandomNet.m' to limit the creation of sparse
%             networks with more nodes than 40 (which are costly to generate with
%             this function)
%     2/2007 Update:
%         1. All of the subfunction files have been deprecated, and their
%             functionality has been combined and added to the end of the
%             'loops_gui.m' and 'run_loops.m' files. The two files 'loops_gui.m'
%             and 'run_loops.m' are now independent, stand-alone m-files
%         2. Added ability to save the network as an edgelist file
%         3. Improved the layout and function of the Loops GUI
%
clc
help readme


function edges = knnGraph(nodes, varargin)
%KNNGRAPH Create the k-nearest neighbors graph of a set of points
%
%   EDGES = knnGraph(NODES)
%
%   Example
%   nodes = rand(10, 2);
%   edges = knnGraph(nodes);
%   drawGraph(nodes, edges);
%
%   See also
%     graphs

% ------
% Author: David Legland
% e-mail: david.legland@nantes.inra.fr
% Created: 2008-07-28,    using Matlab 7.4.0.287 (R2007a)
% Copyright 2008 INRA - BIA PV Nantes - MIAJ Jouy-en-Josas.

% get number of neighbors for each node
k = 2;
if ~isempty(varargin)
    k = varargin{1};
end

% init size of arrays
n       = size(nodes, 1);
edges   = zeros(k*n, 2);

% iterate on nodes
for i = 1:n
    dists = distancePoints(nodes(i,:), nodes);
    [dists, inds]    = sort(dists); %#ok<ASGLU>
    for j = 1:k
        edges(k*(i-1)+j, :) = [i inds(j+1)];
    end
end

% remove double edges
edges = unique(sort(edges, 2), 'rows');


% Test file for @deltafun/restrict.m

function pass = test_restrict(pref)

if (nargin < 1)
    pref = chebfunpref();
end
%%
d = deltafun();
pass(1) = isempty(restrict(d, [-.5, .5]));

f = bndfun(@sin);
d = deltafun(f, struct('deltaMag', 1, 'deltaLoc', 0));
pass(2) = ~isa(restrict(d, [-1, -.5]), 'deltafun');
pass(3) = ~isa(restrict(d, [.5, 1]), 'deltafun');
pass(4) = anyDelta(restrict(d, [-.5, .5]));

f = fun.constructor(@(x) exp(x), struct('domain', [-1, 1]));
d = deltafun(f, struct('deltaMag', [1 1 1 1], 'deltaLoc', [-.5, -.25, 0, 1]));
A = restrict(d, [-1 0 .5 1]);
d1 = A{1};
d2 = A{2};
d3 = A{3};

pass(5) = all(d1.deltaLoc == [-.5 -.25 0] );
pass(6) = all(d2.funPart.domain == [0, .5] );
pass(7) = all(d3.deltaMag == 1 );

%% make sure interior delta functions exactly at break points
% get divided equally in each adjacent deltafun.
f = fun.constructor(@(x) exp(x), struct('domain', [-1, 1]));
data.deltaMag = [1 1 1 1 1 1 1];
data.deltaLoc = [-1, -.5, -.25, 0, .25, .5, 1];
d = deltafun(f, data);
A = restrict(d, [-1 0 .5 1]);
d1 = A{1};
d2 = A{2};
d3 = A{3};

pass(8) = all(d1.deltaLoc == [-1 -.5 -.25 0] ) && all(d1.deltaMag == [1 1 1 .5] );
pass(9) = all(d2.deltaLoc == [0 .25 .5] ) && all(d2.deltaMag == [.5 1 .5] );
pass(10) = all(d3.deltaLoc == [.5 1] ) && all(d3.deltaMag == [.5 1] );
end


function [inH2O] = atm2inH2O(atm)
% Convert pressure from atmospheres to inches of water column.
% Chad A. Greene of chadagreene.com fame
inH2O = atm*406.782;


%compute tail speed in the direction perpendicular to tail-head direction
function [data,units]=compute_veltailperth(trx,n)

larvae=trx.exp2flies{n};
numlarvae=numel(larvae);
veltailperth=cell(1,numlarvae);
%tailheadangperp=cell(1,numlarvae);
for i=1:numlarvae
    larva=larvae(i);
    %tailheadangperp{1,i}=trx(larva).tailheadang-pi/2;
    veltailperth{1,i}=trx(larva).velmagtail.*(cos(trx(larva).velangtail).*cos(trx(larva).tailheadang(1,1:end-1)+pi/2)+sin(trx(larva).velangtail).*sin(trx(larva).tailheadang(1,1:end-1)+pi/2));
end

units=parseunits('mm/s');
data=veltailperth;


close all;
clear;
clc
addpath(genpath('./'));

% Path planner
disp('Planning ...');
% This map is used to generate path and check collision
map_colli = load_map_inflated('maps/map1_full.txt', 0.5, 0.2, 0); % xy res, z res, margin
% This map is for visualization (without boundary obsatacles)
map = load_map('maps/map1.txt', 0.5, 0.15, 0.34); % 3 parameters are dummy

% start = {[5.0 -1 3.5]};
% stop  = {[5.0 19.0 .5]};

start = [5.0 -1 3.5];
stop  = [5.0 19.0 .5];
% stop  = [17.0 4.0 .5];

% Generate way points
path = dijkstra(map_colli, start, stop, true);

figure(1);
plot_path(map, path);
axis equal
grid on
title('Quad Simulator');xlabel('x');ylabel('y');zlabel('z');
hold on
plot3(start(1),start(2),start(3), '*r', 'markersize', 10)
plot3(stop(1),stop(2),stop(3), '*g', 'markersize', 10)
hold off
view(3)
%% Desired trajectory generation
% *Warning*
% Since demonstrated obstacles are dense, including narrow corridors, it
% may take up to a minute to get collision free trajectory.
% It is highly recommended to use simpler map if you just need a
% demonstration.
traj_obj = trajectoryGenerator(map_colli, path);

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%% ODE Simulation %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

tau_vec = traj_obj.tau_vec;
path = traj_obj.path;
ts = [0 cumsum(tau_vec)];

%%%%%%%%%%%%%%%%%%%%%%%%%%% Initial Condition %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% State: [x, v, pitch, roll, yaw, w]'
x0 = [path(1,:) zeros(1,9)]';

% Disturbances on the initial condition
% x0(1:3) = x0(1:3)-[.1 .05 .15]';
% x0(4:6) = x0(4:6)+[.01 .01 .02]';

% Initial error of yaw angle must not exceed 90 degrees
% x0(7:9) = x0(7:9)+ones(3,1)*pi/18;
% x0(10:12) = x0(10:12) + .0;
%%%%%%%%%%%%%%%%%%%%%%%%%%% Initial Condition %%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%% Model parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model_param.grav = 9.807;
model_param.mass = 1.477;
model_param.I = [0.01152 0 0;0 0.01152 0;0 0 0.0218];
model_param.arm_length = 0.263;
model_param.c_tf = 8.004e-4;

% Gain
KK.Kp = 5;
KK.Kv = 5;
KK.KR = 5;
KK.K_omega = 5;
%%%%%%%%%%%%%%%%%%%%%%%%%%%% Model parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% For real time video
target_fps = 30;
t_sim = 0:1/target_fps:sum(tau_vec);

% External force
start_T = 0;
duration = 0;
mag = 0;
direction = [1 0 0]';
Fmat = extForce_gen(t_sim, start_T, duration, mag, direction);

% Run ode45
options =  odeset('RelTol',1e-2,'AbsTol',1e-2);
[tsave, xsave] = quadSim(traj_obj, model_param, KK, t_sim, x0, Fmat, options);

%% Desired trajectory vs actual trajectory
desired_pos = zeros(length(tsave),3);
desired_vel = zeros(length(tsave),3);
desired_acc = zeros(length(tsave),3);
desired_jerk = zeros(length(tsave),3);
desired_yaw = zeros(length(tsave),1);
for i = 1:length(tsave)
    desired_s = desiredState(traj_obj, tsave(i));
    desired_pos(i,:) = desired_s.pos';
    desired_vel(i,:) = desired_s.vel';
    desired_acc(i,:) = desired_s.acc';
    desired_jerk(i,:) = desired_s.jerk';
    desired_yaw(i) = desired_s.yaw;
end

figure(1)
% Desired trajectory
plot3(desired_pos(:,1),desired_pos(:,2),desired_pos(:,3))
hold on
grid on
axis equal
% Actual trajectory
plot3(xsave(:,1),xsave(:,2),xsave(:,3),'--')
% legend('Desired','Actual')
%%
figure(5)
subplot(4,1,1)
plot(tsave,xsave(:,7)*180/pi,'-b','LineWidth',1.0);title('Euler angles');ylabel('roll, \phi')
grid on
subplot(4,1,2)
plot(tsave,xsave(:,8)*180/pi,'-b','LineWidth',1.0);ylabel('pitch, \theta')
grid on
subplot(4,1,3)
plot(tsave,xsave(:,9)*180/pi,'-b','LineWidth',1.0);ylabel('yaw, \psi');%xlabel('time, sec')
grid on

%%
m = model_param.mass;
g = model_param.grav;
% pos error
ep = xsave(:,1:3)-desired_pos;
% vel error
ev = xsave(:,4:6)-desired_vel;
% accel error
calc_acc = diff(xsave(:,4:6))./diff(tsave);
% plot(tsave(1:end-1),calc_acc)
%
ea = calc_acc-desired_acc(1:end-1,:);
%%
% desired force
Fd = -KK.Kp.*ep -KK.Kv.*ev + m*desired_acc;
Fd(:,3) = Fd(:,3) + m*g;
% Desired force derivative
% Fd_dot = -KK.Kp*ev -KK.Kv*ea + m*desired_jerk;
% desired rotation, Rd = [xbd ybd zbd]
zbd = zeros(length(tsave),3);
xcd = zeros(length(tsave),3);
zbdxcd = zeros(length(tsave),3);
norm_zbdxcd = zeros(length(tsave),1);
for i=1:length(tsave)
    zbd(i,:) = Fd(i,:)/norm(Fd(i,:));
    xcd(i,:) = [cos(desired_yaw(i)) sin(desired_yaw(i)) 0];
    dummy = hat_optr(zbd(i,:))*xcd(i,:)';
    zbdxcd(i,:) = dummy';
    norm_zbdxcd(i) = norm(zbdxcd(i,:));
end
figure(5)
subplot(4,1,4)
plot(tsave,norm_zbdxcd,'-','LineWidth',1.5)
grid on
%% Video generator
figure(1);
% plot_path(map, path{1});
filename = 'my_video.avi';
video_gen(tsave, xsave, filename, 15, Fmat)


function [res] = tt_mvdot(a_tt,x_tt,y_tt)
%Fast computation of the bilinear form (Ax,y) in the TT-format
%   [RES]=TT_MVDOT(A_TT,X_TT,Y_TT) Computes scalar product(AX,Y), where
%   A_TT is a matrix, X_TT is a vector, Y_TT  is a vector
%
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------
d=size(a_tt,1);
%The c
core=a_tt{1}; matx=x_tt{1}; maty=y_tt{1};
n1=size(core,1); n2=size(core,2);
rc=size(core,3);
rx=size(matx,2); ry=size(maty,2);
core=maty'*reshape(core,[n1,n2*rc]); %core is ryxn2xrc
core=reshape(permute(reshape(core,[ry,n2,rc]),[3,1,2]),[rc*ry,n2]);
phi=permute(reshape(core*matx,[rc,ry,rx]),[1,3,2]); %core is rcxryxrx-> rcxrx*ry

for i=2:d-1
  core=a_tt{i}; core_x = x_tt{i}; core_y=y_tt{i};
  %Decipher sizes
 n1=size(core,1); n2=size(core,2); rc1=size(core,3); rc2=size(core,4);
 rx1=size(core_x,2); rx2=size(core_x,3); ry1=size(core_y,2); ry2=size(core_y,3);
 %Convolve phi & core over the first index
 %core is n1xn2xrc1xrc2 phi is rc1x rx1 x ry1
  core=reshape(permute(core,[3,1,2,4,5]),[rc1,n1*n2*rc2]);
  phi=reshape(phi,[rc1,rx1*ry1])'*core; %Is rx1 x ry1 x n1 x n2 x rc2
  %Convolve over n2 & rx1
  phi=permute(reshape(phi,[rx1,ry1,n1,n2,rc2]),[4,1,2,3,5]);
  %n2 x rx1 x ry1 x n1 x rc2;
  phi=reshape(core_x,[n2*rx1,rx2])'*reshape(phi,[n2*rx1,ry1*n1*rc2]);
  %phi is now rx2 x ry1 x n1 x rc2
  %Convolve over n1 & ry1
  phi=reshape(phi,[rx2,ry1*n1,rc2]); phi=permute(phi,[3,1,2]);
  %keyboard;
  phi=reshape(phi,[rc2*rx2,ry1*n1])*reshape(permute(core_y,[2,1,3]),[ry1*n1,ry2]);
  phi=reshape(phi,[rc2,rx2,ry2]);
end
  core=a_tt{d}; mat_x=x_tt{d}; mat_y=y_tt{d};
  n1=size(core,1); n2=size(core,2); rc=size(core,3);
  rx=size(mat_x,2); ry=size(mat_y,2);
  %Convolve phi & core over aux index
  %core is n1 x n2 x rc
  phi=reshape(core,[n1*n2,rc])*reshape(phi,[rc,rx*ry]);
  %phi is n1 x n2 x rx x ry
  %Convolve over n2 & rx
  phi=permute(reshape(phi,[n1,n2*rx,ry]),[1,3,2]);
  phi=reshape(phi,[n1*ry,n2*rx])*reshape(mat_x,[n2*rx,1]);
  %phi is a column of length
  res= phi'*reshape(mat_y,[n1*ry,1]);

return

end


%+========================================================================+
%|                                                                        |
%|            This script uses the GYPSILAB toolbox for Matlab            |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%|             francois.alouges@polytechnique.edu                         |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : nrtDomRegularize3D.m                          |
%|    #    |   VERSION    : 0.50                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 25.11.2018                                    |
%| ( === ) |   SYNOPSIS   : Singular kernel regularization (in debug mode)|
%|  `---'  |                                                              |
%+========================================================================+

% Cleaning
clear all
close all
clc

% Gypsilab path
run('../../addpathGypsilab.m')

% Parameters
N   = 1e2;
gss = 3;

% Spherical mesh
sphere = mshSphere(N,1);

% Square mesh
square = mshSquare(2*N,[3 3]);

% Graphical representation
figure
plot(sphere)
hold on
plot(square)
axis equal

% Domain
sigma = dom(sphere,gss);

% Finite elements
u = fem(sphere,'P0');
v = fem(sphere,'P1');
w = fem(sphere,'RWG');

% Gren kernel
Gxy   = @(X,Y) femGreenKernel(X,Y,'[1/r]',[]);
dyGxy = @(X,Y) femGreenKernel(X,Y,'grady[1/r]1',[]);

% Single radiation P0
M  = integral(square.vtx,sigma,Gxy,u);
Mr = regularize(square.vtx,sigma,'[1/r]',u);
norm(M+Mr,'inf')

% Single radiation P1
M  = integral(square.vtx,sigma,Gxy,v);
Mr = regularize(square.vtx,sigma,'[1/r]',v);
norm(M+Mr,'inf')

% Single radiation RWG
M  = integral(square.vtx,sigma,Gxy,div(w));
Mr = regularize(square.vtx,sigma,'[1/r]',div(w));
norm(M+Mr,'inf')

% Single layer P0
M  = integral(sigma,sigma,u,Gxy,u);
Mr = regularize(sigma,sigma,u,'[1/r]',u);
norm(M+Mr,'inf')

% Single layer P1
M  = integral(sigma,sigma,v,Gxy,v);
Mr = regularize(sigma,sigma,v,'[1/r]',v);
norm(M+Mr,'inf')

% Single layer RWG
M  = integral(sigma,sigma,div(w),Gxy,div(w));
Mr = regularize(sigma,sigma,div(w),'[1/r]',div(w));
norm(M+Mr,'inf')

% Double radiation P0
M  = integral(square.vtx,sigma,dyGxy,u);
Mr = regularize(square.vtx,sigma,'grady[1/r]1',u);
norm(M+Mr,'inf')

% Double layer P0
M   = integral(sigma,sigma,u,dyGxy,u);
Mr  = regularize(sigma,sigma,u,'grady[1/r]1',u);
norm(M+Mr,'inf')

% Stokeslet P0
for i = 1:3
    for j = 1:3
        name  = ['[ij/r+rirj/r^3]',num2str(i),num2str(j)];
        green = @(X,Y) femGreenKernel(X,Y,name,[]);
        M     = integral(sigma,sigma,u,green,u);
        Mr    = regularize(sigma,sigma,u,name,u);
        norm(M+Mr,'inf')
    end
end

disp('~~> Michto gypsilab !')


function concurrencyPoints = traceConcurrencyPoints( boundaryPoints, cornerPoints )
%TRACECONCURRENCYPOINTS
%
variance = 0;

sizeOfBoundaryPoints = size(boundaryPoints); % 1 element of the vector is the number of row
sizeOfCornerPoints = size(cornerPoints); % 1 element of the vector is the number of row
numberOfConcurrencyPoints = 0;

numberOfToleratedPoints = 0;

    for i=1:sizeOfBoundaryPoints(1)
        for j=1:sizeOfCornerPoints(1)
            if boundaryPoints(i,1) == cornerPoints(j,1) & boundaryPoints(i,2) == cornerPoints(j,2)
                numberOfConcurrencyPoints = numberOfConcurrencyPoints + 1;
                concurrencyPoints (numberOfConcurrencyPoints,1) = cornerPoints(j,1);
                concurrencyPoints (numberOfConcurrencyPoints,2) = cornerPoints(j,2);
            end

            % tolerance implicated
            if (abs(boundaryPoints(i,1) - cornerPoints(j,1)) + abs(boundaryPoints(i,2) - cornerPoints(j,2))) < variance
            %if (abs(boundaryPoints(i,1) - cornerPoints(j,1)) < variance)
            %    if (abs(boundaryPoints(i,2) - cornerPoints(j,2)) < variance)
                numberOfConcurrencyPoints = numberOfConcurrencyPoints + 1;
                                numberOfToleratedPoints = numberOfToleratedPoints +1;

                concurrencyPoints (numberOfConcurrencyPoints,1) = cornerPoints(j,1);
                concurrencyPoints (numberOfConcurrencyPoints,2) = cornerPoints(j,2);
            %    end
            %end
            end

        end
    end

    disp(numberOfToleratedPoints);
end


function [xUpdate, SUpdate,innov,Szz,W]=sqrtKalmanUpdateWithPred(z,SR,zPred,otherInfo)
%%SQRTKALMANUPDATEWITHPRED Given the output of the measurement prediction
%           step from sqrtKalmanMeasPred and a measurement, complete the
%           measurement update step of the square-root Kalman filter.
%           Separating the measurement prediction step from the rest of the
%           update step can make the creation of multiple measurement
%           association hypotheses from a single target prediction more
%           efficient. The full measurement update function is
%           sqrtKalmanUpdate.
%
%INPUTS: z The zDimX1 measurement vector.
%       SR The zDimXzDim lower-triangular square root of the measurement
%          covariance matrix in the native coordinate system of the
%          measurement.
%    zPred The zDimXnumComp measurement predictions from the filter.
%   PzPred The zDimXzDimXnumComp covariance matrices associated with zPred.
% otherInfo The intermediate results returned in the otherInfo output of
%          the sqrtKalmanMeasPred function.
%
%OUTPUTS: xUpdate The xDimXnumComp updated state vectors.
%         SUpdate The updated xDimXxDimXnumComp lower-triangular square-
%                 root state covariance matrices.
%      innov, Szz The zDimXnumComp innovations and the zDimXzDimXnumComp
%                 square-root innovation covariance matrix are returned in
%                 case one wishes to analyze the consistency of the
%                 estimator or use those values in gating or likelihood
%                 evaluation.
%               W The xDimXzDimXnumComp gain used in the update. This can
%                 be useful when gating and using the function
%                 calcMissedGateCov.
%
%See the comments to the function sqrtKalmanMeasPred for an example of
%usage of this function. See the comments to sqrtKalmanUpdate for more
%information on the algorithm.
%
%June 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

xPred=otherInfo.xPred;
SPred=otherInfo.SPred;
Pxz=otherInfo.Pxz;
H=otherInfo.H;

xDim=size(xPred,1);
numComp=size(xPred,2);
zDim=size(z,1);

xUpdate=zeros(xDim,numComp);
SUpdate=zeros(xDim,xDim,numComp);
innov=zeros(zDim,numComp);
Szz=zeros(zDim,zDim,numComp);
W=zeros(xDim,zDim,numComp);

for k=1:numComp
    Szz(:,:,k)=tria([H*SPred(:,:,k),SR]);
    W=(Pxz(:,:,k)/Szz(:,:,k)')/Szz(:,:,k);
    innov(:,k)=z-zPred(:,k);
    xUpdate(:,k)=xPred(:,k)+W(:,:,k)*innov(:,k);
    temp=W(:,:,k)*H;
    SUpdate(:,:,k)=tria([(eye(size(temp))-temp)*SPred,W(:,:,k)*SR]);
end
end

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


function rinexe(ephemerisfile, outputfile)
%RINEXE Reads a RINEX Navigation Message file version 3.03 and
%	        reformats the data into a matrix with 21 rows and a column
%           for each satellite.  The matrix is stored in outputfile

%Typical call: rinexe('pta.96n','pta.nav')

%Kai Borre 04-18-96
%Copyright (c) by Kai Borre
%$Revision: 1.0 $  $Date: 1997/09/24  $
% Revision September 11, 2015 at Samara

% Units are either seconds, meters, or radians
fide = fopen(ephemerisfile);
head_lines = 0;
while 1  % We skip header
   head_lines = head_lines+1;
   line = fgetl(fide);
   answer = findstr(line,'END OF HEADER');
   if ~isempty(answer), break;	end;
end;
head_lines;
noeph = -1;
while 1
   noeph = noeph+1;
   line = fgetl(fide);
   if line == -1, break;  end
end;
noeph = noeph/8
frewind(fide);
for i = 1:head_lines, line = fgetl(fide); end;

% Set aside memory for the input
svprn	 = zeros(1,noeph);
weekno	 = zeros(1,noeph);
t0c	 = zeros(1,noeph);
tgd	 = zeros(1,noeph);
aodc	 = zeros(1,noeph);
toe	 = zeros(1,noeph);
af2	 = zeros(1,noeph);
af1	 = zeros(1,noeph);
af0	 = zeros(1,noeph);
aode	 = zeros(1,noeph);
deltan	 = zeros(1,noeph);
M0	 = zeros(1,noeph);
ecc	 = zeros(1,noeph);
roota	 = zeros(1,noeph);
toe	 = zeros(1,noeph);
cic	 = zeros(1,noeph);
crc	 = zeros(1,noeph);
cis	 = zeros(1,noeph);
crs	 = zeros(1,noeph);
cuc	 = zeros(1,noeph);
cus	 = zeros(1,noeph);
Omega0	 = zeros(1,noeph);
omega	 = zeros(1,noeph);
i0	 = zeros(1,noeph);
Omegadot = zeros(1,noeph);
idot	 = zeros(1,noeph);
accuracy = zeros(1,noeph);
health	 = zeros(1,noeph);
fit	 = zeros(1,noeph);

for i = 1:noeph
   line = fgetl(fide);	  %
   svprn(i) = str2num(line(2:3)); % a G for GPS in front is omitted
   year = line(5:8);
   month = line(10:11);
   day = line(13:14);
   hour = line(16:17);
   minute = line(19:20);
   second = line(22:23);
   af0(i) = str2num(line(24:42));
   af1(i) = str2num(line(43:61));
   af2(i) = str2num(line(62:80));
   line = fgetl(fide);	  %
   IODE = line(5:23);
   crs(i) = str2num(line(24:42));
   deltan(i) = str2num(line(43:61));
   M0(i) = str2num(line(62:80));
   line = fgetl(fide) ;	  %
   cuc(i) = str2num(line(5:23));
   ecc(i) = str2num(line(24:42));
   cus(i) = str2num(line(43:61));
   roota(i) = str2num(line(62:80));
   line=fgetl(fide);
   toe(i) = str2num(line(5:23));
   cic(i) = str2num(line(24:42));
   Omega0(i) = str2num(line(43:61));
   cis(i) = str2num(line(62:80));
   line = fgetl(fide);	    %
   i0(i) =  str2num(line(5:23));
   crc(i) = str2num(line(24:42));
   omega(i) = str2num(line(43:61));
   Omegadot(i) = str2num(line(62:80));
   line = fgetl(fide);	    %
   idot(i) = str2num(line(5:23));
   codes = str2num(line(24:42));
   weekno = str2num(line(43:61));
   L2flag = str2num(line(62:80));
   line = fgetl(fide);	    %
   svaccur = str2num(line(5:23));
   svhealth = str2num(line(24:42));
   tgd(i) = str2num(line(43:61));
   iodc = line(62:80);
   line = fgetl(fide);	    %
   tom(i) = str2num(line(5:23));
   spare = line(24:42);
%   spare = line(43:61);
%   spare = line(62:80);
end
status = fclose(fide);

%  Description of variable eph.
eph(1,:)  = svprn;
eph(2,:)  = af2;
eph(3,:)  = M0;
eph(4,:)  = roota;
eph(5,:)  = deltan;
eph(6,:)  = ecc;
eph(7,:)  = omega;
eph(8,:)  = cuc;
eph(9,:)  = cus;
eph(10,:) = crc;
eph(11,:) = crs;
eph(12,:) = i0;
eph(13,:) = idot;
eph(14,:) = cic;
eph(15,:) = cis;
eph(16,:) = Omega0;
eph(17,:) = Omegadot;
eph(18,:) = toe;
eph(19,:) = af0;
eph(20,:) = af1;
eph(21,:) = toe;

fidu = fopen(outputfile,'w');
count = fwrite(fidu,[eph],'double');
fclose all;
%%%%%%%%% end rinexe.m %%%%%%%%%


function [pvec, pstruct] = tapas_hgf_jget_transp(r, ptrans)
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2013 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

pvec    = NaN(1,length(ptrans));
pstruct = struct;

l = r.c_prc.n_levels;

pvec(1:l)         = ptrans(1:l);                                  % mux_0
pstruct.mux_0     = pvec(1:l);
pvec(l+1:2*l)     = exp(ptrans(l+1:2*l));                         % sax_0
pstruct.sax_0     = pvec(l+1:2*l);
pvec(2*l+1:3*l)   = ptrans(2*l+1:3*l);                            % mua_0
pstruct.mua_0     = pvec(2*l+1:3*l);
pvec(3*l+1:4*l)   = exp(ptrans(3*l+1:4*l));                       % saa_0
pstruct.saa_0     = pvec(3*l+1:4*l);
pvec(4*l+1)       = exp(ptrans(4*l+1));                           % kau
pstruct.kau       = pvec(4*l+1);
pvec(4*l+2:5*l)   = exp(ptrans(4*l+2:5*l));                       % kax
pstruct.kax       = pvec(4*l+2:5*l);
pvec(5*l+1:6*l-1) = exp(ptrans(5*l+1:6*l-1));                     % kaa
pstruct.kaa       = pvec(5*l+1:6*l-1);
pvec(6*l)         = ptrans(6*l);                                  % omu
pstruct.omu       = pvec(6*l);
pvec(6*l+1:7*l)   = ptrans(6*l+1:7*l);                            % omx
pstruct.omx       = pvec(6*l+1:7*l);
pvec(7*l+1:8*l)   = ptrans(7*l+1:8*l);                            % oma
pstruct.oma       = pvec(7*l+1:8*l);

return;


% Copyright 2013 The MathWorks, Inc
%% Ground-Based Monostatic Radar - Range and Doppler Estimation
% This example shows how to model a ground-based monostatic pulse radar to
% estimate the range and speed of fluctuating targets.
%% Model
maxrange  = 8000; % Maximum range (m)
range_res = 50;   % Range resolution (m)
pd        = 0.9;  % Probability of detection
pfa       = 1e-6; % Probability of false alarm
nint      = 48;   % Number of pulses to integrate

% The transmitted waveform is a chirp. An S-band pyramidal antenna array is
% used in this case. The antenna is mounted on a vehicle moving at 67 mph
% (30 m/s).
apos = [0; 0; 0];                  % Antenna position
avel = [30*cos(30);30*sin(30);0];  % Antenna velocity
fc   = 3e9;                        % Operating frequency
sAnt = lowProfileArray('FrequencyRange',[2/3*fc 4/3*fc],'ViewArray',false);
[sWav,sTx,sAntPlat,sRad,fs,prf] = setupTx(maxrange,range_res,pd,pfa,nint,sAnt,apos,avel,fc);

% Three targets are set in motion in a free space environment.
tgtRCS = [1.2 1.1 1.05];
tgtpos = [2000 4560 5825; 0 0 0; 0 0 0];
tgtvel = [100 -400 350;0 0 0; 0 0 0];    % m/s
[sTgt,sTgtMotion,sChan] = setupTheater(tgtRCS,tgtpos,tgtvel,fc,fs);

% On the receiver side, matched filter, time-varying gain control and
% non-coherent pulse integration are applied to improve SNR. A
% range-Doppler map is generated and a Neyman-Pearson (NP) decision rule is
% used to achieve the desired Pfa.
nf          = 0;                                   % Noise figure
fast_time   = 0:1/fs:1/prf-1/fs;                   % Fast time grid
range_gates = physconst('LightSpeed')*fast_time/2; % Range gates
pulses      = zeros(numel(fast_time),nint);        % Pre-allocate
intpulses   = zeros(numel(fast_time),1);
[sCol,sRx,sRD,sMFilt,sTVG,threshold] = setupRx(nint,nf,pfa,maxrange,range_gates,sWav,sAnt,fc);

%% Simulate
rsig = zeros(336,3);
ang  = zeros(2,3);
for m = 1:2000
    [s,tx_status] = step(sTx,step(sWav));               % Transmit pulse
    [apos,avel]   = step(sAntPlat,1/prf);               % Move antenna
    for n = 1:3                                         % For each target
        [tpos,tvel]   = step(sTgtMotion{n},1/prf);      % Move target
        [~, ang(:,n)] = rangeangle(tpos,apos);          % Angle between antenna and target
        tsig          = step(sRad,s,ang(:,n));          % Radiate signal
        tsig          = step(sChan{n},tsig,apos,tpos,avel,tvel); % Propagate two ways
        rsig(:,n)     = step(sTgt{n},tsig,true);        % Reflect off target
    end
    rsig = step(sCol,rsig,ang);                         % Collect
    rsig = sum(rsig,2);                                 % Beamform
    nn   = mod(m-1,nint)+1;
    pulses(:,nn) = step(sRx,rsig,~(tx_status>0));       % Receiver pre-amp
    pulses(:,nn) = step(sTVG,pulses(:,nn));             % Time varying gain
    [rdmap,rgrid,sgrid] = step(sRD,pulses,sMFilt.Coefficients); % Range-Doppler estimate
    pulses(:,nn) = step(sMFilt,pulses(:,nn));           % Matched filter
    if nn == nint
        intpulses  = pulsint(pulses,'noncoherent');     % Pulse integration
        [pmax,detect] = findpeaks(intpulses,'MinPeakHeight',sqrt(threshold)); % Detection
        tgtrange   = range_gates(detect-(numel(sMFilt.Coefficients)-1));      % Range estimation
    end
    viewSignals
end


function [h,t,w,w_times,halfh, auc] = fir2htw2(b,varargin)
% Estimates height, time to peak, and width of FIR response
%
% :Usage:
% ::
%
%     [h,t,w,w_times,halfh, auc] = fir2htw2(b,[hconstraint],[doplot],[colors cell])
%
% :Inputs:
%
%   **b:**
%        beta/estimate series for hemodynamic response curve
%
%   hconstraint:**
%        max time in samples that can be considered the peak
%        (default = last sample)
%
%   doplot:**
%        flag for plot, 1/0
%
%   colors:**
%        cell vector of colors for plot
%
% ..
%    tor wager, 2/14/05
% ..
%
% minh = min height
%
% This version uses turning points (zero gradient) to find the largest
% "hump" in the data and the time it occurs.
%
% :Example:
% ::
%
%    hrf = spm_hrf(.5); hrf = hrf ./ max(hrf); hrf = hrf + .1 * randn(length(hrf), 1);
%    create_figure('hrf'); plot(hrf);
%    [h,t,w,w_times,halfh, auc] = fir2htw2(hrf, [], 1);
%

    if size(b,2) < length(b), b = b'; end

    if ~isempty(varargin) && ~isempty(varargin{1})
        hconstraint = varargin{1};
    else
        hconstraint = length(b);
    end

    if hconstraint > length(b)
        warning('fir2htw is trying to use more betas than there are!  limiting.')
        hconstraint = length(b);
    end

    if length(varargin) > 1,
        doplot = varargin{2};
    else
        doplot = 1;
    end

    % exit if empty
    if isempty(b) || all(b==0) || all(isnan(b))
        h = NaN; t = h; w = h; w_times = [NaN NaN]; halfh = NaN;
        warning('fir2htw:  Empty, nan, or all zero values!')
        return
    end

    % exit if all vals are same
    if all(b == mean(b)), h = NaN; t = h; w = h; w_times = [NaN NaN]; halfh = NaN;
        warning('fir2htw:  all values are the same!')
        return
    end

    colors = {'ro-'};
    if length(varargin) > 2, colors = varargin{3}; end

    % find turning points in data
    tmp = diff(b);
    turnpts = find([0 diff(sign(tmp))]);

    if isempty(turnpts)
        % monotonic increasing or decreasing function, can't define
        turnpts = [find(b==min(b)) find(b==max(b))];
    end

    dat = b(1:hconstraint) - mean(b(1:2));   %- b(1); % ; - mean(b(1:2));          % deviations from first point (baseline)

    turnpts(find(turnpts > length(dat))) = [];  % eliminate extra turn points after hconst

    if isempty(turnpts)
        % monotonic increasing or decreasing function, can't define
        turnpts = [find(dat==min(dat)) find(dat==max(dat))];
    end

    tpdat = abs(dat(turnpts));

    t = turnpts(tpdat == max(tpdat));       % time of max absolute turning point
    t = t(1);
    h = dat(t);                             % the value at max

    % calculations for width
    halfh = .5 * h;                         % 1/2 the max to min distance

    %d = distance(halfh,dat);

    if h > 0
        wh = find(dat > halfh);
    else
        wh = find(dat < halfh);
    end

    if isempty(wh), wh = NaN; end

    % first half
    x2 = max(wh(1),1);    % first above halfh
    x1 = max(wh(1)-1,1);  % first above-half - 1
    y2 = dat(x2);
    y1 = dat(x1);
    m = y2 - y1;          % slope, x reduces to 1 (x2 - x1 = 1)
    if m == 0
        w_times(1) = x1;    % exact match
    else
        w_times(1) = x1 + (halfh - y1) ./ m; % solve y = mx + b for x* given m; y1 = b
    end

    % second half
    x1 = min(wh(end),hconstraint);      % last one above halfh
    x2 = min(wh(end)+1,hconstraint);    % + 1
    y2 = dat(x2);  y1 = dat(x1);
    m = y2 - y1;                      % slope, x reduces to 1 (x2 - x1 = 1)
    if m == 0
        w_times(2) = x1;    % exact match
    else
        w_times(2) = x1 + (halfh - y1) ./ m;     % solve y = mx + b for x* given m; y1 = b
    end

    w = w_times(2) - w_times(1);        % width in elements

    % Add area under curve measure
    if nargout > 5, auc = sum(dat); end

    if doplot
        minh = mean(b(1:2));
        try

            hold on
            h1 = arrow([t minh],[t h+minh],'Length',12);        % height arrow
            h2 = arrow([0 h+minh],[t h+minh],'Length',12);      % delay arrow
            h3 = arrow([w_times(1) halfh+minh],[w_times(2) halfh+minh],'Length',12);
            h4 = arrow([w_times(2) halfh+minh],[w_times(1) halfh+minh],'Length',12);

            h5 = text(t + .1 * w, halfh+minh + .4*(halfh+minh),'h','FontSize',16,'FontWeight','bold');
            h6 = text(t - .5 * w, h+minh - .2*(halfh+minh),'t','FontSize',16,'FontWeight','bold');
            h7 = text(t - .5 * w, halfh+minh - .2*(halfh+minh),'w','FontSize',16,'FontWeight','bold');

            set(h1,'Color',colors{1}(1))
            set(h2,'Color',colors{1}(1))
            set(h3,'Color',colors{1}(1))
            set(h4,'Color',colors{1}(1))
            set(h5,'Color',colors{1}(1))
            set(h6,'Color',colors{1}(1))
            set(h7,'Color',colors{1}(1))

        catch
            warning('Error drawing arrows!')
        end
    end

end

% OLD EXTRA STUFF

% % range = max(b(1:hconstraint)) - min(b(1:hconstraint));
% %
% % % find all values within some tolerance of the half-max height
% % % if not enough points found to determine width, then interpolate until we
% % % get values.
% % % it would be better to interpolate!
% % tolval = .02; wh = []; resampleval = 1;
% %
% % tol = tolval * range;   % tolerance of tolval % of range
% % wh = find(d <= tol);    % find all points within tolerance
% %
% % while length(find(wh>t)) < 1 | length(find(wh<t)) < 1
% %     resampleval = resampleval + 1;
% %
% %     b2 = resample(b(1:hconstraint),resampleval,1); % interpolate
% %
% %     d = distance(halfh,b2);
% %     wh = find(d <= tol);    % find all points within tolerance
% %     wh = wh ./ resampleval;  % convert back to original time units
% %
% %     if resampleval > 10, warning('Could not find width!'), break, end
% % end
% %
% % % now find nearest elements to peak that are at max height
% % d_from_t = wh - t;
% % w_times = [max(d_from_t(d_from_t < 0)) min(d_from_t(d_from_t > 0))];
% %
% %
% %
% % if length(w_times) > 1
% %     w = w_times(2) - w_times(1);        % width in elements
% %     w_times = t + w_times;              % time indices of elements before and after peak at 1/2 max height
% % else
% %     w_times = [NaN NaN];
% %     w = NaN;                            % width is undefined; 1st or last tp was max
% % end


function test08 ( )

%*****************************************************************************80
%
%% TEST08 tests UNICYCLE_INDEX, UNICYCLE_INDEX_TO_SEQUENCE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 June 2012
%
%  Author:
%
%    John Burkardt
%
  n = 6;
  test_num = 5;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST08\n' );
  fprintf ( 1, '  UNICYCLE_INDEX converts a unicycle to index form.\n' );
  fprintf ( 1, '  UNICYCLE_INDEX_TO_SEQUENCE converts an index to unicycle form.\n' );

  for test = 1 : test_num

    [ u, seed ] = unicycle_random ( n, seed );

    unicycle_print ( n, u, '  The unicycle:' );

    u_index = unicycle_index ( n, u );

    unicycle_index_print ( n, u_index, '  The index form:' );

    u2 = unicycle_index_to_sequence ( n, u_index );

    unicycle_print ( n, u2, '  The unicycle recovered:' );

  end

  return
end


function [cum_ret, cumprod_ret, daily_ret, daily_portfolio] ...
    = corn_run(fid, data, w, c, tc, opts)
% This program simulates the BK strategy on data
%
% function [cum_ret, cumprod_ret, daily_ret, daily_portfolio, exp_ret] ...
%    = corn_run(fid, data, w, c, tc, opts)
%
% cum_ret: a number representing the final cumulative wealth.
% cumprod_ret: cumulative return until each trading period
% daily_ret: individual returns for each trading period
% daily_portfolio: individual portfolio for each trading period
% exp_ret: experts' return
%
% data: market sequence vectors
% fid: handle for write log file
% w: window size
% c: correlation coefficient threshold
% tc: transaction cost rate parameter
% opts: option parameter for behvaioral control
%
% Example: [cum_ret, cumprod_ret, daily_ret, daily_portfolio, exp_ret] ...
%            = corn_run(fid, data, w, c, tc, opts);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This file is part of OLPS: http://OLPS.stevenhoi.org/
% Original authors: Bin LI, Steven C.H. Hoi
% Contributors:
% Change log:
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%[T, N]=size(data);
[n, m] = size(data);

% Variables for return, start with uniform weight
% cumprod_ret = 1;
% daily_ret = 1;
% weight = ones(nStocks, 1)/nStocks;

cum_ret = 1;
cumprod_ret = ones(n, 1);
daily_ret = ones(n, 1);
day_weight = ones(m, 1)/m;  %#ok<*NASGU>
day_weight_o = zeros(m, 1);
daily_portfolio = zeros(n, m);

% print file head
fprintf(fid, '-------------------------------------\n');
if (~opts.quiet_mode)
    fprintf(fid, 'Parameters [w=%d, c=%f, tc=%f\n]', w, c, tc);
    fprintf(fid, 'day\t Daily Return\t Total return\n');
end

for t = 1:1:n,
    % Calculate t's portfolio
    if (t >=2)
        [day_weight] = corn_kernel(data(1:t-1, :), w, c);
    end

    % Normalize the constraint
    day_weight = day_weight./sum(day_weight);
    daily_portfolio(t, :) = day_weight';

    % Cal t's return and total return
    daily_ret(t, 1) = (data(t, :)*day_weight)*(1-tc/2*sum(abs(day_weight-day_weight_o)));
    cum_ret = cum_ret * daily_ret(t, 1);
    cumprod_ret(t, 1) = cum_ret;

    day_weight_o = day_weight.*data(t, :)'/daily_ret(t, 1);

    % Debug information
    fprintf(fid, '%d\t%f\t%f\n', t, daily_ret(t, 1), cum_ret);
    if (~opts.quiet_mode)
        if (~mod(t, opts.display_interval)),
            fprintf(1, '%d\t%f\t%f\n', t, daily_ret(t, 1), cum_ret);
        end
    end
end

% Debug Information
fprintf(fid, 'CORN(w:%d, c:%.2f, tc:%.4f), Final return: %.2f\n', ...
    w, c, tc, cum_ret);
fprintf(fid, '-------------------------------------\n');

fprintf(1, 'CORN(w:%d, c:%.2f, tc:%.4f), Final return: %.2f\n', ...
    w, c, tc, cum_ret);
fprintf(fid, '-------------------------------------\n');
end


function [ ] = loop_compare_diff_domain( cutCostsFile, max_cut_num, search_domain_1, search_domain_2, search_domain_3, max_local_dis )
%LOOP_COMPARE_DIFF_SEARCH Summary of this function goes here
%   Detailed explanation goes here

    DP_cost_list_1 = []; DP_time_list_1 = [];
    DP_cost_list_2 = []; DP_time_list_2 = [];
    DP_cost_list_3 = []; DP_time_list_3 = [];

    av_cost_list = []; av_time_list = [];

    for i = 2:max_cut_num
        [ DP_cost_1, DP_time_1, av_cost, av_time ] = DP_cut_domain( cutCostsFile, i, search_domain_1);
        [ DP_cost_2, DP_time_2, av_cost, av_time ] = DP_cut_domain( cutCostsFile, i, search_domain_2);
        [ DP_cost_3, DP_time_3, av_cost, av_time ] = DP_cut_domain( cutCostsFile, i, search_domain_3);

        DP_cost_list_1(i, :) = DP_cost_1;
        DP_time_list_1(i, :) = DP_time_1 * 1000;

        DP_cost_list_2(i, :) = DP_cost_2;
        DP_time_list_2(i, :) = DP_time_2 * 1000;

        DP_cost_list_3(i, :) = DP_cost_3;
        DP_time_list_3(i, :) = DP_time_3 * 1000;

        av_cost_list(i, :) = av_cost;
        av_time_list(i, :) = av_time * 1000;

    end

%     % for local min cut
%     local_cost_list = []; local_time_list = []; cut_num_list = [];
%     i = 2;
%     exp_num = 2;
%     for l = max_local_dis:-1:1
%         [ local_cost, local_time, cut_num ] = localMinTest( cutCostsFile, l);
%
%         if cut_num >= exp_num
%             local_cost_list(i, :) = local_cost;
%             local_time_list(i, :) = local_time * 1000;
%             cut_num_list(i, :) = cut_num;
%             exp_num = exp_num + 1;
%             i = i + 1;
%         end
%
%         if cut_num >= max_cut_num
%            break;
%         end
%
%     end

    % optimal cut costs, but not unniform obviously
    opti_cost_list = [];
    for i = 2:max_cut_num
       opti_cost_list(i, :) = get_the_optimal(cutCostsFile, i);
    end

    figure;
    plot(DP_cost_list_1, 'g-', 'LineWidth', 2);
    hold on;
    plot(DP_cost_list_2, 'b-', 'LineWidth', 2);
    hold on;
    plot(DP_cost_list_3, 'r-', 'LineWidth', 2);
    hold on;
    plot(av_cost_list, 'k-', 'LineWidth', 2);
    hold on;
    plot(opti_cost_list, 'm-', 'LineWidth', 2)
%     plot(cut_num_list, local_cost_list, 'm-', 'LineWidth', 2);
    grid on;
    axis([2 max_cut_num 0 max(av_cost_list)]);
    xlabel('Section Num', 'FontWeight', 'bold', 'FontSize', 12);
    ylabel('Cut Cost', 'FontWeight', 'bold', 'FontSize', 12);
%     legend('DP 5', 'DP 10', 'DP 30', 'Uniform Cutting', 'Local Minimum');
    legend('DP 5', 'DP 10', 'DP 30', 'Uniform Cutting', 'Minest');

%     figure;
%     plot(DP_time_list_1, 'g-', 'LineWidth', 2);
%     hold on;
%     plot(DP_time_list_2, 'b-', 'LineWidth', 2);
%     hold on;
%     plot(DP_time_list_3, 'r-', 'LineWidth', 2);
%     hold on;
%     plot(av_time_list, 'k-', 'LineWidth', 2);
%     hold on;
% %     plot(cut_num_list, local_time_list, 'c-', 'LineWidth', 2);
%     grid on;
%     axis([2 100 0 2]);
%     xlabel('Section Num', 'FontWeight', 'bold', 'FontSize', 12);
%     ylabel('Time Cost (ms)', 'FontWeight', 'bold', 'FontSize', 12);
% %     legend('Dynamic Programming', 'Uniform Cutting', 'Local Minimum');
%     legend('DP 5', 'DP 10', 'DP 20', 'Uniform Cutting');

end


%compute dytailcentral_mm

function [data,units]=compute_dytailcentral_mm(trx,n)

larvae=trx.exp2flies{n};
numlarvae=numel(larvae);
dytailcentral_mm=cell(1,numlarvae);
for i=1:numlarvae
    larva=larvae(i);
    dytailcentral_mm{1,i}=(trx(larva).ytailcentral_mm(2:end)-trx(larva).ytailcentral_mm(1:end-1))./trx(larva).dt;
end

units=parseunits('mm/s');
data=dytailcentral_mm;