%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: computes the angular displacements of a simple gravity pendulum
%
% PURPOSE: to illustrate no change in oscillatory amplitude under
% variations in the radius of the pendulum bob.
%
% Author: N.A. Battista
% Date: 12/13/2019
% Institution: The College of New Jersey
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function simple_pendulum()

%
% Physical Parameters
%
m = 100;    % Mass of Pendulum bob
g=9.80665;  % Gravitational Acceleration;
L=0.2;      % Length of Pendulum

%
% Vector of Radii
%
rVec = 100*[0.005 0.010 0.015 0.02 0.025];

%
% Temporal Parameters
%
t=0;        % initial time
tFinal = 8; % final-time
dt = 1e-3;  % time-step
ct=1;       % counter
tVec(ct)=t; % time vector storage

%
% Moment of Inertia
%
IVec = m*(0.5*rVec.^2+L^2);

%
% Initial Angular Displacement
%
ang = -(pi/2-pi/5)*ones(1,5);
phi = zeros( size( ang ) );

%
% Perform the time-stepping, e.g., solve the ODEs
%
while t<=tFinal

    % increment the storage counter
    ct = ct+1;

    % increment time
    t = t+dt;

    % save current time
    tVec(ct) = t;

    % solve 1st order ODE for angular displacement
    ang(ct,:) = ang(ct-1,:) + dt * ( phi(ct-1,:) );

    % solve 1st order ODE for angular velocity
    phi(ct,:) = phi(ct-1,:) + dt * ( -m*g*L ./IVec .* sin( ang(ct-1,:) ) );

end

%
% Defining Colors for Plotting Data
%
color2 = [0.635 0.078 0.184];  % dark red
color5 = [0.9 0.525 0.098];    % orange
color6 = [0.929 0.840 0.1250]; % dark yellow
color7 = [0.7 0.7 0];          % 'ugly' green

%
% Note discrepancy is a bit off due to small angle approximation:
% sin(theta) ~ theta gives below period of oscillation
%
T1 = 2*pi*sqrt( IVec(1) / (m*g*L) );
%
% Actually computed value of period is:
T1 = 1.93;

%
% Make Figure of Angular Displacement vs. Non-dimensional Time (# of oscillations of 'r' case)
%
lw=6;
fs=18;
plot(tVec/T1,ang(:,1),'k-','LineWidth',lw); hold on;
plot(tVec/T1,ang(:,2),'b-','LineWidth',lw,'Color',color2); hold on;
plot(tVec/T1,ang(:,3),'g-','LineWidth',lw,'Color',color5); hold on;
plot(tVec/T1,ang(:,4),'r-','LineWidth',lw,'Color',color6); hold on;
plot(tVec/T1,ang(:,5),'c-','LineWidth',lw,'Color',color7); hold on;
xlabel('Non-Dimensional Time (# periods of r-case)');
ylabel('Angular Displacement (Radians)');
axis([0 4*T1 -1 1.25]);
leg=legend('r','2r','3r','4r','5r','Orientation','horizontal');
set(gca,'FontSize',fs);
set(leg,'FontSize',fs);


function point = intersectLines(line1, line2, varargin)
%INTERSECTLINES Return all intersection points of N lines in 2D
%
%   PT = intersectLines(L1, L2);
%   returns the intersection point of lines L1 and L2. L1 and L2 are 1-by-4
%   row arrays, containing parametric representation of each line (in the
%   form [x0 y0 dx dy], see 'createLine' for details).
%
%   In case of colinear lines, returns [Inf Inf].
%   In case of parallel but not colinear lines, returns [NaN NaN].
%
%   If each input is [N*4] array, the result is a [N*2] array containing
%   intersections of each couple of lines.
%   If one of the input has N rows and the other 1 row, the result is a
%   [N*2] array.
%
%   PT = intersectLines(L1, L2, EPS);
%   Specifies the tolerance for detecting parallel lines. Default is 1e-14.
%
%   Example
%   line1 = createLine([0 0], [10 10]);
%   line2 = createLine([0 10], [10 0]);
%   point = intersectLines(line1, line2)
%   point =
%       5   5
%
%   See also
%   lines2d, edges2d, intersectEdges, intersectLineEdge
%   intersectLineCircle
%
%   ---------
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 31/10/2003.
%

%   HISTORY
%   2004-02-19 add support for multiple lines.
%   2007-03-08 update doc
%   2011-10-07 code cleanup

%% Process input arguments

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

% check size of each input
N1 = size(line1, 1);
N2 = size(line2, 1);
N = max(N1, N2);
if N1 ~= N2 && N1*N2 ~= N
    error('matGeom:IntersectLines:IllegalArgument', ...
        'The two input arguments must have same number of lines');
end

%% Check parallel and colinear lines

% coordinate differences of origin points
dx = bsxfun(@minus, line2(:,1), line1(:,1));
dy = bsxfun(@minus, line2(:,2), line1(:,2));

% indices of parallel lines
denom = line1(:,3) .* line2(:,4) - line2(:,3) .* line1(:,4);
par = abs(denom) < tol;

% indices of colinear lines
col = abs(dx .* line1(:,4) - dy .* line1(:,3)) < tol & par ;

% initialize result array
x0 = zeros(N, 1);
y0 = zeros(N, 1);

% initialize result for parallel lines
x0(col) = Inf;
y0(col) = Inf;
x0(par & ~col) = NaN;
y0(par & ~col) = NaN;

% in case all line couples are parallel, return
if all(par)
    point = [x0 y0];
    return;
end

%% Extract coordinates of itnersecting lines

% indices of intersecting lines
inds = ~par;

% extract base coordinates of first lines
if N1 > 1
    line1 = line1(inds,:);
end
x1 =  line1(:,1);
y1 =  line1(:,2);
dx1 = line1(:,3);
dy1 = line1(:,4);

% extract base coordinates of second lines
if N2 > 1
    line2 = line2(inds,:);
end
x2 =  line2(:,1);
y2 =  line2(:,2);
dx2 = line2(:,3);
dy2 = line2(:,4);

% re-compute coordinate differences of origin points
dx = bsxfun(@minus, line2(:,1), line1(:,1));
dy = bsxfun(@minus, line2(:,2), line1(:,2));

%% Compute intersection points

denom = denom(inds);
x0(inds) = (x2 .* dy2 .* dx1 - dy .* dx1 .* dx2 - x1 .* dy1 .* dx2) ./ denom ;
y0(inds) = (dx .* dy1 .* dy2 + y1 .* dx1 .* dy2 - y2 .* dx2 .* dy1) ./ denom ;

% concatenate result
point = [x0 y0];


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
%

p = sigmoid(X * theta);

idx_1 = find(p >= 0.5);
idx_0 = find(p < 0.5);

p(idx_1) = ones(size(idx_1));
p(idx_0) = zeros(size(idx_0));

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

end


function [ value, ifault ] = alogam ( x )

%*****************************************************************************80
%
%% ALOGAM computes the logarithm of the Gamma function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 January 2008
%
%  Author:
%
%    Original FORTRAN77 version by Malcolm Pike, David Hill.
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Malcolm Pike, David Hill,
%    Algorithm 291:
%    Logarithm of Gamma Function,
%    Communications of the ACM,
%    Volume 9, Number 9, September 1966, page 684.
%
%  Parameters:
%
%    Input, real X, the argument of the Gamma function.
%    X should be greater than 0.
%
%    Output, real ALOGAM, the logarithm of the Gamma
%    function of X.
%
%    Output, integer IFAULT, error flag.
%    0, no error.
%    1, X <= 0.
%
  if ( x <= 0.0 )
    ifault = 1;
    value = 0.0;
    return
  end

  ifault = 0;
  y = x;

  if ( x < 7.0 )

    f = 1.0;
    z = y;

    while ( z < 7.0 )
      f = f * z;
      z = z + 1.0;
    end

    y = z;
    f = - log ( f );

  else

    f = 0.0;

  end

  z = 1.0 / y / y;

  value = f + ( y - 0.5 ) * log ( y ) - y ...
    + 0.918938533204673 + ...
    ((( ...
    - 0.000595238095238   * z ...
    + 0.000793650793651 ) * z ...
    - 0.002777777777778 ) * z ...
    + 0.083333333333333 ) / y;

  return
end


function ising_2d_simulation ( m, n, iterations, thresh, seed )

%*****************************************************************************80
%
%% ISING_2D_SIMULATION carries out a 2D Ising simulation.
%
%  Discussion:
%
%    Note that, when all the cells are updated in a single cycle, there is
%    a mathematically stable checkerboard solution, in which all the reds
%    and blacks flip color repeatedly.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 November 2011
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'ISING_2D_SIMULATION\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Monte Carlo simulation of a 2D Ising model.\n' );
%
%  Arguments not supplied are set to default values.
%
  if ( nargin < 1 )
    m = 10;
  else
    m = str2num ( m );
  end

  if ( nargin < 2 )
    n = 10;
  else
    n = str2num ( n );
  end

  if ( nargin < 3 )
    iterations = 15;
  else
    iterations = str2num ( iterations );
  end

  if ( nargin < 4 )
    thresh = 0.50;
  else
    thresh = str2num ( thresh );
  end

  if ( nargin < 5 )
    seed = 123456789;
  else
    seed = str2num ( seed );
  end
