function fem2d_project ( sample_prefix, fem_prefix )

%*****************************************************************************80
%
%% MAIN is the main program for FEM2D_PROJECT.
%
%  Discussion:
%
%    FEM2D_PROJECT reads files defining a sampling of a (scalar or vector)
%    function of 2 arguments, and a list of nodes and triangular elements
%    to use for a finite element representation of the data.
%
%    It computes a set of finite element coefficients to be associated with
%    the given finite element mesh, and writes that information to a file
%    so that an FEM representation is formed by the node, element and value
%    files.
%
%  Usage:
%
%    fem2d_project ( 'sample_prefix', 'fem_prefix' )
%
%    where 'sample_prefix' is the common prefix for the SAMPLE files:
%
%    * sample_prefix_nodes.txt,     the node coordinates where samples were taken;
%    * sample_prefix_elements.txt,  the nodes that make up each element;
%    * sample_prefix_values.txt,    the sample values.
%
%    and 'fem_prefix' is the common prefix for the FEM files:
%
%    * fem_prefix_nodes.txt,    the node coordinates;
%    * fem_prefix_elements.txt, the nodes that make up each element;
%    * fem_prefix_values.txt,   the values defined at each node,
%                               (computed by this program).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 February 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string SAMPLE_PREFIX, the common prefix for sample files.
%
%    Input, string FEM_PREFIX, the common prefix for FEM files.
%
  timestamp ( );

  fprintf ( '\n' );
  fprintf ( 'FEM2D_PROJECT\n' );
  fprintf ( '  MATLAB version.\n' );
  fprintf ( '\n' );
  fprintf ( '  Read files defining a sampling of a function of 2 arguments.\n' );
  fprintf ( '  Read files defining a finite element mesh.\n' );
  fprintf ( '  Project the sample data onto the mesh, and\n' );
  fprintf ( '  write a file of FEM coefficient values.\n' );
%
%  Get the number of command line arguments.
%
  if ( nargin < 1 )

    fprintf ( '\n' );
    sample_prefix = input ( 'Enter the sample file prefix:  ' );

  else

  end

  if ( nargin < 2 )

    fprintf ( '\n' );
    fem_prefix = input ( 'Enter the FEM file prefix:  ' );

  else

  end
%
%  Create the filenames.
%
  sample_node_filename    = strcat ( sample_prefix, '_nodes.txt' );
  sample_element_filename = strcat ( sample_prefix, '_elements.txt' );
  sample_value_filename   = strcat ( sample_prefix, '_values.txt' );

  fem_node_filename       = strcat ( fem_prefix, '_nodes.txt' );
  fem_element_filename    = strcat ( fem_prefix, '_elements.txt' );
  fem_value_filename      = strcat ( fem_prefix, '_values.txt' );
%
%  Read the SAMPLE NODE, ELEMENT and VALUE data.
%
  sample_node_xy = load ( sample_node_filename );
  sample_node_xy = sample_node_xy';
  [sample_node_dim, sample_node_num ] = size ( sample_node_xy );

  fprintf ( '\n' );
  fprintf ( '  Sample node spatial dimension is %d\n', sample_node_dim );
  fprintf ( '  Sample node number is            %d\n', sample_node_num );

  if ( sample_node_dim ~= 2 )
    fprintf ( '\n' );
    fprintf ( 'FEM2D_PROJECT - Fatal error!\n' );
    fprintf ( '  Spatial dimension of the sample nodes is not 2.\n' );
    return
  end

  sample_element_node = load ( sample_element_filename );
  sample_element_node = sample_element_node';
  [ sample_element_order, sample_element_num ] = size ( sample_element_node );

  fprintf ( '\n' );
  fprintf ( '  Sample element order is  %d\n', sample_element_order );
  fprintf ( '  Sample element number is %d\n', sample_element_num );

  if ( sample_element_order ~= 3 )
    fprintf ( '\n' );
    fprintf ( 'FEM2D_PROJECT - Fatal error!\n' );
    fprintf ( '  The sample elements must be of order 3.\n' );
    return
  end

  sample_value = load ( sample_value_filename );
  sample_value = sample_value';
  [ sample_value_dim, sample_value_num ] = size ( sample_value );

  fprintf ( '\n' );
  fprintf ( '  Sample value dimension is        %d\n', sample_value_dim );
  fprintf ( '  Sample value number is           %d\n', sample_value_num );

  if ( sample_value_num ~= sample_node_num )
    fprintf ( '\n' );
    fprintf ( 'FEM2D_PROJECT - Fatal error!\n' );
    fprintf ( '  Number of sample nodes and values are not equal.\n' );
    return
  end

  sample_value_min = min ( sample_value, [], 2 );
  sample_value_max = max ( sample_value, [], 2 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '     Index      SAMPLE Min      SAMPLE Max\n' );
  fprintf ( 1, '\n' );
  for i = 1 : sample_value_dim
    fprintf ( 1, '  %8d  %14f  %14f\n', i, sample_value_min(i), sample_value_max(i) );
  end
%
%  Create the sample element neighbor array.
%
  sample_element_neighbor = triangulation_order3_neighbor_triangles ( ...
    sample_element_num, sample_element_node );

  fprintf ( '\n' );
  fprintf ( '  The element neighbor array has been computed.\n' );
%
%  Read the FEM NODE and ELEMENT data.
%
  fem_node_xy = load ( fem_node_filename );
  fem_node_xy = fem_node_xy';
  [ fem_node_dim, fem_node_num ] = size ( fem_node_xy );

  fprintf ( '\n' );
  fprintf ( '  The FEM node dimension is        %d\n', fem_node_dim );
  fprintf ( '  The FEM node number is           %d\n', fem_node_num );

  if ( fem_node_dim ~= 2 )
    fprintf ( '\n' );
    fprintf ( 'FEM2D_PROJECT - Fatal error!\n' );
    fprintf ( '  Spatial dimension of the nodes is not 2.\n' );
    return
  end

  fem_element_node = load ( fem_element_filename );
  fem_element_node = fem_element_node';
  [ fem_element_order, fem_element_num ] = size ( fem_element_node );

  fprintf ( '  The FEM element order is         %d\n', fem_element_order );
  fprintf ( '  The FEM element number is        %d\n', fem_element_num );

  if ( fem_element_order ~= 3 )
    fprintf ( '\n' );
    fprintf ( 'FEM2D_PROJECT - Fatal error!\n' );
    fprintf ( '  The FEM elements must be of order 3.\n' );
    return
  end
%
%  Compute the FEM values.
%
  fem_value_dim = sample_value_dim;
  fem_value_num = fem_node_num;

  fem_value = fem2d_transfer ( sample_element_order, ...
    sample_element_num, sample_value_dim, ...
    sample_node_xy, sample_element_node, sample_element_neighbor, sample_value, ...
    fem_node_num, fem_element_num, fem_value_dim, fem_node_xy, fem_element_node );
%
%  Print Min, Max.
%
  fem_value_min = min ( fem_value, [], 2 );
  fem_value_max = max ( fem_value, [], 2 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '     Index         FEM Min         FEM Max\n' );
  fprintf ( 1, '\n' );
  for i = 1 : fem_value_dim
    fprintf ( 1, '  %8d  %14f  %14f\n', i, fem_value_min(i), fem_value_max(i) );
  end
%
%  Write the FEM values.
%
  r8mat_write ( fem_value_filename, fem_value_dim, fem_value_num, fem_value );

  fprintf ( '\n' );
  fprintf ( '  FEM value data written to "%s".\n',  fem_value_filename );
%
%  Terminate.
%
  fprintf ( '\n' );
  fprintf ( 'FEM2D_PROJECT\n' );
  fprintf ( '  Normal end of execution.\n' );

  fprintf ( '\n' );
  timestamp ( );

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

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

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

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

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

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

%*****************************************************************************80
%
%% BASIS_MN_T6: all bases for N points in a T6 element.
%
%  Discussion:
%
%    The routine is given the coordinates of the vertices and midside
%    nodes of a triangle.  It works directly with these coordinates, and does
%    not refer to a reference element.
%
%    This routine requires that the midside nodes be "in line"
%    with the vertices, that is, that the sides of the triangle be
%    straight.  However, the midside nodes do not actually have to
%    be halfway along the side of the triangle.
%
%  Physical element T6:
%
%    This picture indicates the assumed ordering of the six nodes
%    of the triangle.
%
%             3
%            / \
%           /   \
%          6     5
%         /       \
%        /         \
%       1-----4-----2
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 February 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real T(2,6), the nodal oordinates of the element.
%    It is common to list these points in counter clockwise order.
%
%    Input, real P(2,N), the evaluation points.
%
%    Output, real PHI(6,N), the basis functions at the evaluation points.
%
%    Output, real DPHIDX(6,N), DPHIDY(6,N), the basis derivatives at the
%    evaluation points.
%
%  Local Parameters:
%
%    Local, real AREA, is (twice) the area of the triangle.
%

%
%  Basis function 1: PHI(X,Y) = G(3,2) * H(6,4) / normalization.
%
  gx(1:n) = ( p(1,1:n) - t(1,2) ) * ( t(2,3)   - t(2,2) ) ...
          - ( t(1,3)   - t(1,2) ) * ( p(2,1:n) - t(2,2) );

  gn(1:n) = ( t(1,1) - t(1,2) ) * ( t(2,3) - t(2,2) ) ...
          - ( t(1,3) - t(1,2) ) * ( t(2,1) - t(2,2) );

  hx(1:n) = ( p(1,1:n) - t(1,4) ) * ( t(2,6)   - t(2,4) ) ...
          - ( t(1,6)   - t(1,4) ) * ( p(2,1:n) - t(2,4) );

  hn(1:n) = ( t(1,1) - t(1,4) ) * ( t(2,6) - t(2,4) ) ...
          - ( t(1,6) - t(1,4) ) * ( t(2,1) - t(2,4) );

  phi(1,1:n) = ( gx(1:n) .* hx(1:n) ) ./ ( gn(1:n) .* hn(1:n) );

  dphidx(1,1:n) =  ( ( t(2,3) - t(2,2) ) * hx(1:n) ...
                    + gx(1:n) * ( t(2,6) - t(2,4) ) ) ...
                  ./ ( gn(1:n) .* hn(1:n) );

  dphidy(1,1:n) = -( ( t(1,3) - t(1,2) ) * hx(1:n) ...
                   + gx * ( t(1,6) - t(1,4) ) ) ...
                  ./ ( gn(1:n) .* hn(1:n) );
%
%  Basis function 2: PHI(X,Y) = G(3,1) * H(4,5) / normalization.
%
  gx(1:n) = ( p(1,1:n) - t(1,1) ) * ( t(2,3)   - t(2,1) ) ...
          - ( t(1,3)   - t(1,1) ) * ( p(2,1:n) - t(2,1) );

  gn(1:n) = ( t(1,2) - t(1,1) ) * ( t(2,3) - t(2,1) ) ...
          - ( t(1,3) - t(1,1) ) * ( t(2,2) - t(2,1) );

  hx(1:n) = ( p(1,1:n) - t(1,5) ) * ( t(2,4)   - t(2,5) ) ...
          - ( t(1,4)   - t(1,5) ) * ( p(2,1:n) - t(2,5) );

  hn(1:n) = ( t(1,2) - t(1,5) ) * ( t(2,4) - t(2,5) ) ...
          - ( t(1,4) - t(1,5) ) * ( t(2,2) - t(2,5) );

  phi(2,1:n) = ( gx(1:n) .* hx(1:n) ) ./ ( gn(1:n) .* hn(1:n) );

  dphidx(2,1:n) =  ( ( t(2,3) - t(2,1) ) * hx(1:n) ...
              + gx(1:n) * ( t(2,4) - t(2,5) ) ) ...
              ./ ( gn(1:n) .* hn(1:n) );

  dphidy(2,1:n) = -( ( t(1,3) - t(1,1) ) * hx(1:n) ...
              + gx(1:n) * ( t(1,4) - t(1,5) ) ) ...
              ./ ( gn(1:n) .* hn(1:n) );
%
%  Basis function 3: PHI(X,Y) = G(1,2) * H(5,6) / normalization.
%
  gx(1:n) = ( p(1,1:n) - t(1,2) ) * ( t(2,1)   - t(2,2) ) ...
          - ( t(1,1)   - t(1,2) ) * ( p(2,1:n) - t(2,2) );

  gn(1:n) = ( t(1,3) - t(1,2) ) * ( t(2,1) - t(2,2) ) ...
          - ( t(1,1) - t(1,2) ) * ( t(2,3) - t(2,2) );

  hx(1:n) = ( p(1,1:n) - t(1,6) ) * ( t(2,5)   - t(2,6) ) ...
          - ( t(1,5)   - t(1,6) ) * ( p(2,1:n) - t(2,6) );

  hn(1:n) = ( t(1,3) - t(1,6) ) * ( t(2,5) - t(2,6) ) ...
          - ( t(1,5) - t(1,6) ) * ( t(2,3) - t(2,6) );

  phi(3,1:n) = ( gx(1:n) .* hx(1:n) ) ./ ( gn(1:n) .* hn(1:n) );

  dphidx(3,1:n) =  ( ( t(2,1) - t(2,2) ) * hx(1:n) ...
              + gx(1:n) * ( t(2,5) - t(2,6) ) ) ...
              ./ ( gn(1:n) .* hn(1:n) );

  dphidy(3,1:n) = -( ( t(1,1) - t(1,2) ) * hx(1:n) ...
              + gx(1:n) * ( t(1,5) - t(1,6) ) ) ...
              ./ ( gn(1:n) .* hn(1:n) );
