function pyramid_exactness ( quad_filename, degree_max )

%*****************************************************************************80
%
%% MAIN is the main program for PYRAMID_EXACTNESS.
%
%  Discussion:
%
%    This program investigates the polynomial exactness of a quadrature
%    rule for the pyramid.
%
%    The integration region is:
%
%      - ( 1 - Z ) <= X <= 1 - Z
%      - ( 1 - Z ) <= Y <= 1 - Z
%                0 <= Z <= 1.
%
%    When Z is zero, the integration region is a square lying in the (X,Y)
%    plane, centered at (0,0,0) with "radius" 1.  As Z increases to 1, the
%    radius of the square diminishes, and when Z reaches 1, the square has
%    contracted to the single point (0,0,1).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 July 2009
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PYRAMID_EXACTNESS\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Investigate the polynomial exactness of\n' );
  fprintf ( 1, '  a quadrature rule for the pyramid.\n' );
%
%  Get the quadrature file root name:
%
  if ( 1 <= nargin )

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, 'PYRAMID_EXACTNESS:\n' );

    quad_filename = input ( '  Enter the "root" name of the quadrature files.' );

  end
%
%  Create the names of:
%    the quadrature X file;
%    the quadrature W file;
%
  quad_x_filename = strcat ( quad_filename, '_x.txt' );
  quad_w_filename = strcat ( quad_filename, '_w.txt' );
%
%  The second command line argument is the maximum degree.
%
  if ( 2 <= nargin )

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, 'PYRAMID_EXACTNESS:\n' );

    degree_max = input ( '  Please enter the maximum total degree to check.' );

  end
%
%  Summarize the input.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PYRAMID_EXACTNESS: User input:\n' );
  fprintf ( 1, '  Quadrature rule X file = "%s".\n', quad_x_filename );
  fprintf ( 1, '  Quadrature rule W file = "%s".\n', quad_w_filename );
  fprintf ( 1, '  Maximum total degree to check = %d', degree_max );
%
%  Read the X file.
%
  [ dim_num, order ] = r8mat_header_read ( quad_x_filename );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Spatial dimension = %d\n', dim_num );
  fprintf ( 1, '  Number of points  = %d\n', order );

  if ( dim_num ~= 3 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'PYRAMID_EXACTNESS - Fatal error!\n' );
    fprintf ( 1, '  The quadrature abscissas must be 3 dimensional.\n' );
    error ( 'PYRAMID_EXACTNESS - Fatal error!' );
  end

  x = r8mat_data_read ( quad_x_filename, dim_num, order );
%
%  Read the W file.
%
  [ dim_num2, order2 ] = r8mat_header_read ( quad_w_filename );

  if ( dim_num2 ~= 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'PYRAMID_EXACTNESS - Fatal error!\n' );
    fprintf ( 1, '  The quadrature weight file should have exactly\n');
    fprintf ( 1, '  one value on each line.\n' );
    error ( 'PYRAMID_EXACTNESS - Fatal error!' );
  end

  if ( order2 ~= order )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'PYRAMID_EXACTNESS - Fatal error!\n' );
    fprintf ( 1, '  The quadrature weight file should have exactly\n' );
    fprintf ( 1, '  the same number of lines as the abscissa file.\n' );
    error ( 'PYRAMID_EXACTNESS - Fatal error!' );
  end

  w = r8mat_data_read ( quad_w_filename, 1, order );
%
%  Explore the monomials.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '      Error    Degree  Exponents\n' );
  fprintf ( 1, '\n' );

  for degree = 0 : degree_max

    expon = [];
    more = 0;
    h = 0;
    t = 0;

    while ( 1 )

      [ expon, more, h, t ] = comp_next ( degree, dim_num, expon, more, h, t );

      v = monomial_value ( dim_num, order, expon, x );

      quad = pyra_unit_volume ( ) * w(1:order) * v(1:order)';

      exact = pyra_unit_monomial ( expon );

      quad_error = abs ( quad - exact );

      fprintf ( 1, '  %24.16f   %2d  ', quad_error, degree );
      for dim = 1 : dim_num
        fprintf ( 1, '%3d', expon(dim) );
      end
      fprintf ( 1, '\n' );

      if ( ~more )
        break
      end

    end

    fprintf ( 1, '\n' );

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

  return
end
function [ a, more, h, t ] = comp_next ( n, k, a, more, h, t )

%*****************************************************************************80
%
%% COMP_NEXT computes the compositions of the integer N into K parts.
%
%  Discussion:
%
%    A composition of the integer N into K parts is an ordered sequence
%    of K nonnegative integers which sum to N.  The compositions (1,2,1)
%    and (1,1,2) are considered to be distinct.
%
%    The routine computes one composition on each call until there are no more.
%    For instance, one composition of 6 into 3 parts is
%    3+2+1, another would be 6+0+0.
%
%    On the first call to this routine, set MORE = FALSE.  The routine
%    will compute the first element in the sequence of compositions, and
%    return it, as well as setting MORE = TRUE.  If more compositions
%    are desired, call again, and again.  Each time, the routine will
%    return with a new composition.
%
%    However, when the LAST composition in the sequence is computed
%    and returned, the routine will reset MORE to FALSE, signaling that
%    the end of the sequence has been reached.
%
%    This routine originally used a SAVE statement to maintain the
%    variables H and T.  I have decided that it is safer
%    to pass these variables as arguments, even though the user should
%    never alter them.  This allows this routine to safely shuffle
%    between several ongoing calculations.
%
%    There are 28 compositions of 6 into three parts.  This routine will
%    produce those compositions in the following order:
%
%     I         A
%     -     ---------
%     1     6   0   0
%     2     5   1   0
%     3     4   2   0
%     4     3   3   0
%     5     2   4   0
%     6     1   5   0
%     7     0   6   0
%     8     5   0   1
%     9     4   1   1
%    10     3   2   1
%    11     2   3   1
%    12     1   4   1
%    13     0   5   1
%    14     4   0   2
%    15     3   1   2
%    16     2   2   2
%    17     1   3   2
%    18     0   4   2
%    19     3   0   3
%    20     2   1   3
%    21     1   2   3
%    22     0   3   3
%    23     2   0   4
%    24     1   1   4
%    25     0   2   4
%    26     1   0   5
%    27     0   1   5
%    28     0   0   6
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 July 2008
%
%  Author:
%
%    FORTRAN77 original version by Albert Nijenhuis and Herbert Wilf
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Albert Nijenhuis, Herbert Wilf,
%    Combinatorial Algorithms for Computers and Calculators,
%    Second Edition,
%    Academic Press, 1978,
%    ISBN: 0-12-519260-6,
%    LC: QA164.N54.
%
%  Parameters:
%
%    Input, integer N, the integer whose compositions are desired.
%
%    Input, integer K, the number of parts in the composition.
%
%    Input, integer A(K), the previous composition.  On the first call,
%    with MORE = FALSE, set A = [].  Thereafter, A should be the
%    value of A output from the previous call.
%
%    Input, logical MORE.  The input value of MORE on the first
%    call should be FALSE, which tells the program to initialize.
%    On subsequent calls, MORE should be TRUE, or simply the
%    output value of MORE from the previous call.
%
%    Input, integer H, T, two internal parameters needed for the
%    computation.  The user may need to initialize these before the
%    very first call, but these initial values are not important.
%    The user should not alter these parameters once the computation
%    begins.
%
%    Output, integer A(K), the next composition.
%
%    Output, logical MORE, will be TRUE unless the composition
%    that is being returned is the final one in the sequence.
%
%    Output, integer H, T, the updated values of the two internal
%    parameters.
%
  if ( ~more )

    t = n;
    h = 0;
    a(1) = n;
    a(2:k) = 0;

  else

    if ( 1 < t )
      h = 0;
    end

    h = h + 1;
    t = a(h);
    a(h) = 0;
    a(1) = t - 1;
    a(h+1) = a(h+1) + 1;

  end

  more = ( a(k) ~= n );

  return
end
function column_num = file_column_count ( input_file_name )

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

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

  while ( 1 )

    line = fgetl ( input_unit );

    if ( line == -1 )
      break;
    end

    if ( s_len_trim ( line ) == 0 )

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

    else
      got_one = TRUE;
      break;
    end

  end

  fclose ( input_unit );

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

  column_num = s_word_count ( line );

  return
end
function row_num = file_row_count ( input_file_name )

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

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

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

  record_num = 0;

  while ( 1 )

    line = fgetl ( input_unit );

    if ( line == -1 )
      break;
    end

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

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

  end

  fclose ( input_unit );

  return
end
function value = monomial_value ( dim_num, point_num, expon, x )

%*****************************************************************************80
%
%% MONOMIAL_VALUE evaluates a monomial.
%
%  Discussion:
%
%    This routine evaluates a monomial of the form
%
%      product ( 1 <= dim <= dim_num ) x(dim)^expon(dim)
%
%    where the exponents are nonnegative integers.  Note that
%    if the combination 0^0 is encountered, it should be treated
%    as 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 May 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer POINT_NUM, the number of points at which the
%    monomial is to be evaluated.
%
%    Input, integer EXPON(DIM_NUM), the exponents.
%
%    Input, real X(DIM_NUM,POINT_NUM), the point coordinates.
%
%    Output, real VALUE(POINT_NUM), the value of the monomial.
%
  value(1:point_num) = 1.0;

  for dim = 1 : dim_num
    if ( 0 ~= expon(dim) )
      value(1:point_num) = value(1:point_num) .* x(dim,1:point_num).^expon(dim);
    end
  end

  return
end
function value = pyra_unit_monomial ( expon )

%*****************************************************************************80
%
%% PYRA_UNIT_MONOMIAL: monomial integral in a unit pyramid.
%
%  Discussion:
%
%    This routine returns the integral of
%
%      product ( 1 <= I <= 3 ) X(I)^EXPON(I)
%
%    over the unit pyramid.
%
%    The unit pyramid is defined as:
%
%    - ( 1 - Z ) <= X <= 1 - Z
%    - ( 1 - Z ) <= Y <= 1 - Z
%              0 <= Z <= 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 April 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Arthur Stroud,
%    Approximate Calculation of Multiple Integrals,
%    Prentice Hall, 1971,
%    ISBN: 0130438936,
%    LC: QA311.S85.
%
%  Parameters:
%
%    Input, integer EXPON(3), the exponents.
%
%    Output, real VALUE, the integral of the monomial.
%
  value = 0.0;

  if ( mod ( expon(1), 2 ) == 0 && mod ( expon(2), 2 ) == 0 )

    i_hi = 2 + expon(1) + expon(2);

    for i = 0 : i_hi
      value = value + r8_mop ( i ) * r8_choose ( i_hi, i ) / ( i + expon(3) + 1 );
    end

    value = value * 2.0 / ( expon(1) + 1 ) * 2.0 / ( expon(2) + 1 );

  end

  return
end
function value = pyra_unit_volume ( )

%*****************************************************************************80
%
%% PYRA_UNIT_VOLUME_3D returns the volume of a unit pyramid.
%
%  Discussion:
%
%    A pyramid with square base can be regarded as the upper half of a
%    3D octahedron.
%
%    The integration region:
%
%      - ( 1 - Z ) <= X <= 1 - Z
%      - ( 1 - Z ) <= Y <= 1 - Z
%                0 <= Z <= 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 March 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real VALUE, the volume of the pyramid.
%
  value = 4.0 / 3.0;

  return
end
function value = r8_choose ( n, k )

%*****************************************************************************80
%
%% R8_CHOOSE computes the binomial coefficient C(N,K).
%
%  Discussion:
%
%    The value is calculated in such a way as to avoid overflow and
%    roundoff.  The calculation is done in integer arithmetic.
%
%    The formula used is:
%
%      C(N,K) = N! / ( K! * (N-K)! )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 March
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    ML Wolfson, HV Wright,
%    Algorithm 160:
%    Combinatorial of M Things Taken N at a Time,
%    Communications of the ACM,
%    Volume 6, Number 4, April 1963, page 161.
%
%  Parameters:
%
%    Input, integer N, K, are the values of N and K.
%
%    Output, real VALUE, the number of combinations of N
%    things taken K at a time.
%
  mn = min ( k, n - k );

  if ( mn < 0 )
    value = 0;
  elseif ( mn == 0 )
    value = 1;
  else
    mx = max ( k, n - k );
    value = mx + 1;
    for i = 2 : mn
      value = ( value * ( mx + i ) ) / i;
    end
  end

  return
