function fem1d_bvp_linear_test03 ( )

%*****************************************************************************80
%
%% FEM1D_BVP_LINEAR_TEST03 carries out test case #3.
%
%  Location:
%
%    http://people.sc.fsu.edu/~jburkardt/m_src/fem1d_bvp_linear/fem1d_bvp_linear_test03.m
%
%  Discussion:
%
%    Use A3, C3, F3, EXACT3, EXACT_UX3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Dianne O'Leary,
%    Scientific Computing with Case Studies,
%    SIAM, 2008,
%    ISBN13: 978-0-898716-66-5,
%    LC: QA401.O44.
%
  n = 11;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'FEM1D_BVP_LINEAR_TEST03\n' );
  fprintf ( 1, '  Solve -( A(x) U''(x) )'' + C(x) U(x) = F(x)\n' );
  fprintf ( 1, '  for 0 < x < 1, with U(0) = U(1) = 0.\n' );
  fprintf ( 1, '  A3(X)  = 1.0\n' );
  fprintf ( 1, '  C3(X)  = 2.0 * X\n' );
  fprintf ( 1, '  F3(X)  = - X * ( 2 * X * X - 3 * X - 3 ) * exp ( X )\n' );
  fprintf ( 1, '  U3(X)  = X * ( 1 - X ) * exp ( X )\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of nodes = %d\n', n );
%
%  Geometry definitions.
%
  x_first = 0.0;
  x_last = 1.0;
  x = linspace ( x_first, x_last, n );
  x = x(:);

  u = fem1d_bvp_linear ( n, @a3, @c3, @f3, x );

  uexact = exact3 ( x );

  fprintf ( 1, '\n' );
  fprintf ( 1, '     I    X         U         Uexact    Error\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    fprintf ( 1, '  %4d  %8f  %8f  %8f  %8e\n', ...
      i, x(i), u(i), uexact(i), abs ( u(i) - uexact(i) ) );
  end

  e1 = l1_error ( n, x, u, @exact3 );
  e2 = l2_error_linear ( n, x, u, @exact3 );
  h1s = h1s_error_linear ( n, x, u, @exact_ux3 );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  l1 norm of error  = %g\n', e1 );
  fprintf ( 1, '  L2 norm of error  = %g\n', e2 );
  fprintf ( 1, '  Seminorm of error = %g\n', h1s  );

  return
end
function value = a3 ( x )

%*****************************************************************************80
%
%% A3 evaluates A function #3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of A(X).
%
  value = 1.0;

  return
end
function value = c3 ( x )

%*****************************************************************************80
%
%% C3 evaluates C function #3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 March 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of C(X).
%
  value = 2.0 * x;

  return
end
function value = exact3 ( x )

%*****************************************************************************80
%
%% EXACT3 evaluates exact solution #3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 March 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of U(X).
%
  value = x .* ( 1.0 - x ) .* exp ( x );

  return
end
function value = exact_ux3 ( x )

%*****************************************************************************80
%
%% EXACT_UX3 evaluates the derivative of exact solution #3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of dUdX(X).
%
  value = ( 1.0 - x - x .* x ) .* exp ( x );

  return
end
function value = f3 ( x )

%*****************************************************************************80
%
%% F3 evaluates right hand side function #3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 March 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Output, real VALUE, the value of F(X).
%
  value = - x .* ( 2.0 * x .* x - 3.0 * x - 3.0 ) .* exp ( x );

  return
end


function fem_basis_q4_display ( prefix )

%*****************************************************************************80
%
%% FEM_BASIS_Q4_DISPLAY displays a finite element Q4 basis function.
%
%  Discussion:
%
%    This program reads a data file defining a set of nodes, and a
%    data file defining the triangulation of those nodes using 3 node triangles
%    (or 6 node triangles, as long as the vertices are listed first).
%
%    The program then asks the user interactively to select one of the
%    nodes.  It computes the basis function associated with that node
%    and displays it over the entire mesh.  Of course, the basis function
%    will only be nonzero over a small number of the elements, but it
%    is instructive to see the entire mesh.
%
%    The display is initially "flat", but by using the manipulator
%    on the graphics menu, the user can easily get some dramatic images
%    of the basis function.
%
%  Usage:
%
%    fem_basis_q4_display ( 'prefix' )
%
%    where 'prefix' is the common prefix for the FEM files:
%
%    * 'prefix'_nodes.txt,    the node coordinates.
%    * 'prefix'_elements.txt, the nodes that make up each element;
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 March 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string PREFIX, the common file prefix.
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FEM_BASIS_Q4_DISPLAY:\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Display basis functions associated with \n' );
  fprintf ( 1, '  a finite element grid of linear quadrilaterals ("Q4").\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The user specifies a basis function by node index, and\n' );
  fprintf ( 1, '  the program displays a surface plot of that basis function.\n' );
  fprintf ( 1, '  (Use the 3D ROTATE option to see the full picture!\n' );
%
%  Get the prefix if missing.
%
  if ( nargin < 1 )
    prefix = input ( '  Enter the common file prefix:  ' );
  end
%
%  Construct the file names.
%
  node_filename = strcat ( prefix, '_nodes.txt' );
  element_filename = strcat ( prefix, '_elements.txt' );