%
%  Basis function 4: PHI(X,Y) = G(1,3) * H(2,3) / normalization.
%
  gx(1:n) = ( p(1,1:n) - t(1,3) ) * ( t(2,1)   - t(2,3) ) ...
          - ( t(1,1)   - t(1,3) ) * ( p(2,1:n) - t(2,3) );

  gn(1:n) = ( t(1,4) - t(1,3) ) * ( t(2,1) - t(2,3) ) ...
          - ( t(1,1) - t(1,3) ) * ( t(2,4) - t(2,3) );

  hx(1:n) = ( p(1,1:n) - t(1,3) ) * ( t(2,2)   - t(2,3) ) ...
          - ( t(1,2)   - t(1,3) ) * ( p(2,1:n) - t(2,3) );

  hn(1:n) = ( t(1,4) - t(1,3) ) * ( t(2,2) - t(2,3) ) ...
          - ( t(1,2) - t(1,3) ) * ( t(2,4) - t(2,3) );

  phi(4,1:n) = ( gx(1:n) .* hx(1:n) ) ./ ( gn(1:n) .* hn(1:n) );

  dphidx(4,1:n) =  ( ( t(2,1) - t(2,3) ) * hx(1:n) ...
              + gx(1:n) * ( t(2,2) - t(2,3) ) ) ...
              ./ ( gn(1:n) .* hn(1:n) );

  dphidy(4,1:n) = -( ( t(1,1) - t(1,3) ) * hx(1:n) ...
              + gx(1:n) * ( t(1,2) - t(1,3) ) ) ...
              ./ ( gn(1:n) .* hn(1:n) );
%
%  Basis function 5: PHI(X,Y) = G(2,1) * H(3,1) / normalization.
%
  gx(1:n) = ( p(1,1:n) - t(1,1) ) * ( t(2,2)   - t(2,1) ) ...
          - ( t(1,2)   - t(1,1) ) * ( p(2,1:n) - t(2,1) );

  gn(1:n) = ( t(1,5) - t(1,1) ) * ( t(2,2) - t(2,1) ) ...
          - ( t(1,2) - t(1,1) ) * ( t(2,5) - t(2,1) );

  hx(1:n) = ( p(1,1:n) - t(1,1) ) * ( t(2,3)   - t(2,1) ) ...
          - ( t(1,3)   - t(1,1) ) * ( p(2,1:n) - t(2,1) );

  hn(1:n) = ( t(1,5) - t(1,1) ) * ( t(2,3) - t(2,1) ) ...
          - ( t(1,3) - t(1,1) ) * ( t(2,5) - t(2,1) );

  phi(5,1:n) = ( gx(1:n) .* hx(1:n) ) ./ ( gn(1:n) .* hn(1:n) );

  dphidx(5,1:n) =  ( ( t(2,2) - t(2,1) ) * hx(1:n) ...
              + gx(1:n) * ( t(2,3) - t(2,1) ) ) ...
               ./ ( gn(1:n) .* hn(1:n) );

  dphidy(5,1:n) = -( ( t(1,2) - t(1,1) ) * hx(1:n) ...
              + gx(1:n) * ( t(1,3) - t(1,1) ) ) ...
              ./ ( gn(1:n) .* hn(1:n) );
%
%  Basis function 6: PHI(X,Y) = G(1,2) * H(3,2) / normalization.
%
  gx(1:n) = ( p(1,1:n) - t(1,2) ) * ( t(2,1)   - t(2,2) ) ...
          - ( t(1,1)   - t(1,2) ) * ( p(2,1:n) - t(2,2) );

  gn(1:n) = ( t(1,6) - t(1,2) ) * ( t(2,1) - t(2,2) ) ...
          - ( t(1,1) - t(1,2) ) * ( t(2,6) - t(2,2) );

  hx(1:n) = ( p(1,1:n) - t(1,2) ) * ( t(2,3)   - t(2,2) ) ...
          - ( t(1,3)   - t(1,2) ) * ( p(2,1:n) - t(2,2) );

  hn(1:n) = ( t(1,6) - t(1,2) ) * ( t(2,3) - t(2,2) ) ...
          - ( t(1,3) - t(1,2) ) * ( t(2,6) - t(2,2) );

  phi(6,1:n) = ( gx(1:n) .* hx(1:n) ) ./ ( gn(1:n) .* hn(1:n) );

  dphidx(6,1:n) =  ( ( t(2,1) - t(2,2) ) * hx(1:n) ...
              + gx(1:n) * ( t(2,3) - t(2,2) ) ) ...
              ./ ( gn(1:n) .* hn(1:n) );

  dphidy(6,1:n) = -( ( t(1,1) - t(1,2) ) * hx(1:n) ...
              + gx(1:n) * ( t(1,3) - t(1,2) ) ) ...
              ./ ( gn(1:n) .* hn(1:n) );

  return
end
function fem_value = fem2d_transfer ( sample_element_order, ...
  sample_element_num, sample_value_dim, sample_node_xy, ...
  sample_element_node, sample_element_neighbor, sample_value, fem_node_num, ...
  fem_element_num, fem_value_dim, fem_node_xy, fem_element_node )

%*****************************************************************************80
%
%% FEM2D_TRANSFER "transfers" from one finite element mesh to another.
%
%  Discussion:
%
%    1) the linear system A*X=B is defined with A being a full storage matrix.
%       This can easily be fixed using MATLAB's SPARSE facility.
%
%    2) the quadrature rule used is low order.
%
%    3) the triangular elements are assumed to be linear.
%
%
%    We are also given a set of "sample" finite element function defined
%    by SAMPLE_NODE_XY, SAMPLE_ELEMENT, and SAMPLE_VALUE.
%
%    We are given a second finite element mesh, FEM_NODE_XY and
%    FEM_ELEMENT_NODE.
%
%    Our aim is to "project" the sample data values into the finite element
%    space, that is, to come up with a finite element function FEM_VALUE which
%    well approximates the sample data.
%
%    Now let W(x,y) represent a function interpolating the sample data, and
%    let Vk(x,y) represent the finite element basis function associated with
%    node K.
%
%    Then we seek the coefficient vector U corresponding to a finite element
%    function U(x,y) of the form:
%
%      U(x,y) = sum ( 1 <= K <= N ) Uk * Vk(x,y)
%
%    To determine the coefficent vector entries U, we form a set of
%    projection equations.  For node K at grid point (I,J), the associated
%    basis function Vk(x,y) is used to pose the equation:
%
%      Integral U(x,y) Vk(x,y) dx dy = Integral W(x,y) Vk(x,y) dx dy
%
%    The left hand side is the usual stiffness matrix times the desired
%    coefficient vector U.  To complete the system, we simply need to
%    determine the right hand side, that is, the integral of the data function
%    W against the basis function Vk.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 February 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer SAMPLE_NODE_NUM, the number of nodes.
%
%    Input, integer SAMPLE_ELEMENT_ORDER, the element order.
%
%    Input, integer SAMPLE_ELEMENT_NUM, the number of elements.
%
%    Input, integer SAMPLE_VALUE_DIM, the value dimension.
%
%    Input, integer SAMPLE_VALUE_NUM, the number of values.
%
%    Input, real SAMPLE_NODE_XY(2,SAMPLE_NODE_NUM), the nodes.
%
%    Input, integer SAMPLE_ELEMENT_NODE(SAMPLE_ELEMENT_ORDER,SAMPLE_ELEMENT_NUM),
%    the nodes that make up each element.
%
%    Input, integer SAMPLE_ELEMENT_NEIGHBOR(3,SAMPLE_ELEMENT_NUM),
%    the neighbor triangles.
%
%    Input, real SAMPLE_VALUE(SAMPLE_VALUE_DIM,SAMPLE_NODE_NUM),
%    the values.
%
%    Input, integer FEM_NODE_NUM, the number of nodes.
%
%    Input, integer FEM_ELEMENT_ORDER, the element order.
%
%    Input, integer FEM_ELEMENT_NUM, the number of elements.
%
%    Input, integer FEM_VALUE_DIM, the value dimension.
%
%    Input, integer FEM_VALUE_NUM, the number of values.
%
%    Input, real FEM_NODE_XY(2,FEM_NODE_NUM), the nodes.
%
%    Input, integer FEM_ELEMENT_NODE(FEM_ELEMENT_ORDER,FEM_ELEMENT_NUM),
%    the nodes that make up each element.
%
%    Output, real FEM_VALUE(FEM_VALUE_DIM,FEM_VALUE_NUM),
%    the values.
%
  project_node_num = 1;
  quad_num = 3;
%
%  Assemble the coefficient matrix A and the right-hand side B.
%
  b = zeros (fem_node_num,fem_value_dim);
%
%  Define A as a sparse matrix.
%
  a = sparse ( [], [], [], fem_node_num, fem_node_num );

% a = zeros (fem_node_num,fem_node_num);

  for element = 1 : fem_element_num

    i1 = fem_element_node(1,element);
    i2 = fem_element_node(2,element);
    i3 = fem_element_node(3,element);

    area = 0.5 * ...
      ( fem_node_xy(1,i1) * ( fem_node_xy(2,i2) - fem_node_xy(2,i3) ) ...
      + fem_node_xy(1,i2) * ( fem_node_xy(2,i3) - fem_node_xy(2,i1) ) ...
      + fem_node_xy(1,i3) * ( fem_node_xy(2,i1) - fem_node_xy(2,i2) ) );
%
%  Consider each quadrature point.
%  Here, we use the midside nodes as quadrature points.
%
    for quad = 1 : quad_num

      q1 =       quad;
      q2 = mod ( quad, quad_num ) + 1;

      nq1 = fem_element_node(q1,element);
      nq2 = fem_element_node(q2,element);

      xq = 0.5 * ( fem_node_xy(1,nq1) + fem_node_xy(1,nq2) );
      yq = 0.5 * ( fem_node_xy(2,nq1) + fem_node_xy(2,nq2) );
      wq = 1.0 / 3.0;
%
%  Consider each test function in the element.
%
      for ti1 = 1 : 3

        ti2 = mod ( ti1,     3 ) + 1;
        ti3 = mod ( ti1 + 1, 3 ) + 1;

        nti1 = fem_element_node(ti1,element);
        nti2 = fem_element_node(ti2,element);
        nti3 = fem_element_node(ti3,element);

        qi = 0.5 * ( ...
            ( fem_node_xy(1,nti3) - fem_node_xy(1,nti2) ) ...
          * ( yq - fem_node_xy(2,nti2) ) ...
          - ( fem_node_xy(2,nti3) - fem_node_xy(2,nti2) ) ...
          * ( xq - fem_node_xy(1,nti2) ) ) / area;