end
function value = r8_mop ( i )

%*****************************************************************************80
%
%% R8_MOP returns the I-th power of -1 as an R8 value.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 November 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer I, the power of -1.
%
%    Output, real R8_MOP, the I-th power of -1.
%
  if ( mod ( i, 2 ) == 0 )
    value = + 1.0;
  else
    value = - 1.0;
  end

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

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

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

  input_unit = fopen ( input_filename );

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

  i = 0;

  while ( i < n )

    line = fgets ( input_unit );

    if ( line == -1 )
      break;
    end

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

    elseif ( s_len_trim ( line ) == 0 )

    else

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

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

    end

  end

  fclose ( input_unit );

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

%*****************************************************************************80
%
%% R8MAT_HEADER_READ reads the header from an R8MAT file.
%
%  Discussion:
%
%    An R8MAT is an array of R8's.
%
%  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 LEN, the length of the string up to the last nonblank.
%
  len = length ( s );

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

  return
end
function word_num = s_word_count ( s )

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

  word_num = 0;
  s_length = length ( s );

  if ( s_length <= 0 )
    return;
  end

  blank = TRUE;

  for i = 1 : s_length

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

  end

  return
end
function timestamp ( )

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

  return
end


function y = gaussian_response(rect_size, sigma)
%GAUSSIAN_RESPONSE create the (fixed) target response of the correlation filter response
    [i1, i2] = circ_grid(rect_size(1), rect_size(2));
    y = exp(-(i1.^2 + i2.^2) / (2 * sigma^2));
end


function loglik = RicianLogLik(meas, signals, sig)
% Computes the log likelihood of the measurements given the model signals
% for the Rician noise model.
%
% loglik = RicianLogLik(meas, signals, sig) returns the likelihood of
% measuring meas given the signals and the noise standard deviation sig.
%
% meas are the measurements
%
% signals are computed from a model
%
% sig is the standard deviation of the Gaussian distributions underlying
% the Rician distribution.
%
% author: Daniel C Alexander (d.alexander@ucl.ac.uk)
%

sumsqsc = (signals.^2 + meas.^2)./(2*sig.^2);
scp = meas.*signals./(sig.^2);
lb0 = logbesseli0(scp);
logliks = - 2*log(sig) - sumsqsc + log(signals) + lb0;
loglik = sum(logliks);


function N = nees(x,m,P)

% NEES  Normalized Estimation Error Squared.
%   N = NEES(X,M,P) computes the Normalized Estimation Error Squared given
%   true state X and a Gaussian estimate of mean M and covariance P:
%
%       NEES = (X-M)'*inv(P)*(X-M);
%
%   See also MAHALANOBIS.

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

N = (x-m)'/P*(x-m);

% ========== 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


%  Figure 10.11      Feedback Control of Dynamic Systems, 6e
%                        Franklin, Powell, Emami
%
%  fig10_11.m is a script to generate Fig. 10.11
%  the frequency response of the notch network
clf;
nnotch=[1/.81 0  1] ;
dnotch=[1/625 2/25  1];
% define frequency range
w=logspace(-1,1);
w(36)=1;
subplot(211)
w=logspace(-1,1);
w(24)=0.89;
% compute Bode
[magn phn]=bode(nnotch,dnotch,w);
% phn=php-360*ones(phn);
magn1=[magn, ones(size(magn))];
subplot(211); loglog(w,magn1); grid;
xlabel('\omega (rad/sec)');
ylabel('Magnitude');
title('Fig. 10.11 Bode plot of a notch filter')
subplot(212); semilogx(w,phn); grid;
xlabel('\omega (rad/sec)');
ylabel('Phase (deg)');
%Bode grid
bodegrid


function fd1d_heat_explicit_test03 ( )

%*****************************************************************************80
%
%% FD1D_HEAT_EXPLICIT_TEST03 does a simple test problem.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FD1D_HEAT_EXPLICIT_TEST03:\n' );
  fprintf ( 1, '  Compute an approximate solution to the time-dependent\n' );
  fprintf ( 1, '  one dimensional heat equation:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    dH/dt - K * d2H/dx2 = f(x,t)\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Run a simple test case.\n' );
%
%  Heat coefficient.
%
  k = k_test03 ( );
%
%  X_NUM is the number of equally spaced nodes to use between 0 and 1.
%
  x_num = 21;
  x_min = -5.0;
  x_max = +5.0;
  dx = ( x_max - x_min ) / ( x_num - 1 );
  x = linspace ( x_min, x_max, x_num );
%
%  T_NUM is the number of equally spaced time points between 0 and 10.0.
%
  t_num = 81;
  t_min = 0.0;
  t_max = 4.0;
  dt = ( t_max - t_min ) / ( t_num - 1 );
  t = linspace ( t_min, t_max, t_num );
%
%  Get the CFL coefficient.
%
  cfl = fd1d_heat_explicit_cfl ( k, t_num, t_min, t_max, x_num, x_min, x_max );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of X nodes = %d\n', x_num );
  fprintf ( 1, '  X interval is [%f,%f]\n', x_min, x_max );
  fprintf ( 1, '  X spacing is %f\n', dx );
  fprintf ( 1, '  Number of T values = %d\n', t_num );
  fprintf ( 1, '  T interval is [%f,%f]\n', t_min, t_max );
  fprintf ( 1, '  T spacing is %f\n', dt );
  fprintf ( 1, '  Constant K = %g\n', k );
  fprintf ( 1, '  CFL coefficient = %g\n', cfl );
%
%  Running the code produces an array H of temperatures H(t,x),
%  and vectors x and t.
%

  hmat = zeros ( x_num, t_num );

  for j = 1 : t_num
    if ( j == 1 )
      h = ic_test03 ( x_num, x, t(j) );
      h = bc_test03 ( x_num, x, t(j), h );
    else
      h = fd1d_heat_explicit ( x_num, x, t(j-1), dt, cfl, @rhs_test03, @bc_test03, h );
    end
    hmat(1:x_num,j) = h(1:x_num);
  end
%
%  Plot the data.
%
  figure ( 3 )
  [ tmat, xmat ] = meshgrid ( t, x );
  mesh ( tmat, xmat, hmat );
  title ( 'H(X,T) for TEST03 computed by FD1D\_HEAT\_EXPLICIT' );
  xlabel ( '<-- Time -->' );
  ylabel ( '<-- X -->' );
  zlabel ( '<--  H(X,T) -->' );
%
%  Write the data to files.
%
  r8mat_write ( 'h_test03.txt', x_num, t_num, hmat );
  r8vec_write ( 't_test03.txt', t_num, t );
  r8vec_write ( 'x_test03.txt', x_num, x );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  H(X,T) written to "h_test03.txt"\n' );
  fprintf ( 1, '  T values written to "t_test03.txt"\n' );
  fprintf ( 1, '  X values written to "x_test3.txt"\n' );
  return
end


function krn = spm_smoothkern(fwhm,x,t)
% Generate a Gaussian smoothing kernel
% FORMAT krn = spm_smoothkern(fwhm,x,t)
% fwhm - full width at half maximum
% x    - position
% t    - either 0 (nearest neighbour) or 1 (linear).
%        [Default: 1]
%
% krn  - value of kernel at position x
%__________________________________________________________________________
%
% For smoothing images, one should really convolve a Gaussian with a sinc
% function. For smoothing histograms, the kernel should be a Gaussian
% convolved with the histogram basis function used. This function returns
% a Gaussian convolved with a triangular (1st degree B-spline) basis
% function (by default). A Gaussian convolved with a hat function (0th
% degree B-spline) can also be returned.
%
% Note that the convolution kernel returned by this function differ from
% the ones that other packages currently use for Gaussian smoothing -
% particularly when the FWHM is small compared with the voxel dimensions.
% The fact that SPM does it differently from other software does not mean
% that it is wrong.
%__________________________________________________________________________
% Copyright (C) 2005-2011 Wellcome Trust Centre for Neuroimaging

% John Ashburner
% $Id: spm_smoothkern.m 7460 2018-10-29 15:55:12Z john $

if nargin<3, t = 1; end

% Variance from FWHM
s = (fwhm/sqrt(8*log(2)))^2+eps;

% The simple way to do it. Not good for small FWHM
% krn = (1/sqrt(2*pi*s))*exp(-(x.^2)/(2*s));

if t==0
    % Gaussian convolved with 0th degree B-spline
    % int(exp(-((x+t))^2/(2*s))/sqrt(2*pi*s),t= -0.5..0.5)
    w1  = 1/sqrt(2*s);
    krn = 0.5*(erf(w1*(x+0.5))-erf(w1*(x-0.5)));
    krn(krn<0) = 0;

elseif t==1
    % Gaussian convolved with 1st degree B-spline
    %  int((1-t)*exp(-((x+t))^2/(2*s))/sqrt(2*pi*s),t= 0..1)
    % +int((t+1)*exp(-((x+t))^2/(2*s))/sqrt(2*pi*s),t=-1..0)
    w1  =  0.5*sqrt(2/s);
    w2  = -0.5/s;
    w3  = sqrt(s/2/pi);
    krn = 0.5*(erf(w1*(x+1)).*(x+1) + erf(w1*(x-1)).*(x-1) - 2*erf(w1*x   ).* x)...
          +w3*(exp(w2*(x+1).^2)     + exp(w2*(x-1).^2)     - 2*exp(w2*x.^2));
    krn(krn<0) = 0;

else
    error('Only defined for nearest neighbour and linear interpolation.');
    % This should probably be based on https://arxiv.org/pdf/1608.05854.pdf:
    %     Martin TB, Prunet S, Drissen L. Optimal fitting of Gaussian-apodized
    %     or under-resolved emission lines in Fourier transform spectra
    %     providing new insights on the velocity structure of NGC 6720. Monthly
    %     Notices of the Royal Astronomical Society. 2016 Sep 14;463(4):4223-38.
    % (thanks for the pointer Guillaume).
end


%  rouwenhorst approximates and AR(1) process with a Markov chain
%
%  ::
%
%    [Thetai,y]=rouwenhorst(mu,rho,sigma)
%    [Thetai,y]=rouwenhorst(mu,rho,sigma,N)
%    [Thetai,y]=rouwenhorst(mu,rho,sigma,N,p)
%    [Thetai,y]=rouwenhorst(mu,rho,sigma,N,p,q)
%
%  Args:
%
%     mu (numeric): unconditonal mean of the ar(1) process
%     rho [numeric): autoregressive coefficient
%     sigma (numeric): standard deviation of the shock
%     N (numeric | {2}): number of states of the markov chain
%     p (numeric | {.5*(1+rho)}): first parameter of the procedure
%     q (numeric | {.5*(1+rho)}): second parameter of the procedure
%
%  Returns:
%     :
%
%     - **Thetai** [numeric] : NxN transition matrix
%     - **y** [numeric] : vector of nodes or states
%
%  Note:
%
%     - The process is assumed to be of the form
%       x_t-mu=rho*(x_{t-1}-mu)+sigma*error_term where error_term ~ N(0,1)
%
%  Reference:
%
%     - Karen A. Kopecky, Richard M. H. Suen (2010): "Finite state
%       Markov-chain approximations to highly persistent processes". Review of
%       Economic Dynamics 13, pp 701-714
%
%


% SOSDEMO4 --- Matrix Copositivity
% Section 3.4 of SOSTOOLS User's Manual
%

clear; echo on;
syms x1 x2 x3 x4 x5;
vartable = [x1; x2; x3; x4; x5];

% The matrix under consideration
J = [1 -1  1  1 -1;
    -1  1 -1  1  1;
     1 -1  1 -1  1;
     1  1 -1  1 -1;
    -1  1  1 -1  1];

% =============================================
% First, initialize the sum of squares program

prog = sosprogram(vartable);     % No decision variables.

% =============================================
% Next, define SOSP constraints

% Constraint : r(x)*J(x) - p(x) = 0
J = [x1^2 x2^2 x3^2 x4^2 x5^2]*J*[x1^2; x2^2; x3^2; x4^2; x5^2];
r = x1^2 + x2^2 + x3^2 + x4^2 + x5^2;

prog = sosineq(prog,r*J);

% =============================================
% And call solver
prog = sossolve(prog);