%
%  Read the nodes.
%
  [ dim_num, node_num ] = r8mat_header_read ( node_filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Spatial dimension = %d\n', dim_num );
  fprintf ( 1, '  Number of nodes = %d\n', node_num );

  if ( dim_num ~= 2 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FEM_BASIS_Q4_DISPLAY - Fatal error!\n' );
    fprintf ( 1, '  The spatial dimension is not 2.\n' );
    error ( 'FEM_BASIS_Q4_DISPLAY - Fatal error!' );
  end

  node_xy = r8mat_data_read ( node_filename, dim_num, node_num );
%
%  Read the elements.
%
  [ element_order, element_num ] = i4mat_header_read ( element_filename );

  if ( element_order ~= 4 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FEM_BASIS_Q4_DISPLAY\n' );
    fprintf ( 1, '  This program requires that the elements have order 4.\n' );
    fprintf ( 1, '  Your elements have order %d\n', element_order );
    error ( 'FEM_BASIS_Q4_DISPLAY - Fatal error!' );
  end

  element_node = i4mat_data_read ( element_filename, element_order, ...
    element_num );
%
%  Set up the graph.
%
  x_min = min ( node_xy(1,:) );
  x_max = max ( node_xy(1,:) );
  y_min = min ( node_xy(2,:) );
  y_max = max ( node_xy(2,:) );
  z_min = 0.0;
  z_max = 1.0;

  node_min = min ( min ( element_node ) );
  node_max = max ( max ( element_node ) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Every basis function is associated with a node.\n' );
  fprintf ( 1, '  To chooose a basis function, you specify a node.\n' );
  fprintf ( 1, '  Nodes range in value from %d to %d.\n', node_min, node_max );

  while ( 1 )

    fprintf ( 1, '\n' );
    prompt = sprintf ( 'Enter a node between %d and %d: ', node_min, node_max );
    node_index = input ( prompt );

    if ( node_index < node_min | node_max < node_index )
      break
    end
%
%  Clear the graphics page.
%
    clf

    fprintf ( 1, '\n' );

    for element = 1 : element_num

      local = 0;
      for j = 1 : element_order
        if ( element_node(j,element) == node_index )
          fprintf ( 1, '  Node %d occurs as local node %d in element %d.\n', ...
            node_index, j, element );
          local = j;
        end
      end

      z(1:2,1:2) = 0.0;

      x(1,1) = node_xy(1,element_node(1,element));
      y(1,1) = node_xy(2,element_node(1,element));
      x(2,1) = node_xy(1,element_node(2,element));
      y(2,1) = node_xy(2,element_node(2,element));
      x(1,2) = node_xy(1,element_node(4,element));
      y(1,2) = node_xy(2,element_node(4,element));
      x(2,2) = node_xy(1,element_node(3,element));
      y(2,2) = node_xy(2,element_node(3,element));

      if ( local ~= 0 )
        if ( local == 1 )
          z(1,1) = 1.0;
        elseif ( local == 2 )
          z(2,1) = 1.0;
        elseif ( local == 3 )
          z(2,2) = 1.0;
        else
          z(1,2) = 1.0;
        end
      end

      caxis ( [ -0.4, 1.2 ] )
      surface ( x, y, z, 'FaceColor', 'interp' )

    end

    axis ( [ x_min, x_max, y_min, y_max, z_min, z_max ] );
    axis equal

    xlabel ( '--X axis--' );
    ylabel ( '--Y axis--' );
    zlabel ( '--Z axis--' );

    title_string = sprintf ( 'Q4 basis function for node %d', node_index );
    title ( title_string );

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

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

  return
end
function column_num = file_column_count ( input_file_name )

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

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

  while ( 1 )

    line = fgetl ( input_unit );

    if ( line == -1 )
      break;
    end

    if ( s_len_trim ( line ) == 0 )

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

    else
      got_one = TRUE;
      break;
    end

  end

  fclose ( input_unit );

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

  column_num = s_word_count ( line );

  return
end
function row_num = file_row_count ( input_file_name )

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

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

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

  record_num = 0;

  while ( 1 )

    line = fgetl ( input_unit );

    if ( line == -1 )
      break;
    end

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

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

  end

  fclose ( input_unit );

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

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

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

  input_unit = fopen ( input_filename );

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

  i = 0;

  while ( i < n )

    line = fgets ( input_unit );

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

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

    elseif ( s_len_trim ( line ) == 0 )

    else

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

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

    end

  end

  fclose ( input_unit );

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

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

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

  n = file_row_count ( input_filename );

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

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

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

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

  input_unit = fopen ( input_filename );

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

  i = 0;

  while ( i < n )

    line = fgets ( input_unit );

    if ( line == -1 )
      break;
    end

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

    elseif ( s_len_trim ( line ) == 0 )

    else

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

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

    end

  end

  fclose ( input_unit );

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

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

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

  n = file_row_count ( input_filename );

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

  return
end
function len = s_len_trim ( s )

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

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

  return
end
function word_num = s_word_count ( s )

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

  word_num = 0;
  s_length = length ( s );

  if ( s_length <= 0 )
    return;
  end

  blank = TRUE;

  for i = 1 : s_length

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

  end

  return
end
function timestamp ( )

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

  return
end


function [intersects, inds] = intersectEdgePolygon(edge, poly, varargin)
%INTERSECTEDGEPOLYGON  Intersection point of an edge with a polygon
%
%   INTER = intersectEdgePolygon(EDGE, POLY)
%   Computes intersection(s) point(s) between the edge EDGE and the polygon
%   POLY. EDGE is given by [x1 y1 x2 y2]. POLY is a N-by-2 array of vertex
%   coordinates.
%   INTER is a M-by-2 array containing coordinates of intersection(s). It
%   can be empty if no intersection is found.
%
%   [INTER, INDS] = intersectEdgePolygon(EDGE, POLY)
%   Also returns index/indices of edge(s) involved in intersections.
%
%   Example
%   % Intersection of an edge with a square
%     poly = [0 0;10 0;10 10;0 10];
%     edge = [9 2 9+3*1 2+3*2];
%     exp = [10 4];
%     inter = intersectEdgePolygon(edge, poly)
%     ans =
%         10   4
%
%   See also
%   edges2d, polygons2d, intersectLinePolygon, intersectRayPolygon
%

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

% get computation tolerance
tol = 1e-14;
if ~isempty(varargin)
    tol = varargin{1};
end

% get supporting line of edge
line = edgeToLine(edge);

% compute all intersections of supporting line with polygon
[intersects, inds] = intersectLinePolygon(line, poly, tol);

% keep only intersection points located on the edge
if ~isempty(intersects)
    pos = linePosition(intersects, line);
    keep = pos >= -tol & pos <= (1+tol);
    intersects = intersects(keep, :);
    inds = inds(keep);
end


% =========================================================================
% (c) 2016 Ronald Nissel, ronald.nissel@gmail.com
% =========================================================================
% This script simulates an FBMC and OFDM transmission over a doubly-flat
% channel, including channel estimation. The pilot symbol aided channel
% estimation in FBMC is based on R. Nissel, M. Rupp, "On Pilot-Symbol Aided
% Channel Estimation in FBMC-OQAM", IEEE ICASSP, 2016.

clear; close all;
addpath('./Theory');

M_SNR_OFDM_dB = [0:5:30];           % Signal-to-Noise Ratio in dB
NrRepetitions = 1000;               % Number of Monte Carlo repetition (different channel realizations)
QAM_ModulationOrder = 16;           % QAM signal constellation order, 4, 16, 64, 256, 1024,...

%% FBMC Object
FBMC = Modulation.FBMC(...
    12,...                          % Number subcarriers
    30,...                          % Number FBMC symbols
    15e3,...                        % Subcarrier spacing (Hz)
    15e3*14*12,...                  % Sampling rate (Samples/s)
    15e3*20,...                     % Intermediate frequency first subcarrier (Hz)
    false,...                       % Transmit real valued signal
    'Hermite-OQAM',...              % Prototype filter (Hermite, PHYDYAS, RRC) and OQAM or QAM,
    8, ...                          % Overlapping factor (corresponding to the prototype filter length)
    0, ...                          % Initial phase shift
    true ...                        % Polyphase implementation
    );

%% OFDM Object
OFDM = Modulation.OFDM(...
    12,...                          % Number subcarriers
    15,...                          % Number OFDM Symbols
    15e3,...                        % Subcarrier spacing (Hz)
    15e3*14*12,...                  % Sampling rate (Samples/s)
    15e3*20,...                     % Intermediate frequency first subcarrier (Hz)
    false,...                       % Transmit real valued signal
    0, ...                          % Cyclic prefix length (s), LTE: 1/15e3/14
    (8-1/2)*1/15e3*1/2 ...          % Zero guard length (s)
    );

%% PAM and QAM Object
PAM = Modulation.SignalConstellation(sqrt(QAM_ModulationOrder),'PAM');
QAM = Modulation.SignalConstellation(QAM_ModulationOrder,'QAM');

%% Channel Estimation Objects
ChannelEstimation_OFDM = ChannelEstimation.PilotSymbolAidedChannelEstimation(...
    'Diamond',...                           % Pilot pattern
    [...                                    % Matrix that represents the pilot pattern parameters
    OFDM.Nr.Subcarriers,...                 % Number of subcarriers
    6; ...                                  % Pilot spacing in the frequency domain
    OFDM.Nr.MCSymbols,...                   % Number of FBMC/OFDM Symbols
    4 ...                                   % Pilot spacing in the time domain
    ],...
    'linear'...                             % Interpolation(Extrapolation) method 'linear','spline','FullAverage,'MovingBlockAverage'
    );
ChannelEstimation_FBMC = ChannelEstimation.PilotSymbolAidedChannelEstimation(...
    'Diamond',...                           % Pilot pattern
    [...                                    % Matrix that represents the pilot pattern parameters
    FBMC.Nr.Subcarriers,...                 % Number of subcarriers
    6; ...                                  % Pilot spacing in the frequency domain
    FBMC.Nr.MCSymbols,...                   % Number of FBMC/OFDM Symbols
    8 ...                                   % Pilot spacing in the time domain
    ],...
    'linear'...                             % Interpolation(Extrapolation) method 'linear','spline','FullAverage,'MovingBlockAverage',...
    );

%% Imaginary Interference Cancellation Objects
AuxiliaryMethod = ChannelEstimation.ImaginaryInterferenceCancellationAtPilotPosition(...
    'Auxiliary', ...                                    % Cancellation method
    ChannelEstimation_FBMC.GetAuxiliaryMatrix(1), ...   % PilotMatrix
    FBMC.GetFBMCMatrix, ...                             % Imaginary interference matrix
    16, ...                                             % Cancel 16 closest interferers
    2 ...                                               % Pilot to data power offset
    );
CodingMethod = ChannelEstimation.ImaginaryInterferenceCancellationAtPilotPosition(...
    'Coding', ...                                       % Cancellation method
    ChannelEstimation_FBMC.PilotMatrix, ...             % PilotMatrix
    FBMC.GetFBMCMatrix, ...                             % Imaginary interference matrix
    16, ...                                             % Cancel 16 closest interferers
    2 ...                                               % Pilot to data power offset
    );

BER_FBMC_Aux = nan(length(M_SNR_OFDM_dB),NrRepetitions);
BER_FBMC_Cod = nan(length(M_SNR_OFDM_dB),NrRepetitions);
BER_FBMC_perfect = nan(length(M_SNR_OFDM_dB),NrRepetitions);
BER_OFDM = nan(length(M_SNR_OFDM_dB),NrRepetitions);
BER_OFDM_perfect = nan(length(M_SNR_OFDM_dB),NrRepetitions);
for i_rep = 1:NrRepetitions
    for i_SNR = 1:length(M_SNR_OFDM_dB)
        SNR_OFDM_dB = M_SNR_OFDM_dB(i_SNR);
        Pn_time = OFDM.PHY.SamplingRate/(OFDM.PHY.SubcarrierSpacing*OFDM.Nr.Subcarriers)*10^(-SNR_OFDM_dB/10);

        %% Generate Random BitStream
        BinaryDataStream_FBMC_Aux = randi([0 1],AuxiliaryMethod.NrDataSymbols*log2(PAM.ModulationOrder),1);
        BinaryDataStream_FBMC_Cod = randi([0 1],CodingMethod.NrDataSymbols*log2(PAM.ModulationOrder),1);
        BinaryDataStream_OFDM     = randi([0 1],(OFDM.Nr.Subcarriers*OFDM.Nr.MCSymbols-ChannelEstimation_OFDM.NrPilotSymbols)*log2(QAM.ModulationOrder),1);

        %% Transmitted Data Symbols
        xD_FBMC_Aux = PAM.Bit2Symbol(BinaryDataStream_FBMC_Aux);
        xD_FBMC_Cod = PAM.Bit2Symbol(BinaryDataStream_FBMC_Cod);
        xD_OFDM     = QAM.Bit2Symbol(BinaryDataStream_OFDM);

        %% Transmitted Pilot Symbols
        xP_FBMC = PAM.SymbolMapping(randi(PAM.ModulationOrder,[ChannelEstimation_FBMC.NrPilotSymbols 1]));
        xP_FBMC = xP_FBMC./abs(xP_FBMC);
        xP_OFDM = QAM.SymbolMapping(randi(QAM.ModulationOrder,[ChannelEstimation_OFDM.NrPilotSymbols 1]));
        xP_OFDM = xP_OFDM./abs(xP_OFDM);

        %% Transmitted Symbols
        x_FBMC_Aux = reshape(AuxiliaryMethod.PrecodingMatrix*[xP_FBMC;xD_FBMC_Aux],[FBMC.Nr.Subcarriers FBMC.Nr.MCSymbols]);
        x_FBMC_Cod = reshape(CodingMethod.PrecodingMatrix*[xP_FBMC;xD_FBMC_Cod],[FBMC.Nr.Subcarriers FBMC.Nr.MCSymbols]);
        x_OFDM = nan(OFDM.Nr.Subcarriers,OFDM.Nr.MCSymbols);
        x_OFDM(ChannelEstimation_OFDM.PilotMatrix==1) = xP_OFDM;
        x_OFDM(ChannelEstimation_OFDM.PilotMatrix==0) = xD_OFDM;

        %% Transmitted FBMC Signal (time domain)
        s_FBMC_Aux = FBMC.Modulation(x_FBMC_Aux);
        s_FBMC_Cod = FBMC.Modulation(x_FBMC_Cod);
        s_OFDM     = OFDM.Modulation(x_OFDM);

        %% Channel (doubly flat fading and AWGN)
        h = sqrt(1/2)*(randn+1j*randn);
    %     h = 1; % Pure AWGN
        n_FBMC = sqrt(Pn_time/2)*(randn(size(s_FBMC_Cod))+1j*randn(size(s_FBMC_Cod)));
        n_OFDM = sqrt(Pn_time/2)*(randn(size(s_OFDM))+1j*randn(size(s_OFDM)));

        r_FBMC_Aux = h*s_FBMC_Aux + n_FBMC;
        r_FBMC_Cod = h*s_FBMC_Cod + n_FBMC;
        r_OFDM     = h*s_OFDM + n_OFDM;

        %% Demodulate OFDM and FBMC signal
        y_FBMC_Aux = FBMC.Demodulation(r_FBMC_Aux);
        y_FBMC_Cod = FBMC.Demodulation(r_FBMC_Cod);
        y_OFDM     = OFDM.Demodulation(r_OFDM);

        %% LS channel estimates at pilot positions
        hP_LS_FBMC_Aux = y_FBMC_Aux(ChannelEstimation_FBMC.PilotMatrix==1)./xP_FBMC/sqrt(AuxiliaryMethod.PilotToDataPowerOffset*AuxiliaryMethod.DataPowerReduction);
        hP_LS_FBMC_Cod = y_FBMC_Cod(ChannelEstimation_FBMC.PilotMatrix==1)./xP_FBMC/sqrt(CodingMethod.PilotToDataPowerOffset);
        hP_LS_OFDM     = y_OFDM(ChannelEstimation_OFDM.PilotMatrix==1)./xP_OFDM;

        %% Channel Estimation using Interpolation
        h_FBMC_Aux = ChannelEstimation_FBMC.ChannelInterpolation(hP_LS_FBMC_Aux);
        h_FBMC_Cod = ChannelEstimation_FBMC.ChannelInterpolation(hP_LS_FBMC_Cod);
        h_OFDM     = ChannelEstimation_OFDM.ChannelInterpolation(hP_LS_OFDM);

        %% Equalized received symbols at data position
        y_EQ_FBMC_Aux = real(y_FBMC_Aux(AuxiliaryMethod.PilotMatrix==0)./h_FBMC_Aux(AuxiliaryMethod.PilotMatrix==0)/sqrt(AuxiliaryMethod.DataPowerReduction));
        y_EQ_FBMC_Cod = real(CodingMethod.PrecodingMatrix(:,CodingMethod.NrPilotSymbols+1:end)'*(y_FBMC_Cod(:)./h_FBMC_Cod(:)));
        y_EQ_FBMC_perfect = real(CodingMethod.PrecodingMatrix(:,CodingMethod.NrPilotSymbols+1:end)'*(y_FBMC_Cod(:)./h));

        y_EQ_OFDM = y_OFDM(ChannelEstimation_OFDM.PilotMatrix==0)./h_OFDM(ChannelEstimation_OFDM.PilotMatrix==0);
        y_EQ_OFDM_perfect = y_OFDM(ChannelEstimation_OFDM.PilotMatrix==0)./h;

        %% Detect BitStream
        DetectedBitStream_FBMC_Aux = PAM.Symbol2Bit(real(y_EQ_FBMC_Aux(:)));
        DetectedBitStream_FBMC_Cod = PAM.Symbol2Bit(real(y_EQ_FBMC_Cod(:)));
        DetectedBitStream_FBMC_perfect = PAM.Symbol2Bit(real(y_EQ_FBMC_perfect(:)));

        DetectedBitStream_OFDM = QAM.Symbol2Bit(y_EQ_OFDM(:));
        DetectedBitStream_OFDM_perfect = QAM.Symbol2Bit(y_EQ_OFDM_perfect(:));

        %% Calculate BER
        BER_FBMC_Aux(i_SNR,i_rep) = mean(BinaryDataStream_FBMC_Aux~=DetectedBitStream_FBMC_Aux);
        BER_FBMC_Cod(i_SNR,i_rep) = mean(BinaryDataStream_FBMC_Cod~=DetectedBitStream_FBMC_Cod);
        BER_FBMC_perfect(i_SNR,i_rep) = mean(BinaryDataStream_FBMC_Cod~=DetectedBitStream_FBMC_perfect);

        BER_OFDM(i_SNR,i_rep) = mean(BinaryDataStream_OFDM~=DetectedBitStream_OFDM);
        BER_OFDM_perfect(i_SNR,i_rep) = mean(BinaryDataStream_OFDM~=DetectedBitStream_OFDM_perfect);

    end

    if mod(i_rep,100)==0
       disp([int2str(i_rep/NrRepetitions*100) '%']);
    end
end

%% Theoretical BEP for perfect channel knowledge
% BEP_4QAM = 1/2-1./(2*sqrt(2*(1+10.^(-M_SNR_OFDM_dB/10))-1));
M_SNR_OFDM_dB_morePoints = min(M_SNR_OFDM_dB):0.5:max(M_SNR_OFDM_dB);
BEP_perfect = BitErrorProbabilityDoublyFlatRayleigh(M_SNR_OFDM_dB_morePoints,QAM.SymbolMapping,QAM.BitMapping);

%% Plot BER and BEP
figure();
semilogy(M_SNR_OFDM_dB,mean(BER_FBMC_Aux,2),'red -o');
hold on;
semilogy(M_SNR_OFDM_dB,mean(BER_FBMC_Cod,2),'blue -o');
semilogy(M_SNR_OFDM_dB,mean(BER_OFDM,2),'black -o');
semilogy(M_SNR_OFDM_dB,mean(BER_FBMC_perfect,2),'blue -x');
semilogy(M_SNR_OFDM_dB,mean(BER_OFDM_perfect,2),'black -x');
semilogy(M_SNR_OFDM_dB_morePoints,BEP_perfect','black');
xlabel('SNR for OFDM (dB)');
ylabel('BER, BEP');
legend('Simulation: FBMC Auxiliary','Simulation: FBMC Coding','Simulation: OFDM', 'Simulation FBMC perfect CSI', 'Simulation OFDM perfect CSI','Theory perfect CSI','Location','SouthWest');

%% Plot Pilot Pattern
figure();
ChannelEstimation_OFDM.PlotPilotPattern;
title('OFDM');
figure();
ChannelEstimation_FBMC.PlotPilotPattern(AuxiliaryMethod.PilotMatrix)
title('FBMC Auxiliary');
figure();
ChannelEstimation_FBMC.PlotPilotPattern(-(CodingMethod.ConsideredInterferenceMatrix<0)+(CodingMethod.ConsideredInterferenceMatrix>0))
title('FBMC Coding');

%% Calculate and Plot Expected Transmit Power Over Time
[Power_FBMC_Aux,t_FBMC] = FBMC.PlotTransmitPower(AuxiliaryMethod.PrecodingMatrix*AuxiliaryMethod.PrecodingMatrix');
[Power_FBMC_Cod,~] = FBMC.PlotTransmitPower(CodingMethod.PrecodingMatrix*CodingMethod.PrecodingMatrix');
[Power_OFDM,t_OFDM] = OFDM.PlotTransmitPower;
figure();
plot(t_FBMC,Power_FBMC_Aux,'red');
hold on;
plot(t_FBMC,Power_FBMC_Cod,'blue');
plot(t_OFDM,Power_OFDM,'black ');
legend({'FBMC Auxiliary','FBMC Coding','OFDM'});
ylabel('Transmit Power');
xlabel('Time(s)');

%% Calculate Power Spectral Density
[PSD_FBMC_Aux,t_FBMC] = FBMC.PlotPowerSpectralDensity(AuxiliaryMethod.PrecodingMatrix*AuxiliaryMethod.PrecodingMatrix');
[PSD_FBMC_Cod,~] = FBMC.PlotPowerSpectralDensity(CodingMethod.PrecodingMatrix*CodingMethod.PrecodingMatrix');
[PSD_OFDM,t_OFDM] = OFDM.PlotPowerSpectralDensity;
figure();
plot(t_FBMC,10*log10(PSD_FBMC_Aux),'red');
hold on;
plot(t_FBMC,10*log10(PSD_FBMC_Cod),'blue');
plot(t_OFDM,10*log10(PSD_OFDM),'black ');
legend({'FBMC Auxiliary','FBMC Coding','OFDM'});
ylabel('Power Spectral Density (dB)');
xlabel('Frequency (Hz)');


function PCs = pca(this, cutoff, pcDimension)
% Computes principal component analysis (PCA) of image time series
%
%   Y = MrImage()
%   PCs = pca(this, cutoff, pcDimension)
%
% This is a method of class MrImage.
%
% Spatial PCA (default):
% The principal component analysis report representative spatial
% distributions ("principal components") of fluctuation patterns that explain most variance in the
% data (over all time points).
% The corresponding temporal evolution of weights (or scores or
% projections) describes how each of these spatial patterns fluctuates over
% time.
%
% Temporal PCA (?)
% The principal component analysis reports the representative time series
% ("principal components") that explain most of the variance in the data
% (pooled over all voxels).
% The corresponding spatial maps of weights (or scores or projections) give
% the relative contribution of this particular time series to the variance
% in each voxel
%
% IN
%   cutoff      value determining number of principal components extracted
%               0 < cutoff < 1      interpreted as relative amount of
%                                   variance explained;
%                                   nPCs will be determined as the number
%                                   of components that explain at least
%                                   cutoff*100 % of the variance in the
%                                   data
%               cutoff = 1,2,3...   interpreted as number of PCs extracted
%   pcDimension 'spatial' (default) or 'temporal'
%               determines which dimension i.e. spatial image or time
%               (volumes) will be principal component, and consequently,
%               which other one will be the projection dimension
%               Technically, the non-PC dimension is the one considered to
%               "generate the variance", in that the covariance matrix
%               entries would relate PC vector components, co-varying over
%               the non-PC dimension,
%               e.g.    spatial PCA: How do two voxels co-vary over time
%                       temporal PCA: How do two time-points co-vary over
%                       space
%               Typically, the 1st projection of the temporal PCA looks
%               like the mean, and for the other components, it usually
%               holds:
%               PC_spatial(n) approximately equal to Proj_temporal(n+1)
% OUT
%   PCs         cell(nPCs,1) of MrImages
%               Each element is a 4D image, the nth volume is computed as
%                   PCs{k}_n = PC{k}*Projection_n,
%               i.e. for
%                   spatial PCA: principal component * nth time point pf
%                   projection
%                   temporal PCA: nth element of principal component *
%                   projection vector of all voxels
%
% EXAMPLE
%   pca
%
%   See also MrImage

% Author:   Lars Kasper
% Created:  2015-08-13
% Copyright (C) 2015 Institute for Biomedical Engineering
%                    University of Zurich and ETH Zurich
%
% This file is part of the TAPAS UniQC Toolbox, which is released
% under the terms of the GNU General Public License (GPL), version 3.
% You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version).
% For further details, see the file COPYING or
%  <http://www.gnu.org/licenses/>.

% if number of components specified explicity, no additional variance
% threshold is needed

if nargin < 2
    cutoff = 1;
end

isSpatialPca = nargin < 3 || strcmpi(pcDimension, 'spatial');

hasVarianceThreshold = cutoff<1;

if hasVarianceThreshold
    nComponents = 1;
else
    nComponents = cutoff;
    cutoff = 0;
end

% created reshaped data matrix for PCA

% data matrix X: [nVoxels, nVolumes] for 4D, [nVoxel2D, nSlices] for 3D...
applicationDimension = find(this.geometry.nVoxels>1, 1, 'last');
X = reshape(this.data, [], this.geometry.nVoxels(applicationDimension));

% [nVolumes, nVoxels] for spatial PCA...
if isSpatialPca
    X = X';
end

% remove invalid data for PCA
X(isinf(X)) = 0;
X(isnan(X)) = 0;

% iteratively increase number of components, if variance threshold given,
% otherwise compute once with number of components specified
doPca = 1;
while doPca

    % Explanation for temporal PCA
    % COEFF = [nVolumes, nPCs]  principal components (PCs) ordered by variance
    %                           explained
    % SCORE = [nVoxel, nPCs]    loads of each component in each voxel, i.e.
    %                           specific contribution of each component in
    %                           a voxel's variance
    % LATENT = [nPCs, 1]        eigenvalues of data covariance matrix,
    %                           stating how much variance was explained by
    %                           each PC overall
    % TSQUARED = [nVoxels,1]    Hotelling's T-Squared test whether PC
    %                           explained significant variance in a voxel
    % EXPLAINED = [nPCs, 1]     relative amount of variance explained (in
    %                           percent) by each component
    % MU = [1, nVolumes]        mean of all time series
    [COEFF, SCORE, LATENT, TSQUARED, EXPLAINED, MU] = ...
        pca(X, 'NumComponents', nComponents);

    explainedVariance   = sum(EXPLAINED(1:nComponents))/100;
    doPca               = hasVarianceThreshold && ...
        (explainedVariance < cutoff);

    if doPca
        nComponents = nComponents + 1;

        % somehow, pca also gives out EXPLAINED variance also for more than one
        % component, jump to 1st achieved cutoff-threshold
        nComponentsTemp = find(cumsum(EXPLAINED) >= cutoff*100, 1, 'first');
        if ~isempty(nComponentsTemp)
            nComponents = nComponentsTemp;
        end
    end
end

% create 4D PCs i.e. 3D Pcs which are co-varied along the volume
% dimension with the computed projections

PCs = cell(nComponents,1);
for c = 1:nComponents
    PCs{c} = this.copyobj;
    PCs{c}.rois = {};
    PCs{c}.name = sprintf(['PC %d (explained variance: %5.2f %%, ', ...
        'cumulative %5.2f %%)) - %s'], ...
        c, EXPLAINED(c), sum(EXPLAINED(1:c)), this.name);

    % multiply the PC with each weight entry to generate whole time series
    Y = kron(SCORE(:,c)', COEFF(:,c));

    if ~isSpatialPca
        Y = Y';
    end

    PCs{c}.data = reshape(Y, this.geometry.nVoxels);
end


function [SNR] = snr_mean( target, masked )
% SNR Computes signal-to-noise ratio.
%
%   S=SNR(TARGET,MASKED) returns signal-to-noise ratio
%   given target and masked speech signals.
%
%   Inputs
%           TARGET is a target signal as vector.
%
%           MASKED is a target+masker signal as vector.
%
%   Outputs
%           S is the signal-to-noise ratio (dB).
%
%   Example
%           % read target and masker signals from wav files
%           [ target, fs ] = wavread( 'sp10.wav' );
%           [ masker, fs ] = wavread( 'ssn.wav' );
%
%           % desired SNR level (dB)
%           dSNR = 5;
%
%           % generate mixture signal: noisy = signal + noise
%           [ masked, masker ] = addnoise( target, masker, dSNR );
%
%           % compute SNR (dB)
%           SNR = snr( target, masked );
%
%           % display the result
%           fprintf( 'SNR: %0.2f dB\n', SNR );
%
%   See also ADDNOISE, SEGSNR.

%   Author: Kamil Wojcicki, November 2011.

    % compute the masker (assumes additive noise model)
    masker = masked(:) - target(:);

    % compute target and masker frame energies
    energy.target = target.' * target;
    energy.masker = masker.' * masker + eps;

    % compute frame signal-to-noise ratio (dB)
    SNR = 10*log10( energy.target ./ energy.masker + eps );
    SNR
%     SNR = 123123123;

% EOF


function [yn,en,S] = FXNLMSadapt(un,dn,S)

% FXNLMSadapt       Normalized FXLMS Algorithm
%
%                   Perform over the entire length of the input sequence. The
%                   history of output, square error and coefficients of FIR
%                   filters are passed out to extenal
% Arguments:
% un                Input signal
% dn                Desired signal
% S                 Adptive filter parameters as defined in NLMSinit.m
% yn                History of output signal
% en                History of error signal
%
% by Lee, Gan, and Kuo, 2008
% Subband Adaptive Filtering: Theory and Implementation
% Publisher: John Wiley and Sons, Ltd

M = length(S.coeffs);                     % Length of input sequence
mu = S.step;                              % Step size of NLMS algorithm (between 0 and 2)
leak = S.leakage;                         % Leaky factor for leaky LMS algorithm
alpha = S.alpha;                          % Small constant
AdaptStart = S.AdaptStart;
w = S.coeffs;                             % Coefficients of FIR filter
u = zeros(M,1);
fu = zeros(M,1);

ITER = length(un);                        % Length of input sequence
yn = zeros(1,ITER);                       % Initialize output sequence to zero
en = zeros(1,ITER);                       % Initialize error sequence to zero
%filt_un = zeros(1,ITER);                 % Initialize filtered input to zero
%est_filt_un = zeros(1,ITER);             % Initialize filtered input to zero (est)
filt_un = filter(S.sec_num,S.sec_den,un); % Filtered input
est_filt_un = filter(S.estsec,1,un);      % Filtered input (est)

if isfield(S,'unknownsys')
    b = S.unknownsys;
    norm_b = norm(b);
    eml = zeros(1,ITER);
    ComputeEML = 1;
else
    ComputeEML = 0;
end

for n = 1:ITER
   u = [filt_un(n); u(1:end-1)];          % Input signal vector (through the actual
                                          %   secondary path)
   fu = [est_filt_un(n); fu(1:end-1)];    % Filtered input signal vector (through
                                          %   the estimated secondary path)
   yn(n) = w'*u;                          % Inner product of filter coefficients and
                                          %   tappd delay line
   en(n) = dn(n)-yn(n);                   % Compute error signal
   if ComputeEML == 1;
        eml(n) = norm(b-w)/norm_b;        % System error norm (normalized)
   end
   if n >= AdaptStart
        w = (1-mu*leak)*w + ((mu*en(n))/(fu'*fu+alpha))*fu;
                                          % LMS algorithm in leaky mode
        S.iter = S.iter + 1;
   end
end

S.coeffs = w;                             % Coefficient values at the final iteration
if ComputeEML == 1;
    S.eml = eml;
end


function M = spectrahedronfactory(n, k)
% Manifold of n-by-n symmetric positive semidefinite matrices of rank k
% with trace (sum of diagonal elements) equal to 1.
%
% function M = spectrahedronfactory(n, k)
%
% A point X on the manifold is parameterized as YY^T where Y is a matrix of
% size nxk. As such, X is symmetric, positive semidefinite. We restrict to
% full-rank Y's, such that X has rank exactly k. The point X is numerically
% represented by Y (this is more efficient than working with X, which may
% be big). Tangent vectors are represented as matrices of the same size as
% Y, call them Ydot, so that Xdot = Y Ydot' + Ydot Y and trace(Xdot) == 0.
% The metric is the canonical Euclidean metric on Y.
%
% The trace constraint on X (trace(X) == 1) translates to a unit Frobenius
% norm constraint on Y: trace(X) = norm(Y, 'fro')^2 == 1. The set of such
% Y's forms the unit sphere in R^(nxk): see spherefactory. But because for
% any orthogonal Q of size k, it holds that (YQ)(YQ)' = YY', we "group" all
% matrices of the form YQ in an equivalence class. The set of equivalence
% classes is a Riemannian quotient manifold, implemented here.
%
%
% Note that this geometry formally breaks down at rank-deficient Y's.
% As an alternative, you may use the sphere manifold (it has larger
% dimension (by 1), but does not break down at rank drop.)
%
% The geometry is taken from the 2010 paper:
% M. Journee, P.-A. Absil, F. Bach and R. Sepulchre,
% "Low-Rank Optimization on the Cone of Positive Semidefinite Matrices".
% Paper link: http://www.di.ens.fr/~fbach/journee2010_sdp.pdf
%
%
% Please cite the Manopt paper as well as the research paper:
%     @Article{journee2010low,
%       Title   = {Low-rank optimization on the cone of positive semidefinite matrices},
%       Author  = {Journ{\'e}e, M. and Bach, F. and Absil, P.-A. and Sepulchre, R.},
%       Journal = {SIAM Journal on Optimization},
%       Year    = {2010},
%       Number  = {5},
%       Pages   = {2327--2351},
%       Volume  = {20},
%       Doi     = {10.1137/080731359}
%     }
%
%
% See also: spherefactory elliptopefactory symfixedrankYYfactory

% This file is part of Manopt: www.manopt.org.
% Original author: Bamdev Mishra, July 11, 2013.
% Contributors: Nicolas Boumal
% Change log:
%
%   Apr. 2, 2015 (NB):
%       Replaced trace(A'*B) by A(:)'*B(:) (equivalent but faster).
%       Updated documentation.
%
%   Apr. 17, 2018 (NB):
%       Removed dependence on lyap.
%
%   Sep.  6, 2018 (NB):
%       Removed M.exp() as it was not implemented.

    M.name = @() sprintf('YY'' quotient manifold of %dx%d psd matrices of rank %d with trace 1', n, k);

    M.dim = @() n*k - 1 - k*(k-1)/2;

    % Euclidean metric on the total space
    M.inner = @(Y, eta, zeta) eta(:)'*zeta(:);

    M.norm = @(Y, eta) sqrt(M.inner(Y, eta, eta));

    M.dist = @(Y, Z) error('spectrahedronfactory.dist not implemented yet.');

    M.typicaldist = @() 10*k;

    M.proj = @projection;
    function etaproj = projection(Y, eta)
        % Projection onto the tangent space, i.e., on the tangent space of
        % ||Y|| = 1

        eta = eta - (eta(:)'*Y(:))*Y;

        % Projection onto the horizontal space
        YtY = Y'*Y;
        SS = YtY;
        AS = Y'*eta - eta'*Y;
        % Omega = lyap(SS, -AS);
        Omega = lyapunov_symmetric(SS, AS);
        etaproj = eta - Y*Omega;
    end

    M.tangent = M.proj;
    M.tangent2ambient = @(Y, eta) eta;

    M.retr = @retraction;
    function Ynew = retraction(Y, eta, t)
        if nargin < 3
            t = 1.0;
        end
        Ynew = Y + t*eta;
        Ynew = Ynew/norm(Ynew, 'fro');
    end

    M.egrad2rgrad = @(Y, eta) eta - (eta(:)'*Y(:))*Y;

    M.ehess2rhess = @ehess2rhess;
    function Hess = ehess2rhess(Y, egrad, ehess, eta)

        % Directional derivative of the Riemannian gradient
        Hess = ehess - (egrad(:)'*Y(:))*eta - ( (ehess(:)'*Y(:)) + (eta(:)'*egrad(:)) )*Y;
        Hess = Hess - (Hess(:)'*Y(:))*Y;

        % Project on the horizontal space
        Hess = M.proj(Y, Hess);

    end

    % Notice that the hash of two equivalent points will be different...
    M.hash = @(Y) ['z' hashmd5(Y(:))];

    M.rand = @random;

    function Y = random()
        Y = randn(n, k);
        Y = Y/norm(Y,'fro');
    end

    M.randvec = @randomvec;
    function eta = randomvec(Y)
        eta = randn(n, k);
        eta = projection(Y, eta);
        nrm = M.norm(Y, eta);
        eta = eta / nrm;
    end

    M.lincomb = @matrixlincomb;

    M.zerovec = @(Y) zeros(n, k);

    M.transp = @(Y1, Y2, d) projection(Y2, d);

    M.vec = @(Y, u_mat) u_mat(:);
    M.mat = @(Y, u_vec) reshape(u_vec, [n, k]);
    M.vecmatareisometries = @() true;

end


function [aVal,aJacob,aHess,papt]=aPoly(x,numDim)
%%APOLY The drift function for a linear continuous-time motion model of a
%       given order in a specified number of Cartesian dimensions. The
%       order of the linear filter, that is the number of moments of
%       position, does not need to be explicitly specified.
%
%INPUTS: x The xDimXN state vector of N targets in the order of
%          [position;velocity;acceleration;etc] for however many
%          derivatives of position there are.
%   numDim The number of dimensions of the simulation problem. If the
%          numDim parameter is omitted, then numDim=3 (3D motion) is
%          assumed. The dimensionality of the state must be an integer
%          multiple of numDim.
%
%OUTPUTS: aVal The xDimXN time-derivative of the N state vectors under the
%              linear motion model.
%       aJacob The xDimXxDim Jacobian of aVal (it is the same for all x and
%              is not repeated N times). This is such that aJacob(:,k) is
%              the partial derivative of aVal with respect to the kth
%              element of x.
%        aHess The xDimXxDimXxDim hypermatrix such that aHess(:,k1,k2) is
%              the second partial derivative of aVal with respect to
%              elements k1 and k2 of x (all zero in this instance). It is
%              the same for all x.
%         papt The xDimX1 partial derivative of aVal with respect to time
%              (all zero in this instance). It is the same for all x.
%
%The drift function corresponds to the state transition given in
%discrete-time by the function FPolyKal.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<2)
    numDim=3;
end

numTar=size(x,2);
xDim=size(x,1);

aVal=[x((numDim+1):(xDim),:);zeros(numDim,numTar)];

if(nargout>1)
    aJacob=[zeros(xDim,numDim),[eye(xDim-numDim,xDim-numDim);zeros(numDim,xDim-numDim)],zeros(xDim,numDim)];

    if(nargout>2)
        aHess=zeros(xDim,xDim,xDim);

        if(nargout>3)
            papt=zeros(xDim,1);
        end
    end
end
end

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


function r_size  = nwspgr_size ( type, dim, k, sym, compress )

%*****************************************************************************80
%
%% NWSPGR_SIZE determines the size of a sparse grid rule.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 February 2014
%
%  Author:
%
%    Original MATLAB version by Florian Heiss, Viktor Winschel.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Florian Heiss, Viktor Winschel,
%    Likelihood approximation by numerical integration on sparse grids,
%    Journal of Econometrics,
%    Volume 144, 2008, pages 62-80.
%
%  Parameters:
%
%    Input, string TYPE, selects the 1D integration rule:
%    func:  any function name. Function must accept level l and return nodes N
%    and weights W for univariate quadrature rule with polynomial exactness
%    2*L-1 as [n, w] = feval(func,level)
%
%    Input, integer DIM, the dimension of the integration problem.
%
%    Input, integer K, the level.
%
%    Input, integer SYM.  If the rule is
%    symmetric, specifying SYM to be 1 will allow the code to run faster.
%    But this also requires that the user quadrature rule function only
%    return the nonnegative abscissas and their weights.
%
%    Input, integer COMPRESS,
%    0, do not compress the rule ( = do not merge duplicate points.)
%    1, compress the rule.
%
%    Output, integer R_SIZE, the "size" of the rule.
%
  sym = logical ( sym );
%
%  Retrieve the 1D rules of levels 1 to K.
%  Create cell arrays X1D and W1D containing the node and weight information.
%  The array N1D stores the length or order of each rule.
%
%  If the user indicates that it's a symmetric rule,
%  keep only the positive orthant.
%
  n1d = zeros ( 1, k );
  x1d = cell ( k, 1 );
  w1d = cell ( k, 1 );

  for level = 1 : k

    [ x, w ] = feval ( type, level );

    if ( sym )
      [ numnew, dummy ] = size ( x );
      [ x, sortvec ] = sortrows ( x );
      w = w ( sortvec );
      x = x((floor(numnew/2)+1):numnew,:);
      w = w((floor(numnew/2)+1):numnew,:);
    end

    n1d(level) = length ( w );
    x1d{level} = x;
    w1d{level} = w;

  end
%
%  Initialization.
%
  minq = max ( 0, k - dim );
  maxq = k - 1;
  nodes = [];
  weights = [];
%
%  Loop for max ( 0, K - DIM ) <= Q <= K - 1.
%
  for q = minq : maxq

    r = length ( weights );
%
%  BQ is the combinatorial coefficient applied to the component
%  product rules which have level Q.
%
    bq = ( -1 )^( maxq - q ) * nchoosek ( dim - 1, dim + q - k );
%
%  Compute the D-dimensional row vectors that sum to DIM+Q.
%
    is = get_seq ( dim, dim + q );
%
%  Preallocate new rows for nodes and weights.
%
    Rq = prod ( n1d(is), 2 );
    sRq = sum ( Rq );
    nodes   = [ nodes;   zeros(sRq,dim) ];
    weights = [ weights; zeros(sRq,1) ];
%
%  Generate each of the product rules indicated by IS, and
%  insert them into NODES and WEIGHTS.
%
    for j = 1 : size(is,1)
      midx = is(j,:);
      [ newn, neww ] = tensor_product ( x1d(midx), w1d(midx) );
      nodes((r+1):(r+Rq(j)),:) = newn;
      weights((r+1):(r+Rq(j))) = bq .* neww;
      r = r + Rq(j);
    end
%
%  Sort the nodes and merge repeated values.
%
    [ nodes, sortvec ] = sortrows ( nodes );
    weights = weights(sortvec);
    keep = 1;
    lastkeep = 1;

    for j = 2 : size(nodes,1)
      if ( compress )

        if ( nodes(j,:) == nodes(j-1,:) )
          weights(lastkeep) = weights(lastkeep) + weights(j);
        else
          lastkeep = j;
          keep = [ keep ; j ];
        end
      else
        lastkeep = j;
        keep = [ keep ; j ];
      end
    end

    nodes = nodes(keep,:);
    weights = weights(keep);

  end
%
%  The rule has been computed.
%  If we used symmetry, we now have to extend the rule.
%
  if ( sym )

    nr = length ( weights );
    m = x1d{1};

    for j = 1 : dim

      keep = zeros(nr,1);
      numnew = 0;

      for r = 1 : nr
        if ( nodes(r,j) ~= m )
          numnew = numnew + 1;
          keep(numnew) = r;
        end
      end

      if ( 0 < numnew )
        nodes = [nodes ; nodes(keep(1:numnew),:)];
        nodes(nr+1:nr+numnew,j) = 2*m - nodes(nr+1:nr+numnew,j);
        weights = [weights ; weights(keep(1:numnew))];
        nr = nr + numnew;
      end

    end

    [ nodes, sortvec ] = sortrows ( nodes );
    weights = weights(sortvec);

  end

  r_size = length ( weights );

  return
end


function [ n_data, x, fx ] = tran09_values ( n_data )

%*****************************************************************************80
%
%% TRAN09_VALUES returns some values of the order 9 transportation function.
%
%  Discussion:
%
%    The function is defined by:
%
%      TRAN09(x) = Integral ( 0 <= t <= x ) t^9 * exp(t) / ( exp(t) - 1 )^2 dt
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Allan McLeod,
%    Algorithm 757, MISCFUN: A software package to compute uncommon
%    special functions,
%    ACM Transactions on Mathematical Software,
%    Volume 22, Number 3, September 1996, pages 288-301.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 20;

  fx_vec = [ ...
     0.26469772870084897671E-22, ...
     0.11367943653594246210E-12, ...
     0.74428246255329800255E-08, ...
     0.48022728485415366194E-03, ...
     0.11700243014358676725E+00, ...
     0.27648973910899914391E+01, ...
     0.24716631405829192997E+02, ...
     0.12827119828849828583E+03, ...
     0.46842894800662208986E+03, ...
     0.31673967371627895718E+04, ...
     0.46140886546630195390E+04, ...
     0.11952718545392302185E+05, ...
     0.20001612666477027728E+05, ...
     0.31011073271851366554E+05, ...
     0.10352949905541130133E+06, ...
     0.19743173017140591390E+06, ...
     0.33826030414658460679E+06, ...
     0.36179607036750755227E+06, ...
     0.36360622124777561525E+06, ...
     0.36360880558827162725E+06 ];

  x_vec = [ ...
       0.0019531250E+00, ...
       0.0312500000E+00, ...
       0.1250000000E+00, ...
       0.5000000000E+00, ...
       1.0000000000E+00, ...
       1.5000000000E+00, ...
       2.0000000000E+00, ...
       2.5000000000E+00, ...
       3.0000000000E+00, ...
       4.0000000000E+00, ...
       4.2500000000E+00, ...
       5.0000000000E+00, ...
       5.5000000000E+00, ...
       6.0000000000E+00, ...
       8.0000000000E+00, ...
      10.0000000000E+00, ...
      15.0000000000E+00, ...
      20.0000000000E+00, ...
      30.0000000000E+00, ...
      50.0000000000E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

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

  return
end


function price = PROJ_Parisian(N, call, down, S_0, W, H, M, r, rnCHF, T, Gamm, resetting, alph)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% About: Pricing Function for Parisian-style barrier options using PROJ method
% Models Supported: Levy Processes, including jump diffusions and Black-Scholes model
% Returns: price of contract
% Author: Justin Lars Kirkby
%
% ----------------------
% Contract/Model Params
% ----------------------
% S_0 = initial stock price (e.g. 100)
% W   = strike  (e.g. 100)
% r   = interest rate (e.g. 0.05)
% T   = time remaining until maturity (in years, e.g. T=1)
% M   = number of subintervals of [0,T] (total of M+1 monitoring points in time grid, including S_0)
% call = 1 for call (else put)
% down = 1 for down and out (otherwise it's up and out)
% H    = barrier
% Gamm = maximum number of discretely monitored excursions into the knockout region allowed (more than this results in knockout)
% resetting = 1 if a reseting type parisian option (otherwise its cumulative, ie never resets)
% rnCHF = risk netural characteristic function (function handle with single argument)
%
% ----------------------
% Numerical (PROJ) Params
% ----------------------
% alph  = grid with is 2*alph
% N     = number of grid/basis points (power of 2, e.g. 2^12), resolution = 2*alph/(N-1)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if ~(down == 1 && call ~= 1) && ~(down ~=1 && call == 1)
    fprintf('Sorry, currently only Up and out calls, and down and out puts have been coded \n')
    return
end

gamm0 = 1; %HARDCODED: this param would allow us to specify an inital consumed budget, in case of in-progress valuation

dt   = T/M;
nrdt = -r*dt;
h = log(H/S_0);
lws   = log(W/S_0);

%%%%%%  Gaussian Quad Constants
q_plus = (1 + sqrt(3/5))/2;  q_minus = (1 - sqrt(3/5))/2;
b3  = sqrt(15); b4 = b3/10;

dx = 2*alph/(N-1);
xmin = -alph/2;

n_h = floor((h-xmin)/dx +1);
xmin = h - (n_h -1)*dx;    %realign so that h is on the grid (this is important for the case where h = 0)

if h~= 0 %Realign so that h and 0 are both members of grid (if possible)
    nnot =  floor(1-xmin/dx);
    if abs(h) > dx  %so that n_h ~= nnot
        dx = (h - 0)/(n_h - nnot);
        xmin = dx*(1-nnot);  %hence nnot should remain on the grid
        %n_h = floor((h-xmin)/dx +1);  %NOT Numerically Stable
        n_h = floor(nnot + h/dx);  %Numerically Stable
    end
else
    nnot = n_h;
end

a    = 1/dx;
a2   = a^2;
zmin = (1 - N/2)*dx;  %Kbar corresponds to zero

%Cons = 24*a2/N;
Cons2 = 24*a2*exp(nrdt)/N;
dw    = 2*pi*a/N;
grand = dw*(1:N-1);
grand = exp(-1i*zmin*grand).*rnCHF(grand).*(sin(grand/(2*a))./grand).^2./(2+cos(grand/a));
beta  = Cons2*real(fft([1/(24*a2) grand]));   %%%%  NOTE: all toep matrices incorporate exp(-r*dt)

interp_Atend = 0;
if 0 < abs(h) && abs(h)<dx
    interp_Atend = 1;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%   DETERMINE COMMON Params
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
K     = N/2;
nbar  = floor(a*(lws - xmin)+1);
rho   = lws - (xmin+(nbar - 1)*dx);
zeta  = a*rho;

toepM = [beta(K:-1:1)'; 0 ; beta(2*K-1:-1:K +1)'];   toepM = fft(toepM);

%%%% PAYOFF CONSTANTS-----------------------------------
varthet_01 = exp(.5*dx)*(5*cosh(b4*dx) - b3*sinh(b4*dx) + 4)/18;
varthet_m10 = exp(-.5*dx)*(5*cosh(b4*dx) + b3*sinh(b4*dx) + 4)/18;
varthet_star = varthet_01 + varthet_m10;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if down == 1 && call ~= 1  %DOP
    %l = log(H/S_0);

    n_l = n_h;

    zeta_plus = zeta*q_plus; zeta_minus = zeta*q_minus;
    rho_plus = rho*q_plus; rho_minus = rho*q_minus;

    ed1 = exp(rho_minus); ed2 = exp(rho/2); ed3 = exp(rho_plus);

    dbar_1 = zeta^2/2;
    dbar_0 = zeta - dbar_1;         %  dbar_1 = zeta + .5*((zeta - 1)^2 - 1);
    d_0    = zeta*(5*( (1-zeta_minus)*ed1 + (1-zeta_plus)*ed3 ) + 4*(2-zeta)*ed2)/18;
    d_1    = zeta*( 5*(zeta_minus*ed1 + zeta_plus*ed3) + 4*zeta*ed2 )/18;

    %%%%Thet(1)        =  W/2 - H*varthet_01;
    Thet = zeros(K,1);
    Thet(1:nbar-1) =  W - exp(xmin +dx*(0:nbar-2))*S_0*varthet_star;
    Thet(nbar)     =  W*(.5 + dbar_0 - exp(-rho)*(varthet_m10 + d_0));
    Thet(nbar + 1) =  W*(dbar_1 - exp(- rho)*d_1);

    %%%%%%% Initialize Val
    Val = zeros(K,Gamm+1);
    p   = ifft(toepM.*fft([Thet;zeros(K,1)]));
    for j=1:Gamm
       Val(:,j) = p(1:K);
    end
    Thet(1:n_l-1) = 0;  %up to the barrier l, the coefficients are zero
    Thet(n_l) = W/2 - H*varthet_01;  %because this is the case where you can knock out at termination
    p   = ifft(toepM.*fft([Thet;zeros(K,1)]));
    Val(:,Gamm+1) = p(1:K);

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%% RESETTING PARISIAN
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    if resetting == 1
        for m=M-2:-1:0
            %These Thet must be kept outside of loop, else they will be inadvertently altered
            Thet(n_l+1:K-1) = (Val(n_l:K-2,1)+10*Val(n_l+1:K-1,1)+Val(n_l+2:K,1))/12;
            Thet(K)         = (13*Val(K,1)+15*Val(K-1,1)-5*Val(K-2,1)+Val(K-3,1))/48;
            ThetPartial     = (13*Val(n_l,1)+15*Val(n_l+1,1)-5*Val(n_l+2,1)+Val(n_l+3,1))/48;
            for j=1:Gamm

                Thet(1)      = (13*Val(1,j+1)+15*Val(2,j+1)-5*Val(3,j+1)+Val(4,j+1))/48;
                Thet(2:n_l-1) = (Val(1:n_l-2,j+1)+10*Val(2:n_l-1,j+1)+Val(3:n_l,j+1))/12;
                Thet(n_l)     = (13*Val(n_l,j+1)+15*Val(n_l-1,j+1)-5*Val(n_l-2,j+1)+Val(n_l-3,j+1))/48 ...
                                 + ThetPartial;

                p    = ifft(toepM.*fft([Thet(1:K);zeros(K,1)]));
                Val(:,j)  = p(1:K);
            end

            %%% Now to Gamm+1
            j = Gamm+1;
            Thet(1:n_l-1)   = 0;
            Thet(n_l)       = ThetPartial;

            p    = ifft(toepM.*fft([Thet; zeros(K,1)]));
            Val(:,j)  = p(1:K);
        end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%% CUMULATIVE PARISIAN
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    else
        for m=M-2:-1:0

            for j=1:Gamm
                Thet(1)      = (13*Val(1,j+1)+15*Val(2,j+1)-5*Val(3,j+1)+Val(4,j+1))/48;
                Thet(2:n_l-1) = (Val(1:n_l-2,j+1)+10*Val(2:n_l-1,j+1)+Val(3:n_l,j+1))/12;

                Thet(n_l)     = (13*Val(n_l,j+1)+15*Val(n_l-1,j+1)-5*Val(n_l-2,j+1)+Val(n_l-3,j+1))/48 ...
                              + (13*Val(n_l,j)+15*Val(n_l+1,j)-5*Val(n_l+2,j)+Val(n_l+3,j))/48;

                Thet(n_l+1:K-1) = (Val(n_l:K-2,j)+10*Val(n_l+1:K-1,j)+Val(n_l+2:K,j))/12;
                Thet(K)        = (13*Val(K,j)+15*Val(K-1,j)-5*Val(K-2,j)+Val(K-3,j))/48;

                p    = ifft(toepM.*fft([Thet(1:K);zeros(K,1)]));
                Val(:,j)  = p(1:K);
            end

            %%% Now to Gamm+1
            j = Gamm+1;
            Thet(1:n_l-1)   = 0;
            Thet(n_l)       = (13*Val(n_l,j)+15*Val(n_l+1,j)-5*Val(n_l+2,j)+Val(n_l+3,j))/48;
            Thet(n_l+1:K-1) = (Val(n_l:K-2,j)+10*Val(n_l+1:K-1,j)+Val(n_l+2:K,j))/12;

            p    = ifft(toepM.*fft([Thet(1:K);zeros(K,1)]));
            Val(:,j)  = p(1:K);
        end
    end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
elseif down ~=1 && call == 1  %UOC
    %u    = log(H/S_0);
    n_u = n_h;

    sigma = 1 - zeta; sigma_plus = (q_plus-.5)*sigma; sigma_minus = (q_minus-.5)*sigma;

    es1 = exp(dx*sigma_plus); es2 = exp(dx*sigma_minus);

    dbar_0 = .5 + zeta*(.5*zeta-1);
    dbar_1 = sigma*(1 - .5*sigma);

    d_0 = exp((rho+dx)*.5)*sigma^2/18*(5*((1-q_minus)*es2 +(1-q_plus)*es1) + 4);
    d_1 = exp((rho-dx)*.5)*sigma/18*(5*( (.5*(zeta+1) +sigma_minus)*es2 + (.5*(zeta+1) +sigma_plus)*es1 ) + 4*(zeta+1) );

    %%%%Thet(1)        =  W/2 - H*varthet_01;
    Thet = zeros(K,1);
    Thet(nbar)     =  W*(exp(-rho)*d_0 - dbar_0);
    Thet(nbar + 1) =  W*(exp(dx-rho)*(varthet_01 +d_1) -(.5 + dbar_1) );
    Thet(nbar + 2:K)  = exp(xmin +dx*(nbar+1:K-1))*S_0*varthet_star - W;

    Val = zeros(K,Gamm+1);
    p   = ifft(toepM.*fft([Thet;zeros(K,1)]));
    for j=1:Gamm
       Val(:,j) = p(1:K);
    end

    Thet(n_u+1:K) = 0;  %after the barrier u, the coefficients are zero
    Thet(n_u) = H*varthet_m10 - .5*W;  %because this is the case where you can knock out at termination
    p   = ifft(toepM.*fft([Thet;zeros(K,1)]));
    Val(:,Gamm+1) = p(1:K);

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%% RESETTING PARISIAN
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%% For the resetting, its more efficient to pull the
    %%% Thet(1:n_u-1) ouside of the loop through j = 1:Gamm
    if resetting == 1
        for m=M-2:-1:0
            %NOTE: these must be defined outside of loop, else we change them and then the next function requires the unchanged value!

            Thet(1)      = (13*Val(1,1)+15*Val(2,1)-5*Val(3,1)+Val(4,1))/48;
            Thet(2:n_u-1) = (Val(1:n_u-2,1)+10*Val(2:n_u-1,1)+Val(3:n_u,1))/12;
            ThetPartial = (13*Val(n_u,1)+15*Val(n_u-1,1)-5*Val(n_u-2,1)+Val(n_u-3,1))/48 ;

            for j=1:Gamm
                Thet(n_u)     = ThetPartial ...
                    + (13*Val(n_u,j+1)+15*Val(n_u+1,j+1)-5*Val(n_u+2,j+1)+Val(n_u+3,j+1))/48;
                Thet(n_u+1:K-1) = (Val(n_u:K-2,j+1)+10*Val(n_u+1:K-1,j+1)+Val(n_u+2:K,j+1))/12;
                Thet(K)        = (13*Val(K,j+1)+15*Val(K-1,j+1)-5*Val(K-2,j+1)+Val(K-3,j+1))/48;

                p    = ifft(toepM.*fft([Thet(1:K);zeros(K,1)]));
                Val(:,j)  = p(1:K);
            end

            %%% Now to Gamm+1
            j = Gamm+1;
            Thet(n_u)     = ThetPartial;
            Thet(n_u+1:K) = 0;

            p    = ifft(toepM.*fft([Thet(1:K);zeros(K,1)]));
            Val(:,j)  = p(1:K);
        end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%% CUMULATIVE PARISIAN
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    else
        for m=M-2:-1:0
            for j=1:Gamm
                Thet(1)      = (13*Val(1,j)+15*Val(2,j)-5*Val(3,j)+Val(4,j))/48;
                Thet(2:n_u-1) = (Val(1:n_u-2,j)+10*Val(2:n_u-1,j)+Val(3:n_u,j))/12;

                Thet(n_u)     = (13*Val(n_u,j)+15*Val(n_u-1,j)-5*Val(n_u-2,j)+Val(n_u-3,j))/48 ...
                              + (13*Val(n_u,j+1)+15*Val(n_u+1,j+1)-5*Val(n_u+2,j+1)+Val(n_u+3,j+1))/48;

                Thet(n_u+1:K-1) = (Val(n_u:K-2,j+1)+10*Val(n_u+1:K-1,j+1)+Val(n_u+2:K,j+1))/12;
                Thet(K)        = (13*Val(K,j+1)+15*Val(K-1,j+1)-5*Val(K-2,j+1)+Val(K-3,j+1))/48;

                p    = ifft(toepM.*fft([Thet(1:K);zeros(K,1)]));
                Val(:,j)  = p(1:K);
            end

            %%% Now to Gamm+1
            j = Gamm+1;
            Thet(1)       = (13*Val(1,j)+15*Val(2,j)-5*Val(3,j)+Val(4,j))/48;
            Thet(2:n_u-1) = (Val(1:n_u-2,j)+10*Val(2:n_u-1,j)+Val(3:n_u,j))/12;
            Thet(n_u)     = (13*Val(n_u,j)+15*Val(n_u-1,j)-5*Val(n_u-2,j)+Val(n_u-3,j))/48;
            Thet(n_u+1:K)  =0;

            p    = ifft(toepM.*fft([Thet(1:K);zeros(K,1)]));
            Val(:,j)  = p(1:K);
        end
    end
end

if interp_Atend ~= 1
    price = Val(nnot,gamm0);

else  %%% INTERPOLATION
%     Use 5 Point Cubic Interpolation
%     xnot = xmin +(nnot-1)*dx;
%     xs = [xnot-2*dx,xnot-dx,xnot,xnot+dx,xnot+2*dx];
%     ys = [Val(nnot-2,1),Val(nnot-1,1),Val(nnot,1),Val(nnot+1,1),Val(nnot+2,1)];
%     price = spline(xs,ys,0);

    dd = 0 - (xmin+ (nnot -1)*dx);
    price = Val(nnot,gamm0) + (Val(nnot+1,gamm0) - Val(nnot,gamm0))*dd/dx;
end

end


%VGG_SINGF_FROM_FF  Linearly combines two 3x3 matrices to a singular one.
%
%   a = vgg_singF_from_FF(F)  computes scalar(s) a such that given two 3x3 matrices F{1} and F{2},
%   it is det( a*F{1} + (1-a)*F{2} ) == 0.

function a = vgg_singF_from_FF(F)

% precompute determinants made from columns of F{1}, F{2}
for i1 = 1:2
  for i2 = 1:2
    for i3 = 1:2
      D(i1,i2,i3) = det([F{i1}(:,1) F{i2}(:,2) F{i3}(:,3)]);
    end
  end
end

% Solve The cubic equation for a
a = roots([-D(2,1,1)+D(1,2,2)+D(1,1,1)+D(2,2,1)+D(2,1,2)-D(1,2,1)-D(1,1,2)-D(2,2,2)
            D(1,1,2)-2*D(1,2,2)-2*D(2,1,2)+D(2,1,1)-2*D(2,2,1)+D(1,2,1)+3*D(2,2,2)
            D(2,2,1)+D(1,2,2)+D(2,1,2)-3*D(2,2,2)
            D(2,2,2)]);
a = a(abs(imag(a))<10*eps);

return


function stat = ft_statfun_bayesfactor(cfg, dat, design)

% FT_STATFUN_BAYESFACTOR computes the Bayes factor for a H0 of the data in two
% conditions having the same mean, versus H1 of the data having different means. This
% function supports both unpaired and paired designs and assumes flat priors.
%
% Lee and Wagenmakers (2013) provide these guidelines for its interpretation
%   IF B10 IS...    THEN YOU HAVE...
%     > 100           Extreme evidence for H1
%     30 – 100        Very strong evidence for H1
%     10 – 30         Strong evidence for H1
%     3 – 10          Moderate evidence for H1
%     1 – 3           Anecdotal evidence for H1
%     1               No evidence
%     1/3 – 1         Anecdotal evidence for H0
%     1/3 – 1/10      Moderate evidence for H0
%     1/10 – 1/30     Strong evidence for H0
%     1/30 – 1/100    Very strong evidence for H0
%     < 1/100         Extreme evidence for H0
%
% Use this function by calling one of the high-level statistics functions as
%   [stat] = ft_timelockstatistics(cfg, timelock1, timelock2, ...)
%   [stat] = ft_freqstatistics(cfg, freq1, freq2, ...)
%   [stat] = ft_sourcestatistics(cfg, source1, source2, ...)
% with the following configuration option:
%   cfg.statistic = 'ft_statfun_bayesfactor'
%
% The experimental design is specified as:
%   cfg.ivar  = independent variable, row number of the design that contains the labels of the conditions to be compared (default=1)
%   cfg.uvar  = optional, row number of design that contains the labels of the units-of-observation, i.e. subjects or trials (default=2)
%
% The labels for the independent variable should be specified as the number 1 and 2.
% The labels for the unit of observation should be integers ranging from 1 to the
% total number of observations (subjects or trials).
%
% The cfg.uvar option is only needed for paired data, you should leave it empty
% for non-paired data.
%
% See https://www.statisticshowto.datasciencecentral.com/bayes-factor-definition/ for some background.
%
% See also FT_TIMELOCKSTATISTICS, FT_FREQSTATISTICS or FT_SOURCESTATISTICS

% Copyright (C) 2020, Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

% ensure that the required toolbox dependency is on the path
ft_hastoolbox('bayesFactor', 1);

% set the defaults
cfg.ivar = ft_getopt(cfg, 'ivar', 1);
cfg.uvar = ft_getopt(cfg, 'uvar', []); % default is empty, which means non-paired

if isempty(cfg.uvar)
  sel1 = find(design(cfg.ivar,:)==1); % select replications that belong to condition 1
  sel2 = find(design(cfg.ivar,:)==2); % select replications that belong to condition 2
  n1 = length(sel1);
  n2 = length(sel2);

  x1 = dat(:,sel1);
  x2 = dat(:,sel2);

  nchan = size(x1, 1);
  bf10  = nan(nchan, 1);
  prob  = nan(nchan, 1);
  ci_lo = nan(nchan, 1);
  ci_hi = nan(nchan, 1);
  tstat = nan(nchan, 1);
  diff  = mean(x1, 2) - mean(x2, 2); % this is the raw effect size

  for i=1:nchan
    [bf10(i), prob(i), CI, stats] = bf.ttest2(x1(i,:), x2(i,:));
    ci_lo(i) = CI(1);
    ci_hi(i) = CI(2);
    tstat(i) = stats.tstat;
  end

else
  subj = unique(design(cfg.uvar,:)); % it can also be paired over trials

  n = length(subj);
  sel1 = nan(size(subj));
  sel2 = nan(size(subj));
  for i=1:n
    sel1(i) = find(design(cfg.uvar,:)==subj(i) & design(cfg.ivar,:)==1);
    sel2(i) = find(design(cfg.uvar,:)==subj(i) & design(cfg.ivar,:)==2);
  end
  x1 = dat(:,sel1);
  x2 = dat(:,sel2);

  nchan = size(x1, 1);
  bf10  = nan(nchan, 1);
  prob  = nan(nchan, 1);
  ci_lo = nan(nchan, 1);
  ci_hi = nan(nchan, 1);
  tstat = nan(nchan, 1);
  diff  = mean(x1 - x2, 2); % this is the raw effect size

  for i=1:nchan
    [bf10(i), prob(i), CI, stats] = bf.ttest(x1(i,:) - x2(i,:));
    ci_lo(i) = CI(1);
    ci_hi(i) = CI(2);
    tstat(i) = stats.tstat;
  end

end % if uvar

% return the results for all channel-time-frequency points, for all voxels, or for all source locations
stat.bf10  = bf10(:);
stat.prob  = prob(:);
stat.diff  = diff(:);
stat.ci_lo = ci_lo(:);
stat.ci_hi = ci_hi(:);
stat.tstat = tstat(:);


function [logp, yhat, res] = tapas_logrt_linear_whatworld(r, infStates, ptrans)
% Calculates the log-probability of log-reaction times y (in units of log-ms) according to the
% linear log-RT model developed with Louise Marshall and Sven Bestmann
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2014 Christoph Mathys, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Transform zetas to their native space
be0  = ptrans(1);
be1  = ptrans(2);
be2  = ptrans(3);
be3  = ptrans(4);
ze   = exp(ptrans(5));

% Initialize returned log-probabilities, predictions,
% and residuals as NaNs so that NaN is returned for all
% irregualar trials
n = size(infStates,1);
logp = NaN(n,1);
yhat = NaN(n,1);
res  = NaN(n,1);

% Weed irregular trials out from responses and inputs
y = r.y(:,1);
y(r.irr) = [];

u = r.u(:,1);
u(r.irr) = [];

% Extract trajectories of interest from infStates
mu1hat = squeeze(infStates(:,1,:,:,1));
mu1    = squeeze(infStates(:,1,:,:,3));
mu2    = squeeze(infStates(:,2,:,:,3));
sa2    = squeeze(infStates(:,2,:,:,4));
mu3    = squeeze(infStates(:,3,1,1,3));

% Surprise
% ~~~~~~~~

% mu1 contains the actually occurring transition -> multiply with
% mu1hat to get probability of that transition (other elements are
% zero)
otp    = mu1.*mu1hat; % observed transition probabilities (3-dim)
otps3  = sum(otp, 3, 'omitnan');      % sum over 3rd dim
otps23 = sum(otps3, 2, 'omitnan');    % sum over 2nd dim

surp = -log(otps23);
surp(r.irr) = [];

% Expected uncertainty
% ~~~~~~~~~~~~~~~~~~~~
euo    = mu1.*sa2;    % expected uncertainty of observed transition (3-dim)
euos3  = sum(euo, 3, 'omitnan');      % sum over 3rd dim
euos23 = sum(euos3, 2, 'omitnan');    % sum over 2nd dim

to     = mu1.*mu2;    % tendency of observed transition (3-dim)
tos3   = sum(to, 3, 'omitnan');       % sum over 3rd dim
tos23  = sum(tos3, 2, 'omitnan');     % sum over 2nd dim

eu = tapas_sgm(tos23,1).*(1-tapas_sgm(tos23,1)).*euos23; % transform down to 1st level
eu(r.irr) = [];

% Unexpected uncertainty
% ~~~~~~~~~~~~~~~~~~~~~~
ueu = tapas_sgm(tos23,1).*(1-tapas_sgm(tos23,1)).*exp(mu3); % transform down to 1st level
ueu(r.irr) = [];

% Calculate predicted log-reaction time
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
logrt = be0 +be1.*surp +be2.*eu +be3.*ueu;

% Calculate log-probabilities for non-irregular trials
% Note: 8*atan(1) == 2*pi (this is used to guard against
% errors resulting from having used pi as a variable).
reg = ~ismember(1:n,r.irr);
logp(reg) = -1/2.*log(8*atan(1).*ze) -(y-logrt).^2./(2.*ze);
yhat(reg) = logrt;
res(reg) = y-logrt;

return;


gammaln(0.1) - 2.2527126517342059598697
gammaln(0.6) - .39823385806923489961685
gammaln(0.7) - .26086724653166651438573
gammaln(1.0)
gammaln(2.0)
gammaln(3.4) - 1.0923280598027415674947
gammaln(4.0) - 1.791759469228055000812477
gammaln(8.0) - 8.525161361065414300165531
gammaln(64.0) - 201.00931639928152667928
gammaln(256.0) - 1161.71210111840065079
if gammaln(0) ~= Inf
  error('gammaln(0) should be Inf');
end
if ~isnan(gammaln(-1))
  error('gammaln(-1) should be NaN');
end
if gammaln(Inf) ~= Inf
  error('gammaln(Inf) should be Inf');
end
% should be NaN?
gammaln(-Inf)
if ~isnan(gammaln(NaN))
  error('gammaln(NaN) should be NaN');
end


function phi = basis_brick27 ( n, p )

%*****************************************************************************80
%
%% BASIS_BRICK20: BRICK20 basis functions at natural coordinates.
%
%  Discussion:
%
%        8----19---7
%       /|         /
%     20 |   26   /|
%     /          / |
%    5----17----6  |
%    |   |      |  |
%    |  16---24-|-15
%    |  /|      | /|
%    |25 |  27  |23|        t
%    |/         |/ |        |   s
%   13----22---14  |        |  /
%    |   |      |  |        | /
%    |   |      |  |        |/
%    |   4--11--|--3        0---------r
%    |  /       | /
%    | 12   21  |10
%    |/         |/
%    1----9-----2
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 March 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real P(3,N), natural coordinates of evaluation points.
%
%    Output, real PHI(27,N), the basis function values.
%
  rm(1:n) = p(1,1:n) + 1.0;
  rz(1:n) = p(1,1:n);
  rp(1:n) = p(1,1:n) - 1.0;

  sm(1:n) = p(2,1:n) + 1.0;
  sz(1:n) = p(2,1:n);
  sp(1:n) = p(2,1:n) - 1.0;

  tm(1:n) = p(3,1:n) + 1.0;
  tz(1:n) = p(3,1:n);
  tp(1:n) = p(3,1:n) - 1.0;

  phi(1,1:n)  =         rz .* rp       .* sz .* sp       .* tz .* tp / 8.0;
  phi(2,1:n)  =   rm .* rz             .* sz .* sp       .* tz .* tp / 8.0;
  phi(3,1:n)  =   rm .* rz       .* sm .* sz             .* tz .* tp / 8.0;
  phi(4,1:n)  =         rz .* rp .* sm .* sz             .* tz .* tp / 8.0;
  phi(5,1:n)  =         rz .* rp       .* sz .* sp .* tm .* tz       / 8.0;
  phi(6,1:n)  =   rm .* rz             .* sz .* sp .* tm .* tz       / 8.0;
  phi(7,1:n)  =   rm .* rz       .* sm .* sz       .* tm .* tz       / 8.0;
  phi(8,1:n)  =         rz .* rp .* sm .* sz       .* tm .* tz       / 8.0;

  phi(9,1:n)  = - rm       .* rp       .* sz .* sp       .* tz .* tp / 4.0;
  phi(10,1:n) = - rm .* rz       .* sm       .* sp       .* tz .* tp / 4.0;
  phi(11,1:n) = - rm       .* rp .* sm .* sz             .* tz .* tp / 4.0;
  phi(12,1:n) = -       rz .* rp .* sm       .* sp       .* tz .* tp / 4.0;
  phi(13,1:n) = -       rz .* rp       .* sz .* sp .* tm       .* tp / 4.0;
  phi(14,1:n) = - rm .* rz             .* sz .* sp .* tm       .* tp / 4.0;
  phi(15,1:n) = - rm .* rz       .* sm .* sz       .* tm       .* tp / 4.0;
  phi(16,1:n) = -       rz .* rp .* sm .* sz       .* tm       .* tp / 4.0;
  phi(17,1:n) = - rm       .* rp       .* sz .* sp .* tm .* tz       / 4.0;
  phi(18,1:n) = - rm .* rz       .* sm       .* sp .* tm .* tz       / 4.0;
  phi(19,1:n) = - rm       .* rp .* sm .* sz       .* tm .* tz       / 4.0;
  phi(20,1:n) = -       rz .* rp .* sm       .* sp .* tm .* tz       / 4.0;

  phi(21,1:n) =   rm       .* rp .* sm       .* sp       .* tz .* tp / 2.0;
  phi(22,1:n) =   rm       .* rp       .* sz .* sp .* tm       .* tp / 2.0;
  phi(23,1:n) =   rm .* rz       .* sm       .* sp .* tm       .* tp / 2.0;
  phi(24,1:n) =   rm       .* rp .* sm .* sz       .* tm       .* tp / 2.0;
  phi(25,1:n) =         rz .* rp .* sm       .* sp .* tm       .* tp / 2.0;
  phi(26,1:n) =   rm       .* rp .* sm       .* sp .* tm .* tz       / 2.0;

  phi(27,1:n) = - rm       .* rp .* sm       .* sp .* tm       .* tp;

  return
end


function alpha = backtracking_line_search_prox(problem, w, alpha, rho)
% This is a proximal backtracking algorithm.
%
% Inputs:
%       problem     function (cost/grad/hess)
%       w      current point
%       alpha       current stepsize
%       rho         shrink constant (<1)
% Output:
%       alpha       new stepsize
%
% References:
%
%
% This file is part of SGDLibrary.
%
% Created by H.Kasai on Nov. 19, 2018.
%
%   F(x) := f(x) + g(x);

    w0 = w;

    %% f0
    % calculate f0
%     F0 = problem.calculate_cost(w0);
%     reg0 = problem.calculate_reg(w0);
%     g0 = problem.lambda * reg0;
%     f0 = F0 - g0;
    f0 = problem.differentiable_cost(w0);

    % calculate g0
    grad0 = problem.full_grad(w0);

    % w = w - alpha * grads0
    w_out = w0 - alpha * grad0;

    % prox
    w_out = problem.prox(w_out, alpha);

    %% f1
    fk = problem.differentiable_cost(w_out);

    diff = w_out - w0;

    while fk > f0 + grad0'*diff + 1/(2*alpha) * (diff'*diff)
        alpha = rho * alpha;

        % w = w - alpha * grads0
        w_out = w0 - alpha * grad0;

        % prox
        w_out = problem.prox(w_out, alpha);

        %% fk
        fk = problem.differentiable_cost(w_out);
        diff = w_out - w0;

    end

%     while fk >= f0 + grad_f(x)'*z + (0.5/step)*norm(z,2)^2
%         lambda = rho * step;
%         z = prox(x - lambda*grad_f(x),lambda*g);
%     end

end


function [a,ass] = bipartiteMatchingIntProg(dst, nmatches)
% BIPARTITEMATCHINGINTPROG Use binary integer programming (linear objective) to solve for optimal linear assignment
% function a = bipartiteMatchingIntProg(dst)
% a(i) = best matching column for row i
%
% This gives the same result as bipartiteMatchingHungarian.
%
% function a = bibpartiteMatchingIntProg(dst, nmatches)
% only matches the specified number (must be <= min(size(dst))).
% This can be used to allow outliers in both source and target.
%
% For details, see Marciel & Costeira, "A global solution to sparse correspondence
% problems", PAMI 25(2), 2003

if nargin < 2, nmatches = []; end

[p1 p2] = size(dst);
p1orig = p1; p2orig = p2;
dstorig = dst;

if isempty(nmatches) % no outliers allowed  (modulo size difference)
  % ensure matrix is square
  m = max(dst(:));
  if p1<p2
    dst = [dst; m*ones(p2-p1, p2)];
  elseif p1>p2
    dst = [dst  m*ones(p1, p1-p2)];
  end
end
[p1 p2] = size(dst);

c = dst(:); % vectorize cost matrix

% row-sum: ensure each column sums to 1
A2 = kron(eye(p2), ones(1,p1));
b2 = ones(p2,1);

% col-sum: ensure each row sums to 1
A3 = kron(ones(1,p2), eye(p1));
b3 = ones(p1,1);

if isempty(nmatches)
  % enforce doubly  stochastic
  A = [A2; A3];
  b = [b2; b3];
  Aineq = zeros(1, p1*p2);
  bineq = 0;
else
  nmatches = min([nmatches, p1, p2]);
  Aineq = [A2; A3];
  bineq = [b2; b3]; % row and col sums <= 1
  A = ones(1,p1*p2);
  b = nmatches; % total num matches = b (otherwise get degenerate soln)
end

ass = bintprog(c, Aineq, bineq, A, b);
ass = reshape(ass, p1, p2);

a = zeros(1, p1orig);
for i=1:p1orig
  ndx = find(ass(i,:)==1);
  if ~isempty(ndx) & (ndx <= p2orig)
    a(i) = ndx;
  end
end