%
%  Define the probability of flipping if you are in a neighborhood of
%  1, 2, 3, 4, or 5 of the same sign.
%
%  This should really be some kind of exponential dependent on "temperature".
%
  prob = [ 0.98, 0.85, 0.50, 0.15, 0.02 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The row dimension M = %d\n', m );
  fprintf ( 1, '  The column dimension N = %d\n', n );
  fprintf ( 1, '  The number of iterations taken is ITERATIONS = %d\n', iterations );
  fprintf ( 1, '  The threshhold THRESH = %f\n', thresh );
  fprintf ( 1, '  The seed SEED = %d\n', seed );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The transition probability table, based on the number of\n' );
  fprintf ( 1, '  neighbors with the same charge.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '        1           2           3           4           5\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  %10.4f  %10.4f  %10.4f  %10.4f  %10.4f\n', prob(1:5) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  An automatic pause of 2 seconds is imposed between \n;' );
  fprintf ( 1, '  successive displays.\n' );
%
%  Initialize C1.
%
  [ c1, seed ] = ising_2d_initialize ( m, n, thresh, seed );
%
%  Do the simulation.
%
  [ c1, seed ] = transition ( m, n, iterations, prob, seed, c1 );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'ISING_2D_SIMULATION\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

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

  return
end
function i4mat_print ( m, n, a, title )

%*****************************************************************************80
%
%% I4MAT_PRINT prints an I4MAT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 September 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns of the matrix.
%
%    Input, integer A(M,N), an M by N matrix to be printed.
%
%    Input, string TITLE, a title.
%
  i4mat_print_some ( m, n, a, 1, 1, m, n, title );

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

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

  fprintf ( 1, '\n' );
  fprintf ( 1, '%s\n', title );

  if ( m <= 0 || n <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  (None)\n' );
    return
  end

  for j2lo = max ( jlo, 1 ): incx : min ( jhi, n )

    j2hi = j2lo + incx - 1;
    j2hi = min ( j2hi, n );
    j2hi = min ( j2hi, jhi );

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

    for j = j2lo : j2hi
      fprintf ( 1, '%7d  ', j );
    end

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

    i2lo = max ( ilo, 1 );
    i2hi = min ( ihi, m );

    for i = i2lo : i2hi

      fprintf ( 1, '%5d: ', i );
      for j = j2lo : j2hi
        fprintf ( 1, '%7d  ', a(i,j) );
      end

      fprintf ( 1, '\n' );

    end

  end

  return
end
function [ c, seed ] = i4mat_uniform ( m, n, a, b, seed )

%*****************************************************************************80
%
%% I4MAT_UNIFORM returns a scaled pseudorandom I4MAT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 November 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Paul Bratley, Bennett Fox, Linus Schrage,
%    A Guide to Simulation,
%    Springer Verlag, pages 201-202, 1983.
%
%    Pierre L'Ecuyer,
%    Random Number Generation,
%    in Handbook of Simulation,
%    edited by Jerry Banks,
%    Wiley Interscience, page 95, 1998.
%
%    Bennett Fox,
%    Algorithm 647:
%    Implementation and Relative Efficiency of Quasirandom
%    Sequence Generators,
%    ACM Transactions on Mathematical Software,
%    Volume 12, Number 4, pages 362-376, 1986.
%
%    Peter Lewis, Allen Goodman, James Miller
%    A Pseudo-Random Number Generator for the System/360,
%    IBM Systems Journal,
%    Volume 8, pages 136-143, 1969.
%
%  Parameters:
%
%    Input, integer M, N, the row and column dimensions of the matrix.
%
%    Input, integer A, B, the minimum and maximum acceptable values.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, integer C(M,N), the randomly chosen integer vector.
%
%    Output, integer SEED, the updated seed.
%
  if ( seed == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4MAT_UNIFORM - Fatal error!\n' );
    fprintf ( 1, '  Input SEED = 0!\n' );
    error ( 'I4MAT_UNIFORM - Fatal error!' );
  end

  seed = floor ( seed );
  a = round ( a );
  b = round ( b );

  for j = 1 : n

    for i = 1 : m

      seed = mod ( seed, 2147483647 );

      if ( seed < 0 )
        seed = seed + 2147483647;
      end

      k = floor ( seed / 127773 );

      seed = 16807 * ( seed - k * 127773 ) - k * 2836;

      if ( seed < 0 )
        seed = seed + 2147483647;
      end

      r = seed * 4.656612875E-10;
%
%  Scale R to lie between A-0.5 and B+0.5.
%
      r = ( 1.0 - r ) * ( min ( a, b ) - 0.5 ) ...
        +         r   * ( max ( a, b ) + 0.5 );
%
%  Use rounding to convert R to an integer between A and B.
%
      value = round ( r );

      value = max ( value, min ( a, b ) );
      value = min ( value, max ( a, b ) );

      c(i,j) = value;

    end
  end

  return
end
function c5 = ising_2d_agree ( m, n, c1 )

%*****************************************************************************80
%
%% ISING_2D_AGREE returns the number of neighbors agreeing with each cell.
%
%  Discussion:
%
%    The count includes the cell itself, so it is between 1 and 5.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 November 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of cells in each spatial dimension.
%
%    Input, integer C1(M,N), an array of 1's and -1's.
%
%    Output, integer C5(M,N), the number of neighbors that agree.
%    1, 2, 3, 4, or 5.
%
  c5 = c1 ...
     + circshift ( c1, [ -1,  0 ] ) ...
     + circshift ( c1, [ +1,  0 ] ) ...
     + circshift ( c1, [  0, -1 ] ) ...
     + circshift ( c1, [  0, +1 ] );

  i = find ( 0 < c1 );
  c5(i) = ( 5 + c5(i) ) / 2;

  i = find ( c1 < 0 );
  c5(i) = ( 5 - c5(i) ) / 2;

  return
end
function ising_2d_display ( step, m, n, c1 )

%*****************************************************************************80
%
%% ISING_2D_DISPLAY displays the current Ising status.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 November 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer STEP, the step number.
%
%    Input, integer M, N, the number of rows and columnss.
%
%    Input, integer C1(M,N), the status of each cell:
%    -1, display as a shade of red.
%    +1, display as a shade of blue.
%
  figure ( 1 )
%
%  Clear the graphics frame.
%
  clf
%
%  Determine the plot range.
%
  margin = 0.05;

  x_axes_min = 1.0 - 0.5 - margin;
  x_axes_max = m   + 0.5 + margin;
  y_axes_min = 1.0 - 0.5 - margin;
  y_axes_max = n   + 0.5 + margin;
%
%  Fill in the background with black.
%
  x1 = x_axes_min;
  x2 = x_axes_max;
  y1 = y_axes_min;
  y2 = y_axes_max;

  rgb = [ 0.5, 0.5, 0.5 ];

  fill ( [ x1, x2, x2, x1 ], [ y1, y1, y2, y2 ], rgb );
  hold on
%
%  Draw a square, representing the bed,
%  with most of the length and width, centered at (I,J).
%
  for i = 1 : m
    for j = 1 : n

      x1 = j - 0.47;
      x2 = j + 0.47;
      y1 = ( m + 1 - i ) - 0.47;
      y2 = ( m + 1 - i ) + 0.47;

      if ( c1(i,j) == - 1 )
        rgb = [ 1.0, 0.0, 0.0 ];
      elseif ( c1(i,j) == + 1 )
        rgb = [ 0.0, 0.0, 1.0 ];
      end

      fill ( [ x1, x2, x2, x1 ], [ y1, y1, y2, y2 ], rgb );
      hold on

    end
  end
%
%  Make a title.
%
  title_string = sprintf ( 'Ising charges +1/-1 on step %d', step );

  title ( title_string )
%
%  Choose the aspect ratio and other plot details.
%
  axis ( [ x_axes_min, x_axes_max, y_axes_min, y_axes_max] );
  axis equal
  axis tight
  axis off

  hold off

  pause ( 2 )

  return
end
function [ c1, seed ] = ising_2d_initialize ( m, n, thresh, seed )

%*****************************************************************************80
%
%% ISING_2D_INITIALIZE initializes the Ising array.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 November 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, real THRESH, the threshhold value, between 0 and 1.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, integer C1(M,N), an array of 1's and -1's.
%
%    Output, integer SEED, a seed for the random number generator.
%
  c1 = ones ( m, n );

  [ r, seed ] = r8mat_uniform_01 ( m, n, seed );

  i = find ( r <= thresh );

  c1(i) = -1;

  return
end
function ising_2d_stats ( step, m, n, c1 )

%*****************************************************************************80
%
%% ISING_2D_STATS prints information about the current step.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 November 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer STEP, the step number.
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, integer C1(M,N), the current state of the system.
%
  if ( step == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Step       Positives       Negatives\n' );
    fprintf ( 1, '             #    %%          #    %%\n' );
    fprintf ( 1, '\n' );
  end

  pos_count = sum ( sum ( 0 < c1 ) );
  neg_count = m * n - pos_count;
  pos_percent = ( 100 * pos_count ) / ( m * n );
  neg_percent = ( 100 * neg_count ) / ( m * n );

  fprintf ( 1, '  %4d  %6d  %6.2f  %6d  %6.2f\n', ...
    step, pos_count, pos_percent, neg_count, neg_percent );

  return
end
function neighbor_2d_display ( step, m, n, c1, c5 )

%*****************************************************************************80
%
%% NEIGHBOR_2D_DISPLAY displays the current Ising neighbor status.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 November 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer STEP, the step number.
%
%    Input, integer M, N, the number of rows and columnss.
%
%    Input, integer C1(M,N), the status of each cell:
%
%    Input, integer C5(M,N), the number of agreeable neighbors.
%
  figure ( 2 )
%
%  Clear the graphics frame.
%
  clf
%
%  Determine the plot range.
%
  margin = 0.05;

  x_axes_min = 1.0 - 0.5 - margin;
  x_axes_max = m   + 0.5 + margin;
  y_axes_min = 1.0 - 0.5 - margin;
  y_axes_max = n   + 0.5 + margin;
%
%  Fill in the background with black.
%
  x1 = x_axes_min;
  x2 = x_axes_max;
  y1 = y_axes_min;
  y2 = y_axes_max;

  rgb = [ 0.5, 0.5, 0.5 ];

  fill ( [ x1, x2, x2, x1 ], [ y1, y1, y2, y2 ], rgb );
  hold on
%
%  Draw a square, representing the bed,
%  with most of the length and width, centered at (I,J).
%
  for i = 1 : m
    for j = 1 : n

      x1 = j - 0.47;
      x2 = j + 0.47;
      y1 = ( m + 1 - i ) - 0.47;
      y2 = ( m + 1 - i ) + 0.47;

      c = c1(i,j) * c5(i,j);

      if ( c == - 5 )
        rgb = [ 1.0, 0.0, 0.0 ];
      elseif ( c == - 4 )
        rgb = [ 1.0, 0.2, 0.2 ];
      elseif ( c == - 3 )
        rgb = [ 1.0, 0.4, 0.4 ];
      elseif ( c == - 2 )
        rgb = [ 1.0, 0.7, 0.7 ];
      elseif ( c == - 1 )
        rgb = [ 1.0, 0.8, 0.8 ];
      elseif ( c == 0 )
        rgb = [ 1.0, 1.0, 1.0 ];
      elseif ( c == + 1 )
        rgb = [ 0.7, 0.7, 1.0 ];
      elseif ( c == + 2 )
        rgb = [ 0.6, 0.6, 1.0 ];
      elseif ( c == + 3 )
        rgb = [ 0.4, 0.4, 1.0 ];
      elseif ( c == + 4 )
        rgb = [ 0.2, 0.2, 1.0 ];
      elseif ( c == + 5 )
        rgb = [ 0.0, 0.0, 1.0 ];
      end

      fill ( [ x1, x2, x2, x1 ], [ y1, y1, y2, y2 ], rgb );
      hold on

    end
  end
%
%  Make a title.
%
  title_string = sprintf ( 'Ising neighborhoods -5 to +5, step %d to step %d', step-1, step );

  title ( title_string )
%
%  Choose the aspect ratio and other plot details.
%
  axis ( [ x_axes_min, x_axes_max, y_axes_min, y_axes_max] );
  axis equal
  axis tight
  axis off

  hold off

  pause ( 2 )

  return
end
function neighbor_2d_stats ( step, m, n, c1, c5 )

%*****************************************************************************80
%
%% NEIGHBOR_2D_STATS prints neighbor statistics about the current step.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 November 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer STEP, the step number.
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, integer C1(M,N), the current state of the system.
%
%    Input, integer C5(M,N), the number of agreeable neighbors.
%
  if ( step == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Step     Neighborhood Charge:\n' );
    fprintf ( 1, ...
      '           -5    -4    -3    -2    -1    +1    +2    +3    +4    +5\n' );
    fprintf ( 1, '\n' );
  end

  fprintf ( 1, '  %4d', step );

  for n = -5 : 5
    if ( n ~= 0 )
      c = sum ( sum ( c1 .* c5 == n ) );
      fprintf ( 1, '  %4d', c );
    end
  end
  fprintf ( 1, '\n' );

  return
end
function [ r, seed ] = r8mat_uniform_01 ( m, n, seed )

%*****************************************************************************80
%
%% R8MAT_UNIFORM_01 returns a unit pseudorandom R8MAT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Paul Bratley, Bennett Fox, Linus Schrage,
%    A Guide to Simulation,
%    Springer Verlag, pages 201-202, 1983.
%
%    Bennett Fox,
%    Algorithm 647:
%    Implementation and Relative Efficiency of Quasirandom
%    Sequence Generators,
%    ACM Transactions on Mathematical Software,
%    Volume 12, Number 4, pages 362-376, 1986.
%
%    Peter Lewis, Allen Goodman, James Miller,
%    A Pseudo-Random Number Generator for the System/360,
%    IBM Systems Journal,
%    Volume 8, pages 136-143, 1969.
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns in the array.
%
%    Input, integer SEED, the integer "seed" used to generate
%    the output random number.
%
%    Output, real R(M,N), an array of random values between 0 and 1.
%
%    Output, integer SEED, the updated seed.  This would
%    normally be used as the input seed on the next call.
%
  r = zeros ( m, n );

  if ( seed == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_UNIFORM_01 - Fatal error!\n' );
    fprintf ( 1, '  Input SEED = 0!\n' );
    error ( 'R8MAT_UNIFORM_01 - Fatal error!' );
  end

  for j = 1 : n
    for i = 1 : m

      seed = floor ( seed );

      seed = mod ( seed, 2147483647 );

      if ( seed < 0 )
        seed = seed + 2147483647;
      end

      k = floor ( seed / 127773 );

      seed = 16807 * ( seed - k * 127773 ) - k * 2836;

      if ( seed < 0 )
        seed = seed + 2147483647;
      end

      r(i,j) = seed * 4.656612875E-10;

    end
  end

  return
end
function timestamp ( )

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

  return
end
function [ c1, seed ] = transition ( m, n, iterations, prob, seed, c1 )

%*****************************************************************************80
%
%% TRANSITION carries out a Monte Carlo simulation of a 2D Ising model.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 November 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of cells in each spatial
%    dimension.
%
%    Input, integer ITERATIONS, the number of iterations to carry out.
%
%    Input, real PROB(1:5).  PROB(I) represents the probability
%    that the spin of a given cell will be reversed, given that it has I immediate
%    neighbors (including itself) with spin the same as its own.
%
%    Input/output, integer SEED, a seed for the random number generator.
%
%    Input/output, integer C1(M,N).  On input, the current state of the
%    system.  On output, the state of the system after the iterations.
%
  step = 0;
  ising_2d_stats ( step, m, n, c1 );
  ising_2d_display ( step, m, n, c1 );

  for step = 1 : iterations
%
%  C5 contains 1 through 5, the number of cells that agree with the center cell.
%
    c5 = ising_2d_agree ( m, n, c1 );

    if ( 0 )
      neighbor_2d_stats ( step, m, n, c1, c5 );
    end

    if ( 0 )
      neighbor_2d_display ( step, m, n, c1, c5 );
    end
%
%  Determine the chances of flipping cell (I,J).
%
    threshhold = zeros ( m, n );

    for j = 1 : 5
      i = find ( c5 == j );
      threshhold(i) = prob(j);
    end

    [ r, seed ] = r8mat_uniform_01 ( m, n, seed );

    i = find ( r < threshhold );

    c1(i) = - c1(i);

    ising_2d_stats ( step, m, n, c1 );
    ising_2d_display ( step, m, n, c1 );

  end

  return
end


function value = i4_characteristic ( q )

%*****************************************************************************80
%
%% I4_CHARACTERISTIC gives the characteristic for an integer.
%
%  Discussion:
%
%    For any positive integer Q, the characteristic is:
%
%    Q, if Q is a prime;
%    P, if Q = P^N for some prime P and some integer N;
%    0, otherwise, that is, if Q is negative, 0, 1, or the product
%       of more than one distinct prime.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 January 2005
%
%  Author:
%
%    FORTRAN77 original version by Bratley, Fox, Niederreiter.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Paul Bratley, Bennett Fox, Harald Niederreiter,
%    Algorithm 738:
%    Programs to Generate Niederreiter's Low-Discrepancy Sequences,
%    ACM Transactions on Mathematical Software,
%    Volume 20, Number 4, pages 494-495, 1994.
%
%  Parameters:
%
%    Input, integer Q, the value to be tested.
%
%    Output, integer VALUE, the characteristic of Q.
%
  if ( q <= 1 )
    value = 0;
    return
  end
%
%  If Q is not prime, then there is at least one prime factor
%  of Q no greater than SQRT(Q)+1.
%
%  A faster code would only consider prime values of I,
%  but that entails storing a table of primes and limiting the
%  size of Q.  Simplicity and flexibility for now!
%
  i_max = floor ( sqrt ( q ) ) + 1;

  for i = 2 : i_max

    if ( mod ( q, i ) == 0 )

      while ( mod ( q, i ) == 0 )
        q = q / i;
      end

      if ( q == 1 )
        value = i;
      else
        value = 0;
      end

      return

    end

  end
%
%  If no factor was found, then Q is prime.
%
  value = q;

  return
end


function [A1, B1, C1, A2, B2, C2] = sss(a, b, c)
%SSS   gives both solutions to the side-side-side problem, in radians.
%
%   SSS(a, b, c) results in NaNs for those indices where the existence
%   condition |pi - a| - |pi - b| <= |pi - c| <= |pi - a| + |pi -b| is not
%   met.
%
%   See also SSSD.

% Rody P.S. Oldenhuis
% Delft University of Technology
% oldenhuis@gmail.com
%
% Created    : 23/Feb/2009
% Last edited: 30/Nov/2012

    % first solution
    A1 = acos2( (cos(a) - cos(b).*cos(c))./(sin(b).*sin(c)), a);
    B1 = acos2( (cos(b) - cos(a).*cos(c))./(sin(a).*sin(c)), b);
    C1 = acos2( (cos(c) - cos(a).*cos(b))./(sin(a).*sin(b)), c);

    % second solution
    A2 = 2*pi - A1;
    B2 = 2*pi - B1;
    C2 = 2*pi - C1;

    % check constraints
    indices = ( ...
        (abs(pi-a) - abs(pi-b)) > abs(pi-c) | ...
        abs(pi-c) > (abs(pi-a) + abs(pi-b)) );
    A1(indices) = NaN;   B1(indices) = NaN;   C1(indices) = NaN;
    A2(indices) = NaN;   B2(indices) = NaN;   C2(indices) = NaN;

end


function proj2d = projection(data3d,param, angle_rad)

proj2d = zeros(param.nu,param.nv,'single');

[uu,vv] = meshgrid(param.us,param.vs);

[xx,yy] = meshgrid(param.xs,param.ys);

rx = xx.*cos(angle_rad) - yy.*sin(angle_rad);
ry = xx.*sin(angle_rad) + yy.*cos(angle_rad);

for iz = 1:param.nz

    data3d(:,:,iz) = interp2(xx,yy ,data3d(:,:,iz),rx,ry,'linear');

end

data3d(isnan(data3d))=0;

data3d = permute(data3d,[1 3 2]);

[xx,zz] = meshgrid(param.xs,param.zs);

for iy = 1:param.ny

    Ratio = (param.ys(iy)+param.DSO)/(param.DSD);

    pu = uu*Ratio;
    pv = vv*Ratio;

    pu = (pu - xx(1,1))/(param.dx)+1;
    pv = (pv - zz(1,1))/(param.dz)+1;

    tmp = interp2(data3d(:,:,iy),pv,pu,'linear');

    tmp(isnan(tmp))=0;

    proj2d = proj2d + tmp';
end

dist = sqrt((param.DSD)^2 + uu.^2 + vv.^2)./(param.DSD)*param.dy;

proj2d = proj2d .* dist';


function value = scnrm2 ( n, x, incx )

%*****************************************************************************80
%
%% SCNRM2 returns the euclidean norm of a complex vector.
%
%  Discussion:
%
%    SCNRM2 := sqrt ( sum ( conjg ( x(1:n) ) * x(1:n) ) )
%            = sqrt ( dot_product ( x(1:n), x(1:n) ) )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 April 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1,
%    LC: QA214.L56.
%
%    Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
%    Basic Linear Algebra Subprograms for FORTRAN usage,
%    ACM Transactions on Mathematical Software,
%    Volume 5, Number 3, pages 308-323, 1979.
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Input, complex X(*), the vector.
%
%    Input, integer INCX, the increment between successive entries of X.
%
%    Output, real VALUE, the norm of the vector.
%
  value = sqrt ( sum ( ( real ( x(1:incx:1+(n-1)*incx) ) ).^2 ) ...
               + sum ( ( imag ( x(1:incx:1+(n-1)*incx) ) ).^2 ) );

  return
end


classdef hessenberg
% Functions to work with Hessenberg forms

methods(Static)

function [Q, R] = qr(A)
    % Givens method for computing the QR factorization A = QR of a Hessenberg matrix.
    % A is modified in the algorithm
    % GVL4: algorithm 5.2.5
    import spx.la.givens.rotation;
    [n,n] = size(A);
    % Space for saving the Q factor
    Q = eye(n, n);
    % iterate over each column from first to last but one.
    for j=1:n-1
        % We need to process only one pair of rows
        % The diagonal element and immediate sub-diagonal element.
        a = A(j,j);
        b = A(j+1,j);
        % Compute the needed rotation for making b 0.
        [c,s] = rotation(a,b);
        % Form the givens rotation matrix.
        G = [c s;-s c];
        % Rotate the two consecutive rows based submatrix
        A(j:j+1,j:n) = G'*A(j:j+1,j:n);
        % Rotate corresponding columns n Q matrix
        % by postmultiplication with G
        Q(:,j:j+1) = Q(:,j:j+1)*G;
    end
    % Return the triangular form
    R = A;
end % function

function [H, cs, ss] = qr_rq(H)
    % Takes a Hessenberg matrix H = QR and returns RQ
    % GVL4: algorithm 7.4.1
    import spx.la.givens.rotation;
    [n,n] = size(H);
    % Space for storing the c and s values
    cs = zeros(1, n);
    ss = zeros(1, n);
    % first the QR factorization
    % iterate over each column from first to last but one.
    for k=1:n-1
        % We need to process only one pair of rows
        % The diagonal element and immediate sub-diagonal element.
        a = H(k,k);
        b = H(k+1,k);
        % Compute the needed rotation for making b 0.
        [c,s] = rotation(a,b);
        % Store the rotation
        cs(k) = c;
        ss(k) = s;
        % Form the givens rotation matrix.
        G = [c s;-s c];
        % Rotate the two consecutive rows based submatrix
        H(k:k+1,k:n) = G'*H(k:k+1,k:n);
    end
    % Now the RQ formation
    for k=1:n-1
        G = [cs(k) ss(k); -ss(k) cs(k)];
        H(1:k+1,k:k+1) = H(1:k+1,k:k+1) * G;
    end
    % The product of these Givens rotations is also upper Hessenberg.
    % Product of two Hessenberg matrices is Hessenberg.
end % function

function [H, U0] = hess(A)
    % Reduction of a square matrix A into Hessenberg form
    % using Householder reflections
    % GVL4: algorithm 7.4.2
    [n,n] = size(A);
    import spx.la.house;
    % Iterate over columns of A
    for k=1:n-2
        % Compute the householder reflector for k-th column
        % covering only sub-diagonal elements
        [v,beta] = house.gen(A(k+1:n,k));
        % Update the submatrix of A by premultiplication with the Projector
        A(k+1:n,k:n) = A(k+1:n,k:n) - (beta*v)*(v'*A(k+1:n,k:n));
        % Postmultiply with the same projector without affecting
        % the 0's in the k-th column
        A(:,k+1:n) = A(:,k+1:n) - (A(:,k+1:n)*v)*(beta*v)';
        % Store the householder reflection vector
        A(k+2:n,k) = v(2:n-k);
    end
    % Extract the Hessenberg form
    H = triu(A,-1);
    if nargout > 1
        % Extract the orthogonal matrix U_0 such that
        % A = U_0 H U_0^T
        U0 = eye(n,n);
        U0(2:n,2:n) = house.q_back_accum_full(A(2:n,1:n-1));
    end
end % function

function [H, i1, i2] = backsearch(H, z)
    % Searches for a subdiagonal entry in H which is
    % negligible. From last row to first row.
    if nargin < 2
        z = size(H, 2);
    end
    i1 = z;
    i2 = z;
    % Compute the Frobenius norm of H
    h_norm = norm(H, 'fro');
    % The threshold for negligible entries
    thr = eps * h_norm
    % iterate over diagonal entries backwards
    while i1 >1
        % check for negligible subdiagonal entry.
        fprintf('%d, %d, %e\n', i1, i1-1, H(i1, i1-1));
        if (abs(H(i1, i1-1)) < thr )
            % Make this entry zero
            H(i1, i1-1) = 0;
            if (i1 == i2)
                i2 = i1 - 1;
                i1 = i1 - 1;
            else
                return;
            end
        else
            i1 = i1 - 1;
        end

    end
end

end % methods

end % classdef


function y=rdct(x,n,a,b)
%RDCT     Discrete cosine transform of real data Y=(X,N,A,B)
% Data is truncated/padded to length N.
%
% This routine is equivalent to multiplying by the matrix
%
%   rdct(eye(n)) = diag([sqrt(2)*B/A repmat(2/A,1,n-1)]) * cos((0:n-1)'*(0.5:n)*pi/n)
%
% Default values of the scaling factors are A=sqrt(2N) and B=1 which
% results in an orthogonal matrix. Other common values are A=1 or N and/or B=1 or sqrt(2).
% If b~=1 then the columns are no longer orthogonal.
%
% see IRDCT for the inverse transform

% BUG: in line 51 we should do chopping after transform and not before

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: rdct.m,v 1.6 2007/05/04 07:01:39 dmb Exp $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program 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 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

fl=size(x,1)==1;
if fl x=x(:); end
[m,k]=size(x);
if nargin<2 n=m;
end
if nargin<4 b=1;
    if nargin<3 a=sqrt(2*n);
    end
    end
if n>m x=[x; zeros(n-m,k)];
elseif n<m x(n+1:m,:)=[];
end

x=[x(1:2:n,:); x(2*fix(n/2):-2:2,:)];
z=[sqrt(2) 2*exp((-0.5i*pi/n)*(1:n-1))].';
y=real(fft(x).*z(:,ones(1,k)))/a;
y(1,:)=y(1,:)*b;
if fl y=y.'; end


function [prices, stdErrs] = Price_MC_Hindsight_Strikes_func(Spath, call, strikes, M, mult, disc)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% About: Calculates Hindsight option prices for vector of strikes, given the simulatd paths
% Author: Justin Lars Kirkby
%
% -----------------
% Params
% -----------------
% Contract Types
% Fixed Strike (Hindsight) Put: (W - min{S_m: 0<=m<=M})^+
% Fixed Strike (Hindsight) Call: (max{S_m: 0<=m<=M} - W)^+
%
% Spath = paths of underlying, dimension N_sim x M+1, where M = number of time steps (since includes S_0)
% call = 1 for call (else put)
% Kvec = strike vector
% M = number of monitoring points, e.g. 252 for "daily" monitoring
% mult = time partitioning multiplier to reduce bias (e.g. mult = 2 or 5)
% S_0 = initial price
% disc = discount factor (e.g. exp(-r*T))
% T = Time (in years)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

N_sim = size(Spath,1);  % number of paths

M_mult = M*mult;  %time partitioning to reduce bias

prices = zeros(length(strikes),1);
stdErrs = zeros(length(strikes),1); %TODO: add code to return std errros

if call == 1
    curr_max = zeros(N_sim, 1);
    for n = 1:N_sim
        curr_max(n) = max(Spath(n, 1:mult:M_mult+1));
    end
else  % find_min
    curr_min = zeros(N_sim, 1);
    for n = 1:N_sim
        curr_min(n) = min(Spath(n, 1:mult:M_mult+1));
    end
end

for k = 1:length(strikes)
    K = strikes(k);
    if call ==1  % Fixed Strike (Hindsight) Call: (max{S_m: 0<=m<=M} - W)^+
       payoffs  = max(0, curr_max - K);

    else  % Fixed Strike (Hindsight) Put: (W - min{S_m: 0<=m<=M})^+
        payoffs  = max(0, K - curr_min);
    end
    prices(k) = disc*mean(payoffs);
    stdErrs(k) = disc*std(payoffs) / sqrt(N_sim);

end

end


function rate_na_lt = awgn_na_lt(snr_db,nn,error)

P=10^(snr_db/10);

rate_na_lt=zeros(1,length(nn));

%find the minimum blocklength required for the long-term power constraint to be beneficial
C=log(1+P);
V = 1-1/(1+P)^2;
n_min = ( (1+P)/P*sqrt(2*3.1415926*V)*(1-error)*exp(qfuncinv(error)^2/2) + qfuncinv(error)/(1+P)^2/sqrt(V))^2;

n_index=find(nn <= n_min);

% for every n<=n_min, the normal approx. for the st power constraint coincides
if length(n_index)>0
    rat_na_lt(n_index) = (log(1+P) - sqrt(1-(1+P)^(-2))./sqrt(nn(n_index)) + 0.5*log(nn(n_index))./nn(n_index))/log(2);
end

R_st = C - sqrt(V./nn)*qfuncinv(error);
C_lt=log(1+P/(1-error));

index_large_n = find(nn>n_min);

for ii=index_large_n
    n=nn(ii);
    for R_star = R_st(ii):0.0001:C_lt
        omega = P: 0.00001:P*1.2; %note that larger endpoing for omega (P*1.2) gives a better estimate for rate_na;

        q_arg=sqrt(n)*(log(1+omega)-R_star)./sqrt(1-(1+omega).^(-2)) ;

        error_temp = min( 1-P./omega + P.*qfunc( q_arg)./omega);
        if error_temp> error
            rate_na_lt(ii)= (R_star +0.5*log(n)./n)./log(2) ;
            break
        end
    end
end


function value = p16_exact ( dim_num )

%*****************************************************************************80
%
%% P16_EXACT returns the exact integral for problem 16.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 March 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Output, real VALUE, the exact value of the integral.
%

%
%  Get the limits of integration.
%
  [ a, b ] = p16_lim ( dim_num );
%
%  Get the location of Z.
%
  z = [];
  z = p16_r8vec ( 'G', 'Z', dim_num, z );
%
%  The value of the DIM_NUM dimensional integral can be broken down
%  into the weighted sum of 1 dimensional integrals.
%
  value = 0.0;

  volume = prod ( b(1:dim_num) - a(1:dim_num) );

  for i = 1 : dim_num
%
%  Z < A < B
%
    if ( z(i) < a(i) )

      integral = 0.5 * ( b(i) - a(i) ) * ( b(i) + a(i) - 2.0 * z(i) );
%
%  A < Z < B
%
    elseif ( z(i) < b(i) )

      integral = 0.5 * ( ( b(i) - z(i) )^2 + ( z(i) - a(i) )^2 );
%
%  A < B < Z
%
    else

      integral = 0.5 * ( b(i) - a(i) ) * ( 2.0 * z(i) - a(i) - b(i) );

    end

    value = value + volume * integral / ( b(i) - a(i) );

  end

  return
end


function f = goldstein_price_xy ( x, y )

%*****************************************************************************80
%
%% GOLDSTEIN_PRICE_XY evaluates the Goldstein-Price polynomial.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 February 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Zbigniew Michalewicz,
%    Genetic Algorithms + Data Structures = Evolution Programs,
%    Third Edition,
%    Springer Verlag, 1996,
%    ISBN: 3-540-60676-9,
%    LC: QA76.618.M53.
%
%  Parameters:
%
%    Input, real X, Y, the arguments of the function.
%
%    Output, real F, the value of the function at X.
%
  a = x + y + 1.0;

  b = 19.0 - 14.0 * x + 3.0 * x * x - 14.0 * y ...
    + 6.0 * x * y + 3.0 * y * y;

  c = 2.0 * x - 3.0 * y;

  d = 18.0 - 32.0 * x + 12.0 * x * x + 48.0 * y ...
    - 36.0 * x * y + 27.0 * y * y;

  f = ( 1.0 + a * a * b ) * ( 30.0 + c * c * d );

  return
end


function v = fdm_2d_vector(n0,f_str)
%
%  Generates a vector v which contains the values of a function f(x,y)
%  on an equidistant grid in the interior of the unit square. The grid
%  points are numbered consistently with those used in the function
%  'fdm_2d_matrix'.
%
%  This function is just used as an easy way to generate test problems
%  rather than to solve PDEs.
%
%  Calling sequence:
%
%    v = fdm_2d_vector( n0, f_str)
%
%  Input:
%
%    n0        number of inner grid points in each dimension;
%    f_str     string describing the function f in the space variables 'x'
%              and 'y', e.g., f_str = 'sin(x+2*y)+3'.
%
%  Output:
%
%    v         vector of order n = n0^2 containing the values of f(x,y).
%
%
%  LYAPACK 1.0 (Thilo Penzl, May 1999)

% Input data not completely checked!

na = nargin;

if na~=2
  error('Wrong number of input parameters.');
end

h = 1.0/(n0+1);

n2 = n0*n0;

v = zeros(n2,1);

i = 0;

for iy = 1:n0
  y = iy*h;
  for ix = 1:n0
    x = ix*h;
    i = i+1;
    v(i) = eval(f_str);
  end
end


function an=euler2axan(phi,theta,psi,M)
% From Euler angles (phi,theta,psi) to axis angle (x y z alpha)/(gamma,delta):

% v=[(cos(al-gm)+1)*(1-cos(bt));
%    sin(al-gm)*(1-cos(bt));
%    sin(bt)*(sin(al)+sin(gm))]

cost=cos(theta);
sint=cos(theta);
omcost=(1-cost);
if sint==0
    x=0;
    y=0;
    z=1;
else
    x = (cos(phi-psi)+1)*omcost;
    y = sin(phi-psi)*omcost;
    z = sint*(sin(phi)+sin(psi));
end

if (x==0)&&(y==0)&&(z==0)
    x=1;
    y=0;
    z=0;
else
    vn=sqrt(x^2+y^2+z^2);
    x=x/vn;
    y=y/vn;
    z=z/vn;
end

v=[x y z]';

%[THETA,PHI,R] = cart2sph(X,Y,Z)

[gamma,delta,tmp] = cart2sph(x,y,z);

cppp=cos(phi+psi);
Tr=cppp+cppp*cost+cost;
alpha=atan2(-(M(1,2)*v(3)+M(2,3)*v(1)+M(3,1)*v(2)-M(1,3)*v(2)-M(3,2)*v(1)-M(2,1)*v(3)),Tr-1);
% if (-(M(1,2)*v(3)+M(2,3)*v(1)+M(3,1)*v(2)-M(1,3)*v(2)-M(3,2)*v(1)-M(2,1)*v(3)))>=0
%     alpha=acos((Tr-1)/2);
% else
%     alpha=-acos((Tr-1)/2);
% end

an={{gamma,delta,alpha},v};


function [Y,T]=derivs(x,dim)
%Syntax: [Y,T]=derivs(x,dim)
%____________________________
%
% The phase space reconstruction of a time series x whith the derivatives approach,
% in embedding dimension m.
%
% Y is the trajectory matrix in the reconstructed phase space.
% T is the phase space length.
% x is the time series.
% dim is the embedding dimension.
%
%
% Reference:
%
% Packard N H, Cruchfield J P, Farmer J D, Shaw R S (1980): Geometry from a Time
% Series. Physical Review Letters 45: 712-715
%
%
% Alexandros Leontitsis
% Department of Education
% University of Ioannina
% 45110 - Dourouti
% Ioannina
% Greece
%
% University e-mail: me00743@cc.uoi.gr
% Lifetime e-mail: leoaleq@yahoo.com
% Homepage: http://www.geocities.com/CapeCanaveral/Lab/1421
%
% 11 Mar 2001.

if nargin<1 | isempty(x)==1
   error('You should provide a time series.');
else
   % x must be a vector
   if min(size(x))>1
      error('Invalid time series.');
   end
   x=x(:);
   % N is the time series length
   N=length(x);
end

if nargin<2 | isempty(dim)==1
    dim=2;
else
    % dim must be scalar
    if sum(size(dim))>2
        error('dim must be scalar.');
    end
    % dim must be an integer
    if dim-round(dim)~=0
        error('dim must be an integer.');
    end
    % dim must be positive
    if dim<=0
        error('dim must be positive.');
    end
end

% Initialize the phase space
Y=zeros(N,dim);

% Phase space reconstruction with the derivatives approach
Y(:,1)=x;
for i=2:dim
    Y(:,i)=[diff(Y(:,i-1));0];
end

% Total points on phase space
T=N-(dim-1);

% Remove the meaningless points
Y(T+1:end,:)=[];


function bvec2 = bvec_complement2 ( n, bvec1 )

%*****************************************************************************80
%
%% BVEC_COMPLEMENT2 computes the two's complement of a binary vector.
%
%  Discussion:
%
%    A BVEC is an integer vector of binary digits, intended to
%    represent an integer.  BVEC(1) is the units digit, BVEC(N-1)
%    is the coefficient of 2^(N-2), and BVEC(N) contains sign
%    information.  It is 0 if the number is positive, and 1 if
%    the number is negative.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 November 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the length of the vectors.
%
%    Input, integer BVEC1(N), the vector to be complemented.
%
%    Output, integer BVEC2(N), the two's complemented vector.
%
  base = 2;

  bvec3(1:n) = ( base - 1 ) - bvec1(1:n);

  bvec4(1) = 1;
  bvec4(2:n) = 0;

  bvec2 = bvec_add ( n, bvec3, bvec4 );

  return
end


function fd1d_heat_implicit_test03 ( )

%*****************************************************************************80
%
%% FD1D_HEAT_IMPLICIT_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_IMPLICIT_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_implicit_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 );
%
%  Get the system matrix.
%
  a = fd1d_heat_implicit_matrix ( x_num, cfl );

  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_implicit ( a, 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\_IMPLICIT' );
  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 jacobi_eigenvalue_test02 ( )

%*****************************************************************************80
%
%% JACOBI_EIGENVALUE_TEST02 uses a 4x4 test matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 July 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'JACOBI_EIGENVALUE_TEST02\n' );
  fprintf ( 1, '  For a symmetric matrix A,\n' );
  fprintf ( 1, '  JACOBI_EIGENVALUE computes the eigenvalues D\n' );
  fprintf ( 1, '  and eigenvectors V so that A * V = D * V.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  As a "sanity check", input a diagonal matrix.\n' );

  n = 4;

  a = [ ...
      4.0, 0.0, 0.0, 0.0;
      0.0, 1.0, 0.0, 0.0;
      0.0, 0.0, 3.0, 0.0;
      0.0, 0.0, 0.0, 2.0 ];

  r8mat_print ( n, n, a, '  Input matrix A:' );

  it_max = 100;

  [ v, d, it_num, rot_num ] = jacobi_eigenvalue ( n, a, it_max );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of iterations = %d\n', it_num );
  fprintf ( 1, '  Number of rotations  = %d\n', rot_num );

  r8vec_print ( n, d, '  Eigenvalues D:' );

  r8mat_print ( n, n, v, '  Eigenvector matrix V:' );
%
%  Compute eigentest.
%
  error_frobenius = r8mat_is_eigen_right ( n, n, a, v, d );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Frobenius norm error in eigensystem A*V-D*V = %g\n', ...
    error_frobenius );

  return
end


function f_lim = sphArrayNoiseThreshold(R, Nmic, maxG_db, maxN, arrayType, dirCoeff)
%SPHARRAYNOISETHRESHOLD Returns freq.limits for noise amplification in an SMA
%
%   SPHARRAYNOISETHRESHOLD returns the frequencies that the noise in the
%   output channels of a SMA, after performing the SHT and equalization of
%   the output signals, reaches a certain user-defined threshold maxG_db.
%   The frequencies are computed only at the lower range of each order,
%   where its response decays rapidly, ignoring for example the nulls of an
%   open array at the higher frequencies. The estimation of the limits are
%   based on a linear approximation of the log-log response found e.g. in
%
%       Sector-based Parametric Sound Field Reproduction in the Spherical Harmonic Domain
%       A Politis, J Vilkamo, V Pulkki
%       IEEE Journal of Selected Topics in Signal Processing 9 (5), 852 - 866
%
%   Inputs:
%         R:          array radius
%         Nmic:       number of microphones
%         maxG_db:    max allowed amplification for the noise level,
%               maxG_db = 20*log10(maxG)
%         maxN:       maximum order that the array supports
%         arrayType:  'open', 'rigid' or 'directional' for an open or rigid spherical
%               array of omnidirectional microphones, or for an array of
%               first-order directional microphones
%         dirCoeff: (optional) if arrayType=='directional', then dirCoeff
%               expresses the first-order directivity of microphones
%               from 0-1 (1:omni, 0.5:cardioid, 0:dipole)
%
%         f_lim:    frequency points that the threhsold is reached,
%               for orders 1:maxN
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% SPHARRAYNOISETHRESHOLD.M - 11/7/2013
% Archontis Politis, archontis.politis@aalto.fi
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin<6
    dirCoeff = [];
end

c = 343;
f_lim = zeros(1,maxN);
for n=1:maxN
    bn = sphModalCoeffs(n, 1, arrayType, dirCoeff)/(4*pi);
    bn = bn(end);
    maxG = 10^(maxG_db/10);
    kR_lim = (maxG*Nmic*abs(bn).^2)^(-10*log10(2)/(6*n));
    f_lim(n) = kR_lim*c/(2*pi*R);
end

end


function q = mat2quat(mat,varargin)
% converts direction cosine matrix to quaternion
%
% Syntax
%   q = mat2quat(mat)
%
% Input
%  mat - vector of matrixes
%
% Output
%  q - @quaternion
%
% See also
%
% quaternion_matrix Euler axis2quat hr2quat
%
% Description
% Wertz says to the algo similar to this with largest divisor
%       q4 = 1/2*sqrt((1+mat(1,1)+mat(2,2)+mat(3,3)));
% Eqn 12-14a - c
%  Quat(1) = (mat(2,3)-mat(3,2))/q4/4;
%  Quat(2) = (mat(3,1)-mat(1,3))/q4/4;
%  Quat(3) = (mat(1,2)-mat(2,1))/q4/4;
%  Quat(4) = q4

Quat=zeros(4,1);

%Compute absolute values of the four quaternions from diags of Eqn 12-13
%absQ=0.5*sqrt([1 -1 -1;-1 1 -1;-1 -1 1;1 1 1]*diag(mat)+1);

absQ(1,:) = 0.5 * sqrt(1+mat(1,1,:)+mat(2,2,:)+mat(3,3,:));
absQ(2,:) = 0.5 * sqrt(1-mat(1,1,:)-mat(2,2,:)+mat(3,3,:));
absQ(3,:) = 0.5 * sqrt(1+mat(1,1,:)-mat(2,2,:)-mat(3,3,:));
absQ(4,:) = 0.5 * sqrt(1-mat(1,1,:)+mat(2,2,:)-mat(3,3,:));

[~,ind]=max(absQ); % Select biggest for best accuracy

qind = ind == 1;
if any(qind)
  Quat(1,qind)=absQ(1,qind);
  Quat(2,qind)=squeeze((mat(2,3,qind)-mat(3,2,qind))).'.*0.25./absQ(1,qind);
  Quat(3,qind)=squeeze((mat(3,1,qind)-mat(1,3,qind))).'.*0.25./absQ(1,qind);
  Quat(4,qind)=squeeze((mat(1,2,qind)-mat(2,1,qind))).'.*0.25./absQ(1,qind);
end

qind = ind == 2;
if any(qind)
  Quat(1,qind)=squeeze(mat(1,2,qind)-mat(2,1,qind)).'.*0.25./absQ(2,qind);
  Quat(2,qind)=squeeze(mat(3,1,qind)+mat(1,3,qind)).'.*0.25./absQ(2,qind);
  Quat(3,qind)=squeeze(mat(3,2,qind)+mat(2,3,qind)).'.*0.25./absQ(2,qind);
  Quat(4,qind)=absQ(2,qind);
end

qind = ind == 3;
if any(qind)
  Quat(1,qind)=squeeze(mat(2,3,qind)-mat(3,2,qind)).'.*0.25./absQ(3,qind);
  Quat(2,qind)=absQ(3,qind);
  Quat(3,qind)=squeeze(mat(1,2,qind)+mat(2,1,qind)).'.*0.25./absQ(3,qind);
  Quat(4,qind)=squeeze(mat(3,1,qind)+mat(1,3,qind)).'.*0.25./absQ(3,qind);
end

qind = ind == 4;
if any(qind)
  Quat(1,qind)=squeeze(mat(3,1,qind)-mat(1,3,qind)).'.*0.25./absQ(4,qind);
  Quat(2,qind)=squeeze(mat(1,2,qind)+mat(2,1,qind)).'.*0.25./absQ(4,qind);
  Quat(3,qind)=absQ(4,qind);
  Quat(4,qind)=squeeze(mat(2,3,qind)+mat(3,2,qind)).'.*0.25./absQ(4,qind);
end

q = quaternion(real(Quat));
q = q./norm(q);
q = inv(q).';


%% DEMO_volumetric_SED_eval
% This demo was developed as part of the paper:
% Moerman et al. "Novel Hyperelastic Models for Large Volumetric
% Deformations".
%
% The demo features:
% * Implementations of hyperelastic volumetric strain energy density
% functions (SEDs)
% * Visualizations of the SED, hydrostatic stress, and tangent as a
% function of the volume ratio.

%% Keywords
%
% * Strain energy density
% * Volumetric
% * Visualization

%%

clear; close all; clc;

%% Plot settings
fontSize=20;
fontSizeInner=fontSize+15;
fontSizeLabel=fontSize+30;
plotColors=gjet(4);
plotColors=plotColors([1 2 4],:);

lineWidth=3;
gridAlpha=0.3;
LineWidthAxis=2;
legendHeight=0.05;
numXTicks=5;

%% Control parameters

formulationCases=1:12;  %Choose formulation 1:12
k=1; %Default bulk modulus (except for hyperfoam)

J_max=2;
numPoints=2000; %Number of points for plotting
J=linspace(0.1,J_max,numPoints)'; %The volume ratios
xtickRange=linspace(0,max(J),numXTicks); %X-axis tick range

%% Get or set formulation specific data and parameters

for formulationCase=formulationCases

    switch formulationCase
        case 1 %Hencky
            formulationName='Hencky';
            parSet(1)=k; %Bulk modulus
        case 2 %Simo
            formulationName='Simo';
            parSet(1)=k; %Bulk modulus
        case 3 %Bischoff
            formulationName='Bischoff';
            b=2; %Beta
            parSet(1)=k; %Bulk modulus
            parSet(2)=b; %Beta
        case 4 %Modified Ogden
            formulationName='Modified Ogden';
            b=2;%Beta
            parSet(1)=k; %Bulk modulus
            parSet(2)=b; %Beta
        case 5 %Hyperfoam
            formulationName='Hyperfoam';
            mu=1;
            a=2; %Alpha
            b=2; %Beta
            parSet(1)=mu; %Mu
            parSet(2)=a; %Alpha
            parSet(3)=b; %Beta
        case 6 %Doll and Schweizerhoff
            formulationName='Doll and Schweizerhoff';
            a=3; %Alpha
            b=2; %Beta
            parSet(1)=k; %Bulk modulus
            parSet(2)=a; %Alpha
            parSet(3)=b; %Beta
        case 7 %Montella et al.
            formulationName='Montella et al.';
            k1=1;
            beta1=1/8;
            k2=1;
            beta2=1/8;
            m=4;
            parSet(1)=k; %Bulk modulus
            parSet(2)=beta1;
            parSet(3)=k2;
            parSet(4)=beta2;
            parSet(5)=m;
        case 8 %Moerman 1
            formulationName='Moerman 1';
            b1=3;
            b2=2;
            parSet(1)=k; %Bulk modulus
            parSet(2)=b1; %Alpha
            parSet(3)=b2; %Beta
        case 9 %Moerman 1A
            formulationName='Moerman 1A';
            b1=3;
            b2=2;
            q=0.5;
            parSet(1)=k; %Bulk modulus
            parSet(2)=b1; %Alpha
            parSet(3)=b2; %Beta
            parSet(4)=q; %Weigthing factor
        case 10 %Moerman 1B
            formulationName='Moerman 1B';
            b1=3;
            b2=2;
            parSet(1)=k; %Bulk modulus
            parSet(2)=b1; %Alpha
            parSet(3)=b2; %Beta
        case 11 %Moerman 2
            formulationName='Moerman 2';
            J1=max(J)+0.1;
            J2=0;
            parSet(1)=k; %Bulk modulus
            parSet(2)=J1; %J1
            parSet(3)=J2; %J2
        case 12 %Moerman 2A
            formulationName='Moerman 2A';
            b1=3;
            b2=2;
            parSet(1)=k; %Bulk modulus
            parSet(2)=b1; %Alpha
            parSet(3)=b2; %Beta
        case 13 %Moerman 3
            formulationName='Moerman 3';
            J1=max(J)+0.1;
            J2=0;
            s1=0.15;
            s2=0.15;
            q1=0.9;
            q2=0.9;
            parSet(1)=k; %Bulk modulus
            parSet(2)=J1; %J1
            parSet(3)=J2; %J2
            parSet(4)=s1; %s1
            parSet(5)=s2; %s2
            parSet(6)=q2; %q1
            parSet(7)=q2; %q2
    end

    %% Calculate SED

    %Get normalized SED
    [W,S,T]=SED_eval(formulationCase,parSet,J);

    %% Visualize data

    hf=cFigure;
    ht=subtitle(formulationName);
    ht.FontSize=fontSizeLabel;
    ht.Interpreter='latex';

    subplot(1,3,1); hold on;
    set(gca,'FontSize',fontSize,'LineWidth',LineWidthAxis,'GridAlpha',gridAlpha);
    xlabel('$J$','FontSize',fontSizeLabel,'Interpreter','latex');
    ylabel('$\Psi/\kappa$','FontSize',fontSizeLabel,'Interpreter','latex');

    hp1=plot(J,W,'k-','LineWidth',lineWidth);
    hp1.Color=plotColors(1,:);

    grid on; axis tight; axis square; box on;
    xlim([0 max(J(:))]);
    set(gca,'XTick',xtickRange);

    subplot(1,3,2); hold on;
    set(gca,'FontSize',fontSize,'LineWidth',LineWidthAxis,'GridAlpha',gridAlpha);
    xlabel('$J$','FontSize',fontSizeLabel,'Interpreter','Latex');
    ylabel('$\sigma_{h}/\kappa$','FontSize',fontSizeLabel,'Interpreter','Latex');
    grid on; axis tight; axis square; box on;

    hp2=plot(J,S,'k-','LineWidth',lineWidth);
    hp2.Color=plotColors(2,:);

    grid on; axis tight; axis square; box on;
    xlim([0 max(J(:))]);
    set(gca,'XTick',xtickRange);

    subplot(1,3,3); hold on;
    set(gca,'FontSize',fontSize,'LineWidth',LineWidthAxis,'GridAlpha',gridAlpha);
    xlabel('$J$','FontSize',fontSizeLabel,'Interpreter','latex');
    ylabel('$\frac{\partial^2 \Psi}{\partial J^2} /\kappa$','FontSize',fontSizeLabel,'Interpreter','Latex');

    hp3=plot(J,T,'k-','LineWidth',lineWidth);
    hp3.Color=plotColors(3,:);

    grid on; axis tight; axis square; box on;
    xlim([0 max(J(:))]);
    ylim([0 max(T(:))]);
    set(gca,'XTick',xtickRange);

    drawnow;

end

%% Evaluate SED

function [W,S,T]=SED_eval(formulationCase,parSet,J)

switch formulationCase
    case 1 %Hencky
        k=parSet(1);
        W=k/2*log(J).^2;
        S=k*log(J)./J;
        T=(k-k*log(J))./J.^2;
    case 2 %Simo
        k=parSet(1);
        W=(k/2).*(J-1).^2;
        S=(k/2).*(2*J-2);
        T=k*ones(size(J));
    case 3 %Bischoff
        k=parSet(1);
        b=parSet(2);
        W=(k./b.^2).*(cosh(b*(J-1))-1);
        S=(k./b)   .* sinh(b*(J-1));
        T=k           .* cosh(b*(J-1));
    case 4 %Modified Ogden
        k=parSet(1);
        b=parSet(2);
        W=(k./b.^2).*(J.^-b - 1 + b.*log(J));
        S=(k./b)   .*(1./J     - J.^(-b-1));
        T=(k./b)   .*(-1./J.^2  + (b+1).*J.^(-b-2));
    case 5 %Hyperfoam
        mu=parSet(1);
        a=parSet(2);
        b=parSet(3);
        k=mu.*(b+1/3);
        W=(2*mu./(a.^2)).*( 3*(J.^(a/3)-1)...
            +(1./b.*( (J.^(-a.*b))-1 )) );
        S=(1./J).*(2*mu./a).*(J.^(a/3)...
            -J.^(-a.*b) );
        T=(1./(J.^2)).*(2*mu/a).*((a./3-1).*J.^(a./3) +...
            (a.*b+1).*J.^(-a*b) );
    case 6 %Doll and Schweizerhoff
        k=parSet(1);
        a=parSet(2);
        b=parSet(3);
        W=( (k/(a+b)).*( ((1/(a+1)).*(J.^(a+1))) + ((1/(b-1)).*(J.^(-b+1)))) )...
            -(k.*(1/(a+1)).*(1/(b-1)));
        S=(k/(a+b)).*(J.^a-J.^(-b));
        T=(k/(a+b)).*(a*J.^(a-1)+b*J.^(-b-1));
    case 7
        k=parSet(1);
        beta1=parSet(2);
        k2=parSet(3);
        beta2=parSet(4);
        m=parSet(5);

        %SED
        W1=k./(2.*beta1).*exp(beta1.*   (log(J).^2))-(k/(2*beta1));
        W2=k2./(m.*beta2).*exp(beta2.*abs(log(J).^m))-(k2/(m*beta2));
        W=W1+W2;

        %Stress
        S1=(k./J).*(exp(beta1.*    log(J).^2) .*     log(J));
        S2=(k2./J).*(exp(beta2.*abs(log(J)).^m).*(abs(log(J)).^(m-1)).*sign(log(J)));
        S=S1+S2;

        %Tangent
        T1=(k./J.^2) .* exp(beta1.*    log(J) .^2).*(beta1.*2.*    log(J) .^2 - log(J) + 1);
        T2=(k2./J.^2) .* exp(beta2.*abs(log(J)).^m).*(beta2.*m.*abs(log(J)).^m - log(J) + m -1).*abs(log(J)).^(m-2);
        T=T1+T2;

    case 8 %Moerman 1
        k=parSet(1);
        b1=parSet(2);
        b2=parSet(3);
        W=(k/4)      .*( (1/b1^2).*((J.^ b1)-1).^2 + ...
            (1/b2^2).*((J.^-b2)-1).^2 );
        S=(k/2)./J   .*( (1/b1  ).*(J.^( 2*b1) - J.^b1 ) - ...
            (1/b2  ).*(J.^(-2*b2) - J.^-b2) );
        T=(k/2)./J.^2.*( ((2-1/b1)*J.^( 2*b1)-(1-1/b1)*J.^b1) + ...
            ((2+1/b2)*J.^(-2*b2)-(1+1/b2)*J.^-b2) );
    case 9 %Moerman 1A
        k=parSet(1);
        b1=parSet(2);
        b2=parSet(3);
        q=parSet(4);

        W1=(k/(2*b1^2)).*(  q).*((J.^ b1)-1).^2;
        W2=(k/(2*b2^2)).*(1-q).*((J.^-b2)-1).^2;
        W=W1+W2;

        S1= (k/b1)./J.*(  q).*(J.^( 2*b1)-J.^b1 );
        S2=-(k/b2)./J.*(1-q).*(J.^(-2*b2)-J.^-b2);
        S=S1+S2;

        T1=(k./J.^2).*(  q).*((2-1/b1)*J.^( 2*b1)-(1-1/b1)*J.^b1 );
        T2=(k./J.^2).*(1-q).*((2+1/b2)*J.^(-2*b2)-(1+1/b2)*J.^-b2);
        T=T1+T2;
    case 10 %Moerman 1B
        k=parSet(1);
        b1=parSet(2);
        b2=parSet(3);

        L1=(J>=1);
        L2=(J<1);

        W=zeros(size(J));
        W(L1)=(k/(2*b1^2)).*((J(L1).^ b1)-1).^2;
        W(L2)=(k/(2*b2^2)).*((J(L2).^-b2)-1).^2;

        S=zeros(size(J));
        S(L1)= (k/b1).*(J(L1).^( 2*b1-1)-J(L1).^( b1-1));
        S(L2)=-(k/b2).*(J(L2).^(-2*b2-1)-J(L2).^(-b2-1));

        T=zeros(size(J));
        T(L1)=(k/b1).*((2*b1-1)*J(L1).^( 2*b1-2)-(b1-1)*J(L1).^( b1-2));
        T(L2)=(k/b2).*((2*b2+1)*J(L2).^(-2*b2-2)-(b2+1)*J(L2).^(-b2-2));
    case 11 %Moerman 2
        k=parSet(1);
        J1=parSet(2);
        J2=parSet(3);

        a1=(2/pi)*(J1-1);
        a2=(2/pi)*(J2-1);

        W1=(-k*a1.^2)*log(cos((J-1)/a1));
        W2=(-k*a2.^2)*log(cos((J-1)/a2));

        W=zeros(size(W1));
        W(J>=1)=W1(J>=1);
        W(J<1)=W2(J<1);
        W=real(W);
        W(J>=J1)=inf;
        W(J<=J2)=inf;

        S1=(k.*a1).*tan((J-1)/a1);
        S2=(k.*a2).*tan((J-1)/a2);
        S=zeros(size(S1));
        S(J>=1)=S1(J>=1);
        S(J<1)=S2(J<1);
        S(J>=J1)=inf;
        S(J<=J2)=-inf;

        T1=k*sec((J-1)/a1).^2;
        T2=k*sec((J-1)/a2).^2;
        T=zeros(size(T1));
        T(J>=1)=T1(J>=1);
        T(J<1)=T2(J<1);
        T(J>=J1)=inf;
        T(J<=J2)=inf;
    case 12 %Moerman 2A
        k=parSet(1);
        b1=parSet(2);
        b2=parSet(3);

        W1=(k./b1.^2).*(cosh(b1*(J-1))-1);
        S1=(k./b1)   .* sinh(b1*(J-1));
        T1=k            .* cosh(b1*(J-1));

        W2=(k./b2.^2).*(cosh(b2*(J-1))-1);
        S2=(k./b2)   .* sinh(b2*(J-1));
        T2=k            .* cosh(b2*(J-1));

        W3=k.*(-4/pi^2) .*log(cos(pi/2*(1-J)));
        S3=k.*(-2/pi)   .*tan(pi/2*(1-J));
        T3=k            .*sec(pi/2*(1-J)).^2;

        W=W1;
        S=S1;
        T=T1;

        W(J<1)=W2(J<1)/2+W3(J<1)/2;
        S(J<1)=S2(J<1)/2+S3(J<1)/2;
        T(J<1)=T2(J<1)/2+T3(J<1)/2;
    case 13 %Moerman 3
        k=parSet(1);
        J1=parSet(2);
        J2=parSet(3);
        s_1=parSet(4);
        s_2=parSet(5);
        q1=parSet(6);
        q2=parSet(7);

        L=J<1;

        % PART 1
        a1=(pi/2)*(1/(J1-1));
        a2=(pi/2)*(1/(J2-1));

        %SED
        W11=(-1/a1.^2)*log(cos((J-1).*a1));
        W21=(-1/a2.^2)*log(cos((J-1).*a2));
        W1=zeros(size(J));
        W1(~L)=W11(~L);
        W1(L)=W21(L);
        W1=real(W1);
        W1(J>J1)=inf;
        W1(J<J2)=inf;

        %Stress
        S11=(1./a1).*tan((J-1).*a1);
        S21=(1./a2).*tan((J-1).*a2);
        S1=zeros(size(J));
        S1(~L)=S11(~L);
        S1(L)=S21(L);
        S1(J>J1)=inf;
        S1(J<J2)=-inf;

        %Tangent
        T11=sec((J-1).*a1).^2;
        T21=sec((J-1).*a2).^2;
        T1=zeros(size(J));
        T1(~L)=T11(~L);
        T1(L)=T21(L);
        T1(J>J1)=inf;
        T1(J<J2)=inf;

        % PART 2
        b1= k/(s_1);
        b2= k/(s_2);

        W12=(1/b1.^2)*log(cosh((J-1).*b1));
        W22=(1/b2.^2)*log(cosh((J-1).*b2));
        W2=zeros(size(J));
        W2(~L)=W12(~L);
        W2(L)=W22(L);
        W2=real(W2);

        S12=1/b1*tanh((J-1)*b1);
        S22=1/b2*tanh((J-1)*b2);
        S2=zeros(size(J));
        S2(~L)=S12(~L);
        S2(L)=S22(L);

        T12=sech((J-1).*b1).^2;
        T22=sech((J-1).*b2).^2;
        T2=zeros(size(J));
        T2(~L)=T12(~L);
        T2(L)=T22(L);

        % SUM

        W(~L)=k*((1-q1)*W1(~L)+q1*W2(~L));
        S(~L)=k*((1-q1)*S1(~L)+q1*S2(~L));
        T(~L)=k*((1-q1)*T1(~L)+q1*T2(~L));

        W(L)=k*((1-q2)*W1(L)+q2*W2(L));
        S(L)=k*((1-q2)*S1(L)+q2*S2(L));
        T(L)=k*((1-q2)*T1(L)+q2*T2(L));

        %%

        W(J>=J1)=inf;
        W(J<=J2)=inf;

        S(J>=J1)=inf;
        S(J<=J2)=-inf;

        T(J>=J1)=inf;
        T(J<=J2)=inf;

end

%Normalise based on bulk-modulus
W=W/k; S=S/k; T=T/k;

end

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


% Bezier parameters
% Copyright by Nguyen Quoc Duan - EMMC11

% Purpose : to compute the parameter terms in Bezier curves
% n : the polynomial degree of Bezier curve

function B_para=B_para(i,n,t)
B_para=(1-t)^(n-i)*t^i;


function [Au, PHIu, Av, PHIv, w, TWOCIR]=ep2ap(SEMA, ECC, INC, PHA, plot_demo)
%
% Convert tidal ellipse parameters into amplitude and phase lag parameters.
% Its inverse is app2ep.m. Please refer to app2ep for the meaning of the
% inputs and outputs.
%
% Zhigang Xu
% Oct. 20, 2000
%
% Document:  tidal_ellipse.ps
%
if nargin < 5
     plot_demo=0;  % by default, no plot for the ellipse
end

   Wp = (1+ECC)/2 .*SEMA;
   Wm = (1-ECC)/2 .*SEMA;
   THETAp = INC-PHA;
   THETAm = INC+PHA;

   %convert degrees into radians
   THETAp = THETAp/180*pi;
   THETAm = THETAm/180*pi;

   %Calculate wp and wm.
   wp = Wp.*exp(i*THETAp);
   wm = Wm.*exp(i*THETAm);

   if nargout >= 5
      ndot=36;
      dot = 2*pi/ndot;
      ot = [0:dot:2*pi-dot];
      w = wp(:)*exp(i*ot)+wm(:)*exp(-i*ot);
      w=reshape(w, [size(wp) ndot]);
   end

   % Calculate cAu, cAv --- complex amplitude of u and v
   cAu = wp+conj(wm);
   cAv = -i*(wp-conj(wm));
   Au  = abs(cAu);
   Av  = abs(cAv);
   PHIu = -angle(cAu)*180/pi;
   PHIv = -angle(cAv)*180/pi;

   % flip angles in the range of [-180 0) to the range of [180 360).
   id = PHIu < 0; PHIu(id) = PHIu(id) + 360;
   id = PHIv < 0; PHIv(id) = PHIv(id) + 360;

  if any(plot_demo)
     plot_ell(SEMA,ECC,INC,PHA,plot_demo);
  end

  if nargout == 6
      TWOCIR=struct('Wp', Wp, 'THETAp', THETAp, 'wp', ...
            wp, 'Wm', Wm, 'THETAm', THETAm, 'wm', wm, 'ot', ot, 'dot', dot);
 end

%Authorship Copyright:
%
%    The author of this program retains the copyright of this program, while
% you are welcome to use and distribute this program as long as you credit
% the author properly and respect the program name itself. Particularly,
% you are expected to retain the original author's name in this original
% version of the program or any of its modified version that you might make.
% You are also expected not to essentially change the name of the programs
% except for adding possible extension for your own version you might create,
% e.g. app2ep_xx is acceptable.  Any suggestions are welcome and enjoy my
% program(s)!
%
%
%Author Info:
%_______________________________________________________________________
%  Zhigang Xu, Ph.D.
%  (pronounced as Tsi Gahng Hsu)
%  Research Scientist
%  Coastal Circulation
%  Bedford Institute of Oceanography
%  1 Challenge Dr.
%  P.O. Box 1006                    Phone  (902) 426-2307 (o)
%  Dartmouth, Nova Scotia           Fax    (902) 426-7827
%  CANADA B2Y 4A2                   email xuz@dfo-mpo.gc.ca
%_______________________________________________________________________
%
%Release Date: Nov. 2000


function x = gendatav(pc,R0,nobs)
%function x = gendatav(pc,R0,nobs)
%
%  Simulates nobs observations of a vector AR-process
%  with partial correlations pc and covariance matrix R0.
%
%  See also FILTERV, RANDN.

%  S. de Waele, FEB 2001.

if ~isstatv(pc), error('Partial correlations non-stationairy!'), end
order = kingsize(pc,3)-1;
dim = size(pc,1);
I = eye(dim);
[rc,rcb] = pc2rcv(pc,R0);
[Pf,Pb] = pc2resv(pc,R0);
par = zeros(dim,dim,order+1);
parb = zeros(dim,dim,order+1);

x = zeros(dim,1,nobs); %The signal in matrix notation
%First observation
innovation = randncov(Pf(:,:,1));
x(:,:,1) = innovation;

par(:,:,1) = I;
parb(:,:,1)= I;
if order,
	par(:,:,2) = rc(:,:,2);
	parb(:,:,2)= rcb(:,:,2);
	par_o  = par;
	parb_o = parb;
end

%observation 2 to order
for obs = 2:order,
   innovation = randncov(Pf(:,:,obs));
   prediction = -prodsumv(flipdim(par(:,:,2:obs),3),x(:,:,1:obs-1));
   x(:,:,obs) = prediction+innovation;

   p=obs; %The next observations requires the AR(obs)-model
   %par(:,:,2:p) =  par_o(:,:,2:p) +flipdim(filterv(rc(:,:,p+1),1,parb_o(:,:,2:p)),3);
   par(:,:,2:p) =  par_o(:,:,2:p) +flipdim(armafilterv(parb_o(:,:,2:p),1,rc(:,:,p+1)),3);
   par(:,:,p+1)= rc(:,:,p+1);
   parb(:,:,2:p) =  parb_o(:,:,2:p) +flipdim(armafilterv(par_o(:,:,2:p),1,rcb(:,:,p+1)),3);
   parb(:,:,p+1)= rcb(:,:,p+1);

   par_o  = par;
   parb_o = parb;

end %for obs = 2:order,

%observations order+1 to nobs
innovation = zeros(dim,1,nobs-order);
for i = 1:nobs-order,
   innovation(:,:,i) = randncov(Pf(:,:,order+1));
end
x(:,:,order+1:end) = armafilterv(innovation,par,I,x(:,:,1:order));


% SOSDEMO6 --- MAX CUT
% Section 3.6 of SOSTOOLS User's Manual
%

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

% Number of cuts
f = 2.5 - 0.5*x1*x2 - 0.5*x2*x3 - 0.5*x3*x4 - 0.5*x4*x5 - 0.5*x5*x1;

% Boolean constraints
bc{1} = x1^2 - 1 ;
bc{2} = x2^2 - 1 ;
bc{3} = x3^2 - 1 ;
bc{4} = x4^2 - 1 ;
bc{5} = x5^2 - 1 ;

% =============================================
% First, initialize the sum of squares program
prog = sosprogram(vartable);

% =============================================
% Then define SOSP variables

% -- p1(x) -- : sum of squares
% Monomial vector: 5 independent variables, degree <= 1
Z = monomials(vartable,[0 1]);
[prog,p{1}] = sossosvar(prog,Z);

% -- p2(x) ... p6(x) : polynomials
% Monomial vector: 5 independent variables, degree <= 2
Z = monomials(vartable,0:2);
for i = 1:5
    [prog,p{1+i}] = sospolyvar(prog,Z);
end;

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

% Constraint : p1(x)*(gamma - f(x)) +  p2(x)*bc1(x)
%               + ... + p6(x)*bc5(x) - (gamma-f(x))^2 >= 0
gamma = 4;

expr = p{1}*(gamma-f);
for i = 2:6
    expr = expr + p{i}*bc{i-1};
end;
expr = expr - (gamma-f)^2;

prog = sosineq(prog,expr);

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

% =============================================
% Program is feasible, thus 4 is an upper bound for the cut.
echo off


function ppwd = pwd_imp_circexp(pm,Npw)
%PWD_IMP_CIRCEXP converts a circular basis expansion of a sound field to its
%two-dimensional plane wave decomposition
%
%   Usage: ppwd = pwd_imp_circexp(pm,[Npw])
%
%   Input parameters:
%       pm      - circular basis expansion [N x (M+1)]
%       Npw     - number of equi-angular distributed plane waves, optional,
%                 default: 2*M+1
%
%   Output parameters:
%       ppwd    - plane wave decomposition [N x Npw]
%
%   See also: driving_function_imp_localwfs_sbl_ps,
%   driving_function_imp_localwfs_sbl_pw

%*****************************************************************************
% The MIT License (MIT)                                                      *
%                                                                            *
% Copyright (c) 2010-2019 SFS Toolbox Developers                             *
%                                                                            *
% Permission is hereby granted,  free of charge,  to any person  obtaining a *
% copy of this software and associated documentation files (the "Software"), *
% to deal in the Software without  restriction, including without limitation *
% the rights  to use, copy, modify, merge,  publish, distribute, sublicense, *
% and/or  sell copies of  the Software,  and to permit  persons to whom  the *
% Software is furnished to do so, subject to the following conditions:       *
%                                                                            *
% The above copyright notice and this permission notice shall be included in *
% all copies or substantial portions of the Software.                        *
%                                                                            *
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
% IMPLIED, INCLUDING BUT  NOT LIMITED TO THE  WARRANTIES OF MERCHANTABILITY, *
% FITNESS  FOR A PARTICULAR  PURPOSE AND  NONINFRINGEMENT. IN NO EVENT SHALL *
% THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
% LIABILITY, WHETHER  IN AN  ACTION OF CONTRACT, TORT  OR OTHERWISE, ARISING *
% FROM,  OUT OF  OR IN  CONNECTION  WITH THE  SOFTWARE OR  THE USE  OR OTHER *
% DEALINGS IN THE SOFTWARE.                                                  *
%                                                                            *
% The SFS Toolbox  allows to simulate and  investigate sound field synthesis *
% methods like wave field synthesis or higher order ambisonics.              *
%                                                                            *
% https://sfs.readthedocs.io                            sfstoolbox@gmail.com *
%*****************************************************************************

%% ===== Checking of input parameters ==================================
nargmin = 1;
nargmax = 2;
narginchk(nargmin,nargmax);
isargmatrix(pm);
M = size(pm,2)-1;
if nargin == nargmin
  Npw = 2*M+1;
else
  isargpositivescalar(Npw);
end

%% ===== Computation ====================================================
% Implementation of
%                 ___
% _               \
% p(phipw, t) =   /__     p (t) i^m  e^(-i m phipw)
%               m=-M..M    m
% with
%
% phipw = n * 2*pi/Npw

pm = [conj(pm(:,end:-1:2)), pm];  % append coefficients for negative m
ppwd = inverse_cht(bsxfun(@times,pm,1i.^(-M:M)),Npw);


function [ vX, mX ] = SolveLsL1ComplexCd( mA, vB, lambdaFctr, numIterations )
% ----------------------------------------------------------------------------------------------- %
%[ vX, mX ] = SolveLsL1ComplexPgm( mA, vB, lambdaFctr, numIterations )
% Solves the 0.5 * || A x - b ||_2 + \lambda || x ||_1 problem using
% Coordinate Descent Method. The model allows A, b and x to be Complex.
% Input:
%   - mA                -   Model Matrix.
%                           The model matrix.
%                           Structure: Matrix (m X n).
%                           Type: 'Single' / 'Double' (Complex).
%                           Range: (-inf, inf).
%   - vB                -   Input Vector.
%                           The model known data.
%                           Structure: Vector (m X 1).
%                           Type: 'Single' / 'Double' (Complex).
%                           Range: (-inf, inf).
%   - paramLambda       -   Parameter Lambda.
%                           The L1 Regularization parameter.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: (0, inf).
%   - numIterations     -   Number of Iterations.
%                           Number of iterations of the algorithm.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range {1, 2, ...}.
% Output:
%   - vX                -   Output Vector.
%                           Structure: Vector (n X 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
% References
%   1.  Wikipedia Coordinate Descent Method - https://en.wikipedia.org/wiki/Coordinate_descent.
% Remarks:
%   1.  Coordienat Descent is basically Steepest Descnt in L1 Norm.
% Known Issues:
%   1.  A
% TODO:
%   1.  B
% Release Notes:
%   -   1.0.000     07/11/2016
%       *   First realease version.
% ----------------------------------------------------------------------------------------------- %

vANorm = sum(mA .* conj(mA), 1);
numElements = size(mA, 2); %<! Size of solution

vX  = pinv(mA) * vB; %<! Dealing with "Fat Matrix"

mX = zeros([size(vX, 1), numIterations]);
mX(:, 1) = vX;

for ii = 2:numIterations

    for jj = 1:numElements
         vA = mA(:, jj);
         colNormSqr = vANorm(jj);
         vExcCoord = [1:jj - 1, jj + 1:numElements];

         vR = vB - (mA(:, vExcCoord) * vX(vExcCoord));
         vBeta = vA' * vR;

         vX(jj) = ProxL1(vBeta, lambdaFctr) / colNormSqr;
    end

    mX(:, ii) = vX;

end

end

function [ vX ] = ProxL1( vX, lambdaFactor )

% Soft Thresholding - Complex Domain -> Keep Phase, Soft Threshold the
% Modulus

% vXAbs   = abs(vX);
% vXPhase = angle(vX);
%
% vX = max(vXAbs - lambdaFactor, 0) .* exp(1i * vXPhase);

vXAbs = abs(vX);

vX              = (vX ./ vXAbs) .* max((vXAbs - lambdaFactor), 0);
vX(vXAbs == 0)  = 0;

end


function M = compute_compressed_sensing_matrix(p,n, type, options)

% compute_compressed_sensing_matrix - compute a CS matrix
%
%   M = compute_compressed_sensing_matrix(p,n, type, options);
%
%   M is a (p,n) matrix.
%   type can be
%       'gaussian': gaussian entries
%       'randnormed': random unit norm columns
%       'bernouilli': random +-1/sqrt(p)
%       'orthoproj': random projector M'*M=Id
%       'fourier': random Fourier rows
%       'sincos': random cosine/sine rows
%
%   Copyright (c) 2008 Gabriel Peyre

if nargin<3
    type = 'gaussian';
end

options.null = 0;
switch lower(type)
    case 'gaussian'
    	M = randn(p,n) / sqrt(p);
    case {'randnormed' 'rand'}
        M = randn(p,n);
        M = M ./ repmat( sqrt(sum(M.^2)), [p 1] );
    case 'bernouilli'
        M = sign(randn(p,n)) / sqrt(p);
    case {'orthoproj' 'randproj'}
        M = randn(p,n);
        [M,R] = qr(M'); M = M(:,1:p)';
    case 'fourier'
        [X,Y] = meshgrid(0:n-1:0:n-1);
        M = exp( 2i*pi/n * X.*Y ) / sqrt(p);
        sel = randperm(n); sel = sel(1:p);
        M = M(sel,:);
    case 'sincos'
        [X,Y] = meshgrid(0:n-1,0:n/2);
        M = cos( 2*pi/n * X.*Y );
        [X,Y] = meshgrid(0:n-1,1:n/2-1);
        M = [M; sin( 2*pi/n * X.*Y )];
        M = M ./ repmat( sqrt(sum(M.^2,2)), [1 n] );
        sel = randperm(n); sel = sel(1:p);
        M = M(sel,:) * sqrt( n/p );
    case 'projoptim'
        niter = getoptions(options, 'niter_projoptim', 40);
        M = randn(p,n);
        for i=1:niter
            % unit normed
            M = M ./ repmat( sqrt(sum(M.^2)), [p 1] );
            % projector
            [U,S,V] = svd(M, 'econ');
            S = diag(diag(S)*0+1);
            M = U*S*V';
        end
    otherwise
        error('Unknown matrice type');
end

return;
%% old code

if nargin<4
    normtype = 'normalize';
end

switch lower(type)
    case 'random'
        M = randn(n,m);
    case 'bumps'
        normtype = 'none';
        % m bumps
        sigma = 0.05;
        x = linspace(0,1,m+1)'; x(end) = []; % bump centers
        M = zeros(n,m);
        t = linspace(0,1,n+1)'; t(end) = [];
        for i=1:m
            u = t-x(i); u(u>1/2) = 1-u(u>1/2);
            M(:,i) = exp( -u.^2 / (2*sigma^2)  );
        end
end

switch lower(normtype)
    case 'normalize'
        % lines should be of unit norm
        d = sqrt( sum(M.^2,1) );
        M = M ./ repmat( d, [size(M,1) 1] );
    case 'orthogonalize'
        % M*M' = Id
        M = orth(M')';
    case 'none'
    otherwise
        error('Unknown normalization');
end


function [xi,w]=fifthOrder2DCubPoints()
%%FIFTHORDER2DCUBPOINTS Generate fifth-order cubature points
%               for integration over a 2-dimensional cube with bounds of
%               (-1,-1), (-1,1), (1,1), and (1,-1).
%
%INPUTS: None
%
%OUTPUTS: xi This is a 2XnumCubPoints set of points for the standard
%            square.
%          w A 1XnumCubPoints set of cubature weights. This sums to the
%            volume of the standard square (4).
%
%This function implements the points given in [1] (8 points).
%
%EXAMPLE:
%We compare a 4th-order moment computed using these cubature points
%to one computed using monomialIntCube (a 5th order moment would have just
%been 0). The results are the same within typical finite precision limits.
% [xi,w]=fifthOrder2DCubPoints();
% alpha=[2;2];
% theMoment=findMomentFromSamp(alpha,xi,w);
% intVal=monomialIntCube(alpha);
% RelErr=(theMoment-intVal)/intVal
%
%REFERENCES:
%[1] F. D. Witherden and P. E. Vincent, "On the identification of symmetric
%    quadrature rules for finite element methods," Computer and Mathematics
%    with Applications, vol. 69, no. 10, pp. 1232-1241, May 2015.
%
%October 2022 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

M=[0.68313005106397322554806924536807013272,                                          0,    0.8163265306122448979591836734693877551;
                                        0,   0.68313005106397322554806924536807013272,    0.8163265306122448979591836734693877551;
-0.68313005106397322554806924536807013272,                                          0,    0.8163265306122448979591836734693877551;
                                        0,  -0.68313005106397322554806924536807013272,    0.8163265306122448979591836734693877551;
  0.8819171036881968635005385845464201419,    0.8819171036881968635005385845464201419,    0.1836734693877551020408163265306122449;
  0.8819171036881968635005385845464201419,   -0.8819171036881968635005385845464201419,    0.1836734693877551020408163265306122449;
 -0.8819171036881968635005385845464201419,    0.8819171036881968635005385845464201419,    0.1836734693877551020408163265306122449;
 -0.8819171036881968635005385845464201419,   -0.8819171036881968635005385845464201419,    0.1836734693877551020408163265306122449];
w=M(:,3);
xi=M(:,1:2)';

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.


%% Data Structure: Boundary Conditions
%
% We use |bdFlag(1:NT,1:3)| to record the type of three edges of each
% triangle. Similarly in 3-D, we use |bdFlag(1:NT,1:4)| to record the type
% of four faces of each tetrahedron. The value is the type of boundary
% condition.
%
% * 0: non-boundary, i.e., an interior edge or face.
% * 1: first type, i.e., a Dirichlet boundary edge or face.
% * 2: second type, i.e., a Neumann boundary edge or face.
% * 3: third type, i.e., a Robin boundary edge or face.

%% Local labeling of edges and faces
% We label three edges of a triangle such that |bdFlag(t,i)| is the edge
% opposite to the i-th vertex. Similarly |bdFlag(t,i)| is the face opposite
% to the i-th vertex.

node = [1,0; 1,1; 0,0];
elem = [1 2 3];
edge = [2 3; 1 3; 1 2];
showmesh(node,elem);
findnode(node);
findedge(node,edge);

%% Set up boundary conditions
%
% The function |setboundary| is to set up the bdFlag matrix for a 2-D
% triangulation and |setboundary3| for a 3-D triangulation.
%
help setboundary

%%
% Note that if the i-th edge of t is on the boundary but |bdFlag(t,i)=0|,
% it is equivalent to use homogenous Neumann boundary condition (zero
% flux).

%% Example: Crack Domain
node = [1,0; 0,1; -1,0; 0,-1; 0,0; 1,0];        % nodes
elem = [5,1,2; 5,2,3; 5,3,4; 5,4,6];            % elements
elem = label(node,elem);                        % label the mesh
figure;
showmesh(node,elem);                            % plot mesh
findelem(node,elem);                            % plot element indices
findnode(node,2:6);                             % plot node indices
text(node(6,1),node(6,2)+0.075,int2str(1),'FontSize',16,'FontWeight','bold');
hold on;
plot([node(1,1),node(5,1)], [node(1,2),node(5,2)],'r-', 'LineWidth',3);
bdFlag = setboundary(node,elem,'Dirichlet');                % Dirichlet boundary condition
display(elem)
display(bdFlag)
%%
% node 1 and node 6 are the same point (1,0)

%%
bdFlag = setboundary(node,elem,'Dirichlet','abs(x) + abs(y) == 1','Neumann','y==0');
display(bdFlag)

%% Example: Prism Domain
node = [-1,-1,-1; 1,-1,-1; 1,1,-1; -1,1,-1; -1,-1,1; 1,-1,1; 1,1,1; -1,1,1];
elem = [1,2,3,7; 1,6,2,7; 1,5,6,7];
elem = label3(node,elem);
figure;
showmesh3(node,elem);
view([-53,8]);
findnode3(node,[1 2 3 5 6 7]);
findelem3(node,elem);
bdFlag = setboundary3(node,elem,'Dirichlet','(z==1) | (z==-1)');
display(elem)
display(bdFlag)

%%
% The top and bottom of the prism is set as Dirichlet boundary condition
% and other faces are zero flux boundary condition.

%% Remark
% It would save storage if we record boundary edges or faces only. The
% current data structure is convenient for the local refinement and
% coarsening since the boundary can be easily update along with the change
% of elements. The matrix |bdFlag| is sparse but we use a dense matrix to
% store it. We do not save |bdFlag| as a sparse matrix since updating
% sparse matrix is time consuming. We set up the type of |bdFlag| or
% |bdFlag| to |uint8| to minimize the waste of spaces.


function [ x, odata, opts ] = solver_TraceLS( A, b, lambda, x0, opts )
% SOLVER_TRACELS Unconstrained form of trace-regularized least-squares problem.
% [ x, odata, opts ] = solver_TraceLS( A, b, lambda, x0, opts )
%    Solves the trace-regularized least squares problem
%        minimize (1/2)*norm( A * X - b )^2 + lambda * trace( X )
%        with the constraint that X is positive semi-definite.
%    A must be a matrix or a linear operator, b must be a vector,
%    and lambda must be a real positive scalar.
%    The initial point x0 and option structure opts are
%    both optional.
%
%    If "A" is a sparse matrix, and nnz(Z) = length(b), then it is assumed
%    that the nonzero entries correspond to samples, and then the
%    corresponding sampling operator is used.
%
%    If opts.largescale = true, then uses an iterative method
%    to compute the eigenvalue decomposition used with prox_trace.
%
%   See also solver_L1RLS (aka The Lasso)

% Added Feb 7, 2011
error(nargchk(3,5,nargin));
if nargin < 4, x0 = []; end
if nargin < 5, opts = []; end
if ~isfield( opts, 'restart' ),
    opts.restart = 100;
end
if issparse(A) && nnz(A) == length(b)
    A = linop_subsample(A);
end
if isfield( opts,'largescale')
    prx = prox_trace( lambda, opts.largescale );
    opts = rmfield(opts,'largescale');
else
    prx = prox_trace( lambda );
end
% Note: the proximity operator of trace is really a combination
%  of the proximity operator of trace and the indicator
%  set of positive semi-definite matrices.
%  So we do not need to *explicitly* include the positive semi-definite constraint.

[x,odata,opts] = tfocs( smooth_quad, { A, -b }, prx, x0, opts );

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


function result = sphere_monomial_int_nd ( n, r, e )

%*****************************************************************************80
%
%% SPHERE_MONOMIAL_INT_ND integrates a monomial on a sphere in ND.
%
%  Discussion:
%
%    The sphere may have nonunit radius, but it must be centered at 0.
%
%    The integration region is
%
%      sum ( x(i)**2 ) = R**2.
%
%    The monomial is F(X) = X(1)**E(1) * X(2)**E(2) * ... * X(N)**E(N).
%
%    This routine is useful for testing the accuracy of quadrature
%    rules on the sphere.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 January 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Philip Davis, Philip Rabinowitz,
%    Methods of Numerical Integration,
%    Second Edition,
%    Academic Press, 1984, page 263.
%
%  Parameters:
%
%    Input, integer N, the dimension of the space.
%
%    Input, real R, the radius of the sphere.
%
%    Input, integer E(N), the exponents of X, Y and Z in the monomial.
%    Each exponent must be nonnegative.
%
%    Output, real RESULT, the integral.
%
  if ( any ( e(1:n) < 0 ) )
    result = -r8_huge ( result );
    fprintf ( 1, '\n' );
    fprintf ( 1, 'SPHERE_MONOMIAL_INT_ND - Fatal error!\n' );
    fprintf ( 1, '  All exponents must be nonnegative.\n' );
    error ( 'SPHERE_MONOMIAL_INT_ND - Fatal error!' );
  end

  if ( all ( e(1:n) == 0 ) )

    result = 2.0 * sqrt ( pi^n ) / gamma ( 0.5 * n );

  elseif ( any ( mod ( e(1:n), 2 ) == 1 ) )

    result = 0.0;

  else

    result = 2.0;

    for i = 1 : n
      result = result * gamma ( 0.5 * ( e(i) + 1 ) );
    end

    result = result / gamma ( 0.5 * ( sum ( e(1:n) + 1 ) ) );

  end

  result = result * r^( sum ( e(1:n) ) + 2 );

  return
end


function degree = r8poly_degree ( na, a )

%*****************************************************************************80
%
%% R8POLY_DEGREE returns the degree of a polynomial in power sum form.
%
%  Discussion:
%
%    The power sum form of a polynomial is:
%
%      p(x) = a(0) + a(1) * x + ... + a(n-1) * x**(n-1) + a(n) * x**(n)
%
%    The degree of a polynomial is the index of the highest power
%    of X with a nonzero coefficient.
%
%    The degree of a constant polynomial is 0.  The degree of the
%    zero polynomial is debatable, but this routine returns the
%    degree as 0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 June 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NA, the dimension of A.
%
%    Input, real A(1:NA+1), the coefficients of the polynomials.
%
%    Output, integer DEGREE, the degree of A.
%
  degree = na;

  while ( 0 < degree )

    if ( a(degree+1) ~= 0.0 )
      return
    end

    degree = degree - 1;

  end

  return
end


function fem2d_pack_test17 ( )

%*****************************************************************************80
%
%% TEST17 demonstrates S_L2NORM.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  element_num = 40;
  psi_num = element_num + 1;
  quad_num = 5;

  x_max = 1.0;
  x_min = 0.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST17\n' );
  fprintf ( 1, '  S_L2NORM computes the L2 norm of a scalar function\n' );
  fprintf ( 1, '  S(X) over a region (of any dimension), assuming:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  * that there is a set of finite element basis\n' );
  fprintf ( 1, '    functions PSI;\n' );
  fprintf ( 1, '  * that the region is decomposed into a number of\n' );
  fprintf ( 1, '    elements whose areas are known;\n' );
  fprintf ( 1, '  * that the integral is to be computed by a quadrature\n' );
  fprintf ( 1, '    rule applied in the same way to each element;\n' );
  fprintf ( 1, '  * that the value of the basis functions is given\n' );
  fprintf ( 1, '    at every quadrature node in every element;\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Our example will have one spatial dimension.\n' );
  fprintf ( 1, '  XMIN = %f\n', x_min );
  fprintf ( 1, '  XMAX = %f\n', x_max );
  fprintf ( 1, '  The number of intervals will be    %d\n', element_num );
  fprintf ( 1, '  The number of basis functions is   %d\n', psi_num );
  fprintf ( 1, '  The number of quadrature points is %d\n', quad_num );
%
%  Set the nodes.
%
  for i = 1 : psi_num
    node_x(i) = ( ( psi_num - i     ) * x_min   ...
                + (           i - 1 ) * x_max ) ...
                / ( psi_num     - 1 );
  end
%
%  Set the element "areas".
%
  for i = 1 : element_num
    element_area(i) = node_x(i+1) - node_x(i);
  end
%
%  Set the quadrature weights and abscissas.
%
  quad_weight(1) = 0.236926885056189087514264040720;
  quad_weight(2) = 0.478628670499366468041291514836;
  quad_weight(3) = 0.568888888888888888888888888889;
  quad_weight(4) = 0.478628670499366468041291514836;
  quad_weight(5) = 0.236926885056189087514264040720;

  quad_weight(1:5) = quad_weight(1:5) / 2.0;

  quad_x(1) = - 0.906179845938663992797626878299;
  quad_x(2) = - 0.538469310105683091036314420700;
  quad_x(3) =   0.0;
  quad_x(4) =   0.538469310105683091036314420700;
  quad_x(5) =   0.906179845938663992797626878299;
%
%  Set the finite element coefficients of S.  In our formulation,
%  these finite element coefficients are simply the function values
%  at the nodes.
%
  for i = 1 : psi_num
    s_coef(i) = sin ( node_x(i) );
  end
%
%  For each basis function I,
%    for each element J,
%      for each quadrature point K,
%
%  ...evaluate the basis function in the element at the quadrature point.
%
  psi_quad(1:psi_num,1:element_num,1:quad_num) = 0.0;

  for psi = 1 : psi_num

    for element = 1 : element_num

      if ( element < psi - 1 )

        continue

      elseif ( element == psi - 1 )

        xl = node_x(element);
        xr = node_x(element+1);

        for quad = 1 : quad_num

          x = ( ( 1.0 - quad_x(quad) ) * xl   ...
              + ( 1.0 + quad_x(quad) ) * xr ) ...
              /   2.0;

          psi_quad(psi,element,quad) = ( x - xl ) / ( xr - xl );

        end

      elseif ( element == psi )

        xl = node_x(element);
        xr = node_x(element+1);

        for quad = 1 : quad_num

          x = ( ( 1.0 - quad_x(quad) ) * xl   ...
              + ( 1.0 + quad_x(quad) ) * xr ) ...
              /   2.0;

          psi_quad(psi,element,quad) = ( xr - x ) / ( xr - xl );

        end

      elseif ( psi < element )
        continue
      end

    end

  end
%
%  Ask S_L2NORM to compute the integral of L2 norm of S.
%
  l2norm = s_l2norm ( psi_num, element_num, quad_num, element_area, ...
    quad_weight, psi_quad, s_coef );

  fprintf ( 1, '\n');
  fprintf ( 1, '  The computed L2 norm is %f\n', l2norm );
%
%  The integral of (sin(x))**2 is x/2 - sin(2x)/4
%
  exact = sqrt ( ...
      ( 0.5 * x_max - 0.25 * sin ( 2.0 * x_max ) ) ...
    - ( 0.5 * x_min - 0.25 * sin ( 2.0 * x_min ) ) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The exact value is      %f\n', exact );

  return
end