% =============================================
% Program is feasible. The matrix J is copositive.

echo off


function fx = edcheb ( x, coef, npl )

%*****************************************************************************80
%
%% EDCHEB evaluates the derivative of a Chebyshev series at a point.
%
%  Discussion:
%
%    This routine evaluates the derivative of a Chebyshev series
%    at a point in [-1,+1].
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Roger Broucke.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Roger Broucke,
%    Algorithm 446:
%    Ten Subroutines for the Manipulation of Chebyshev Series,
%    Communications of the ACM,
%    October 1973, Volume 16, Number 4, pages 254-256.
%
%  Parameters:
%
%    Input, real ( kind = 8 ) X, the evaluation point.
%    -1 <= X <= +1.
%
%    Input, real ( kind = 8 ) COEF(NPL), the Chebyshev series.
%
%    Input, integer ( kind = 4 ) NPL, the number of terms in the
%    Chebyshev series.
%
  xjp2 = 0.0;
  xjpl = 0.0;
  bjp2 = 0.0;
  bjpl = 0.0;
  n = npl - 1;

  for k = 1 : n
    j = npl - k;
    dj = j;
    xj = 2.0 * coef(j+1) * dj + xjp2;
    bj = 2.0 * x * bjpl - bjp2 + xj;
    bf = bjp2;
    bjp2 = bjpl;
    bjpl = bj;
    xjp2 = xjpl;
    xjpl = xj;
  end

  fx = 0.5 * ( bj - bf );

  return
end


function backtrack_binary_rc_test01 ( )

%*****************************************************************************80
%
%% BACKTRACK_BINARY_RC_TEST01 seeks binary powers that have a given sum.
%
%  Discussion:
%
%    We consider the binary powers 1, 2, 4, ... 2^(n-1).
%
%    We wish to select some of these powers, so that the sum is equal
%    to a given target value.  We are actually simply seeking the binary
%    representation of an integer.
%
%    A partial solution is acceptable if it is less than the target value.
%
%    We list the powers in descending order, so that the bactracking
%    procedure makes the most significant choices first, thus quickly
%    eliminating many unsuitable choices.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 December 2013
%
%  Author:
%
%    John Burkardt
%
  n = 8;
  test_num = 3;
  targets = [ 73, 299, -3 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'BACKTRACK_BINARY_RC_TEST01\n' );
  fprintf ( 1, '  Use BACKBIN_RC to find the binary expansion of\n' );
  fprintf ( 1, '  an integer between 0 and 255.\n' );
  fprintf ( 1, '  The choices are 0/1 for the 8 digits.\n' );

  for test = 1 : test_num

    target = targets(test);
    fprintf ( 1, '\n' );
    fprintf ( 1, '  TARGET = %d\n', target );
    call_num = 0;
    reject = 0;
    n2 = -1;
    choice = [];

    while ( 1 )

      [ n2, choice ] = backbin_rc ( n, reject, n2, choice );
      call_num = call_num + 1;

      if ( n2 == -1 )
        fprintf ( 1, '  Termination without solution.\n' );
        break
      end
%
%  Evaluate the integer determined by the choices.
%
      factor = 1;
      for i = n : -1 : n2 + 1
        factor = factor * 2;
      end

      result = 0;
      for i = 1 : n2
        result = result * 2 + choice(i);
      end

      result = result * factor;
%
%  If the integer is too big, then we reject it, and
%  all the related integers formed by making additional choices.
%
      reject = ( target < result );
%
%  If we hit the target, then in this case, we can exit because
%  the solution is unique.
%
      if ( result == target )
        break
      end

    end

    fprintf ( 1, '  Number of calls = %d\n', call_num );
    fprintf ( 1, '  Binary search space = %d\n', 2 ^ n );
    fprintf ( 1, '  ' );
    for i = 1 : n
      fprintf ( 1, '%2d', choice(i) );
    end
    fprintf ( 1, '\n' );

  end

  return
end


function legendre_polynomial_test04 ( )