%
%  The projection takes place here.  The finite element code needs the value
%  of the sample function at the point (XQ,YQ).  The call to PROJECTION
%  locates (XQ,YQ) in the triangulated mesh of sample data, and returns a
%  value produced by piecewise linear interpolation.
%
        project_node_xy(1,1) = xq;
        project_node_xy(2,1) = yq;

        project_value = projection ( sample_node_xy, ...
          sample_element_order, sample_element_num, sample_element_node, ...
          sample_element_neighbor, sample_value_dim, sample_value, ...
          project_node_num, project_node_xy );

        b(nti1,1:fem_value_dim) = b(nti1,1:fem_value_dim) ...
          + area * wq * ( project_value(1:fem_value_dim,1)' * qi );
%
%  Consider each basis function in the element.
%
        for tj1 = 1 : 3

          tj2 = mod ( tj1,     3 ) + 1;
          tj3 = mod ( tj1 + 1, 3 ) + 1;

          ntj1 = fem_element_node(tj1,element);
          ntj2 = fem_element_node(tj2,element);
          ntj3 = fem_element_node(tj3,element);

          qj = 0.5 * ( ...
              ( fem_node_xy(1,ntj3) - fem_node_xy(1,ntj2) ) ...
            * ( yq - fem_node_xy(2,ntj2) ) ...
            - ( fem_node_xy(2,ntj3) - fem_node_xy(2,ntj2) ) ...
            * ( xq - fem_node_xy(1,ntj2) ) ) / area;

          a(nti1,ntj1) = a(nti1,ntj1) + area * wq * ( qi * qj );

        end

      end

    end

  end
%
%  Solve the linear system A * X = B.
%
  fem_value = ( a \ b )';

  return
end
function isgn = i4col_compare ( m, n, a, i, j )

%*****************************************************************************80
%
%% I4COL_COMPARE compares columns I and J of a integer array.
%
%  Example:
%
%    Input:
%
%      M = 3, N = 4, I = 2, J = 4
%
%      A = (
%        1  2  3  4
%        5  6  7  8
%        9 10 11 12 )
%
%    Output:
%
%      ISGN = -1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 June 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, integer A(M,N), an array of N columns of vectors of length M.
%
%    Input, integer I, J, the columns to be compared.
%    I and J must be between 1 and N.
%
%    Output, integer ISGN, the results of the comparison:
%    -1, column I < column J,
%     0, column I = column J,
%    +1, column J < column I.
%

%
%  Check.
%
  if ( i < 1)
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4COL_COMPARE - Fatal error!\n' );
    fprintf ( 1, '  Column index I = %d < 1.\n', i );
    error ( 'I4COL_COMPARE - Fatal error!' );
  end

  if ( n < i )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4COL_COMPARE - Fatal error!\n' );
    fprintf ( 1, '  N = %d < column index I = %d.\n', n, i );
    error ( 'I4COL_COMPARE - Fatal error!' );
  end

  if ( j < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4COL_COMPARE - Fatal error!\n' );
    fprintf ( 1, '  Column index J = %d < 1.\n', j );
    error ( 'I4COL_COMPARE - Fatal error!' );
  end

  if ( n < j )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4COL_COMPARE - Fatal error!\n' );
    fprintf ( 1, '  N = %d < column index J = %d.\n', n, j );
    error ( 'I4COL_COMPARE - Fatal error!' );
  end

  isgn = 0;

  if ( i == j )
    return
  end

  k = 1;

  while ( k <= m )

    if ( a(k,i) < a(k,j) )
      isgn = -1;
      return
    elseif ( a(k,j) < a(k,i) )
      isgn = +1;
      return
    end

    k = k + 1;

  end

  return
end
function a = i4col_sort_a ( m, n, a )

%*****************************************************************************80
%
%% I4COL_SORT_A ascending sorts an I4COL.
%
%  Discussion:
%
%    In lexicographic order, the statement "X < Y", applied to two real
%    vectors X and Y of length M, means that there is some index I, with
%    1 <= I <= M, with the property that
%
%      X(J) = Y(J) for J < I,
%    and
%      X(I) < Y(I).
%
%    In other words, the first time they differ, X is smaller.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 February 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the number of rows of A, and the length of
%    a vector of data.
%
%    Input, integer N, the number of columns of A.
%
%    Input, integer A(M,N), the array of N columns of M-vectors.
%
%    Output, integer A(M,N), the columns of A have been sorted in ascending
%    lexicographic order.
%
  if ( m <= 0 )
    return
  end

  if ( n <= 1 )
    return
  end
%
%  Initialize.
%
  indx = 0;
  isgn = 0;
%
%  Call the external heap sorter.
%
  while ( 1 )

    [ indx, i, j ] = sort_heap_external ( n, indx, isgn );
%
%  Interchange the I and J objects.
%
    if ( 0 < indx )

      a = i4col_swap ( m, n, a, i, j );
%
%  Compare the I and J objects.
%
    elseif ( indx < 0 )

      isgn = i4col_compare ( m, n, a, i, j );

    elseif ( indx == 0 )

      break

    end

  end

  return
end
function a = i4col_swap ( m, n, a, i, j )

%*****************************************************************************80
%
%% I4COL_SWAP swaps columns I and J of a integer array of column data.
%
%  Example:
%
%    Input:
%
%      M = 3, N = 4, I = 2, J = 4
%
%      A = (
%        1  2  3  4
%        5  6  7  8
%        9 10 11 12 )
%
%    Output:
%
%      A = (
%        1  4  3  2
%        5  8  7  6
%        9 12 11 10 )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 February 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns in the array.
%
%    Input, integer A(M,N), an array of N columns of length M.
%
%    Input, integer I, J, the columns to be swapped.
%
%    Output, integer A(M,N), the array, with columns I and J swapped.
%
  if ( i < 1 || n < i || j < 1 || n < j )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4COL_SWAP - Fatal error!\n' );
    fprintf ( 1, '  I or J is out of bounds.\n' );
    fprintf ( 1, '  I =    %d\n', i );
    fprintf ( 1, '  J =    %d\n', j );
    fprintf ( 1, '  N =    %d\n', n );
    error ( 'I4COL_SWAP - Fatal error!' );
  end

  if ( i == j )
    return
  end

  col(1:m) = a(1:m,i)';
  a(1:m,i) = a(1:m,j);
  a(1:m,j) = col(1:m)';

  return
end
function sample_value = projection ( fem_node_xy, ...
  fem_element_order, fem_element_num, fem_element_node, ...
  fem_element_neighbor, fem_value_dim, fem_value, sample_node_num, ...
  sample_node_xy )

%*****************************************************************************80
%
%% PROJECTION evaluates an FEM function on a T3 or T6 triangulation.
%
%  Discussion:
%
%    Note that the sample values returned are true values of the underlying
%    finite element function.  They are NOT produced by constructing some
%    other function that interpolates the data at the finite element nodes
%    (something which MATLAB's griddata function can easily do.)  Instead,
%    each sampling node is located within one of the associated finite
%    element triangles, and the finite element function is developed and
%    evaluated there.
%
%    MATLAB's scattered data interpolation is wonderful, but it cannot
%    be guaranteed to reproduce the finite element function corresponding
%    to nodal data.  This routine can (or at least tries to%).
%
%    So if you are using finite elements, then using THIS routine
%    (but not MATLAB's griddata function), what you see is what you have%
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 June 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer FEM_NODE_NUM, the number of nodes.
%
%    Input, real FEM_NODE_XY(2,FEM_NODE_NUM), the coordinates
%    of the nodes.
%
%    Input, integer FEM_ELEMENT_ORDER, the order of the elements,
%    either 3 or 6.
%
%    Input, integer FEM_ELEMENT_NUM, the number of triangles.
%
%    Input, integer FEM_ELEMENT_NODE(FEM_ELEMENT_ORDER,FEM_ELEMENT_NUM), the
%    nodes that make up each triangle.
%
%    Input, integer FEM_ELEMENT_NEIGHBOR(3,FEM_ELEMENT_NUM), the
%    index of the neighboring triangle on each side, or -1 if no neighbor there.
%
%    Input, integer FEM_VALUE_DIM, the "dimension" of the values.
%
%    Input, real FEM_VALUE(FEM_VALUE_DIM,FEM_NODE_NUM), the
%    finite element coefficient values at each node.
%
%    Input, integer SAMPLE_NODE_NUM, the number of sample nodes.
%
%    Input, real SAMPLE_NODE_XY(2,SAMPLE_NODE_NUM), the sample nodes.
%
%    Output, real SAMPLE_VALUE(FEM_VALUE_DIM,SAMPLE_NODE_NUM),
%    the sampled values.
%
  sample_value = zeros(fem_value_dim,sample_node_num);
%
%  For each sample point: find the triangle T that contains it,
%  and evaluate the finite element function there.
%
  for j = 1 : sample_node_num

    p_xy(1:2,1) = sample_node_xy(1:2,j);
%
%  Find the triangle T that contains the point.
%
    t = triangulation_search_delaunay ( ...
      fem_node_xy, fem_element_num, fem_element_node, ...
      fem_element_neighbor, p_xy );

    if ( t == -1 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'PROJECTION - Fatal error!\n' );
      fprintf ( 1, '  Triangulation search failed!\n' );
      error ( 'PROJECTION - Fatal error!' );
    end
%
%  Evaluate the finite element basis functions at the point in T.
%
    t_node(1:fem_element_order) = fem_element_node(1:fem_element_order,t);

    t_xy(1:2,1:fem_element_order) = fem_node_xy(1:2,t_node);

    if ( fem_element_order == 3 )
      b = basis_mn_t3 ( t_xy, 1, p_xy );
    elseif ( fem_element_order == 6 )
      b = basis_mn_t6 ( t_xy, 1, p_xy );
    end
%
%  Multiply by the finite element values to get the sample values.
%
    for i = 1 : fem_value_dim
      sample_value(i,j) = fem_value(i,t_node(1:fem_element_order)) ...
        * b(1:fem_element_order);
    end

  end

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

%*****************************************************************************80
%
%% R8MAT_WRITE writes an R8MAT file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 June 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string OUTPUT_FILENAME, the output filename.
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of points.
%
%    Input, real TABLE(M,N), the points.
%

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

  if ( output_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_WRITE - Error!\n' );
    fprintf ( 1, '  Could not open the output file.\n' );
    error ( 'R8MAT_WRITE - Error!' );
  end
%
%  Write the data.
%
%  For greater precision, try:
%
%     fprintf ( output_unit, '  %24,16f', table(i,j) );
%
  for j = 1 : n
    for i = 1 : m
      fprintf ( output_unit, '  %14f', table(i,j) );
    end
    fprintf ( output_unit, '\n' );
  end
%
%  Close the file.
%
  fclose ( output_unit );

  return
end
function [ indx, i, j ] = sort_heap_external ( n, indx, isgn )

%*****************************************************************************80
%
%% SORT_HEAP_EXTERNAL externally sorts a list of items into ascending order.
%
%  Discussion:
%
%    The actual list of data is not passed to the routine.  Hence this
%    routine may be used to sort integers, reals, numbers, names,
%    dates, shoe sizes, and so on.  After each call, the routine asks
%    the user to compare or interchange two items, until a special
%    return value signals that the sorting is completed.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 February 2004
%
%  Author:
%
%    Original FORTRAN77 version by Albert Nijenhuis, Herbert Wilf.
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Albert Nijenhuis, Herbert Wilf.
%    Combinatorial Algorithms,
%    Academic Press, 1978, second edition,
%    ISBN 0-12-519260-6.
%
%  Parameters:
%
%    Input, integer N, the number of items to be sorted.
%
%    Input, integer INDX, the main communication signal.
%    The user must set INDX to 0 before the first call.
%    Thereafter, the user should set the input value of INDX
%    to the output value from the previous call.
%
%    Input, integer ISGN, results of comparison of elements I and J.
%    (Used only when the previous call returned INDX less than 0).
%    ISGN <= 0 means I is less than or equal to J;
%    0 <= ISGN means I is greater than or equal to J.
%
%    Output, integer INDX, the main communication signal.
%    If INDX is
%
%      greater than 0, the user should:
%      * interchange items I and J;
%      * call again.
%
%      less than 0, the user should:
%      * compare items I and J;
%      * set ISGN = -1 if I < J, ISGN = +1 if J < I;
%      * call again.
%
%      equal to 0, the sorting is done.
%
%    Output, integer I, J, the indices of two items.
%    On return with INDX positive, elements I and J should be interchanged.
%    On return with INDX negative, elements I and J should be compared, and
%    the result reported in ISGN on the next call.
%
  persistent i_save;
  persistent j_save;
  persistent k;
  persistent k1;
  persistent n1;

  if ( isempty ( i_save ) )
    i_save = -1;
  end

  if ( isempty ( j_save ) )
    j_save = -1;
  end
%
%  INDX = 0: This is the first call.
%
  if ( indx == 0 )

    k = floor ( n / 2 );
    k1 = k;
    n1 = n;
%
%  INDX < 0: The user is returning the results of a comparison.
%
  elseif ( indx < 0 )

    if ( indx == -2 )

      if ( isgn < 0 )
        i_save = i_save + 1;
      end

      j_save = k1;
      k1 = i_save;
      indx = -1;
      i = i_save;
      j = j_save;
      return;
    end

    if ( 0 < isgn )
      indx = 2;
      i = i_save;
      j = j_save;
      return;
    end

    if ( k <= 1 )

      if ( n1 == 1 )
        i_save = 0;
        j_save = 0;
        indx = 0;
      else
        i_save = n1;
        n1 = n1 - 1;
        j_save = 1;
        indx = 1;
      end

      i = i_save;
      j = j_save;
      return;

    end

    k = k - 1;
    k1 = k;
%
%  0 < INDX, the user was asked to make an interchange.
%
  elseif ( indx == 1 )

    k1 = k;

  end

  while ( 1 )

    i_save = 2 * k1;

    if ( i_save == n1 )
      j_save = k1;
      k1 = i_save;
      indx = -1;
      i = i_save;
      j = j_save;
      return;
    elseif ( i_save <= n1 )
      j_save = i_save + 1;
      indx = -2;
      i = i_save;
      j = j_save;
      return;
    end

    if ( k <= 1 )
      break;
    end

    k = k - 1;
    k1 = k;

  end

  if ( n1 == 1 )
    i_save = 0;
    j_save = 0;
    indx = 0;
    i = i_save;
    j = j_save;
  else
    i_save = n1;
    n1 = n1 - 1;
    j_save = 1;
    indx = 1;
    i = i_save;
    j = j_save;
  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 triangle_neighbor = triangulation_order3_neighbor_triangles ( ...
  triangle_num, triangle_node )

%*****************************************************************************80
%
%% TRIANGULATION_ORDER3_NEIGHBOR_TRIANGLES determines triangle neighbors.
%
%  Discussion:
%
%    A triangulation of a set of nodes can be completely described by
%    the coordinates of the nodes, and the list of nodes that make up
%    each triangle.  However, in some cases, it is necessary to know
%    triangle adjacency information, that is, which triangle, if any,
%    is adjacent to a given triangle on a particular side.
%
%    This routine creates a data structure recording this information.
%
%    The primary amount of work occurs in sorting a list of 3 * TRIANGLE_NUM
%    data items.
%
%    This routine was modified to use columns instead of rows.
%
%  Example:
%
%    The input information from TRIANGLE_NODE:
%
%    Triangle   Nodes
%    --------   ---------------
%     1         3      4      1
%     2         3      1      2
%     3         3      2      8
%     4         2      1      5
%     5         8      2     13
%     6         8     13      9
%     7         3      8      9
%     8        13      2      5
%     9         9     13      7
%    10         7     13      5
%    11         6      7      5
%    12         9      7      6
%    13        10      9      6
%    14         6      5     12
%    15        11      6     12
%    16        10      6     11
%
%    The output information in TRIANGLE_NEIGHBOR:
%
%    Triangle  Neighboring Triangles
%    --------  ---------------------
%
%     1        -1     -1      2
%     2         1      4      3
%     3         2      5      7
%     4         2     -1      8
%     5         3      8      6
%     6         5      9      7
%     7         3      6     -1
%     8         5      4     10
%     9         6     10     12
%    10         9      8     11
%    11        12     10     14
%    12         9     11     13
%    13        -1     12     16
%    14        11     -1     15
%    15        16     14     -1
%    16        13     15     -1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 February 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer TRIANGLE_NUM, the number of triangles.
%
%    Input, integer TRIANGLE_NODE(3,TRIANGLE_NUM), the nodes that make up each triangle.
%
%    Output, integer TRIANGLE_NEIGHBOR(3,TRIANGLE_NUM), the three triangles that are direct
%    neighbors of a given triangle.  TRIANGLE_NEIGHBOR(1,I) is the index of the triangle
%    which touches side 1, defined by nodes 2 and 3, and so on.  TRIANGLE_NEIGHBOR(1,I)
%    is negative if there is no neighbor on that side.  In this case, that
%    side of the triangle lies on the boundary of the triangulation.
%

%
%  Step 1.
%  From the list of nodes for triangle T, of the form: (I,J,K)
%  construct the three neighbor relations:
%
%    (I,J,1,T) or (J,I,1,T),
%    (J,K,2,T) or (K,J,2,T),
%    (K,I,3,T) or (I,K,3,T)
%
%  where we choose (I,J,1,T) if I < J, or else (J,I,1,T)
%
  col = zeros ( 4, 3 * triangle_num );

  for tri = 1 : triangle_num

    i = triangle_node(1,tri);
    j = triangle_node(2,tri);
    k = triangle_node(3,tri);

    if ( i < j )
      col(1:4,1+3*(tri-1)) = [ i, j, 1, tri ]';
    else
      col(1:4,1+3*(tri-1)) = [ j, i, 1, tri ]';
    end

    if ( j < k )
      col(1:4,2+3*(tri-1)) = [ j, k, 2, tri ]';
    else
      col(1:4,2+3*(tri-1)) = [ k, j, 2, tri ]';
    end

    if ( k < i )
      col(1:4,3+3*(tri-1)) = [ k, i, 3, tri ]';
    else
      col(1:4,3+3*(tri-1)) = [ i, k, 3, tri ]';
    end

  end
%
%  Step 2. Perform an ascending dictionary sort on the neighbor relations.
%  We only intend to sort on rows 1 and 2; the routine we call here
%  sorts on rows 1 through 4 but that won't hurt us.
%
%  What we need is to find cases where two triangles share an edge.
%  Say they share an edge defined by the nodes I and J.  Then there are
%  two columns of COL that start out ( I, J, ?, ? ).  By sorting COL,
%  we make sure that these two columns occur consecutively.  That will
%  make it easy to notice that the triangles are neighbors.
%
  col = i4col_sort_a ( 4, 3*triangle_num, col );
%
%  Step 3. Neighboring triangles show up as consecutive columns with
%  identical first two entries.  Whenever you spot this happening,
%  make the appropriate entries in TRIANGLE_NEIGHBOR.
%
  triangle_neighbor(1:3,1:triangle_num) = -1;

  icol = 1;

  while ( 1 )

    if ( 3 * triangle_num <= icol )
      break
    end

    if ( col(1,icol) ~= col(1,icol+1) || col(2,icol) ~= col(2,icol+1) )
      icol = icol + 1;
      continue
    end

    side1 = col(3,icol);
    tri1 =  col(4,icol);
    side2 = col(3,icol+1);
    tri2 =  col(4,icol+1);

    triangle_neighbor(side1,tri1) = tri2;
    triangle_neighbor(side2,tri2) = tri1;

    icol = icol + 2;

  end

  return
end
function [ triangle_index, edge ] = ...
  triangulation_search_delaunay ( node_xy, ...
  triangle_num, triangle_node, triangle_neighbor, p )

%*****************************************************************************80
%
%% TRIANGULATION_SEARCH_DELAUNAY searches a Delaunay triangulation for a point.
%
%  Purpose:
%
%    The algorithm "walks" from one triangle to its neighboring triangle,
%    and so on, until a triangle is found containing point P, or P is found
%    to be outside the convex hull.
%
%    The algorithm computes the barycentric coordinates of the point with
%    respect to the current triangle.  If all three quantities are positive,
%    the point is contained in the triangle.  If the I-th coordinate is
%    negative, then P lies on the far side of edge I, which is opposite
%    from vertex I.  This gives a hint as to where to search next.
%
%    For a Delaunay triangulation, the search is guaranteed to terminate.
%    For other triangulations, a cycle may occur.
%
%    Note the surprising fact that, even for a Delaunay triangulation of
%    a set of points, the nearest point to P need not be one of the
%    vertices of the triangle containing P.
%
%    The code can be called for triangulations of any order, but only
%    the first three nodes in each triangle are considered.  Thus, if
%    higher order triangles are used, and the extra nodes are intended
%    to give the triangle a polygonal shape, these will have no effect,
%    and the results obtained here might be misleading.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 January 2010
%
%  Author:
%
%    Original FORTRAN77 version by Barry Joe.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Barry Joe,
%    GEOMPACK - a software package for the generation of meshes
%    using geometric algorithms,
%    Advances in Engineering Software,
%    Volume 13, pages 325-331, 1991.
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, real NODE_XY(2,NODE_NUM), the vertices.
%
%    Input, integer TRIANGLE_ORDER, the order of the triangles.
%
%    Input, integer TRIANGLE_NUM, the number of triangles in the triangulation.
%
%    Input, integer TRIANGLE_NODE(TRIANGLE_ORDER,TRIANGLE_NUM),
%    the nodes that make up each triangle.
%
%    Input, integer TRIANGLE_NEIGHBOR(3,TRIANGLE_NUM), the triangle
%    neighbor list.
%
%    Input, real P(2), the coordinates of a point.
%
%    Output, integer TRIANGLE_INDEX, the index of the triangle where the
%    search ended.  If a cycle occurred, then TRIANGLE_INDEX = -1.
%
%    Output, integer EDGE, indicates the position of the point P in
%    triangle TRIANGLE_INDEX:
%    0, the interior or boundary of the triangle;
%    -1, outside the convex hull of the triangulation, past edge 1;
%    -2, outside the convex hull of the triangulation, past edge 2;
%    -3, outside the convex hull of the triangulation, past edge 3.
%
  persistent triangle_index_save;

  if ( isempty ( triangle_index_save ) )
    triangle_index_save = -1;
  end

  count = 0;
  edge = 0;

  if ( triangle_index_save < 1 || triangle_num < triangle_index_save )
    triangle_index = floor ( ( triangle_num + 1 ) / 2 );
  else
    triangle_index = triangle_index_save;
  end

  while ( 1 )

    count = count + 1;

    if ( triangle_num < count )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'TRIANGULATION_SEARCH_DELAUNAY - Fatal error!\n' );
      fprintf ( 1, '  The algorithm seems to be cycling.\n' );
      triangle_index = -1;
      edge = -1;
      return
    end
%
%  Get the vertices of triangle TRIANGLE_INDEX.
%
    a = triangle_node(1,triangle_index);
    b = triangle_node(2,triangle_index);
    c = triangle_node(3,triangle_index);
%
%  Using vertex C as a base, compute the distances to vertices A and B,
%  and the point P.
%
    dxa = node_xy(1,a) - node_xy(1,c);
    dya = node_xy(2,a) - node_xy(2,c);

    dxb = node_xy(1,b) - node_xy(1,c);
    dyb = node_xy(2,b) - node_xy(2,c);

    dxp = p(1)         - node_xy(1,c);
    dyp = p(2)         - node_xy(2,c);

    det = dxa * dyb - dya * dxb;
%
%  Compute the barycentric coordinates of the point P with respect
%  to this triangle.
%
    alpha = ( dxp * dyb - dyp * dxb ) / det;
    beta =  ( dxa * dyp - dya * dxp ) / det;
    gamma = 1.0 - alpha - beta;
%
%  If the barycentric coordinates are all positive, then the point
%  is inside the triangle and we're done.
%
    if ( 0.0 <= alpha && 0.0 <= beta && 0.0 <= gamma )
      break
    end
%
%  At least one barycentric coordinate is negative.
%
%  If there is a negative barycentric coordinate for which there exists
%  an opposing triangle neighbor closer to the point, move to that triangle.
%
%  (Two coordinates could be negative, in which case we could go for the
%  most negative one, or the most negative one normalized by the actual
%  distance it represents).
%
    if ( alpha < 0.0 && 0 < triangle_neighbor(2,triangle_index) )
      triangle_index = triangle_neighbor(2,triangle_index);
      continue;
    elseif ( beta < 0.0 && 0 < triangle_neighbor(3,triangle_index) )
      triangle_index = triangle_neighbor(3,triangle_index);
      continue;
    elseif ( gamma < 0.0 && 0 < triangle_neighbor(1,triangle_index) )
      triangle_index = triangle_neighbor(1,triangle_index);
      continue;
    end
%
%  All negative barycentric coordinates correspond to vertices opposite
%  sides on the convex hull.
%
%  Note the edge and exit.
%
    if ( alpha < 0.0 )
      edge = -2;
      break
    elseif ( beta < 0.0 )
      edge = -3;
      break
    elseif ( gamma < 0.0 )
      edge = -1;
      break
    end

  end

  triangle_index_save = triangle_index;

  return
end


%% Selecting Grains
%
%%
% In this section we discuss how to select grains by properties. We start
% our discussion by reconstructing the grain structure from a sample EBSD
% data set.

% load sample EBSD data set
mtexdata forsterite silent

% restrict it to a subregion of interest.
ebsd = ebsd(inpolygon(ebsd,[5 2 10 5]*10^3));

% remove all not indexed pixels
ebsd = ebsd('indexed');

% reconstruct grains
[grains, ebsd.grainId] = calcGrains(ebsd,'angle',5*degree);

% smooth them
grains = smooth(grains,5);

% plot the orientation data of the Forsterite phase
plot(ebsd('fo'),ebsd('fo').orientations)

% plot the grain boundary on top of it
hold on
plot(grains.boundary,'lineWidth',2)
hold off

%% Selecting grains by mouse
% The most easiest way to select a grain is by using the mouse and the
% command <grain2d.selectInteractive.html selectInteractive> which allows
% you to select an arbitrary amount of grains. The index of the selected
% grains appear as the global variable |indSelected| in your workspace

selectInteractive(grains,'lineColor','gold')

% this simulates a mouse click
pause(0.1)
simulateClick(9000,3500)
pause(0.1)

global indSelected;
grains(indSelected)

hold on
plot(grains(indSelected).boundary,'lineWidth',4,'lineColor','gold')
hold off

%% Indexing by orientation or position
% One can also to select a grain by spatial coordinates without user
% interaction. This is done using the syntax |grains(x,y)|, i.e.,

x = 12000; y = 4000;

hold on
plot(grains(x,y).boundary,'linewidth',4,'linecolor','blue')

plot(x,y,'marker','s','markerfacecolor','k',...
  'markersize',10,'markeredgecolor','w')
hold off

%%
% Alternatively one can also select all grains with a certain orientation.
% Lets find all grains with a similar orientation as the one marked in
% gold. As threshold we shall use 20 degree

% select grains by orientation
grains_selected = grains.findByOrientation(grains(indSelected).meanOrientation,20*degree)

hold on
plot(grains_selected.boundary,'linewidth',4,'linecolor','gold')
hold off

%% Indexing by a Property
% In order the generalize the above concept lets remember that the variable
% |grains| is essentially a large vector of grains. Thus when applying a
% function like <grain2d.area.html area> to this variable we obtain a
% vector of the same lenght with numbers representing the area of each
% grain

grain_area = grains.area;

%%
% As a first rather simple application we could colorize the grains
% according to their area, i.e., according to the numbers stored in
% |grain_area|

plot(grains,grain_area)

%%
% As a second application, we can ask for the largest grain within our data
% set. The maximum value and its position within a vector are found by the
% Matlab command |max|.

[max_area,max_id] = max(grain_area)

%%
% The number |max_id| is the position of the grain with a maximum area within
% the variable |grains|. We can access this specific grain by direct
% indexing

grains(max_id)

%%
% and so we can plot it

hold on
plot(grains(max_id).boundary,'linecolor','red','linewidth',4)
hold off

%%
% Note that this way of addressing individual grains can be generalized to
% many grains. E.g. assume we are interested in the largest 5 grains. Then
% we can sort the vector |grain_area| and take the indices of the 5 largest
% grains.

[sorted_area,sorted_id] = sort(grain_area,'descend');

large_grain_id = sorted_id(2:5);

hold on
plot(grains(large_grain_id).boundary,'linecolor','Orange','linewidth',4)
hold off

%% Indexing by a Condition
% By the same syntax as above we can also single out grains that satisfy a
% certain condition. I.e., to access are grains that are at least one
% quarter as large as the largest grain we can do

condition = grain_area > max_area/4;

hold on
plot(grains(condition).boundary,'linecolor','Yellow','linewidth',4)
hold off

%%
% This is a very powerful way of accessing grains as the condition can be
% build up using any grain property. As an example let us consider the
% phase. The phase of the first five grains we get by

grains(1:5).phase

%%
% Now we can access or grains of the first phase Forsterite by the
% condition

condition = grains.phase == 1;
plot(grains(condition))

%%
% To make the above more directly you can use the mineral name for indexing

grains('forsterite')

%%
% Logical indexing allows also for more complex queries, e.g. selecting all
% grains perimeter larger than 6000 and at least 600 measurements within

condition = grains.perimeter>6000 & grains.grainSize >= 600;

selected_grains = grains(condition)

plot(selected_grains)

%% The grainId and how to select EBSD inside specific grains
%
% Besides, the list of grains the command <EBSD.calcGrains.html calcGrains>
% returns also two other output arguments.

plot(grains)
largeGrains = grains(grains.grainSize > 50);

text(largeGrains,largeGrains.id)

%%
% The second output argument grainId is a list with the same size as the
% EBSD measurements that stores for each measurement the corresponding
% grainId. The above syntax stores this list directly inside the ebsd
% variable. This enables MTEX to select EBSD data by grains. The following
% command returns all the EBSD data that belong to grain number 33.

ebsd(grains(33))

%%
% and is equivalent to the command

ebsd(ebsd.grainId == 33)

%%
% The following picture plots the largest grains together with its
% individual orientation measurements.

plot(ebsd(grains(max_id)),ebsd(grains(max_id)).orientations)
hold on
plot(grains(max_id).boundary,'lineWidth',2)
hold off

%% Boundary grains
% Sometimes it is desirable to remove all boundary grains as they might
% distort grain statistics. To do so one should remember that each grain
% boundary has a property |grainId| which stores the ids of the neigbouring
% grains. In the case of an outer grain boundary, one of the neighbouring
% grains has the id zero. We can filter out all these boundary segments by

% ids of the outer boundary segment
outerBoundary_id = any(grains.boundary.grainId==0,2);

% plot the outer boundary segments
plot(grains)
hold on
plot(grains.boundary(outerBoundary_id),'linecolor','red','linewidth',2)
hold off

%%
% Now |grains.boundary(outerBoundary_id).grainId| is a list of grain ids
% where the first column is zero, indicating the outer boundary, and the
% second column contains the id of the boundary grain. Hence, it remains to
% remove all grains with these ids.

% next we compute the corresponding grain_id
grain_id = grains.boundary(outerBoundary_id).grainId;

% remove all zeros
grain_id(grain_id==0) = [];

% and plot the boundary grains
plot(grains(grain_id))

%%
% finally, we could remove the boundary grains by
%
%   grains(grain_id) = []
%
% However, boundary grains can be selected more easily be the command
% |<grain2d/isBoundary.html isBoundary>|.

plot(grains(~grains.isBoundary))


function [NV,NF,OV,OE,OV1,OE1,epsilon] = remesh_at_handles(V,F,C,P,BE,CE)
  % REMESH_AT_HANDLES  remesh a given mesh using its outline and constraining
  % interrior controls at given handles. Point handles are met exactly and
  % bones are sample at the given rate.
  %
  % [NV,NF] = remesh_at_handles(V,F,C,P,BE,CE)
  %
  % Inputs:
  %  V  list of vertex positions
  %  F  list of face indices
  %  C  list of control vertex positions
  %  P  list of indices into C for point controls, { 1:size(C,1) }
  %  BE  list of bones, pairs of indices into C, connecting control vertices,
  %    { [] }
  %  CE  list of "cage edges", pairs of indices into ***P***, connecting
  %    control ***points***. A "cage edge" just tells point boundary conditions
  %    to vary linearly along straight lines between the end points, and to be
  %    zero for all other handles. { [] }
  % Outputs
  %  NV  new list of vertex positions
  %  NF  new list of face indices
  %  OV  list of vertex positions sent to triangle
  %  OE  list of edges sent to triangle
  %  OV1 list of vertex positions before performing close point collapses and
  %    edge splits
  %  OE1 list of edges before performing close point collapses and edge splits
  %  epsilon  used to collapse points and split edges
  %
  % Example:
  %  [NV,NF,OV,OE,OV1,OE1] = remesh_at_handles(V,F,C,P,BE,CE);
  %  % show a plot of what's sent to triangle
  %  subplot(3,1,1);
  %  plot([OV1(OE1(:,1),1) OV1(OE1(:,2),1)]',[OV1(OE1(:,1),2) OV1(OE1(:,2),2)]','-','LineWidth',1);
  %  hold on;
  %  plot(OV1(:,1),OV1(:,2),'.');
  %  hold off;
  %  axis equal;
  %  title('Outline + handle samples');
  %  subplot(3,1,2);
  %  plot([OV(OE(:,1),1) OV(OE(:,2),1)]',[OV(OE(:,1),2) OV(OE(:,2),2)]','-','LineWidth',1);
  %  hold on;
  %  plot(OV(:,1),OV(:,2),'.');
  %  hold off;
  %  axis equal;
  %  title('Input to triangle (collapsed points + split edges)');
  %  % show a plot of the result
  %  subplot(3,1,3);
  %  tsurf(NF,NV);
  %  hold on;
  %  plot([OV(OE(:,1),1) OV(OE(:,2),1)]',[OV(OE(:,1),2) OV(OE(:,2),2)]','-','LineWidth',2);
  %  hold off;
  %  axis equal;
  %  title('Output of triangle');

  %% THIS DOES NOT WORK FOR MESHES THAT AREN"T TOPOLOGICAL DISKS!! HOLES WILL
  %% BE CLOSED

  dim = size(V,2);
  assert(dim == size(C,2));
  % only work with 2D
  assert(dim == 2);

  % Find all edges in mesh, note internal edges are repeated
  E = sort([F(:,1) F(:,2); F(:,2) F(:,3); F(:,3) F(:,1)]')';
  % determine uniqueness of edges
  [u,m,n] = unique(E,'rows');
  % determine counts for each unique edge
  counts = accumarray(n(:), 1);
  % extract edges that only occurred once
  O = u(counts==1,:);
  % extract unique vertex indices on outline
  [u,m,n] = unique(O(:));
  % original map O = IM(O)
  IM = 1:size(V,1);
  IM(O(:)) = n;
  OV = V(u,:);
  OE = IM(O);

  samples_per_edge = 10;
  % Bone samples
  [BES,BESE] = sample_edges(C,BE,samples_per_edge);
  % Cage edge samples
  [CES,CESE] = sample_edges(C(P,:),CE,samples_per_edge);
  % Append handle samples
  OV = [OV;C(P,:);BES;CES];
  OE = [ ...
    OE; ...
    (size(OV,1)-size(BES,1)-size(CES,1))+BESE; ...
    (size(OV,1)-size(CES,1))+CESE];

  % save old values
  OV1 = OV;
  OE1 = OE;

  % phony counts to enter while loop
  prev_v_count = size(OV,1)+1;
  prev_e_count = size(OE,1)+1;
  % this epsilon is a parameter that probably needs to be tweaked and should at
  % least be exposed.
  % Use fraction of average edge length
  epsilon = mean(sqrt(sum((OV(OE(:,1),:) - OV(OE(:,2),:)).^2,2)))/8;
  % continue to collapse points and split edges until nothing more to do
  while( prev_v_count ~= size(OV,1) || prev_e_count ~= size(OE,1))
    prev_v_count = size(OV,1);
    prev_e_count = size(OE,1);
    [OV,OE] = collapse_close_points(OV,OE,epsilon);
    [OV,OE] = snap_points_to_close_edges(OV,OE,epsilon);
  end

  % Again, max area term here is a heuristic
  %sum((OV(OE(:,1)) - OV(OE(:,2))).^2,2)
  % use a multiple of min edge length
  %max_area = 2*(sqrt(3)/4)*min(sum((OV(OE(:,1),:) - OV(OE(:,2),:)).^2,2));
  max_area = 8*(sqrt(3)/4)*min(sum((OV(OE(:,1),:) - OV(OE(:,2),:)).^2,2));
  [NV,NF] = triangle(OV,OE,[],'Quality',30,'MaxArea',max_area);
end


function Rob = createRobots(Robot)

% CREATEROBOTS Create robots structure array.
%   Rob = CREATEROBOTS(Robot) creates the Rob() structure array to be used
%   as SLAM data. The input Robot{}  is a cell array of structures as
%   specified by the user in userData.m. There must be one Robot{} per each
%   robot considered in the simulation. See userData.m for details.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

for rob = 1:numel(Robot)

    Ri = Robot{rob}; % input robot structure

    % identification
    Ro.rob     = rob;
    Ro.id      = Ri.id;
    Ro.name    = Ri.name;
    Ro.type    = Ri.type;
    Ro.motion  = Ri.motion;

    Ro.sensors = [];

    % Robot frame in quaternion form
    ep = [Ri.position;deg2rad(Ri.orientationDegrees)];
    EP = diag([Ri.positionStd;deg2rad(Ri.orientationStd)].^2);
    [qp,QP] = propagateUncertainty(ep,EP,@epose2qpose); % frame and cov. in quaternion

    % control and rest of the state
    switch Ri.motion

        case {'constVel'}
            % control
            Ro.con.u    = [Ri.dv;deg2rad(Ri.dwDegrees)];
            Ro.con.uStd = [Ri.dvStd;deg2rad(Ri.dwStd)];
            Ro.con.U    = diag(Ro.con.uStd.^2);
            Ro.com.W    = 1/Ro.con.U;  % Information matrix

            % velocity states
            v = [Ri.velocity;deg2rad(Ri.angularVelDegrees)];
            V = diag([Ri.velStd;deg2rad(Ri.angVelStd)].^2);

            % state
            Ro.state.x    = [qp;v]; % state
            Ro.state.P    = blkdiag(QP,V);
            Ro.state.dx   = zeros(12,1);

        case {'odometry'}
            % control
            Ro.con.u    = [Ri.dx;deg2rad(Ri.daDegrees)];
            Ro.con.uStd = [Ri.dxStd;deg2rad(Ri.daStd)];
            Ro.con.U    = diag(Ro.con.uStd.^2);
            Ro.com.W    = Ro.con.U^-1;  % Information matrix

            % state
            Ro.state.x    = qp; % state
            Ro.state.P    = EP;
            Ro.state.dx   = zeros(6,1);

        otherwise
            error('Unknown motion model ''%s'' for robot %d.',Robot{rob}.motion,Robot{rob}.id);
    end

    Ro.state.r  = []; % Points  Map.x  into  state.dx
    Ro.state.size = numel(Ro.state.x);   % state size
    Ro.state.dsize = numel(Ro.state.dx);

    Ro.frame.r = [];
    Ro.frame.x = qp;
    Ro.frame.P = QP;
    Ro.frame = updateFrame(Ro.frame);

    Rob(rob) = Ro; % output robot structure

end

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

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

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function cx = cscal ( n, ca, cx, incx )

%*****************************************************************************80
%
%% CSCAL scales a complex vector by a constant.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1,
%    LC: QA214.L56.
%
%    Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
%    Basic Linear Algebra Subprograms for Fortran Usage,
%    Algorithm 539,
%    ACM Transactions on Mathematical Software,
%    Volume 5, Number 3, September 1979, pages 308-323.
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Input, complex CA, the multiplier.
%
%    Input, complex CX(*), the vector to be scaled.
%
%    Input, integer INCX, the increment between successive entries of CX.
%
%    Output, complex CX(*), the scaled vector.
%
  cx(1:incx:1+(n-1)*incx) = ca * cx(1:incx:1+(n-1)*incx);

  return
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Spherical Harmonic Modeling and Analysis Toolkit (SPHARM-MAT) is a 3D
% shape modeling and analysis toolkit.
% It is a software package developed at Shenlab in Center for Neuroimaging,
% Indiana University (SpharmMat@gmail.com, http://www.iupui.edu/~shenlab/)
% It is available to the scientific community as copyright freeware
% under the terms of the GNU General Public Licence.
%
% Copyright 2009, 2010, ShenLab, Center for Neuroimaging, Indiana University
%
% This file is part of SPHARM-MAT.
%
% SPHARM-MAT is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% SPHARM-MAT is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with SPHARM-MAT. If not, see <http://www.gnu.org/licenses/>.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Goal: Fix a binary image by removing bad connectivities
% inObject: filename string of a binary object in .MAT format
% info = varargin{1}: connectivity
% info can be one of four values between 1 and 4 - '1=(6+,18), 2=(18,6+),
% 3=(6,26), 4=(26,6)'
% epsilon = varargin{2}: hole size, holes bigger than epsilon won't be
% filled
%
% 04/15/2002 - created by Li Shen
% Modified by Sungeun Kim (10-09-08)

function [bim, origin, vxsize, new_name] = fix_bad_topology(inObject, confs)

epsilon = confs.Epsilon;
switch deblank(char(confs.Connectivity))
    case '(6+,18)'
        conn = 1;
    case '(18,6+)'
        conn = 2;
    case '(6,26)'
        conn = 3;
    case '(26,6)'
        conn = 4;
end

% Read input object dataset
if ischar(inObject)
    [path,name,ext] = fileparts(inObject);

    if ~strcmp(ext, '.mat')
        disp('Input object data should be Matlab data file (*.mat)');
        return;
    end

    load(inObject);
    roi = bim;
else
    disp('Input Object should be the filename of a binary object (_bim.mat).');
    return;
end

tic;

infostr{1} = 'outlier or hole';
infostr{2} = 'vertex conn.';
infostr{3} = 'edge conn.';
infostr{4} = 'ring or hole';
infostr{5} = 'single ring';

% display information
disp('Make a simply connected volume by removing bad voxels');
disp(sprintf('(1) %s, (2) %s, (3) %s, (4) %s, (5) s', ...
              infostr{1}, infostr{2}, infostr{3}, infostr{4}, infostr{5}));

% Make binary image, in case reslice generates 2's
ind = find(roi>0); roi(ind) = 1;
vol = length(ind);
ind = find(roi<1); roi(ind) = 0;

% routine fix
roi = routine_fix(roi);
[vertices, faces] = gen_surf_data(roi,origin,vxsize);

% fill 3d holes
for k=1:3
    if (size(vertices,1)-size(faces,1)==2)
        disp('===> No 3D holes, skip');
        break;
    end
    disp('===> fixing 3D holes ...');
    roi = fill3dholes(roi,conn,epsilon,name);
    disp('===> routine fix again ...');
    roi = routine_fix(roi);
    [vertices, faces] = gen_surf_data(roi,origin,vxsize);
end

fvn = length(find(roi~=bim));
bim = roi;

disp(sprintf('fix_vox_num (%d) / vol (%d) = %f%%, save ...', ...
                      fvn, vol, fvn*100/vol));

% Save surface object to a new file
postfix = name(end-2:end);
if strcmp(postfix, 'bim') | strcmp(postfix,'fix')
    new_name = [confs.OutDirectory '/' name(1:end-3) 'fix'];
else
    new_name = [confs.OutDirectory '/' name '_fix'];
end

if exist(new_name,'file')
    prompt = {'Enter new filename:'};
    dlg_title = 'New File Name';
    num_lines = 1;
    def = {new_name};
    answer = inputdlg(prompt,dlg_title,num_lines,def);
    new_name = answer{1};
end

save(new_name, 'bim', 'origin', 'vxsize');

seconds = toc;
hours = seconds/3600;
disp(['seconds=', num2str(seconds), ' hours=', num2str(hours)]);

return;

%
% routine fix
%

function roi = routine_fix(roi)

DIM = size(roi);

fixset = [];

for j = 1:10
    [roi, fs1] = outl_hole(roi, DIM,1);
    [roi, fs2] = vertex_conn(roi, DIM,2);
    [roi, fs3] = edge_conn(roi, DIM,3);
    if (isempty(fs1) & isempty(fs2) & isempty(fs3))
        break;
    else
        if (~isempty(fs1))
            fixset(end+1:end+size(fs1,1),:) = fs1;
        end
        if (~isempty(fs2))
            fixset(end+1:end+size(fs2,1),:) = fs2;
        end
        if (~isempty(fs3))
            fixset(end+1:end+size(fs3,1),:) = fs3;
        end
    end
end

[roi, fs4] = ring_hole(roi, DIM, 4);
if (~isempty(fs4))
    fixset(end+1:end+size(fs4,1),:) = fs4;
    for j = 1:10
        [roi, fs1] = outl_hole(roi, DIM,1);
        [roi, fs2] = vertex_conn(roi, DIM,2);
        [roi, fs3] = edge_conn(roi, DIM,3);
        [roi, fs4] = ring_hole(roi, DIM,4);
        if (isempty(fs1) & isempty(fs2) & isempty(fs3) & isempty(fs4))
            break;
        else
            if (~isempty(fs1))
                fixset(end+1:end+size(fs1,1),:) = fs1;
            end
            if (~isempty(fs2))
                fixset(end+1:end+size(fs2,1),:) = fs2;
            end
            if (~isempty(fs3))
                fixset(end+1:end+size(fs3,1),:) = fs3;
            end
            if (~isempty(fs4))
                fixset(end+1:end+size(fs4,1),:) = fs4;
            end
        end
    end
end

fix_vox_num = size(fixset,1);

return;

%
% remove disconnected small components and holes
%

function [roi, fixset] = outl_hole(roi, d, infoid)

roi = reshape(roi,d);

fixset = [];

% check seperated small components (k=1)
% and then check holes
val = [0 1];
for k = 1:2
	[L, num] = bwlabeln(roi,6);

	if num>1
		% find the background
		for i = 1:num
            cnt(i) = length(find(L == i));
		end
		[bkgd_size, bkgd_ind] = max(cnt);
        sigind = find(L~=0 & L~=bkgd_ind);
        roi(sigind) = 0;
        [xs,ys,zs] = ind2sub(d, sigind);
        fixset(end+1:end+length(xs),:) = [[xs,ys,zs], xs*0+val(k), xs*0+infoid];
	end

	roi = 1-roi;
end

return;

%
% fix bad vertex connectivities
%

function [roi, fixset] = vertex_conn(roi, d, infoid)

roi = reshape(roi,d);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% work on 0 vertex connectivies
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% make a work area so that all border voxels belong to the background
w = ones(d+2);
w(2:d(1)+1,2:d(2)+1,2:d(3)+1) = roi;
wd = d+2;
ind = find(w == 0);
w = zeros(d+2);
w(2:d(1)+1,2:d(2)+1,2:d(3)+1) = roi;

[xs,ys,zs] = ind2sub(wd,ind);

h = sum([w(sub2ind(wd,xs-1,ys,zs)), ...
         w(sub2ind(wd,xs+1,ys,zs)), ...
         w(sub2ind(wd,xs,ys-1,zs)), ...
         w(sub2ind(wd,xs,ys+1,zs)), ...
         w(sub2ind(wd,xs,ys,zs-1)), ...
         w(sub2ind(wd,xs,ys,zs+1))]')';

fixset = [];

% fix ring 0 voxels (single ring)
ind1 = ind(find(h==4 & ...
                w(sub2ind(wd,xs-1,ys,zs))==0 & ...
                w(sub2ind(wd,xs+1,ys,zs))==0 ...
           ));
ind2 = ind(find(h==4 & ...
                w(sub2ind(wd,xs,ys-1,zs))==0 & ...
                w(sub2ind(wd,xs,ys+1,zs))==0 ...
           ));
ind3 = ind(find(h==4 & ...
                w(sub2ind(wd,xs,ys,zs-1))==0 & ...
                w(sub2ind(wd,xs,ys,zs+1))==0 ...
           ));
ind1 = [ind1;ind2;ind3];
n4 = length(ind1);
if (n4~=0)
	w(ind1) = 1;
    [a, b, c] = ind2sub(wd,ind1);
	fixset(end+1:end+n4,:) = [[a, b, c]-1, ind1*0+1, ind1*0+5];
end

% right up forward, right up backward, right down forward, right down backward
ruf = find(w(sub2ind(wd,xs-1,ys+1,zs+1)) == 0 & ...
           sum([w(sub2ind(wd,xs-1,ys,zs)), ...
                w(sub2ind(wd,xs,ys+1,zs)), ...
                w(sub2ind(wd,xs-1,ys+1,zs)), ...
                w(sub2ind(wd,xs,ys,zs+1)), ...
                w(sub2ind(wd,xs-1,ys,zs+1)), ...
                w(sub2ind(wd,xs,ys+1,zs+1))]')' == 6);

rub = find(w(sub2ind(wd,xs-1,ys+1,zs-1)) == 0 & ...
           sum([w(sub2ind(wd,xs-1,ys,zs)), ...
                w(sub2ind(wd,xs,ys+1,zs)), ...
                w(sub2ind(wd,xs-1,ys+1,zs)), ...
                w(sub2ind(wd,xs,ys,zs-1)), ...
                w(sub2ind(wd,xs-1,ys,zs-1)), ...
                w(sub2ind(wd,xs,ys+1,zs-1))]')' == 6);

rdf = find(w(sub2ind(wd,xs+1,ys+1,zs+1)) == 0 & ...
           sum([w(sub2ind(wd,xs+1,ys,zs)), ...
                w(sub2ind(wd,xs,ys+1,zs)), ...
                w(sub2ind(wd,xs+1,ys+1,zs)), ...
                w(sub2ind(wd,xs,ys,zs+1)), ...
                w(sub2ind(wd,xs+1,ys,zs+1)), ...
                w(sub2ind(wd,xs,ys+1,zs+1))]')' == 6);

rdb = find(w(sub2ind(wd,xs+1,ys+1,zs-1)) == 0 & ...
           sum([w(sub2ind(wd,xs+1,ys,zs)), ...
                w(sub2ind(wd,xs,ys+1,zs)), ...
                w(sub2ind(wd,xs+1,ys+1,zs)), ...
                w(sub2ind(wd,xs,ys,zs-1)), ...
                w(sub2ind(wd,xs+1,ys,zs-1)), ...
                w(sub2ind(wd,xs,ys+1,zs-1))]')' == 6);

cube = [];

if (~isempty(ruf))
    cube(end+1:end+length(ruf),:) = ...
        [xs(ruf),ys(ruf),zs(ruf), ...
         xs(ruf)-1,ys(ruf)+1,zs(ruf)+1];
end

if (~isempty(rub))
    cube(end+1:end+length(rub),:) = ...
        [xs(rub),ys(rub),zs(rub), ...
         xs(rub)-1,ys(rub)+1,zs(rub)-1];
end

if (~isempty(rdf))
    cube(end+1:end+length(rdf),:) = ...
        [xs(rdf),ys(rdf),zs(rdf), ...
         xs(rdf)+1,ys(rdf)+1,zs(rdf)+1];
end

if (~isempty(rdb))
    cube(end+1:end+length(rdb),:) = ...
        [xs(rdb),ys(rdb),zs(rdb), ...
         xs(rdb)+1,ys(rdb)+1,zs(rdb)-1];
end

% adjust according to cube (3: fix bad 0 vertex connectivities)
if (~isempty(cube))
    len = size(cube,1);
    for i = 1:len
        k = cube(i,:);
        if (w(k(1),k(2),k(3))==0 & w(k(4),k(5),k(6))==0)
            if (nb_sum(w,k(1),k(2),k(3)) >= nb_sum(w,k(4),k(5),k(6)))
                w(k(1),k(2),k(3)) = 1;
                fixset(end+1,:) = [[k(1), k(2), k(3)]-1, 1, infoid];
            else
                w(k(4),k(5),k(6)) = 1;
                fixset(end+1,:) = [[k(4), k(5), k(6)]-1, 1, infoid];
            end
        end
    end
end

cnts = [length(ruf), length(rub), length(rdf), length(rdb)];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% work on 1 vertex connectivies
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

ind = find(w);
[xs,ys,zs] = ind2sub(wd,ind);

% right up forward, right up backward, right down forward, right down backward
ruf = find(w(sub2ind(wd,xs-1,ys+1,zs+1)) == 1 & ...
           sum([w(sub2ind(wd,xs-1,ys,zs)), ...
                w(sub2ind(wd,xs,ys+1,zs)), ...
                w(sub2ind(wd,xs-1,ys+1,zs)), ...
                w(sub2ind(wd,xs,ys,zs+1)), ...
                w(sub2ind(wd,xs-1,ys,zs+1)), ...
                w(sub2ind(wd,xs,ys+1,zs+1))]')' == 0);

rub = find(w(sub2ind(wd,xs-1,ys+1,zs-1)) == 1 & ...
           sum([w(sub2ind(wd,xs-1,ys,zs)), ...
                w(sub2ind(wd,xs,ys+1,zs)), ...
                w(sub2ind(wd,xs-1,ys+1,zs)), ...
                w(sub2ind(wd,xs,ys,zs-1)), ...
                w(sub2ind(wd,xs-1,ys,zs-1)), ...
                w(sub2ind(wd,xs,ys+1,zs-1))]')' == 0);

rdf = find(w(sub2ind(wd,xs+1,ys+1,zs+1)) == 1 & ...
           sum([w(sub2ind(wd,xs+1,ys,zs)), ...
                w(sub2ind(wd,xs,ys+1,zs)), ...
                w(sub2ind(wd,xs+1,ys+1,zs)), ...
                w(sub2ind(wd,xs,ys,zs+1)), ...
                w(sub2ind(wd,xs+1,ys,zs+1)), ...
                w(sub2ind(wd,xs,ys+1,zs+1))]')' == 0);

rdb = find(w(sub2ind(wd,xs+1,ys+1,zs-1)) == 1 & ...
           sum([w(sub2ind(wd,xs+1,ys,zs)), ...
                w(sub2ind(wd,xs,ys+1,zs)), ...
                w(sub2ind(wd,xs+1,ys+1,zs)), ...
                w(sub2ind(wd,xs,ys,zs-1)), ...
                w(sub2ind(wd,xs+1,ys,zs-1)), ...
                w(sub2ind(wd,xs,ys+1,zs-1))]')' == 0);

cube = [];

if (~isempty(ruf))
    cube(end+1:end+length(ruf),:) = ...
        [xs(ruf),ys(ruf),zs(ruf), ...
         xs(ruf)-1,ys(ruf)+1,zs(ruf)+1];
end

if (~isempty(rub))
    cube(end+1:end+length(rub),:) = ...
        [xs(rub),ys(rub),zs(rub), ...
         xs(rub)-1,ys(rub)+1,zs(rub)-1];
end

if (~isempty(rdf))
    cube(end+1:end+length(rdf),:) = ...
        [xs(rdf),ys(rdf),zs(rdf), ...
         xs(rdf)+1,ys(rdf)+1,zs(rdf)+1];
end

if (~isempty(rdb))
    cube(end+1:end+length(rdb),:) = ...
        [xs(rdb),ys(rdb),zs(rdb), ...
         xs(rdb)+1,ys(rdb)+1,zs(rdb)-1];
end

% adjust according to cube (4: remove bad 1 voxel connectivities)
if (~isempty(cube))
    len = size(cube,1);
    for i = 1:len
        k = cube(i,:);
        if (w(k(1),k(2),k(3))==1 & w(k(4),k(5),k(6))==1)
            if (nb_sum(w,k(1),k(2),k(3)) <= nb_sum(w,k(4),k(5),k(6)))
                w(k(1),k(2),k(3)) = 0;
                fixset(end+1,:) = [[k(1), k(2), k(3)]-1, 0, infoid];
            else
                w(k(4),k(5),k(6)) = 0;
                fixset(end+1,:) = [[k(4), k(5), k(6)]-1, 0, infoid];
            end
        end
    end
end

cnts = [length(ruf), length(rub), length(rdf), length(rdb)];

% make roi consistent with w
roi = w(2:d(1)+1,2:d(2)+1,2:d(3)+1);

return;

%
% fix bad edge connectivities
%

function [roi, fixset] = edge_conn(roi, d, infoid)

roi = reshape(roi,d);

% make a work area so that all border voxels belong to the background
w = zeros(d+2);
w(2:d(1)+1,2:d(2)+1,2:d(3)+1) = roi;
wd = d+2;

ind = find(w);
[xs,ys,zs] = ind2sub(wd,ind);

squa = [];

% right up and right down
ru = find(w(sub2ind(wd,xs-1,ys,zs))   == 0 & ...
          w(sub2ind(wd,xs,ys+1,zs))   == 0 & ...
          w(sub2ind(wd,xs-1,ys+1,zs)) == 1);

if (~isempty(ru))
    squa(end+1:end+length(ru),:) = ...
                [xs(ru),ys(ru),zs(ru),...
                 xs(ru)-1,ys(ru)+1,zs(ru),...
                 xs(ru)-1,ys(ru),zs(ru),...
                 xs(ru),ys(ru)+1,zs(ru)];
end

rd = find(w(sub2ind(wd,xs+1,ys,zs))   == 0 & ...
          w(sub2ind(wd,xs,ys+1,zs))   == 0 & ...
          w(sub2ind(wd,xs+1,ys+1,zs)) == 1);

if (~isempty(rd))
    squa(end+1:end+length(rd),:) = ...
                [xs(rd),ys(rd),zs(rd),...
                 xs(rd)+1,ys(rd)+1,zs(rd),...
                 xs(rd)+1,ys(rd),zs(rd),...
                 xs(rd),ys(rd)+1,zs(rd)];
end

% forward up and forward down
fu = find(w(sub2ind(wd,xs-1,ys,zs))   == 0 & ...
          w(sub2ind(wd,xs,ys,zs+1))   == 0 & ...
          w(sub2ind(wd,xs-1,ys,zs+1)) == 1);

if (~isempty(fu))
    squa(end+1:end+length(fu),:) = ...
                [xs(fu),ys(fu),zs(fu),...
                 xs(fu)-1,ys(fu),zs(fu)+1,...
                 xs(fu)-1,ys(fu),zs(fu),...
                 xs(fu),ys(fu),zs(fu)+1];
end

fd = find(w(sub2ind(wd,xs+1,ys,zs))   == 0 & ...
          w(sub2ind(wd,xs,ys,zs+1))   == 0 & ...
          w(sub2ind(wd,xs+1,ys,zs+1)) == 1);

if (~isempty(fd))
    squa(end+1:end+length(fd),:) = ...
                [xs(fd),ys(fd),zs(fd),...
                 xs(fd)+1,ys(fd),zs(fd)+1,...
                 xs(fd)+1,ys(fd),zs(fd),...
                 xs(fd),ys(fd),zs(fd)+1];
end

% left forward and right forward
lf = find(w(sub2ind(wd,xs,ys-1,zs))   == 0 & ...
          w(sub2ind(wd,xs,ys,zs+1))   == 0 & ...
          w(sub2ind(wd,xs,ys-1,zs+1)) == 1);

if (~isempty(lf))
    squa(end+1:end+length(lf),:) = ...
                [xs(lf),ys(lf),zs(lf),...
                 xs(lf),ys(lf)-1,zs(lf)+1,...
                 xs(lf),ys(lf)-1,zs(lf),...
                 xs(lf),ys(lf),zs(lf)+1];
end

rf = find(w(sub2ind(wd,xs,ys+1,zs))   == 0 & ...
          w(sub2ind(wd,xs,ys,zs+1))   == 0 & ...
          w(sub2ind(wd,xs,ys+1,zs+1)) == 1);

if (~isempty(rf))
    squa(end+1:end+length(rf),:) = ...
                [xs(rf),ys(rf),zs(rf),...
                 xs(rf),ys(rf)+1,zs(rf)+1,...
                 xs(rf),ys(rf)+1,zs(rf),...
                 xs(rf),ys(rf),zs(rf)+1];
end

% adjust according to squa (5: remove bad edge connectivities)
fixset = [];
if (~isempty(squa))
    len = size(squa,1);
    for i = 1:len
        k = squa(i,:);
        if (sum([w(k(1),k(2),k(3)) ...
                 w(k(4),k(5),k(6)) ...
                 w(k(7),k(8),k(9)) ...
                 w(k(10),k(11),k(12)) ...
                 ]==[1 1 0 0])==4)
            [start, val] = decide_squa(w,k);
            w(k(start),k(start+1),k(start+2)) = val;
            fixset(end+1,:) = [[k(start), k(start+1), k(start+2)]-1, val, infoid];
        end
    end
end

cnts = [length(ru),length(rd),length(fu),length(fd),length(lf),length(rf)];

% make roi consistent with w
roi = w(2:d(1)+1,2:d(2)+1,2:d(3)+1);

return;

%
% check ring on one slice, and hole between two slices
%

function [roi, fixset] = ring_hole(roi, d, infoid)

roi = reshape(roi,d);

% make a work area so that all border voxels belong to the background
w = zeros(d+2);
w(2:d(1)+1,2:d(2)+1,2:d(3)+1) = roi;
wd = d+2;

% exchange background and object
w = 1-w;

fixset = [];

for k = 1:3
    % label each slice
	for i = 1:wd(k)
        switch k
        case 1
            im = reshape(w(i,:,:),wd(2),wd(3));
        case 2
            im = reshape(w(:,i,:),wd(1),wd(3));
        case 3
            im = reshape(w(:,:,i),wd(1),wd(2));
        end
        [lab, num(i)] = bwlabel(im,4);
        L{i} = lab;
	end
    % original roi background should be labeled as '1'
    ind = find(num>1);
    for i = 1:length(ind);
        lab = L{ind(i)};
        for j = 1:num(ind(i))
            cnt(j) = length(find(lab == j));
        end
        [mval, mind] = max(cnt);
        if (mind > 1)
            disp('Make background labeled as 1');
            ind1 = find(lab == 1);
            ind2 = find(lab == mind);
            lab(ind2) = 1; % background
            lab(ind1) = mind;
            L{ind(i)} = lab;
        end
    end
    % check multiple component slices
    for i = 1:length(ind);
        lab = L{ind(i)};
        for j = 2:num(ind(i))
            % connect to background in the previous slice
            con_prev = find(lab == j & L{ind(i)-1} == 1);
            % connect to background in the next slice
            con_next = find(lab == j & L{ind(i)+1} == 1);
            if (~isempty(con_prev) & ~isempty(con_next))
                [xs, ys] = find(lab == j);
                switch k
                case 1
                    im = reshape(w(ind(i),:,:),wd(2),wd(3));
                    sigind = sub2ind(wd,xs*0+ind(i),xs,ys);
                case 2
                    im = reshape(w(:,ind(i),:),wd(1),wd(3));
                    sigind = sub2ind(wd,xs,xs*0+ind(i),ys);
                case 3
                    im = reshape(w(:,:,ind(i)),wd(1),wd(2));
                    sigind = sub2ind(wd,xs,ys,xs*0+ind(i));
                end
                w(sigind) = 0; % note that really it is assigned 1 to roi
                [xs,ys,zs] = ind2sub(wd,sigind);
                fixset(end+1:end+length(xs),:) = [[xs,ys,zs]-1, xs*0+1, xs*0+infoid];
            end
        end
    end
    % check hole between slices
    for i = 2:(wd(k)-2)
        overlap = L{i}*0;
        overlap_bkgd = find(L{i} == 1 & L{i+1} == 1);
        overlap(overlap_bkgd) = 1;
        [lab, m] = bwlabel(overlap,4);
        if (m>1)
            % deal with only the minimum one;
            for j = 1:m
                cnt(j) = length(find(lab == j));
            end
            [mval, mind] = min(cnt);
            % connect to background in the previous slice
            con_prev = find(lab == mind & L{i-1} == 1);
            % connect to background in the current slice
            con_curr = find(lab == mind & L{i} == 1);
            % connect to background in the next slice
            con_next = find(lab == mind & L{i+1} == 1);
            % connect to background in the next next slice
            con_next_next = find(lab == mind & L{i+2} == 1);
            if ((~isempty(con_curr) | ~isempty(con_prev)) & ...
                (~isempty(con_next) | ~isempty(con_next_next)))
                [xs, ys] = find(lab == mind);
                switch k
                case 1
                    sigind = [sub2ind(wd,xs*0+i,xs,ys);sub2ind(wd,xs*0+i+1,xs,ys)];
                case 2
                    sigind = [sub2ind(wd,xs,xs*0+i,ys);sub2ind(wd,xs,xs*0+i+1,ys)];
                case 3
                    sigind = [sub2ind(wd,xs,ys,xs*0+i);sub2ind(wd,xs,ys,xs*0+i+1)];
                end
                w(sigind) = 0; % note that really it is assigned 1 to roi
                [xs,ys,zs] = ind2sub(wd,sigind);
                fixset(end+1:end+length(xs),:) = [[xs,ys,zs]-1, xs*0+1, xs*0+infoid];
            end
        end
    end
    L = [];
    num = [];
end

% make roi consistent with w
roi = 1 - w(2:d(1)+1,2:d(2)+1,2:d(3)+1);

return;

%
% select a point to change its value
% according to maximum number of different neighbours
%

function [start, val] = decide_squa(w,k)

for i = 1:3:10
    cnt((i-1)/3+1) = nb_sum(w, k(i), k(i+1), k(i+2));
end

cnt(1:2) = 6 - cnt(1:2);

[v, in] = max(cnt);

start = (in-1)*3+1;
if (in < 3)
    val = 0;
else
    val = 1;
end

return;

%
% calculate the sum of the direct neighbour
%
function b = nb_sum(w, x, y, z)

b = sum([w(x-1,y,z), ...
         w(x+1,y,z), ...
         w(x,y-1,z), ...
         w(x,y+1,z), ...
         w(x,y,z-1), ...
         w(x,y,z+1)]);

return;


%% Efficacy of coadministration of Drug X with Statin on cholesterol reduction
% Copyright 2010 - 2011 MathWorks, Inc.

%% Abstract
% Statins are the most common class of drugs used for treating
% hyperlipdemia. However, studies have shown that even at their maximum
% dosage of 80 mg, many patients do not reach LDL cholesterol goals
% recommended by the National Cholesterol Education Program Adult Treatment
% Panel. Combination therapy, in which a second cholesterol-reducing agent that
% acts via a complementary pathway is coadmininstered with statin, is one
% alternative of achieving higher efficacy at lower statin dosage.
%
% In this example, we test the primary hypothesis that coadminstering drug
% X with statin is more effective at reducing cholesterol levels than
% statin monotherapy.
%
% *NOTE The dataset used in this example is purely fictitious*.
%
% The analysis presented in this example is adapted from the following
% publication.
%
% *Reference* Ballantyne CM, Houri J, Notarbartolo A, Melani L, Lipka LJ,
% Suresh R, Sun S, LeBeaut AP, Sager PT, Veltri EP; _Ezetimibe Study Group.
% Effect of ezetimibe coadministered with atorvastatin in 628 patients with
% primary hypercholesterolemia: a prospective, randomized, double-blind
% trial._ Circulation. 2003 May 20;107(19):2409-15.

%% Data
% 650 patients were randomly assigned to one of the following 10 treatment
% groups (65 subjects per group)
%
% * Placebo
% * Drug X (10 mg)
% * Statin (10, 20, 40 or 80 mg)
% * Drug X (10 mg) + Statin (10, 20, 40 or 80 mg)
%
% Lipid profile (LDL cholesterol, HDL CHolesterol and Triglycerides) was
% measured at baseline (BL) and at 12 weeks (after the start of treatment).
% In addition to the lipid profile, patients age, gender and Cardiac Heart
% Disease (CHD) risk category was also logged at baseline.
%
% The data from the study is stored in a Microsoft Excel (R) file. Note
% that the  data could also be imported from other sources such as text
% files, any JDBC/ODBC compliant database, SAS transport files, etc.
%
% The columns in the data are as follows:
%
% * ID     - Patient ID
% * Group  - Treatment group
% * Dose_A - Dosage of Statin (mg)
% * Dose_X - Dosage of Drug X (mg)
% * Age    - Patient Age
% * Gender - Patient Gender
% * Risk   - Patient CHD risk category (1 is high risk, and 3 is low risk)
% * LDL_BL - HDL_BL & TC_BL - Lipid levels at baseline
% * LDL_12wks , HDL_12wks & TC_12wks - Lipid levels after treatment

%%
% We will import the data into a dataset array that affords better data
% managemment and organization.

% Import data from an Excel file
  ds = dataset('xlsfile', 'Data.xls') ;

%% Preliminary analysis
% Our primary efficacy endpoint is the level of LDL cholesterol. Let us
% compare the LDL C levels at baseline to LDL C levels after treatment

% Use custom scatter plot
  LDLplot(ds.LDL_BL, ds.LDL_12wk, 50, 'g')

%%
% The mean LDL C level at baseline is around 4.2 and mean level after
% treatment is 2.5. So, at least for the data pooled across all the
% treatment groups, it seems that the treatment causes lowering of the LDL
% cholesterol levels

% Use a grouped scatter plot
  figure
  gscatter(ds.LDL_BL, ds.LDL_12wk, ds.Group)

%%
% The grouped plot shows that LDL C levels before the start of treatment
% have similar means. However, the LDL C levels after treatment show
% difference across treatment groups. The Placebo group show no
% improvement. Statin monotherapy seems to outperform the Drug X
% monotherapy. There is overlap between the Statin and Statin + X groups;
% however, it the combination treatment does seem to perform better that
% the statin monotherapy. Remember that the "Statin" and "Statin + X"
% groups are further split based on Statin dose.
%
% In this example, we will use percentage change of LDL C from the baseline
% level as the primary metric of efficacy.

% Calculate the percentage improvement over baseline level

  ds.Change_LDL = ( ds.LDL_BL - ds.LDL_12wk ) ./ ds.LDL_BL * 100 ;

%%
% In the following graph, we can see that
%
% # In the "Statin" and "Statin + X" group, there appears to be a positive
% linear correlation between percentage improvement and statin dose
% # Even at the smallest dose of 10 mg, monotherapy with statin seems to be
% better than the Drug X monotherapy group

% Visualize effect of treatment and statin dose on perecentage LDL reduction
  figure
  gscatter(ds.ID, ds.Change_LDL, {ds.Group, ds.Dose_S})
  legend('Location', 'Best')

%% Pooled comparison: Is the combination therapy better than statin monotherapy ?
%
% First, we will extract percent change in LDL C level for the Statin and
% the Statin + X groups only.  We will test the null hypothesis that the
% percent change in LDL C level for the "Statin + X" groups is greater than
% that in the "Statin + X" using pooled data. We use a 2 sample t-test to
% test this hypothesis.

% Convert Group into a categorical variable
ds.Group = nominal(ds.Group) ;

grp1 = ds.Change_LDL(ds.Group == 'Statin')         ;
grp2 = ds.Change_LDL(ds.Group == 'Statin + X')     ;

[h, p] = ttest2(grp1, grp2, .01, 'left')

%%
% We performed a tailed hypothesis to see if Statin + X group (grp2) is
% better than the Statin group (grp1). We test against the alternative that
% that mean LDL change of grp1 (Statin only) is less than mean LDL change
% of grp2 (Statin + X)
%
% The null hypothesis is rejected (p < 0.01), implying that grp1 mean is
% less that grp2 mean, i.e. the Statin group is less effective at lowering
% LDL C levels than the Statin + X group.
%
% The pooled analysis shows that coadministering drug X with statin is more
% effective than statin monotherapy.

%% Effect of Treatment, Statin Dose and Dose by Treatment interaction
% Our analysis so far was done on pooled data. We analysed the effect of
% treatment (statin alone (X = 0) vs. statin + 10 mg X) on the LDL C
% levels. We ignored levels of statin dose within each treatment group
%
% Next, we will perform a 2-way ANOVA (analysis of variance) to
% simultaneously understand the effect of both factors - statin dose (4
% levels -  10 20, 40, 80 mg) and Treatment (2 level - statin only or
% Statin + 10 mg X ) - on the percentage change of LDL C levels.
%

% First, we filter the data to include only the Statin and Statin + X groups
ds1       = ds(ds.Group == 'Statin' | ds.Group == 'Statin + X', :)    ;

anovan(ds1.Change_LDL , {ds1.Dose_S, ds1.Group }         , ...
        'varnames'    , {'Statin Dose', 'Treatment'}    ) ;

%% Effect of Statin Dose on incremental increase in percentage LDL reduction
% The ANOVA results indicate that statin dose is a significant factor, but
% it doesn't compare means across individual dose-treatment level
% combination. Let's look at the individual cell means.

ds2 = grpstats(ds1 , {'Dose_X', 'Dose_S'}, '', 'DataVars', 'Change_LDL')

%%
% Convert to wide format
ds2 = unstack(ds2, 'mean_Change_LDL' , 'Dose_X', ...
                   'NewDataVarNames' , {'Change_LDL_St', 'Change_LDL_St_X'} )

%%
% From the above table, we can clearly see that the average efficacy of the
% combination therapy is better than statin monotherapy at all statin
% dosages.
%
% In the plot of the individual means, notice that the percentage reduction
% in LDL C levels achieved in the low dose combination therapy group (~50.5
% %) is comparable to that achieved in the higher dose Statin monotherapy
% group (~ 49.4 %). Thus combination therapy with Drug X could help
% patients that cannot tolerate high statin doses.

figure
bar([ds2.Change_LDL_St, ds2.Change_LDL_St_X])
set(gca, 'XTickLabel', [10, 20 40, 80])
colormap summer
xlabel('Statin Dose Groups(mg)')
ylabel('Percentage reduction of LDL C from Baseline (mmol/L)')
legend('Statin', 'Statin + X')

%% Regression analysis: Effect of statin dose on percent LDL C reduction
% In the above graph, there appears to be linear improvement in the
% effectiveness metric for both treatment groups. In general it seems that
% for every doubling of the statin dose, there is a 5-6 point improvement
% in the percentage LDL C reduction. Let's fit a linear regression line to
% the entire dataset, instead of to the mean level.
%
x = ds1.Dose_S    (  ds1.Group == 'Statin' ) ;
y = ds1.Change_LDL(  ds1.Group == 'Statin' ) ;

x1 = ds1.Dose_S    (ds1.Group == 'Statin + X')  ;
y1 = ds1.Change_LDL(ds1.Group == 'Statin + X')  ;

%%

cftool

%%
% The regression line for the Statin and the Statin + X group run almost
% parallel. This probably indicates mechanism of actions of drug X and
% statins are independent.

% Fit
[m1, m2] = createFit(x,y,x1, y1)

%% Secondary Analysis: Consistency of effect across subgroups, age and gender
% Finally, we will make a visual check to ensure that the efficacy of the
% Statin + X treatment at various statin doses is consistent across gender
% and age subgroups. We will perform this check for only the Statin + X
% treatment group.

idx = ds.Group == 'Statin + X' ;
boxplot(ds.Change_LDL(idx), { ds.Dose_S(idx), ds.Gender(idx)} )

%%
% We will convert the continuous age variable into a catergorical variable,
% with 2 categories: Age < 65 and Age >= 65

% Convert age into a ordinal array
ds.AgeGroup = ordinal(ds.Age ,{'< 65', '>= 65'} , [] ,[0 65 100] )  ;

% Plot
boxplot(ds.Change_LDL(idx), { ds.Dose_S(idx), ds.AgeGroup(idx)} )


function [taumat, thetamat] ...
         = SimulateControl(thetalist, dthetalist, g, Ftipmat, Mlist, ...
                           Glist, Slist, thetamatd, dthetamatd, ...
                           ddthetamatd, gtilde, Mtildelist, Gtildelist, ...
                           Kp, Ki, Kd, dt, intRes)
% *** CHAPTER 11: ROBOT CONTROL ***
% Takes thetalist: n-vector of initial joint variables,
%       dthetalist: n-vector of initial joint velocities,
%       g: Actual gravity vector g,
%       Ftipmat: An N x 6 matrix of spatial forces applied by the
%                end-effector (If there are no tip forces, the user should
%                input a zero and a zero matrix will be used),
%       Mlist: Actual list of link frames i relative to i? at the home
%              position,
%       Glist: Actual spatial inertia matrices Gi of the links,
%       Slist: Screw axes Si of the joints in a space frame, in the format
%              of a matrix with the screw axes as the columns,
%       thetamatd: An Nxn matrix of desired joint variables from the
%                  reference trajectory,
%       dthetamatd: An Nxn matrix of desired joint velocities,
%       ddthetamatd: An Nxn matrix of desired joint accelerations,
%       gtilde: The gravity vector based on the model of the actual robot
%               (actual values given above),
%       Mtildelist: The link frame locations based on the model of the
%                   actual robot (actual values given above),
%       Gtildelist: The link spatial inertias based on the model of the
%                   actual robot (actual values given above),
%       Kp: The feedback proportional gain (identical for each joint),
%       Ki: The feedback integral gain (identical for each joint),
%       Kd: The feedback derivative gain (identical for each joint),
%       dt: The timestep between points on the reference trajectory.
%       intRes: Integration resolution is the number of times integration
%               (Euler) takes places between each time step. Must be an
%               integer value greater than or equal to 1.
% Returns taumat: An Nxn matrix of the controller commanded joint
%                 forces/torques, where each row of n forces/torques
%                 corresponds to a single time instant,
%         thetamat: An Nxn matrix of actual joint angles.
% The end of this function plots all the actual and desired joint angles.
% Example Usage
%
% clc; clear;
% thetalist = [0.1; 0.1; 0.1];
% dthetalist = [0.1; 0.2; 0.3];
% %Initialize robot description (Example with 3 links)
% g = [0; 0; -9.8];
% M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.089159]; [0, 0, 0, 1]];
% M12 = [[0, 0, 1, 0.28]; [0, 1, 0, 0.13585]; [-1, 0 ,0, 0]; [0, 0, 0, 1]];
% M23 = [[1, 0, 0, 0]; [0, 1, 0, -0.1197]; [0, 0, 1, 0.395]; [0, 0, 0, 1]];
% M34 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.14225]; [0, 0, 0, 1]];
% G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]);
% G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]);
% G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]);
% Glist = cat(3, G1, G2, G3);
% Mlist = cat(3, M01, M12, M23, M34);
% Slist = [[1; 0; 1;      0; 1;     0], ...
%        [0; 1; 0; -0.089; 0;     0], ...
%        [0; 1; 0; -0.089; 0; 0.425]];
% dt = 0.01;
% %Create a trajectory to follow
% thetaend =[pi / 2; pi; 1.5 * pi];
% Tf = 1;
% N = Tf / dt;
% method = 5;
% thetamatd = JointTrajectory(thetalist, thetaend, Tf, N, method);
% dthetamatd = zeros(N, 3);
% ddthetamatd = zeros(N, 3);
% dt = Tf / (N - 1);
% for i = 1: N - 1
%   dthetamatd(i + 1, :) = (thetamatd(i + 1, :) - thetamatd(i, :)) / dt;
%   ddthetamatd(i + 1, :) = (dthetamatd(i + 1, :) ...
%                           - dthetamatd(i, :)) / dt;
% end
% %Possibly wrong robot description (Example with 3 links)
% gtilde = [0.8; 0.2; -8.8];
% Mhat01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.1]; [0, 0, 0, 1]];
% Mhat12 = [[0, 0, 1, 0.3]; [0, 1, 0, 0.2]; [-1, 0 ,0, 0]; [0, 0, 0, 1]];
% Mhat23 = [[1, 0, 0, 0]; [0, 1, 0, -0.2]; [0, 0, 1, 0.4]; [0, 0, 0, 1]];
% Mhat34 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.2]; [0, 0, 0, 1]];
% Ghat1 = diag([0.1, 0.1, 0.1, 4, 4, 4]);
% Ghat2 = diag([0.3, 0.3, 0.1, 9, 9, 9]);
% Ghat3 = diag([0.1, 0.1, 0.1, 3, 3, 3]);
% Gtildelist = cat(3, Ghat1, Ghat2, Ghat3);
% Mtildelist = cat(4, Mhat01, Mhat12, Mhat23, Mhat34);
% Ftipmat = ones(N, 6);
% Kp = 20;
% Ki = 10;
% Kd = 18;
% intRes = 8;
% [taumat, thetamat] ...
% = SimulateControl(thetalist, dthetalist, g, Ftipmat, Mlist, Glist, ...
%                 Slist, thetamatd, dthetamatd, ddthetamatd, gtilde, ...
%                 Mtildelist, Gtildelist, Kp, Ki, Kd, dt, intRes);
%

Ftipmat = Ftipmat';
thetamatd = thetamatd';
dthetamatd = dthetamatd';
ddthetamatd = ddthetamatd';
n = size(thetamatd, 2);
taumat = zeros(size(thetamatd));
thetamat = zeros(size(thetamatd));
thetacurrent = thetalist;
dthetacurrent = dthetalist;
eint = zeros(size(thetamatd, 1), 1);
for i=1: n
    taulist ...
    = ComputedTorque(thetacurrent, dthetacurrent, eint, gtilde, ...
                     Mtildelist, Gtildelist, Slist, thetamatd(:, i), ...
                     dthetamatd(:, i), ddthetamatd(:, i), Kp, Ki, Kd);
    for j=1: intRes
        ddthetalist ...
        = ForwardDynamics(thetacurrent, dthetacurrent, taulist, g, ...
                          Ftipmat(:, i), Mlist, Glist, Slist);
        [thetacurrent, dthetacurrent] ...
        = EulerStep(thetacurrent, dthetacurrent, ddthetalist, ...
                    (dt / intRes));
    end
    taumat(:, i) = taulist;
    thetamat(:, i) = thetacurrent;
    eint = eint + (dt*(thetamatd(:, i) - thetacurrent));
end
%Output using matplotlib
links = size(thetamat, 1);
leg = cell(1, 2 * links);
time=0: dt: dt * n - dt;
timed=0: dt: dt * n - dt;
figure
hold on
for i=1: links
    col = rand(1, 3);
    plot(time, (thetamat(i, :)'), '-', 'Color', col)
    plot(timed, (thetamatd(i, :)'), '.', 'Color', col)
    leg{2 * i - 1} = (strcat('ActualTheta', num2str(i)));
    leg{2 * i} = (strcat('DesiredTheta', num2str(i)));
end
title('Plot of Actual and Desired Joint Angles')
xlabel('Time')
ylabel('Joint Angles')
legend(leg, 'Location', 'NorthWest')
taumat = taumat';
thetamat = thetamat';
end