%*****************************************************************************80
%
%% LEGENDRE_POLYNOMIAL_TEST04 tests P_QUADRATURE_RULE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 March 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'LEGENDRE_POLYNOMIAL_TEST04:\n' );
  fprintf ( 1, '  P_QUADRATURE_RULE computes the quadrature rule\n' );
  fprintf ( 1, '  associated with P(n,x)\n' );

  n = 5;
  [ x, w ] = p_quadrature_rule ( n );

  r8vec2_print ( n, x, w,  '      X            W' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Use the quadrature rule to estimate:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    Q = Integral ( -1 <= X < +1 ) X^E dx\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   E       Q_Estimate      Q_Exact\n' );
  fprintf ( 1, '\n' );

  for e = 0 : 2 * n - 1
    if ( e == 0 )
      f = ones ( n, 1 );
    else
      f = x .^ e;
    end
    q = w' * f;
    q_exact = p_integral ( e );
    fprintf ( 1, '  %2d  %14g  %14g\n', e, q, q_exact );
  end

  return
end


function test07()
% function test07()
%
% Computes a robust mean of angles
%

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

    % Generate the data
    N = 250;
    theta_true = pi*(2*rand(1)-1);
    thetas = zeros(N,1);
    p_true = .25;
    for i = 1 : N
        if rand(1) < p_true
            thetas(i) = theta_true + .3*randn(1);
        else
            thetas(i) = pi*(2*rand(1)-1);
        end
    end
    X = [cos(thetas') ; sin(thetas)'];

    % Pick the manifold
    problem.M = spherefactory(2);

    % Parameters
    p = p_true;
    kappa = 3;
    c2 = besseli(0, 2*kappa);

    % Define the problem cost function
    problem.cost = @cost;
    function [val store] = cost(x, store)

        if ~isfield(store, 'fi')
            store.fi = (p/c2)*exp(2*kappa*X'*x);
        end
        fi = store.fi;

        val = -sum(log(fi + (1-p)));

    end

    % And its gradient
    problem.grad = @grad;
    function [g store] = grad(x, store)

        if ~isfield(store, 'fi')
            store.fi = (p/c2)*exp(2*kappa*X'*x);
        end
        fi = store.fi;

        g = -X*(2*kappa*(fi./(fi+(1-p))));
        g = g - (x'*g)*x;

    end

    % Check differentials consistency.
    % checkgradient(problem);

    % Solve with trust-regions and FD approximation of the Hessian
    warning('off', 'manopt:getHessian:approx');

    % Test many random initial guess
%     best_cost = inf;
%     best_x = [];
%     for i = 1 : 5
%         [x cst] = trustregions(problem);
%         if cst < best_cost
%             best_cost = cst;
%             best_x = x;
%         end
%     end
%     theta_found = angle(best_x(1)+1i*best_x(2));

    % Do a histogram of the data and select the initial guess as the peak
    [freqs, bins] = hist(thetas, 30);
    [~, ind] = max(freqs);
    x0 = [cos(bins(ind)) ; sin(bins(ind))];
    x = trustregions(problem, x0);
    theta_found = angle(x(1)+1i*x(2));

    fprintf('True theta: %g\nHist theta: %g\nFound theta: %g\n', theta_true, angle(x0(1)+1i*x0(2)), theta_found);

    subplot(2,1,1);
    hist(thetas, 30);
    xlim([-pi pi]);

    th = linspace(-pi, pi);
    fth = zeros(size(th));
    for i = 1 : length(th)
        fth(i) = cost([cos(th(i));sin(th(i))], struct());
    end
    subplot(2,1,2);
    plot(th, -fth);
    xlim([-pi pi]);

%     keyboard;

end


function outputarray = linarray(start,spacing,numpoints)
% outputarray = linarray(start,spacing,numpoints)
%
% The function linarray generates an row vector based on a start value, a
% spacing, and a number of points.  For example: linarray(1,.1,5) returns:
% [1.0000    1.1000    1.2000    1.3000    1.4000]
%
% 3/2/11 (c) James F. Mack

outputarray = [start:spacing:start+(numpoints-1)*spacing];


function [rhsHx, rhsHy, rhsHz, rhsEx, rhsEy, rhsEz] = MaxwellRHS3D(Hx,Hy,Hz,Ex,Ey,Ez)

% function [rhsHx, rhsHy, rhsHz, rhsEx, rhsEy, rhsEz] = MaxwellRHS3D(Hx,Hy,Hz,Ex,Ey,Ez)
% Purpose  : Evaluate RHS flux in 3D Maxwell equations

Globals3D;

% storage for field differences at faces
dHx = zeros(Nfp*Nfaces,K); dHy = dHx; dHz = dHx;
dEx = zeros(Nfp*Nfaces,K); dEy = dEx; dEz = dEx;

% form field differences at faces
dHx(:)  = Hx(vmapP)-Hx(vmapM);  dEx(:)  = Ex(vmapP)-Ex(vmapM);
dHy(:)  = Hy(vmapP)-Hy(vmapM); 	dEy(:)  = Ey(vmapP)-Ey(vmapM);
dHz(:)  = Hz(vmapP)-Hz(vmapM);  dEz(:)  = Ez(vmapP)-Ez(vmapM);

% make boundary conditions all reflective (Ez+ = -Ez-)
dHx(mapB) = 0;  dEx(mapB) = -2*Ex(vmapB);
dHy(mapB) = 0;  dEy(mapB) = -2*Ey(vmapB);
dHz(mapB) = 0;  dEz(mapB) = -2*Ez(vmapB);

alpha=1; % => full upwinding

ndotdH = nx.*dHx + ny.*dHy + nz.*dHz;
ndotdE = nx.*dEx + ny.*dEy + nz.*dEz;

fluxHx = -ny.*dEz + nz.*dEy + alpha*(dHx - ndotdH.*nx);
fluxHy = -nz.*dEx + nx.*dEz + alpha*(dHy - ndotdH.*ny);
fluxHz = -nx.*dEy + ny.*dEx + alpha*(dHz - ndotdH.*nz);

fluxEx =  ny.*dHz - nz.*dHy + alpha*(dEx - ndotdE.*nx);
fluxEy =  nz.*dHx - nx.*dHz + alpha*(dEy - ndotdE.*ny);
fluxEz =  nx.*dHy - ny.*dHx + alpha*(dEz - ndotdE.*nz);

% evaluate local spatial derivatives
[curlHx,curlHy,curlHz] = Curl3D(Hx,Hy,Hz);
[curlEx,curlEy,curlEz] = Curl3D(Ex,Ey,Ez);

% calculate Maxwell's right hand side
rhsHx = -curlEx + LIFT*(Fscale.*fluxHx/2);
rhsHy = -curlEy + LIFT*(Fscale.*fluxHy/2);
rhsHz = -curlEz + LIFT*(Fscale.*fluxHz/2);

rhsEx =  curlHx + LIFT*(Fscale.*fluxEx/2);
rhsEy =  curlHy + LIFT*(Fscale.*fluxEy/2);
rhsEz =  curlHz + LIFT*(Fscale.*fluxEz/2);
return;


function D = EuDist2(fea_a,fea_b,bSqrt)
% Euclidean Distance matrix
%   D = EuDist(fea_a,fea_b)
%   fea_a:    nSample_a * nFeature
%   fea_b:    nSample_b * nFeature
%   D:      nSample_a * nSample_a
%       or  nSample_a * nSample_b

if ~exist('bSqrt','var')
    bSqrt = 1;
end

if (~exist('fea_b','var')) | isempty(fea_b)
    [nSmp, nFea] = size(fea_a);

    aa = sum(fea_a.*fea_a,2);
    ab = fea_a*fea_a';

    aa = full(aa);
    ab = full(ab);

    if bSqrt
        D = sqrt(repmat(aa, 1, nSmp) + repmat(aa', nSmp, 1) - 2*ab);
        D = real(D);
    else
        D = repmat(aa, 1, nSmp) + repmat(aa', nSmp, 1) - 2*ab;
    end

    D = max(D,D');
    D = D - diag(diag(D));
    D = abs(D);
else
    [nSmp_a, nFea] = size(fea_a);
    [nSmp_b, nFea] = size(fea_b);

    aa = sum(fea_a.*fea_a,2);
    bb = sum(fea_b.*fea_b,2);
    ab = fea_a*fea_b';

    aa = full(aa);
    bb = full(bb);
    ab = full(ab);

    if bSqrt
        D = sqrt(repmat(aa, 1, nSmp_b) + repmat(bb', nSmp_a, 1) - 2*ab);
        D = real(D);
    else
        D = repmat(aa, 1, nSmp_b) + repmat(bb', nSmp_a, 1) - 2*ab;
    end

    D = abs(D);
end


function g = p42_g ( n, x )

%*****************************************************************************80
%
%% P42_G evaluates the gradient for problem 42.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 March 2002
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the values of the variables.
%
%    Output, real G(N), the gradient of the objective function.
%
  g = zeros ( n, 1 );

  g(1) = 2.0 * ( x(1) - x(2) ) / ( 1.0 + ( x(1) - x(2) )^2 )^2;

  g(2) = 2.0 * ( x(2) - x(1) ) / ( 1.0 + ( x(1) - x(2) )^2 )^2 ...
    - 0.5 * pi * x(3) * cos ( 0.5 * pi * x(2) * x(3) );

  g(3) = - 0.5 * pi * x(2) * cos ( 0.5 * pi * x(2) * x(3) );

  if ( x(2) ~= 0.0 )

    arg = ( x(1) + 2.0 * x(2) + x(3) ) / x(2);
    term = exp ( - arg^2 );

    g(1) = g(1) + 2.0 * term * ( x(1) + 2.0 * x(2) + x(3) ) / x(2)^2;
    g(2) = g(2) - 2.0 * term * ( x(1) + 2.0 * x(2) + x(3) ) ...
      * ( x(1) + x(3) ) / x(2)^3;
    g(3) = g(3) + 2.0 * term * ( x(1) + 2.0 * x(2) + x(3) ) / x(2)^2;

  end

  return
end


%%  Introduction to *wavelet_layer_2d*
% *wavelet_layer_2d* is computing the wavelet transform coefficient from
% the coefficient of the previous layer. It should be applied iteratively
% to an input signal.
%
%% Usage
% [A, V] = *wavelet_layer_2d*(U, filters, options), documentation is given in
% <matlab:doc('wavelet_layer_2d') wavelet_layer_2d>
%
%% Description
% It is possible to create some wavelet filters with wavelet_factory_2d for
% instance. The filters size have to be adapted to the size of the input
% signal $x$.
clear; close all;

x = mandrill;

% Create $ U[\empty]x $
U{1}.signal{1} = x;
U{1}.meta.j = zeros(0,1);
U{1}.meta.q = zeros(0,1);
U{1}.meta.resolution=0;
filters = morlet_filter_bank_2d(size(x));

% A corresponds to the output scattering coefficients, and V to the wavelet
% coefficients.
[A, V] = wavelet_layer_2d(U{1}, filters);

colormap gray
subplot(121)
imagesc(real(V.signal{1}))
axis off
title('Real part of the first wavelet transform coefficient');
subplot(122)
imagesc(imag(V.signal{1}))
axis off
title('Imaginary part of the first wavelet transform coefficient');

%% Options
% The options are the same as in *wavelet_2d*.


function param = copulaparam(type,tau)
%COPULAPARAM Copula parameter, given Kendall's rank correlation.
%   RHO = COPULAPARAM('Gaussian',TAU) returns the linear correlation
%   parameter RHO corresponding to a Gaussian copula having Kendall's rank
%   correlation TAU.  If TAU is a scalar correlation coefficient, RHO is a
%   scalar correlation coefficient corresponding to a bivariate copula.  If
%   TAU is a P-by-P correlation matrix, RHO is a P-by-P correlation matrix
%   corresponding to a P-variate copula.
%
%   RHO = COPULAPARAM('t',TAU) returns the linear correlation parameter RHO
%   corresponding to a t copula having Kendall's rank correlation TAU.  If
%   TAU is a scalar correlation coefficient, RHO is a scalar correlation
%   coefficient corresponding to a bivariate copula.  If TAU is a P-by-P
%   correlation matrix, RHO is a P-by-P correlation matrix corresponding to
%   a P-variate copula.
%
%   ALPHA = COPULAPARAM(TYPE,TAU) returns the copula parameter ALPHA
%   corresponding to a bivariate Archimedean copula having Kendall's rank
%   correlation TAU.  TYPE is one of 'Clayton', 'Frank', or 'Gumbel'.
%
%   Example:
%      % Determine the linear correlation coefficient corresponding to a
%      % bivariate Gaussian copula having a rank correlation of -0.5
%      tau = -0.5
%      rho = copulaparam('gaussian',tau)
%
%      % Generate dependent beta random values using that copula
%      u = copularnd('gaussian',rho,100)
%      b = betainv(u,2,2)
%
%      % Verify that those pairs have a sample rank correlation approximately
%      % equal to tau
%      tau_sample = kendall(b)

%   Written by Peter Perkins, The MathWorks, Inc.
%   Revision: 1.0  Date: 2003/09/05
%   This function is not supported by The MathWorks, Inc.
%
%   Requires MATLAB R13.

if nargin < 2
    error('Requires two input arguments.');
end

switch lower(type)
case {'gaussian' 't'}
    if ((numel(tau) == 1) && (tau < -1 | 1 < tau)) || ((numel(tau) ~= 1) && ~iscor(tau))
        error('TAU must be a correlation coefficient between -1 and 1, or a positive semidefinite correlation matrix.');
    end
    param = sin(tau.*pi./2);

case {'clayton' 'frank' 'gumbel'}
    if (numel(tau) ~= 1) || (tau < -1 | 1 < tau)
        error('TAU must be a correlation coefficient between -1 and 1.');
    end
    switch lower(type)
    case 'clayton'
        if tau < 0
            error('TAU must be nonnegative for the Clayton copula.');
        end
        param = 2*tau ./ (1-tau);
    case 'frank'
        if tau == 0
            param = 0;
        elseif abs(tau) < 1
            % There's no closed form for alpha in terms of tau, so alpha has to be
            % determined numerically.
            warn = warning('off','MATLAB:fzero:UndeterminedSyntax');
            param = fzero(@frankRootFun,sign(tau),[],tau);
            warning(warn);
        else
            param = sign(tau).*Inf;
        end
    case 'gumbel'
        if tau < 0
            error('TAU must be nonnegative for the Gumbel copula.');
        end
        param = 1 ./ (1-tau);
    end

otherwise
    error('Unrecognized copula type: ''%s''',type);
end

function err = frankRootFun(alpha,targetTau)
if abs(alpha) < realmin
    tau = 0;
else
    tau = 1 + 4 .* (debye1(alpha)-1) ./ alpha;
end
err = tau - targetTau;


function asset_path_test ( )

%*****************************************************************************80
%
%% ASSET_PATH_TEST tests ASSET_PATH.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 February 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'ASSET_PATH_TEST:\n' );
  fprintf ( 1, '  Demonstrate the simulation of an asset price path.\n' );

  s0 = 2.0;
  mu = 0.1;
  sigma = 0.3;
  n = 100;
  t0 = 0.0;
  t1 = 1.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The asset price at time 0,     S0    = %f\n', s0 );
  fprintf ( 1, '  The asset expected growth rate MU    = %f\n', mu );
  fprintf ( 1, '  The asset volatility           SIGMA = %f\n', sigma );
  fprintf ( 1, '  The expiry date                T1    = %f\n', t1 );
  fprintf ( 1, '  The number of time steps       N     = %d\n', n );

  s = asset_path ( s0, mu, sigma, t1, n );
%
%  Plot.
%
  figure ( 1 )
  t = ( linspace ( t0, t1, n + 1 ) )';
  plot ( t, s )
  grid on
  xlabel ( '<-- Time -->' )
  ylabel ( '<-- Value --> ' )
  title ( 'Simulated asset path' )
%
%  Print a little.
%
  r8vec_print_part ( n + 1, s, 10, '  Partial results:' );
%
%  Write to a file.
%
  output_filename = 'asset_path.txt';

  r8vec_write ( output_filename, n + 1, s );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Full results written to "%s".\n', output_filename );

  return
end


function x = r8vec_chebyspace ( n, a, b )

%*****************************************************************************80
%
%% R8VEC_CHEBYSPACE creates a vector of Chebyshev spaced values in [A,B].
%
%  Discussion:
%
%    An R8VEC is a vector of R8's.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 June 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Input, real A, B, the first and last entries.
%
%    Output, real X(N,1), a vector of Chebyshev spaced data.
%
  x = zeros ( n, 1 );

  if ( n == 1 )

    x(1) = ( a + b ) / 2.0;

  else

    for i = 1 : n

      theta = ( n - i ) * pi / ( n - 1 );

      c = cos ( theta );

      if ( mod ( n, 2 ) == 1 )
        if ( 2 * i - 1 == n )
          c = 0.0;
        end
      end

      x(i) = ( ( 1.0 - c ) * a  ...
             + ( 1.0 + c ) * b ) ...
             /   2.0;

    end

  end

  return
end


function [ mO ] = ApplyLocalLinearKernel( mI, filterRadius, regFctr )
% ----------------------------------------------------------------------------------------------- %
%[ mOutputImage ] = ApplyLocalLinearFilter( mInputImage, filterRadius, regFctr )
% Applying Linear Edge Preserving Smoothing Filter based on Local Linear (Affine) model.
% Input:
%   - mI            -   Input Image.
%                       Structure: Image Matrix (Single Channel).
%                       Type: 'Single' / 'Double'.
%                       Range: [0, 1].
%   - filterRadius  -   Filter Radius.
%                       The filter radius.
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range: {1, 2, ...}.
%   - regFctr       -   Regularization Factor.
%                       Regularize the local covariance (Variance).
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range: (0, inf).
% Output:
%   - mO            -   Output Image.
%                       Structure: Image Matrix (Single Channel).
%                       Type: 'Single' / 'Double'.
%                       Range: [0, 1].
% References:
%   1.  "Guided Image Filtering".
% Remarks:
%   1.  This is basically estimating the Linear Function (Affine)
%       parameters for the Local Window. Namely, the ouput is a Linear
%       combination of the input Window and a DC Factor. The final step is
%       aggregation (Uniform) off all estimations of the parameters.
%   2.  Prefixes:
%       -   'v' - Vector.
%       -   'm' - Matrix.
%       -   't' - Tensor (Multi Dimension Matrix)
%       -   's' - Struct.
%       -   'c' - Cell Array.
%   3.  The calculation of the Local Variance might be negative due to
%       numerical diffuculties. If artifacts appear, this might be the
%       casue. Usually using matrices of type 'double' solves it.
%   4.  This implemnetation is `ApplyGuidedFilter` where the Guiding Image
%       is the Input Image.
%   5.  Speed otimizzation can be achived by wiser use of 'mNumEffPixels'.
%       Instead of dividing by it calculate its reciprocal once. Moreover,
%       it can be used only once in the aggregation process.
% TODO:
%   1.  Create Multi Variable Linear Model.
%   2.  Some speed potimization could be made (Taking advantage of 'mNumEffPixels').
% Release Notes:
%   -   1.0.000     05/01/2016  Royi Avital
%       *   First release version
% ----------------------------------------------------------------------------------------------- %

FALSE   = 0;
TRUE    = 1;

OFF = 0;
ON  = 1;

BORDER_TYPE_CONSTANT    = 1;
BORDER_TYPE_CIRCULAR    = 2;
BORDER_TYPE_REPLICATE   = 3;
BORDER_TYPE_SYMMETRIC   = 4;

numRows = size(mI, 1);
numCols = size(mI, 2);

borderType      = BORDER_TYPE_CONSTANT;
borderValue     = 0;
normalizeFlag   = OFF;

mNumEffPixels = ApplyBoxFilter(ones([numRows, numCols]), filterRadius, borderType, borderValue, normalizeFlag);

mLocalMean          = ApplyBoxFilter(mI, filterRadius, borderType, borderValue, normalizeFlag) ./ mNumEffPixels;
mLocalMeanSquare    = ApplyBoxFilter((mI .* mI), filterRadius, borderType, borderValue, normalizeFlag) ./ mNumEffPixels;
mLocalCovariance    = mLocalMeanSquare - (mLocalMean .* mLocalMean);

mO = zeros([numRows, numCols]);

for jj = 1:numCols
    for ii = 1:numRows
        % The coordinate i, j are set.

        sumWeights = 0;

        % Running on the Neighborhood of the pixel
        for ll = -filterRadius:filterRadius
            for kk = -filterRadius:filterRadius

                jRowIdx = ii + kk;
                jColIdx = jj + ll;

                if((jColIdx >= 1) && (jColIdx <= numCols) && (jRowIdx >= 1) && (jRowIdx <= numRows))
                    % Valid Pixel
                    numPixels       = 0;
                    jPixelWeight    = 0;

                    % Running on Wk Window
                    for nn = -filterRadius:filterRadius
                        for mm = -filterRadius:filterRadius

                            % K Index
                            kRowIdx = jRowIdx + mm;
                            kColIdx = jColIdx + nn;

                            if((kColIdx >= 1) && (kColIdx <= numCols) && (kRowIdx >= 1) && (kRowIdx <= numRows) && (abs(kColIdx - jj) <= filterRadius) && (abs(kRowIdx - ii) <= filterRadius))

                                numPixels = numPixels + 1;

                                % Weight
                                jPixelWeight = jPixelWeight + 1 + (((mI(ii, jj) - mLocalMean(kRowIdx, kColIdx)) * (mI(jRowIdx, jColIdx) - mLocalMean(kRowIdx, kColIdx))) / (mLocalCovariance(kRowIdx, kColIdx) + regFctr));
                            end
                        end
                    end

                    % jPixelWeight = jPixelWeight / (numPixels * numPixels);
                    mO(ii, jj) = mO(ii, jj) + (jPixelWeight * mI(jRowIdx, jColIdx));

                    sumWeights = sumWeights + jPixelWeight;

                end
            end
        end

        mO(ii, jj) = mO(ii, jj) / sumWeights;

    end
end

end


function scMove(Pos1,length,LinCol)
%scMove : Move along the transmission line on smith chart to transform impedance
%
%
%  SYNOPSIS:
%     Transform a point to another one as one moves along a lossless transmission line on
%     smith chart.
%
%     See also scDraw, scInv, scConCirc, scMatchCirc
%
%  SYNTAX:
%     scMove(Pos1, length, Direction, LinCol)
%
%  INPUT ARGUMENTS:
%     Pos1      : Starting point coordinates [r x] normalized
%     length    : Length of the transmission line normalized to wavelength
%     LinCol     : Color of the end-ray emanating from origin
%     Direction : Towards(+1) or away from the generator(-1) %%%%TO BE INCORPORATED YET.
%
%  OUTPUT ARGUMENT:
%     none
%
%  EXAMPLE:
%         The Command sequence
%         scDraw;
%         scMove([2 3],.3)
%         will draw a blank smith chart and will plot the point
%         corresponding to impedance (2+j*3)*Z_L and its transformed value
%         after moving 0.3*lambda towards the generator along the
%         transmission line
%
%     Mohammad Ashfaq - (31-05-2000)
%     Mohammad Ashfaq - (13-04-2006) Modified (example included)
%

if nargin == 2
    LinCol = 'm';
end

 r1       = Pos1(1);
 x1       = Pos1(2);
 [u1, v1] = scPOI(r1,x1);

 % MARK STARTING POINT
 scRay(Pos1);

 r        = sqrt(u1^2+v1^2);
 Theta1   = atan2(v1,u1);
 length1  = (1-Theta1/pi)/4;
 length2  = length1 + length;
 Theta2   = (1-4*length2)*pi;
 u2       = r * cos(Theta2);
 v2       = r * sin(Theta2);

 % MARK END POINT
 plot(u2, v2,'r*')

 Theta2d  = Theta2*180/pi;
 if (Theta2d<-180)
    Theta2d  = 360 + Theta2d;
 elseif (Theta2d>180)
    Theta2d  = 360 - Theta2d;
 end
 length2 = length2-floor(length2/.5)*.5;

 plot([0 u2],[0 v2],LinCol);
 plot([1.25*cos(Theta2) u2],[1.25*sin(Theta2) v2],'k');
 string = ['\theta=', num2str(Theta2d,'%3.2f'), '  l/\lambda=', num2str(length2, '%0.3f')];

 if abs(Theta2d)>90
    Theta2d  = Theta2d+180;
    h = text(1.25*cos(Theta2),1.25*sin(Theta2), string);
    set(h,'rotation', Theta2d, 'Fontsize', 8, 'HorizontalAlignment','right');
 else
    h = text(1.25*cos(Theta2),1.25*sin(Theta2), string);
    set(h,'rotation', Theta2d, 'Fontsize', 8);
 end


function C= train_QDA(xTr, yTr, varargin)
%TRAIN_QDA - Quadratic Discriminant Analysis
%
%Synopsis:
%   C = train_QDA(XTR, YTR)
%   C = train_QDA(XTR, YTR, PRIOR)
%
%Arguments:
% XTR: DOUBLE [NxM] - Data matrix, with N feature dimensions, and M
%                     training points/examples.
% YTR: INT [CxM] - Class membership labels of points in X_TR. C by M matrix
%                  of training labels, with C representing the number of
%                  classes and M the number of training examples/points.
%                  Y_TR(i,j)==1 if the point j belongs to class i.
% PRIOR: DOUBLE - (default ones(nClasses, 1)/nClasses): Empirical class
%                 priors
%
%Returns:
%   C: STRUCT - Trained classifier structure, with the hyperplane given by
%               fields C.w, C.b and C.sq
%
%See also:
%   APPLY_QDA

if size(yTr,1)==1,
  nClasses= 2;
  clInd{1}= find(yTr==-1);
  clInd{2}= find(yTr==1);
  N= [length(clInd{1}) length(clInd{2})];
else
  nClasses= size(yTr,1);
  clInd= cell(nClasses,1);
  N= zeros(nClasses, 1);
  for ci= 1:nClasses,
    clInd{ci}= find(yTr(ci,:));
    N(ci)= length(clInd{ci});
  end
end

if nargin==2
  priorP= ones(nClasses,1)/nClasses;
else
  priorP= varargin{1};
end
if isequal(priorP, '*')
  priorP = N/sum(N);
end

d= size(xTr,1);
C.w= zeros(d, nClasses);
C.b= zeros(1, nClasses);
C.sq= zeros(d, d, nClasses);
for ci= 1:nClasses,
  cli= clInd{ci};
  C.w(:,ci)= mean(xTr(:,cli), 2);
  yc= xTr(:,cli) - C.w(:,ci)*ones(1,N(ci));
  Sq= yc*yc';
  C.sq(:,:,ci)= Sq / (N(ci)-1);
end
C.b= zeros(1, nClasses);
for ci= 1:nClasses,
  S= C.sq(:,:,ci);
  S= pinv(S);
  C.sq(:,:,ci)= -0.5*S;
  C.b(ci)=  -0.5*C.w(:,ci)' * S*C.w(:,ci) + ...
            0.5*log(max([det(S),realmin])) + log(priorP(ci));
  C.w(:,ci)= S*C.w(:,ci);
end
C.b=C.b';

if nClasses==2,
  sq(:,:)= C.sq(:,:,2) - C.sq(:,:,1);
  C.sq= sq;
  C.w= C.w(:,2)-C.w(:,1);
  C.b= C.b(2) - C.b(1);
end


function pde = Stokesdata0

%
nu = 1;
pde = struct('f',@f,'g',@g,'exactp', @exactp, ...
             'exactu', @exactu,'g_D', @g_D, 'nu', nu);

    function z = f(p)
        x = p(:,1); y = p(:,2);
        z(:,1) = -4*pi^2*(2*cos(2*pi*x)-1).*sin(2*pi*y)+x.^2;
        z(:,2) = 4*pi^2*(2*cos(2*pi*y)-1).*sin(2*pi*x);
    end

    function z = g(p)
        z = zeros(size(p,1),1);
    end

    function z = exactu(p)
        x = p(:,1); y = p(:,2);
        z(:,1) = (1-cos(2*pi*x)).*sin(2*pi*y);
        z(:,2) = -(1-cos(2*pi*y)).*sin(2*pi*x);
    end

    function z = exactp(p)
        x = p(:,1); % y = p(:,2);
        z = 1/3*x.^3;
    end

    function z = g_D(p) % Dirichlet boundary condition
        z = exactu(p);
    end
end


function b = isPointOnLine(point, line, varargin)
%ISPOINTONLINE Test if a point belongs to a line.
%
%   B = isPointOnLine(POINT, LINE)
%   with POINT being [xp yp], and LINE being [x0 y0 dx dy].
%   Returns 1 if point lies on the line, 0 otherwise.
%
%   If POINT is an N-by-2 array of points, B is a N-by-1 array of booleans.
%
%   If LINE is a N-by-4 array of line, B is a 1-by-N array of booleans.
%
%   B = isPointOnLine(POINT, LINE, TOL)
%   Specifies the tolerance used for testing location on 3D line. Default value is 1e-14.
%
%   See also
%   lines2d, points2d, isPointOnEdge, isPointOnRay, isLeftOriented
%

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2003-10-31
% Copyright 2003-2022 INRA - TPV URPOI - BIA IMASTE

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

% test if lines are colinear, using third coordinate of 3D cross-product
% same test as:
% b = abs((xp-x0).*dy-(yp-y0).*dx)./hypot(dx, dy).^2 < tol;
b = bsxfun(...
    @rdivide, abs(...
    bsxfun(@times, bsxfun(@minus, point(:,1), line(:,1)'), line(:,4)') - ...
    bsxfun(@times, bsxfun(@minus, point(:,2), line(:,2)'), line(:,3)')), ...
    (line(:,3).^2 + line(:,4).^2)') < tol;


function [gradISE,gradJhr,gradJrr]=GaussMixISEMeanGrads(w1,mu1,P1,w2,mu2,P2,PDFVals12,PDFVals22)
%%GAUSSMIXISEMEANGRADS Compute the gradient of the non-normalized
%               integrated squared error (ISE) between two Gaussian mixture
%               PDFs with respect to the mean vectors of the individual
%               components of the second Gaussian mixture. This gradient
%               can arise when optimizing over the ISE with respect to the
%               means.
%
%INPUTS: w1 The N1X1 or 1XN1 vector of weights for the first Gaussian
%           mixture. All w1>0 and sum(w1)=1.
%       mu1 The xDimXN1 set of mean vectors for the first Gaussian mixture
%           distribution.
%        P1 The xDimXxDimXN1 set of positive definite covariance matrices
%           for the first Gaussian mixture distirbution.
% w2, mu2, P2, The length N2, xDimXN2, and xDimXxDimXN2 set of weights,
%           mean vectors and positive-definite covariance matrices for the
%           second Gaussian mixture distribution.
% PDFVals12 A matrix such that the value in element (i,j) is
%           N(mu1(:,i);mu2(:,j),P1(:,:,i)+P2(:,:,j)), where N indicates the
%           multivariate Gaussian PDF evaluated at the first argument with
%           the second and third arguments being the mean and covarince
%           matrix. This parameter is returned by computeGaussMixISE.
% PDFVals22 A matrix such that the value in element (i,j) is
%           N(mu2(:,i);mu2(:,j),P2(:,:,i)+P2(:,:,j)). This parameter is
%           returned by computeGaussMixISE.
%
%OUTPUTS: gradISE The xDimXn2 set of derivatives of the ISE with respect to
%                 the elements of q (the square roots of w2).
% gradJhr, gradJrr In Chapter 3 of [1], the ISE is expressed in terms of
%                 Jhr and Jrr terms. These are the xDimXn2 gradients of
%                 those terms.
%
%Formule for gradJhr and gradJrr are Equation 3.34 in Section 3.3.3.2 of
%[1]. They relate to the ISE via Equation 3.20. See the function
%computeGaussMixISE to compute the ISE.
%
%EXAMPLE 1:
%In this example with a scalar random variable, we verify that the gradient
%obtained from this function is consistent with numerical differentiation.
% w1=[0.03,0.18,0.12,0.19,0.02,0.16,0.06,0.1,0.08,0.06];
% n1=length(w1);
% mu1=[1.45,2.20,0.67,0.48,1.49,0.91,1.01,1.42,2.77,0.89];
% P1=[0.0487,0.0305,0.1171,0.0174,0.0295,0.0102, 0.0323, 0.0380, 0.0115, 0.0679];
% P1=reshape(P1,[1,1,n1]);
%
% %The second PDF is the first with the five least-weight components deleted.
% w2=[0.18,0.12,0.19,0.16,0.1,0.08];
% w2=w2/sum(w2);
% n2=length(w2);
% mu2=[2.20,0.67,0.48,0.91,1.42,2.77];
% P2=[0.0305,0.1171,0.0174,0.0102,0.0380,0.0115];
% P2=reshape(P2,[1,1,n2]);
%
% [ISEVal,PDFVals12,PDFVals22]=computeGaussMixISE(w1,mu1,P1,w2,mu2,P2);
%
% epsVal=1e-8;
% gradISENumDiff=zeros(1,n2);
% for j=1:n2
%     muCur=mu2;
%     muCur(1,j)=muCur(1,j)+epsVal;
% 	ISEValCur=computeGaussMixISE(w1,mu1,P1,w2,muCur,P2);
%     gradISENumDiff(1,j)=(ISEValCur-ISEVal)/epsVal;
% end
% gradISE=GaussMixISEMeanGrads(w1,mu1,P1,w2,mu2,P2,PDFVals12,PDFVals22);
% RelErr=max(abs((gradISENumDiff-gradISE)./gradISENumDiff))
%The relative error will be about 5.4042e-6, which indicates good numeric
%agreement.
%
%EXAMPLE 2:
%In this example with a bivariate distribution, we verify that the gradient
%obtained from this function is consistent with numerical differentiation.
% w1=[0.25;0.5;0.25];
% mu1=zeros(2,2);
% mu1(:,1)=[1;-1];
% mu1(:,2)=[-1;1];
% mu1(:,3)=[0;0];
% P1=zeros(2,2,2);
% P1(:,:,1)=[4/9,  14/45;
%            14/45,4/9];
% P1(:,:,2)=[4/9, 0;
%            0, 4/9];
% P1(:,:,3)=[2/9, -1/9;
%           -1/9, 3/9];
%
% %The second distribution just throws out the first component.
% w2=w1(2:3);
% w2=w2/sum(w2);
% mu2=mu1(:,2:3);
% P2=P1(:,:,2:3);
%
% n2=length(w2);
% [ISEVal,PDFVals12,PDFVals22]=computeGaussMixISE(w1,mu1,P1,w2,mu2,P2);
%
% numDim=size(mu1,1);
% epsVal=1e-8;
% gradISENumDiff=zeros(numDim,n2);
% for curDim=1:numDim
%     for j=1:n2
%         muCur=mu2;
%         muCur(curDim,j)=muCur(curDim,j)+epsVal;
%         ISEValCur=computeGaussMixISE(w1,mu1,P1,w2,muCur,P2);
%         gradISENumDiff(curDim,j)=(ISEValCur-ISEVal)/epsVal;
%     end
% end
% gradISE=GaussMixISEMeanGrads(w1,mu1,P1,w2,mu2,P2,PDFVals12,PDFVals22);
% RelErr=max(max(abs((gradISENumDiff-gradISE)./gradISENumDiff)))
%The relative error will be about 1.2876e-7, which indicates good numeric
%agreement.
%
%REFERENCES:
%[1] J. L. Williams, "Gaussian mixture reduction for tracking multiple
%    maneuvering targets in clutter," Master's thesis, Air Force Institute
%    of Technology, Mar. 2003. [Online].
%    Available: http://www.dtic.mil/srch/doc?collection=t3&id=ADA415317
%
%May 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

Nh=length(w1);
Nr=length(w2);
xDim=size(mu1,1);

%The formulae for the gradient of Jhr and Jrr are given in Equation 3.34 of
%Section 3.3.3.2 of [1].
gradJhr=zeros(xDim,Nr);
gradJrr=zeros(xDim,Nr);

for j=1:Nr
    for i=1:Nh
        gradJhr(:,j)=gradJhr(:,j)+(P1(:,:,i)+P2(:,:,j))\(mu2(:,j)-mu1(:,i))*PDFVals12(i,j)*w1(i);
    end
    gradJhr(:,j)=-w2(j)*gradJhr(:,j);
end

for j=1:Nr
    for i=1:Nr
        gradJrr(:,j)=gradJrr(:,j)+(P2(:,:,i)+P2(:,:,j))\(mu2(:,j)-mu2(:,i))*PDFVals22(i,j)*w2(i);
    end
    gradJrr(:,j)=-2*w2(j)*gradJrr(:,j);
end

gradISE=gradJrr-2*gradJhr;
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 [n,V,p] = affine_fit(X,W)
    %Computes the plane that fits best (lest square of the normal distance
    %to the plane) a set of sample points.
    %INPUTS:
    %
    %X: a N by 3 matrix where each line is a sample point
    %
    %OUTPUTS:
    %
    %n : a unit (column) vector normal to the plane
    %V : a 3 by 2 matrix. The columns of V form an orthonormal basis of the
    %plane
    %p : a point belonging to the plane
    %
    %NB: this code actually works in any dimension (2,3,4,...)
    %Author: Adrien Leygue
    %Date: August 30 2013

    if nargin<2
      W = 1/size(X,1)*ones(size(X,1),1);
    end
    W = W(:)./sum(W);

    %the mean of the samples belongs to the plane
    p = sum(bsxfun(@times,W,X));

    %The samples are reduced:
    R = bsxfun(@times,bsxfun(@minus,X,p),W);
    %Computation of the principal directions if the samples cloud
    [V,D] = eig(R'*R);
    %Extract the output from the eigenvectors
    n = V(:,1);
    V = V(:,2:end);
end

% Copyright (c) 2013, Adrien Leygue
% All rights reserved.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are
% met:
%
%     * Redistributions of source code must retain the above copyright
%       notice, this list of conditions and the following disclaimer.
%     * Redistributions in binary form must reproduce the above copyright
%       notice, this list of conditions and the following disclaimer in
%       the documentation and/or other materials provided with the distribution
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
% POSSIBILITY OF SUCH DAMAGE.


function B = barymat(y, x, w, s, r, doFlip)
%BARYMAT  Barycentric Interpolation Matrix.
%   BARYMAT(Y, X, W), where Y is a column vector of length M and X and W are
%   column and row vectors of length N, respectively, returns the M*N matrix
%   which interpolates data from the grid X to the grid Y using the 2nd-kind
%   barycentric interpolation formula with barycentric weights W. If W is not
%   supplied it is assumed to be the weights for polynomial interpolation at a
%   2nd-kind Chebyshev grid: W(j) = (-1)^j, W([1, N]) = 0.5*W([1, N]).
%
%   BARYMAT(Y, X, W, S, R) is the same, where S = acos(Y) and R = acos(X). The
%   purpose of this is that Y(j) - X(k) can be more accurately computed in this
%   'theta space'. This is sometimes referred to as the 'trig trick' in spectral
%   collocation. BARYMAT(Y, X, W, S, R, 1) also performs the 'flipping trick',
%   which takes advantage of the fact that the smaller entries in R and S can be
%   computed more accurately. Note that X and Y should be symmetric about zero
%   for this work, and it is assumed that S and R are sorted in descending
%   order.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

if ( isempty(y) || isempty(x) )
    % Nothing to do here!
    B = [];
    return
end

if ( size(x, 2) > 1 || size(y, 2) > 1 )
    error('CHEBFUN:barymat:dims', 'Inputs should be column vectors.');
end

% Obtain lengths of inputs:
N = length(x);
M = length(y);

% Nothing to do here!
if ( M == N && all(x == y) )
    B = eye(N);
    return
end

% Default to the Chebyshev barycentric weights:
if ( nargin < 3 || isempty(w) )
    w = ones(1, N);
    w(2:2:end) = -1;
    w([1, N]) = 0.5*w([1, N]);
else
    % Ensure w is a row vector:
    w = reshape(w, 1, N);
end

% Repmat(Y-X'):
if ( nargin < 5 )
    B = bsxfun(@minus, y, x.');
else
    % Use the 'trig trick' that y-x = cos(s)-cos(r) = 2*sin((s+r)/2)*sin((r-s)/2).
    B = 2*bsxfun(@(r, s) sin((s+r)/2).* sin((r-s)/2), r.', s);
end

% Construct the matrix:
if ( M >= 500 && N >= 1000 )            % <-- Experimentally determined.
    % Testing shows BSXFUN is faster in this regime
    B = bsxfun(@rdivide, w, B);         % w(k)/(y(j)-x(k))
    B = bsxfun(@rdivide, B, sum(B, 2)); % Normalisation.
else
    % Else use FOR loops
    for k = 1:N
        B(:,k) = w(k)./B(:,k);          % w(k)/(y(j)-x(k))
    end
    c = 1./sum(B, 2);                   % Normalisation.
    for j = 1:M
        B(j,:) = B(j,:)*c(j);
    end
end

% Where points coincide there will be division by zeros (as with bary.m).
% Replace these entries with the identity:
B(isnan(B)) = 1;

% Flipping trick:
if ( nargin > 5 && doFlip )
    ii = logical(rot90(tril(ones(M, N)), 2));
    ii = fliplr(ii);
    rot90D = rot90(B, 2);
    B(ii) = rot90D(ii);
    B(isnan(B)) = 1;
end

end


%CORRECTMATCHES  Refines coordinates of corresponding points
%
%     [newPoints1, newPoints2] = cv.correctMatches(F, points1, points2)
%
% ## Input
% * __F__ 3x3 fundamental matrix.
% * __points1__ first set of 2D points. A numeric Nx2/Nx1x2/1xNx2 array or a
%   cell array of 2-element vectors `{[x,y], ...}` (floating-point precision).
% * __points2__ second set of 2D points. Same size and type as `points1`.
%
% ## Output
% * __newPoints1__ The optimized `points1`. Similar in shape to `points1`
%   (either Nx2/1xNx2 numeric array or cell array of 2D points).
% * __newPoints2__ The optimized `points2`.
%
% The function implements the Optimal Triangulation Method (see [Hartley2004]
% for details). For each given point correspondence `points1[i] <-> points2[i]`,
% and a fundamental matrix `F`, it computes the corrected correspondences
% `newPoints1[i] <-> newPoints2[i]` that minimize the geometric error:
%
%     d(points1[i], newPoints1[i])^2 + d(points2[i], newPoints2[i])^2
%
% (where `d(a,b)` is the geometric distance between points `a` and `b`)
% subject to the epipolar constraint
%
%     newPoints2' * F * newPoints1 = 0
%
% ## References
% [Hartley2004]:
% > R.I. Hartley and A. Zisserman. "Multiple View Geometry in Computer Vision"
% > Cambridge University Press, 2004.
%
% See also: cv.findFundamentalMat
%


function jac = p17_jac ( neqn, t, y )

%*****************************************************************************80
%
%% P17_JAC evaluates the jacobian for problem p17.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 February 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NEQN, the number of equations.
%
%    Input, real T, Y(NEQN), the arguments of the jacobian.
%
%    Output, real JAC(NEQN,NEQN), the jacobian matrix.
%
  jac = zeros ( neqn, neqn );

  d = ( sqrt ( ( y(1).^2 + y(2).^2 ) ) ).^5;

  jac(1,3) = 1.0;
  jac(2,4) = 1.0;
  jac(3,1) = ( 2.0 * y(1).^2 - y(2).^2 ) / d;
  jac(3,2) = 3.0 * y(1) * y(2) / d;
  jac(4,1) = 3.0 * y(1) * y(2) / d;
  jac(4,2) = ( - y(1).^2 + 2.0 * y(2).^2 ) / d;

  return
end


function [ld, UC] = logdet(A, UC)

% LOGDET The log of the determinant when argument is positive definite.
% FORMAT
% DESC returns the log determinant of a positive definite matrix. If
% the matrix isn't quite positive definite the function adds 'jitter'
% to make it positive definite and gives out a warning message (this
% is done through JITCHOL).
% ARG A : the input positive definite matrix for which the log
% determinant is required.
% RETURN d : the log determinant of A computed using Cholesky
% decomposition.
% RETURN U : the Cholesky decomposition of A.
%
% FORMAT
% DESC returns the log determinant of a positive definite matrix given
% the Cholesky decomposition of A. If jitter is used then the
% amount of jitter used is returned.
% ARG A : the input positive definite matrix for which the log
% determinant is required.
% ARG U : the Cholesky decomposition of A.
% RETURN d : the log determinant of A computed using Cholesky
% decomposition.
% RETURN U : the Cholesky decomposition of A.
% RETURN jitter : the amount of jitter added.
%
% SEEALSO : jitChol, pdinv, chol
%
% COPYRIGHT : Neil D. Lawrence, 2003, 2004, 2005, 2006

% NDLUTIL

if nargin < 2
  UC=[];
end

% Obtain a Cholesky decomposition.
if isempty(UC)
  if nargout > 2
    [UC, jitter] = jitChol(A);
  else
    UC = jitChol(A);
  end
end

ld = 2*sum(log(diag(UC)));


function [J grad] = nnCostFunction(nn_params, ...
                                   input_layer_size, ...
                                   hidden_layer_size, ...
                                   num_labels, ...
                                   X, y, lambda)
%NNCOSTFUNCTION Implements the neural network cost function for a two layer
%neural network which performs classification
%   [J grad] = NNCOSTFUNCTON(nn_params, hidden_layer_size, num_labels, ...
%   X, y, lambda) computes the cost and gradient of the neural network. The
%   parameters for the neural network are "unrolled" into the vector
%   nn_params and need to be converted back into the weight matrices.
%
%   The returned parameter grad should be a "unrolled" vector of the
%   partial derivatives of the neural network.
%

% Reshape nn_params back into the parameters Theta1 and Theta2, the weight matrices
% for our 2 layer neural network
Theta1 = reshape(nn_params(1:hidden_layer_size * (input_layer_size + 1)), ...
                 hidden_layer_size, (input_layer_size + 1));

Theta2 = reshape(nn_params((1 + (hidden_layer_size * (input_layer_size + 1))):end), ...
                 num_labels, (hidden_layer_size + 1));

% Setup some useful variables
m = size(X, 1);

% You need to return the following variables correctly
J = 0;
Theta1_grad = zeros(size(Theta1));
Theta2_grad = zeros(size(Theta2));

% ====================== YOUR CODE HERE ======================
% Instructions: You should complete the code by working through the
%               following parts.
%
% Part 1: Feedforward the neural network and return the cost in the
%         variable J. After implementing Part 1, you can verify that your
%         cost function computation is correct by verifying the cost
%         computed in ex4.m

% Add ones to the X data matrix
X = [ones(m, 1) X];
% Convert y from (1-10) class into num_labels vector
yd = eye(num_labels);
y = yd(y,:);

%%% Map from Layer 1 to Layer 2
a1=X;
% Coverts to matrix of 5000 examples x 26 thetas
z2=X*Theta1';
% Sigmoid function converts to p between 0 to 1
a2=sigmoid(z2);

%%% Map from Layer 2 to Layer 3
% Add ones to the h1 data matrix
a2=[ones(m, 1) a2];
% Converts to matrix of 5000 exampls x num_labels
z3=a2*Theta2';
% Sigmoid function converts to p between 0 to 1
a3=sigmoid(z3);

% Compute cost
%logisf=(-y)'*log(a3)-(1-y)'*log(1-a3);
logisf=(-y).*log(a3)-(1-y).*log(1-a3); % Becos y is now a matrix, so use dot product, unlike above
%J=((1/m).*sum(sum(logisf)));	% This line is correct if there is no regularization
% Try with ...
% J=((1/m).*sum((logisf)));
% That will give J in 10 columns (it has summed m samples), so need to sum again

%% Regularized cost
Theta1s=Theta1(:,2:end);
Theta2s=Theta2(:,2:end);
J=((1/m).*sum(sum(logisf)))+(lambda/(2*m)).*(sum(sum(Theta1s.^2))+sum(sum(Theta2s.^2)));

% Part 2: Implement the backpropagation algorithm to compute the gradients
%         Theta1_grad and Theta2_grad. You should return the partial derivatives of
%         the cost function with respect to Theta1 and Theta2 in Theta1_grad and
%         Theta2_grad, respectively. After implementing Part 2, you can check
%         that your implementation is correct by running checkNNGradients
%
%         Note: The vector y passed into the function is a vector of labels
%               containing values from 1..K. You need to map this vector into a
%               binary vector of 1's and 0's to be used with the neural network
%               cost function.
%
%         Hint: We recommend implementing backpropagation using a for-loop
%               over the training examples if you are implementing it for the
%               first time.
%
% Set all the D to zeros
tridelta_1=0;
tridelta_2=0;

% Compute delta, tridelta and big D
	delta_3=a3-y;
    z2=[ones(m,1) z2];
	delta_2=delta_3*Theta2.*sigmoidGradient(z2);
    delta_2=delta_2(:,2:end);
	tridelta_1=tridelta_1+delta_2'*a1; % Same size as Theta1_grad (25x401)
    tridelta_2=tridelta_2+delta_3'*a2; % Same size as Theta2_grad (10x26)
	Theta1_grad=(1/m).*tridelta_1;
    Theta2_grad=(1/m).*tridelta_2;
    %Theta1_grad=0;
	%Theta2_grad=0;
%end

% Part 3: Implement regularization with the cost function and gradients.
%
%         Hint: You can implement this around the code for
%               backpropagation. That is, you can compute the gradients for
%               the regularization separately and then add them to Theta1_grad
%               and Theta2_grad from Part 2.
%

% -------------------------------------------------------------

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

% Unroll gradients
grad = [Theta1_grad(:) ; Theta2_grad(:)];

end


function [model, L] = ppcaVb(X, q, prior)
% Perform variatioanl Bayeisan inference for probabilistic PCA model.
% Input:
%   X: d x n data matrix
%   q: dimension of target space
% Output:
%   model: trained model structure
%   L: variantional lower bound
% Reference:
%   Pattern Recognition and Machine Learning by Christopher M. Bishop
% Written by Mo Chen (sth4nth@gmail.com).
[m,n] = size(X);
if nargin < 3
    a0 = 1e-4;
    b0 = 1e-4;
    c0 = 1e-4;
    d0 = 1e-4;
else
    a0 = prior.a;
    b0 = prior.b;
    c0 = prior.c;
    d0 = prior.d;
end

if nargin < 2
    q = m-1;
end
tol = 1e-6;
maxIter = 500;
L = -inf(1,maxIter);

mu = mean(X,2);
Xo = bsxfun(@minus, X, mu);
s = dot(Xo(:),Xo(:));
I = eye(q);
% init parameters
a = a0+m/2;
c = c0+m*n/2;
Ealpha = 1e-4;
Ebeta = 1e-4;
EW = rand(q,m);
EWo = bsxfun(@minus,EW,mean(EW,2));
EWW = EWo*EWo'/m+EW*EW';
for iter = 2:maxIter
%     q(z)
    LZ = I+Ebeta*EWW;
    V = inv(chol(LZ));                   % inv(LZ) = V*V';
    EZ = LZ\EW*Xo*Ebeta;
    EZZ = n*(V*V')+EZ*EZ';
    KLZ = n*sum(log(diag(V)));           % KLZ = 0.5*n*log(det(inv(LZ)));
%     q(w)
    LW = diag(Ealpha)+Ebeta*EZZ;
    V = inv(chol(LW));                   % inv(LW) = V*V';
    EW = LW\EZ*Xo'*Ebeta;
    EWW = m*(V*V')+EW*EW';
    KLW = m*sum(log(diag(V)));           % KLW = 0.5*n*log(det(inv(LW)));
%     q(alpha)
    b = b0+diag(EWW)/2;
    Ealpha = a./b;
    KLalpha = -sum(a*log(b));
%     q(beta)
    WZ = EW'*EZ;
    d = d0+(s-2*dot(Xo(:),WZ(:))+dot(EWW(:),EZZ(:)))/2;
    Ebeta = c/d;
    KLbeta = -c*log(d);
%     q(mu)
%     Emu = Ebeta/(lambda+n*Ebeta)*sum(X-WZ,2);

%     lower bound
    L(iter) = KLalpha+KLbeta+KLW+KLZ;
    if L(iter)-L(iter-1) < tol*abs(L(iter-1)); break; end
end
L = L(2:iter);

model.Z = EZ;
model.W = EW;
model.apha = Ealpha;
model.beta = Ebeta;
model.a = a;
model.b = b;
model.c = c;
model.d = d;
model.mu = mu;


function ellipse_t = ellFit(x,y,axis_handle,colorStr)
% Finds the best fit to an ellipse for the given set of points.
% http://www.mathworks.com/matlabcentral/fileexchange/loadFile.do?objectId=3215&objectType=File
%
% Format:   ellipse_t = ellFit( x,y, [axis_handle])
%
% Input:    x,y         - a set of points in 2 column vectors. AT LEAST 5 points are needed !
%           axis_handle - optional. a handle to an axis, at which the estimated ellipse
%                         will be drawn along with it's axes
%
% Output:   ellipse_t - structure that defines the best fit to an ellipse
%                       a           - sub axis (radius) of the X axis of the non-tilt ellipse
%                       b           - sub axis (radius) of the Y axis of the non-tilt ellipse
%                       phi         - orientation in radians of the ellipse (tilt)
%                       X0          - center at the X axis of the non-tilt ellipse
%                       Y0          - center at the Y axis of the non-tilt ellipse
%                       X0_in       - center at the X axis of the tilted ellipse
%                       Y0_in       - center at the Y axis of the tilted ellipse
%                       long_axis   - size of the long axis of the ellipse
%                       short_axis  - size of the short axis of the ellipse
%                       status      - status of detection of an ellipse
%
% Note:     if an ellipse was not detected (but a parabola or hyperbola), then
%           an empty structure is returned
%
% Example:  (Verify recovery)
%     a = 1.45; b = 3.2; theta = 0.15; radSpacing = 0.05;
%     [x,y] = ellipsePoints(a,b,theta,radSpacing);
%     figure(1), plot(x,y,'-'); axis equal
%     ellS = ellFit(x,y);
%     [ellS.a, ellS.b, ellS.phi]
%     [a, b, theta]
%
%     a = 1.45; b = 3.2; theta = pi/5; radSpacing = 0.05;
%     [x,y] = ellipsePoints(a,b,theta,radSpacing);
%     figure(1), plot(x,y,'-'); axis equal
%     for ii=1:10
%       ellS = ellFit(x + randn(size(x))/5, y + randn(size(y))/5);
%       aHat(ii) = ellS.a; bHat(ii) = ellS.b; thetaHat(ii) =  ellS.phi;
%     end
%     [mean(aHat), mean(bHat), mean(thetaHat)]
%
%     ellS = ellFit(x,y,figure(1));

%
% =====================================================================================
%                  Ellipse Fit using Least Squares criterion
% =====================================================================================
% We will try to fit the best ellipse to the given measurements. the mathematical
% representation of use will be the CONIC Equation of the Ellipse which is:
%
%    Ellipse = a*x^2 + b*x*y + c*y^2 + d*x + e*y + f = 0
%
% The fit-estimation method of use is the Least Squares method (without any weights)
% The estimator is extracted from the following equations:
%
%    g(x,y;A) := a*x^2 + b*x*y + c*y^2 + d*x + e*y = f
%
%    where:
%       A   - is the vector of parameters to be estimated (a,b,c,d,e)
%       x,y - is a single measurement
%
% We will define the cost function to be:
%
%   Cost(A) := (g_c(x_c,y_c;A)-f_c)'*(g_c(x_c,y_c;A)-f_c)
%            = (X*A+f_c)'*(X*A+f_c)
%            = A'*X'*X*A + 2*f_c'*X*A + N*f^2
%
%   where:
%       g_c(x_c,y_c;A) - vector function of ALL the measurements
%                        each element of g_c() is g(x,y;A)
%       X              - a matrix of the form: [x_c.^2, x_c.*y_c, y_c.^2, x_c, y_c ]
%       f_c            - is actually defined as ones(length(f),1)*f
%
% Derivation of the Cost function with respect to the vector of parameters "A" yields:
%
%   A'*X'*X = -f_c'*X = -f*ones(1,length(f_c))*X = -f*sum(X)
%
% Which yields the estimator:
%
%       ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%       |  A_least_squares = -f*sum(X)/(X'*X) ->(normalize by -f) = sum(X)/(X'*X)  |
%       ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%
% (We will normalize the variables by (-f) since "f" is unknown and can be accounted for later on)
%
% NOW, all that is left to do is to extract the parameters from the Conic Equation.
% We will deal the vector A into the variables: (A,B,C,D,E) and assume F = -1;
%
%    Recall the conic representation of an ellipse:
%
%       A*x^2 + B*x*y + C*y^2 + D*x + E*y + F = 0
%
% We will check if the ellipse has a tilt (=orientation). The orientation is present
% if the coefficient of the term "x*y" is not zero. If so, we first need to remove the
% tilt of the ellipse.
%
% If the parameter "B" is not equal to zero, then we have an orientation (tilt) to the ellipse.
% we will remove the tilt of the ellipse so as to remain with a conic representation of an
% ellipse without a tilt, for which the math is more simple:
%
% Non tilt conic rep.:  A`*x^2 + C`*y^2 + D`*x + E`*y + F` = 0
%
% We will remove the orientation using the following substitution:
%
%   Replace x with cx+sy and y with -sx+cy such that the conic representation is:
%
%   A(cx+sy)^2 + B(cx+sy)(-sx+cy) + C(-sx+cy)^2 + D(cx+sy) + E(-sx+cy) + F = 0
%
%   where:      c = cos(phi)    ,   s = sin(phi)
%
%   and simplify...
%
%       x^2(A*c^2 - Bcs + Cs^2) + xy(2A*cs +(c^2-s^2)B -2Ccs) + ...
%           y^2(As^2 + Bcs + Cc^2) + x(Dc-Es) + y(Ds+Ec) + F = 0
%
%   The orientation is easily found by the condition of (B_new=0) which results in:
%
%   2A*cs +(c^2-s^2)B -2Ccs = 0  ==> phi = 1/2 * atan( b/(c-a) )
%
%   Now the constants   c=cos(phi)  and  s=sin(phi)  can be found, and from them
%   all the other constants A`,C`,D`,E` can be found.
%
%   A` = A*c^2 - B*c*s + C*s^2                  D` = D*c-E*s
%   B` = 2*A*c*s +(c^2-s^2)*B -2*C*c*s = 0      E` = D*s+E*c
%   C` = A*s^2 + B*c*s + C*c^2
%
% Next, we want the representation of the non-tilted ellipse to be as:
%
%       Ellipse = ( (X-X0)/a )^2 + ( (Y-Y0)/b )^2 = 1
%
%       where:  (X0,Y0) is the center of the ellipse
%               a,b     are the ellipse "radiuses" (or sub-axis)
%
% Using a square completion method we will define:
%
%       F`` = -F` + (D`^2)/(4*A`) + (E`^2)/(4*C`)
%
%       Such that:    a`*(X-X0)^2 = A`(X^2 + X*D`/A` + (D`/(2*A`))^2 )
%                     c`*(Y-Y0)^2 = C`(Y^2 + Y*E`/C` + (E`/(2*C`))^2 )
%
%       which yields the transformations:
%
%           X0  =   -D`/(2*A`)
%           Y0  =   -E`/(2*C`)
%           a   =   sqrt( abs( F``/A` ) )
%           b   =   sqrt( abs( F``/C` ) )
%
% And finally we can define the remaining parameters:
%
%   long_axis   = 2 * max( a,b )
%   short_axis  = 2 * min( a,b )
%   Orientation = phi
%
%

if notDefined('colorStr');
    colorStr = 'r';
end
% initialize
orientation_tolerance = 1e-3;

% empty warning stack
warning( '' );

% prepare vectors, must be column vectors
x = x(:);
y = y(:);

% remove bias of the ellipse - to make matrix inversion more accurate. (will be added later on).
mean_x = mean(x);
mean_y = mean(y);
x = x-mean_x;
y = y-mean_y;

% the estimation for the conic equation of the ellipse
X = [x.^2, x.*y, y.^2, x, y ];
a = sum(X)/(X'*X);

% check for warnings
if ~isempty( lastwarn )
    disp( 'stopped because of a warning regarding matrix inversion' );
    ellipse_t = [];
    return
end

% extract parameters from the conic equation
[a,b,c,d,e] = deal( a(1),a(2),a(3),a(4),a(5) );

% remove the orientation from the ellipse
if ( min(abs(b/a),abs(b/c)) > orientation_tolerance )

    orientation_rad = 1/2 * atan( b/(c-a) );
    cos_phi = cos( orientation_rad );
    sin_phi = sin( orientation_rad );
    [a,b,c,d,e] = deal(...
        a*cos_phi^2 - b*cos_phi*sin_phi + c*sin_phi^2,...
        0,...
        a*sin_phi^2 + b*cos_phi*sin_phi + c*cos_phi^2,...
        d*cos_phi - e*sin_phi,...
        d*sin_phi + e*cos_phi );
    [mean_x,mean_y] = deal( ...
        cos_phi*mean_x - sin_phi*mean_y,...
        sin_phi*mean_x + cos_phi*mean_y );
else
    orientation_rad = 0;
    cos_phi = cos( orientation_rad );
    sin_phi = sin( orientation_rad );
end

% check if conic equation represents an ellipse
test = a*c;
switch (1)
case (test>0),  status = '';
case (test==0), status = 'Parabola found';  warning( 'fit_ellipse: Did not locate an ellipse' );
case (test<0),  status = 'Hyperbola found'; warning( 'fit_ellipse: Did not locate an ellipse' );
end

% if we found an ellipse return it's data
if (test>0)

    % make sure coefficients are positive as required
    if (a<0), [a,c,d,e] = deal( -a,-c,-d,-e ); end

    % final ellipse parameters
    X0          = mean_x - d/2/a;
    Y0          = mean_y - e/2/c;
    F           = 1 + (d^2)/(4*a) + (e^2)/(4*c);
    [a,b]       = deal( sqrt( F/a ),sqrt( F/c ) );
    long_axis   = 2*max(a,b);
    short_axis  = 2*min(a,b);

    % rotate the axes backwards to find the center point of the original TILTED ellipse
    R           = [ cos_phi sin_phi; -sin_phi cos_phi ];
    P_in        = R * [X0;Y0];
    X0_in       = P_in(1);
    Y0_in       = P_in(2);

    % pack ellipse into a structure
    ellipse_t = struct( ...
        'a',a,...
        'b',b,...
        'phi',orientation_rad,...
        'X0',X0,...
        'Y0',Y0,...
        'X0_in',X0_in,...
        'Y0_in',Y0_in,...
        'long_axis',long_axis,...
        'short_axis',short_axis,...
        'status','' );
else
    % report an empty structure
    ellipse_t = struct( ...
        'a',[],...
        'b',[],...
        'phi',[],...
        'X0',[],...
        'Y0',[],...
        'X0_in',[],...
        'Y0_in',[],...
        'long_axis',[],...
        'short_axis',[],...
        'status',status );
end

% check if we need to plot an ellipse with it's axes.
if (nargin>2) && ~isempty( axis_handle ) && (test>0)

    % rotation matrix to rotate the axes with respect to an angle phi
    R = [ cos_phi sin_phi; -sin_phi cos_phi ];

    % the axes
    ver_line        = [ [X0 X0]; Y0+b*[-1 1] ];
    horz_line       = [ X0+a*[-1 1]; [Y0 Y0] ];
    new_ver_line    = R*ver_line;
    new_horz_line   = R*horz_line;

    % the ellipse
    theta_r         = linspace(0,2*pi);
    ellipse_x_r     = X0 + a*cos( theta_r );
    ellipse_y_r     = Y0 + b*sin( theta_r );
    rotated_ellipse = R * [ellipse_x_r;ellipse_y_r];

    % draw
    hold_state = get( axis_handle,'NextPlot' );
    set( axis_handle,'NextPlot','add' );
%     plot( new_ver_line(1,:),new_ver_line(2,:),colorStr );
%     plot( new_horz_line(1,:),new_horz_line(2,:),colorStr );
    plot( rotated_ellipse(1,:),rotated_ellipse(2,:),colorStr );
    set( axis_handle,'NextPlot',hold_state );
end


function determ = herndon_determinant ( n )

%*****************************************************************************80
%
%% HERNDON_DETERMINANT returns the determinant of the Herndon matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real DETERM, the determinant.
%
  determ = 6.0 / ( ( n + 1 ) * n * ( 5 - 2 * n ) );

  return
end


function p = predict(theta, X)
%PREDICT Predict whether the label is 0 or 1 using learned logistic
%regression parameters theta
%   p = PREDICT(theta, X) computes the predictions for X using a
%   threshold at 0.5 (i.e., if sigmoid(theta'*x) >= 0.5, predict 1)

m = size(X, 1); % Number of training examples

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

% ====================== YOUR CODE HERE ======================
% Instructions: Complete the following code to make predictions using
%               your learned logistic regression parameters.
%               You should set p to a vector of 0's and 1's
%

h = sigmoid(X*theta);
ids = find(h>=0.5);
p(ids) = 1;
ids = find(h<0.5);
p(ids) = 0;

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

end


function determ = wilk21_determinant ( n )

%*****************************************************************************80
%
%% WILK21_DETERMINANT computes the determinant of the WILK21 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 November 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Output, real DETERM, the determinant.
%
  d = zeros ( n, 1 );

  for i = 1 : n
    d(i) = round ( abs ( i - ( n + 1 ) / 2 ) );
  end

  determ_nm1 = d(n);

  if ( n == 1 )
    determ = determ_nm1;
    return
  end

  determ_nm2 = determ_nm1;
  determ_nm1 = d(n-1) * d(n) - 1.0;

  if ( n == 2 )
    determ = determ_nm1;
    return
  end

  for i = n - 2 : -1 : 1

    determ = d(i) * determ_nm1 - determ_nm2;

    determ_nm2 = determ_nm1;
    determ_nm1 = determ;

  end

  return
end


function turnDir=turnOrientation(v1,v2,v3)
%%TURNORIENTATION Given 3 two-dimensional vertices in order, determine
%                 whether they form a left turn (go counterclockwise).
%                 Such a determination is necessary for determining
%                 whether a polygon is convex and plays a role in
%                 finding the convex hull of a set of points.
%
%INPUTS: v1, v2, v3 A set of 3 2XN matrices, of N vertex sets in the order
%                   [x;y] These are 2-dimensional vertices in order,
%                   whereby one wishes to determine whether the angle
%                   v1-v2-v3 is going counterclockwise (left) or clockwise
%                   (right) for each set of vertices.
%
%OUTPUTS: turnDir  An NX1 vector where the ith element is is 1 if the ith
%                  set of vertices form a counterclockwise angle, -1 if
%                  they are going clockwise and 0 if they are collinear or
%                  if two of them coincide.
%
%An implementation is given ehre in Matlab. However, the implementation in
%C++ might be more accurate, assuming that one's compilter implements the
%long double datatype as an IEEE-754 souble extended precision datatype.
%A notable exception is Microsoft's Visual Studio (as of 2017), which maps
%long doubles to doubles (64 bits rather than the 80-bit size of Intel/
%AMD floating point registers) as noted in
%https://docs.microsoft.com/en-us/cpp/c-language/type-long-double
%Additionally, the function exactSignOfSumCPP is used to provide the sign
%of an error-free sum. Unfortunately, since the IEEE-754 double extended
%precision datatype only provides a 64-bit mantissa, which is not much of
%an extension over the 54-bit mantissa of a standard double precision
%floating point number, and is not enough to assure no finite precision
%rounding in the products that go into the sum, one cannot guarantee zero
%finite precision errors in this function. On the other hand, if the
%inputs are single-precision (promoted to doubles, since the function does
%not take singles) floating point numbers, the result is exact.
%
%The cross product rule for 3D vectors a and b says that
%norm(cross(a,b))=norm(a)*norm(b)*sin(theta)
%where theta is the positive angle between the vectors. However, If the
%vectors are 2D, (set the z-components to zero), then the cross product
%only have one nonzero component in the z-direction and that component is
%equal to det([a,b]) (for 2D a and b). The interesting thing now, is that
%the sign of the determinant will tell you whether the subsequent vectors
%are going counterclockwise or clockwise. This function returns -1 if the
%vectors are going counterclockwise, 0 if they are exactly collinear and 1
%if they are clockwise.
%
%The Matlab implementation can be used without a compilation. The compiled
%version of the algorithm can be obtained using the
%CompileCLibraries function.
%
%The algorithm is run in Matlab using the command format
%turnDir=turnOrientation(v1,v2,v3);
%
%December 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    S=zeros(6,1);
    S(1)=v3(1)*v1(2);
    S(2)=v1(1)*v2(2);
    S(3)=v2(1)*v3(2);
    S(4)=-v3(1)*v2(2);
    S(5)=-v1(1)*v3(2);
    S(6)=-v2(1)*v1(2);

    turnDir=exactSignOfSum(S);
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.