function R = rotation_matrix(phi,dim,orientation)
%ROTATION_MATRIX 3D rotation matrix
%
%   Usage: R = rotation_matrix(phi,[dim,[orientation]])
%
%   Input parameters:
%       phi         - angle to rotate the given dim dimension vector / rad
%       dim         - dimension to turn around (default: 3, z-axis)
%       orientation - orientation of the rotation, 'clockwise' or
%                     'counterclockwise' (default: 'counterclockwise')
%
%   Output parameters:
%       R       - 3x3 rotation matrix to apply to your vector to
%                 rotate: R*y
%
%
%   ROTATION_MATRIX(phi,dimension,orientation) returns a rotation matrix R,
%   which is able to rotate a vector around the given dimension about phi.
%
%   See also: sin, cos

%*****************************************************************************
% 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 = 3;
narginchk(nargmin,nargmax);
isargscalar(phi)
if nargin<3
    % Set defualt orientation of the rotation
    orientation = 'counterclockwise';
end
if nargin<2
    % set default rotation dimension to z-axis
    dim = 3;
end
isargchar(orientation);
isargpositivescalar(dim);

%% ===== Computation ====================================================
% Rotation matrix (see: https://en.wikipedia.org/wiki/Rotation_matrix)
% get single matrix entries
r1 = cos(phi);
r4 = cos(phi);
if strcmp('counterclockwise',orientation)
    r2 = -sin(phi);
    r3 =  sin(phi);
elseif strcmp('clockwise',orientation)
    r2 =  sin(phi);
    r3 = -sin(phi);
else
    error('%s: the given orientation "%s" is not known.', ...
        upper(mfilename),orientation);
end
% Fill up matrix to rotate around the given axis
if dim==1

    R = [1 0  0;  ...
         0 r1 r2; ...
         0 r3 r4];
elseif dim==2
    R = [r1 0 r2; ...
         0  1 0;  ...
         r3 0 r4];
elseif dim==3
    R = [r1 r2 0; ...
         r3 r4 0; ...
         0  0  1];
else
    error('%s: dim has to be 1,2, or 3 and not %i',upper(mfilename),dim);
end


function [q,qd,q2d] = mixed_traj(t,C,A,B,w,N)
% ---------------------------------------------------------------------
% This function computes "mixed trajectory" meaning the trajectory
% that consistes of finite fourier series and fifth order polynomial
% Inputs:
%   t - time instants at which trajectory should be evaluated
%   C - coefficients of the fifth order polynomail
%   A - coeffincients of the sine in finite fourier series
%   B - coefficinets of the cosine in finitne fourier series
%   w - fundamental frequency
%   N - number of harmonics
% ---------------------------------------------------------------------
% finite fourier series
[qh,qhd,qh2d] = fourier_series_traj(t,zeros(6,1),A,B,w,N);

% fifth order polynomail trajectory
qp = C(:,1) + C(:,2).*t + C(:,3).*t.^2 + C(:,4).*t.^3 + ...
                                    C(:,5).*t.^4 + C(:,6).*t.^5;
qpd = C(:,2) + 2*C(:,3).*t + 3*C(:,4).*t.^2 + ...
                                4*C(:,5).*t.^3 + 5*C(:,6).*t.^4;
qp2d = 2*C(:,3) + 6*C(:,4).*t + 12*C(:,5).*t.^2 + 20*C(:,6).*t.^3;

q = qh + qp;
qd = qhd + qpd;
q2d = qh2d + qp2d;


function [max_val,ind] = mDmax(x)
%MDMAX Largest component in a multidimensional matrix.
%   For a matrix X, MDMAX(X) is the largest element in X.
%
%   [Y,I] = MDMAX(X) returns the indices of the maximum value in vector I.
%
%   When X is complex, the maximum is computed using the magnitude
%   MAX(ABS(X)). In the case of equal magnitude elements, then the phase
%   angle MAX(ANGLE(X)) is used.
%
%   NaN's are ignored when computing the maximum. When all elements in X
%   are NaN's, then the first one is returned as the maximum.
%
%   Example: If X = [2 8 4;   then mDmax(X) is 9,
%                    7 3 9]
%   and
%            [val,ind] = mDmax(X) returns val=9 and ind=[2 3]
%
%   Note: The vector ind can be used to access elements from X using this
%   neat trick:
%              sub = num2cell(ind)
%              val = x(sub{:});

[max_val,position] = max(x(:));
ind = myind2sub(size(x),position);

%--------------------------------------------------------------
function ind = myind2sub(siz,ndx)

dim = numel(siz);
sub = cell(dim,1);
[sub{:}] = ind2sub(siz,ndx);
ind = cell2mat(sub);

% Old implementation
% siz = double(siz);
%
% n = length(siz);
% k = [1 cumprod(siz(1:end-1))];
% for i = n:-1:1,
%     vi = rem(ndx-1, k(i)) + 1;
%     vj = (ndx - vi)/k(i) + 1;
%     ind(:,i) = vj;
%     ndx = vi;
% end


function x = pwPoly2(tGrid,xGrid,t)
% x = pwPoly2(tGrid,xGrid,t)
%
% This function does piece-wise quadratic interpolation of a set of data.
%
% INPUTS:
%   tGrid = [1, 2*n-1] = time grid, knot idx = 1:2:end
%   xGrid = [m, 2*n-1] = function at each grid point in time
%   t = [1, k] = vector of query times (must be contained within tGrid)
%
% OUTPUTS:
%   x = [m, k] = function value at each query time
%
% NOTES:
%   If t is out of bounds, then all corresponding values for x are replaced
%   with NaN
%

nGrid = length(tGrid);
if mod(nGrid-1,2)~=0 || nGrid < 3
    error('The number of grid-points must be odd and at least 3');
end

% Figure out sizes
n = floor((length(tGrid)-1)/2);
m = size(xGrid,1);
k = length(t);
x = zeros(m, k);

% Figure out which segment each value of t should be on
edges = [-inf, tGrid(1:2:end), inf];
[~, bin] = histc(t,edges);

% Loop over each quadratic segment
for i=1:n
    idx = bin==(i+1);
    if sum(idx) > 0
        gridIdx = 2*(i-1) + [1,2,3];
        x(:,idx) = quadInterp(tGrid(gridIdx),xGrid(:,gridIdx),t(idx));
    end
end

% Replace any out-of-bounds queries with NaN
outOfBounds = bin==1 | bin==(n+2);
x(:,outOfBounds) = nan;

end

function x = quadInterp(tGrid,xGrid,t)
%
% This function computes the interpolant over a single interval
%
% INPUTS:
%   tGrid = [1, 3] = time grid
%   xGrid = [m, 3] = function grid
%   t = [1, p] = query times, spanned by tGrid
%
% OUTPUTS:
%   x = [m, p] = function at query times
%

% Rescale the query points to be on the domain [-1,1]
t = 2*(t-tGrid(1))/(tGrid(3)-tGrid(1)) - 1;

% Compute the coefficients:
a = 0.5*(xGrid(:,3) + xGrid(:,1)) - xGrid(:,2);
b = 0.5*(xGrid(:,3)-xGrid(:,1));
c = xGrid(:,2);

% Evaluate the polynomial for each dimension of the function:
p = length(t);
m = size(xGrid,1);
x = zeros(m,p);
tt = t.^2;
for i=1:m
    x(i,:) = a(i)*tt + b(i)*t + c(i);
end

end


%
% lellippi(phi, k, errtol)
%
% Inputs:
%
%   phi     Input angle vector size 1 or 1xN.
%   k       Input parameter vector size 1 or 1xN.
%   n       Input parameter vector size 1 or 1xN.
%   errtol  Error tolerance for Carlson's algorithms.
%
% Matlab function to compute Legendre's (incomplete) elliptic integral
% Pi(phi, k, n).  Uses a vectorized implementation of Carlson's Duplication Algorithms
% for symmetric elliptic integrals as found in "Computing Elliptic
% Integrals by Duplication," by B. C. Carlson, Numer. Math. 33, 1-16 (1979)
% and also found in ACM TOMS Algorithm 577.  Section 4 in the paper cited
% here describes how to convert between the symmetric elliptic integrals
% and Legendre's elliptic integrals.
%
% Returns NaN's for any argument values outside input range.
%

function f = lellippi(phi, k, n, errtol)
% Argument checking for vectorization:
lphi = length(phi);
lk = length(k);
ln = length(n);
errflag = logical(0);
if ( ~ ((lphi==lk) & (lphi==ln) & (lk==ln)) )
    if ( (lk==1) & (ln==1) )
        kvec = k * ones(1,lphi);
        nvec = n * ones(1,lphi);
        phivec = phi;
    elseif ( (lphi==1) & (ln==1) )
        phivec = phi * ones(1,lk);
        nvec = n * ones(1,lk);
        kvec = k;
    elseif ( (lphi==lk) & (ln==1) )
        nvec = n * ones(1,lphi);
        kvec = k;
        phivec = phi;
    elseif ( (lphi==1) & (lk==1) )
        phivec = phi * ones(1,ln);
        kvec = k * ones(1,lk);
        nvec = n;
    elseif ( (lphi==ln) & (lk==1) )
        kvec = k * ones(1,lphi);
        phivec = phi;
        nvec = n;
    elseif ( (lk==ln) & (lphi==1) )
        phivec = phi * ones(1,lk);
        kvec = k;
        nvec = n
    else
        disp('Incompatible input vector dimensions in lellipf!');
        errflag = logical(1);
    end
else
    phivec = phi;
    kvec = k;
    nvec = n;
end
if (~errflag)
    snphi = sin(phivec);
    csphi = cos(phivec);
    snphi2 = snphi.^2;
    csphi2 = csphi.^2;
    k2 = kvec.^2;
    y = 1.0 - k2.*snphi2;
    p = 1.0 + nvec .* snphi2;
    onesvec = ones(1,length(phivec));
    f = snphi .* rf(csphi2,  y, onesvec, errtol) - ...
        nvec .* snphi .* snphi2 .* rj(csphi2, y, onesvec, p, errtol) / 3.0;
else
    f = NaN;
end


%KGAUSS Gaussian kernel
%
% K = KGAUSS(SIGMA) is a 2-dimensional Gaussian kernel of standard deviation
% SIGMA, and  centred within the matrix K whose half-width is H=2xSIGMA and
% W=2xH+1.
%
% K = KGAUSS(SIGMA, H) as above but the half-width H is specified.
%
% Notes::
% - The volume under the Gaussian kernel is one.
%
% See also KDGAUSS, KDOG, KLOG, ICONV.

% Copyright (C) 1993-2011, by Peter I. Corke
%
% This file is part of The Machine Vision Toolbox for Matlab (MVTB).
%
% MVTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% MVTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with MVTB.  If not, see <http://www.gnu.org/licenses/>.

function m = kgauss(sigma, w)

    if nargin == 1,
        w = ceil(3*sigma);
    end
    ww = 2*w + 1;

    [x,y] = meshgrid(-w:w, -w:w);

    m = 1/(2*pi*sigma^2) * exp( -(x.^2 + y.^2)/2/sigma^2);

    % area under the curve should be 1, but the discrete case is only
    % an approximation, correct it
    %m = m / sum(m(:));


function qwgw_test07 ( )

%*****************************************************************************80
%
%% TEST07 tests QWGW for the Jacobi weight.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 February 2014
%
%  Author:
%
%    John Burkardt
%

%
%  Set the quadrature interval and number of points.
%
  a = -1.0;
  b = +1.0;
  n = 5;
%
%  Set the weight function parameters.
%
  alpha = 0.25;
  beta = 0.75;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST07:\n' );
  fprintf ( 1, '  Compute points and weights for Gauss quadrature\n' );
  fprintf ( 1, '  with the Jacobi weight w(x) = (1-x^2)^alpha*(1+x)^beta\n' );
  fprintf ( 1, '  Order N = %d\n', n );
  fprintf ( 1, '  ALPHA = %g\n', alpha );
  fprintf ( 1, '  BETA =  %g\n', beta );
  fprintf ( 1, '  Interval = [%g,%g]\n', a, b );
%
%  Set the recursion coefficients.
%
  aj = zeros ( n, 1 );
  bj = zeros ( n, 1 );

  for j = 1 : n
    jr = j;
    aj(j) = ( beta - alpha ) * ( beta + alpha ) ...
      / ( alpha + beta + 2.0 * jr - 2.0 ) ...
      / ( alpha + beta + 2.0 * jr );
  end

  for j = 1 : n - 1
    jr = j;
    bj(j) = 4.0 * jr * ( alpha + jr ) * ( beta + jr ) ...
      * ( alpha + beta + jr ) ...
      / ( ( alpha + beta + 2.0 * jr )^2 - 1.0 ) ...
      / ( alpha + beta + 2.0 * jr )^2;
  end
  bj(n) = 0.0;

  bj(1:n) = sqrt ( bj(1:n) );

  mu0 = 2.0 ^ ( alpha + beta + 1.0 ) ...
    * gamma ( alpha + 1.0 ) * gamma ( beta + 1.0 ) ...
    / gamma ( alpha + beta + 2.0 );
%
%  Compute the points and weights.
%
  [ x, w ] = sgqf ( n, aj, bj, mu0 );

  r8vec_print ( n, x, '  Abscissas:' );
  r8vec_print ( n, w, '  Weights:' );

  return
end


%% CUBEPOISSONQ1 Poisson equation in a CUBE domain using trilinear cube element
%
%   cubePoissonQ1 computes trilinear finite element approximations of the
%   Poisson equation in the unit cube on a sequence of hex meshes obtained by
%   uniform refinement. It plots the approximation err vs the number of
%   degree of freedoms.
%
% See also
%   cubePoisson, cubePoissonP2
%
% Copyright (C)  Long Chen. See COPYRIGHT.txt for details.

close all;

%% Parameters
maxIt = 4;
N = zeros(maxIt,1);
h = zeros(maxIt,1);

%% Domain and pde
h0 = 1.0/2.0;
cube = [0 1 0 1 0 1];

pde = sincosdata3;
option.solver = 'amg';
% option.isoparametric = true;

%% Finite Element Method
errL2 = zeros(maxIt,1);
errH1 = zeros(maxIt,1);
erruIuh = zeros(maxIt,1);
for k = 1:maxIt
    [node,elem] = cubehexmesh(cube,h0/(2.0^k));
    bdFlag = setboundary3(node,elem,'Dirichlet','abs(z)>eps','Neumann','abs(z)<eps');
%     bdFlag = setboundary3(node,elem,'Dirichlet');
%     [uh,Du,eqn,info] = Poisson3Q1(node,elem,pde,bdFlag,option);
    [uh,Du,eqn,info] = Poisson3T1(node,elem,pde,bdFlag,option);
    N(k) = size(node,1);
    h(k) = 1./(size(node,1)^(1/3)-1);
    uI = pde.exactu(node);
    erruIuh(k) = sqrt((uh-uI)'*eqn.A*(uh-uI));
    errH1(k) = getH1error3Q1(node,elem,pde.Du,uh);
    errL2(k) = getL2error3Q1(node,elem,pde.exactu,uh);
end

%% Plot convergence rates
figure;
showrateh3(h,errH1,2,'-*', '$|| Du - Du_h ||', ...
           h,errL2,2,'k-+', '|| u - u_h ||', ...
           h,erruIuh,2,'m-+','|| D u_I - D u_h ||');

fprintf('\n');
disp('Table: Error')
colname = {'#Dof','h','|| u-u_h ||','|| Du-Du_h ||','|| Du_I-Du_h ||'};
disptable(colname,N,[],h,'%0.3e',errL2,'%0.5e',errH1,'%0.5e',erruIuh,'%0.5e');


function [is,os,str] = strengths_dir(CIJ)
%STRENGTHS_DIR      Instrength and outstrength
%
%   [is,os,str] = strengths_dir(CIJ);
%
%   Node strength is the sum of weights of links connected to the node. The
%   instrength is the sum of inward link weights and the outstrength is the
%   sum of outward link weights.
%
%   Input:      CIJ,    directed weighted connection matrix
%
%   Output:     is,     node instrength
%               os,     node outstrength
%               str,    node strength (instrength + outstrength)
%
%   Notes:  Inputs are assumed to be on the columns of the CIJ matrix.
%
%
%   Olaf Sporns, Indiana University, 2002/2006/2008

% compute strengths
is = sum(CIJ,1);    % instrength = column sum of CIJ
os = sum(CIJ,2)';   % outstrength = row sum of CIJ
str = is+os;        % strength = instrength+outstrength


function [rhsE, rhsH] = MaxwellRHS1D(E,H,eps,mu)

% function [rhsE, rhsH] = MaxwellRHS1D(E,H,eps,mu)
% Purpose  : Evaluate RHS flux in 1D Maxwell

Globals1D;

% Compute impedance
Zimp = sqrt(mu./eps);

% Define field differences at faces
dE = zeros(Nfp*Nfaces,K); dE(:) = E(vmapM)-E(vmapP);
dH = zeros(Nfp*Nfaces,K); dH(:) = H(vmapM)-H(vmapP);
Zimpm = zeros(Nfp*Nfaces,K); Zimpm(:) = Zimp(vmapM);
Zimpp = zeros(Nfp*Nfaces,K); Zimpp(:) = Zimp(vmapP);
Yimpm = zeros(Nfp*Nfaces,K); Yimpm(:) = 1./Zimpm(:);
Yimpp = zeros(Nfp*Nfaces,K); Yimpp(:) = 1./Zimpp(:);

% Homogeneous boundary conditions, Ez=0
Ebc = -E(vmapB); dE (mapB) = E(vmapB) - Ebc;
Hbc =  H(vmapB); dH (mapB) = H(vmapB) - Hbc;

% evaluate upwind fluxes
fluxE = 1./(Zimpm + Zimpp).*(nx.*Zimpp.*dH - dE);
fluxH = 1./(Yimpm + Yimpp).*(nx.*Yimpp.*dE - dH);

% compute right hand sides of the PDE's
rhsE = (-rx.*(Dr*H) + LIFT*(Fscale.*fluxE))./eps;
rhsH = (-rx.*(Dr*E) + LIFT*(Fscale.*fluxH))./mu;
return


function qwv_test02 ( )

%*****************************************************************************80
%
%% QWV_TEST02 tests QWV for a Clenshaw-Curtis rule.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 February 2014
%
%  Author:
%
%    John Burkardt
%
  a =  -1.0;
  b = +1.0;
  n = 5;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'QWV_TEST02\n' );
  fprintf ( 1, '  Use the Vandermonde procedure to compute the\n' );
  fprintf ( 1, '  quadrature weights for a Clenshaw-Curtis rule.\n' );
  fprintf ( 1, '  Order N = %d\n', n );
  fprintf ( 1, '  Interval is [%g,%g]\n', a, b );
%
%  Set the points.
%
  x = zeros ( n, 1 );

  for i = 1 : n

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

    x(i) = ( ( 1 - cos ( theta ) ) * a   ...
           + ( 1 + cos ( theta ) ) * b ) ...
           /   2.0;

  end

  r8vec_print ( n, x, '  Abscissas:' );
%
%  Determine the corresponding weights.
%
  w = qwv ( n, a, b, x );

  r8vec_print ( n, w, '  Weights:' )

  return
end


clc;
clearvars;
close all;
rng default;

basis = orth(randn(3, 2))
basis'*basis

rng(10);
A = orth(randn(3, 2))
e1 = A(:, 1);
e2 = A(:, 2);
corners = [e1+e2, e2-e1, -e1-e2, -e2+e1];
spx.graphics.figure.full_screen;
fill3(corners(1,:),corners(2,:),corners(3,:),'r');
grid on;
hold on;
alpha(0.3);
quiver3(0, 0, 0, e1(1), e1(2), e1(3), 'color', 'r');
quiver3(0, 0, 0, e2(1), e2(2), e2(3), 'color', 'r');
saveas(gcf, 'images/random_subspace_a_3d.png');
rng(30);
B = orth(randn(3, 2));
e1 = B(:, 1);
e2 = B(:, 2);
corners = [e1+e2, e2-e1, -e1-e2, -e2+e1];
fill3(corners(1,:),corners(2,:),corners(3,:),'g');
alpha(0.3);
quiver3(0, 0, 0, e1(1), e1(2), e1(3), 'color', spx.graphics.rgb('DarkGreen'));
quiver3(0, 0, 0, e2(1), e2(2), e2(3), 'color', spx.graphics.rgb('DarkGreen'));
saveas(gcf, 'images/random_subspace_a_b_3d.png');

% subspace dimension
D = 4;
% ambient dimension
M = 10;
% Number of subspaces
K = 2;
import spx.data.synthetic.subspaces.random_subspaces;
bases = random_subspaces(M, K, D);
A = bases{1};
B = bases{2};
G = A' * B
sigmas = svd(G)'
largest_product = sigmas(1)
smallest_angle_rad  = acos(largest_product)
smallest_angle_deg = rad2deg(smallest_angle_rad)


function [mi, loc] = kminima(a, k, d)
% KMINIMA specified number of minima
%     For vectors, KMINIMA(X,K) is the vector of K smallest elements in X.
%     For matrices, Y = KMINIMA(X,K) is a matrix with K rows; Y(i,:)
%     contains the i-th minimum element from each column. For N-D arrays,
%     KMINIMA(X,K) operates along the first non-singleton dimension.
%
%     [Y,I] = KMINIMA(X,K) returns the indices of the minima in vector I.
%
%     [Y,I] = KMAXIMA(X,K,DIM) operates along the dimension DIM.
%
%     When complex, the magnitude ABS(X) is considered when computing the
%     minima, and the angle ANGLE(X) is ignored.  NaN's are also ignored .

% Mukhtar Ullah
% mukhtar.ullah@informatik.uni-rostock.de
% November 17, 2004

a(isnan(a)) = inf;
if isvector(a) && nargin < 3
    [b,ix] = sort(a);
    mi = b(1:k);
    loc = ix(1:k);
else
    if nargin < 3, d = 1; end
    [b,ix] = sort(a, d);
    n = ndims(a);
    if n < 3
        if d < 2
            mi = b(1:k, :);
            loc = ix(1:k, :);
        else
            mi = b(:, 1:k);
            loc = ix(:, 1:k);
        end
    else
        C = cell(1, n);
        if d > 1, C(1:d-1) = {':,'}; end
        if d < n, C(d+1:n) = {',:'}; end
        C(d) = {'1:k'};
        S = ['(' [C{:}] ')'];
        mi = eval(['b' S]);
        if nargout > 1, loc = eval(['ix' S]); end
    end
end


function pdf = pareto_pdf ( x, a, b )

%*****************************************************************************80
%
%% PARETO_PDF evaluates the Pareto PDF.
%
%  Formula:
%
%    PDF(X)(A,B) = B * A**B / X**(B+1).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the argument of the PDF.
%    A <= X
%
%    Input, real A, B, the parameters of the PDF.
%    0.0 < A.
%    0.0 < B.
%
%    Output, real PDF, the value of the PDF.
%
  if ( x < a )
    pdf = 0.0;
  else
    pdf = b * a^b / x^( b + 1.0 );
  end

  return
end


function y=evalSinCosSeries(c,cosX,sinX)
%%EVALSINCOSSERIES Evaluate the cosine series
%                  y=sum_{i=1}^Nc(i)*cos((i-1)*x)
%                  or the sine series
%                  y=sum_{i=1}^(N+1)c(i)*sin(i*x)
%                  efficiently using Clenshaw summation.
%
%INPUTS: c An NX1 or a 1XN vector of the coefficients in the sum.
%     cosX The value cos(x). This value is always required. To evaluate
%          multiple sums at once, this can be a vector or a matrix.
%     sinX If this value is provided, it is sin(x) and the sum evaluated by
%          this function will be a sine series. Otherwise, if this
%          parameter is omitted or an empty matrix is passed, this function
%          will evaluate a cosine series. If this parameter is provided, it
%          must be the same size as cosX.
%
%OUTPUTS: y The value of the sum. If cosX was a matrix, then this will be a
%           matrix.
%
%This function chooses the correct parameters and calls the function
%evalClenshawRecurSeries.
%
%EXAMPLE:
% N=200;
% c=rand(N,1);
% x=2*pi*rand(4,4);
% cosX=cos(x);
% yF=evalSinCosSeries(c,cosX);
% ySum=zeros(4,4);
% for k=1:N
%    ySum=ySum+c(k)*cos((k-1)*x);
% end
% relativeError=max(max(abs(ySum-yF)./yF))
% %This will typically be on the order of 1e-12 or less, due simply to
% %finite precision differences.
% %Similarly, if we wanted the sine series.
% sinX=sin(x);
% yF=evalSinCosSeries(c,cosX,sinX);
% ySum=zeros(4,4);
% for k=1:N
%     ySum=ySum+c(k)*sin(k*x);
% end
% relativeError=max(max(abs(ySum-yF)./yF))
% %This will again be on the order of 1e-12 or less, due simply to finite
% %precision differences.
%
%July 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

alphaVal=2*cosX;
betaVal=-1;

if(nargin<3||isempty(sinX))
    %A cosine series.
    F0=ones(size(cosX));%=cos(0)
    F1=cosX;
    y=evalClenshawRecurSeries(c,alphaVal,betaVal,F0,F1);
else%A sine series
    F0=sinX;
    F1=2*sinX.*cosX;
    y=evalClenshawRecurSeries(c,alphaVal,betaVal,F0,F1);
end
end

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


function cadj=ref_spreadadj_4(coef)
%REF_SPREADADJ_4  Symbol of adjoint spreading function.
%   Usage: cadj=ref_spreadadj_4(c,number);
%
%   Development version by FJ for comparison of different implementations
%   cadj=SPREADADJ(c) will compute the symbol cadj of the spreading
%   operator that is the adjoint of the spreading operator with symbol c.
%
%   Improved implementation of the direct formula with higher memory
%   needs (but better speed)
%
%   This implementation uses the same improvements as case 3, but
%   also avoid the use of loop by using matrix pointwise
%   multiplication, which improves the computation time, but also
%   requires more memory due to the contruction of the (L-1)x(L-1)
%   matrix temp
%

L=size(coef,1);

cadj=zeros(L);

% Proceesing for ii==0 or jj==0
cadj(1,1)=conj(coef(1,1));
cadj(2:end,1)=conj(coef(end:-1:2,1));
cadj(1,2:end,1)=conj(coef(1,end:-1:2));

% Processing for ii~=0 and jj~=0

% Precomputation for exponential term

% Optimization note : As said in note of case 3 ,we are computing
% the Lth root of unity which have special properties and symetries
% that could be exploited to highly reduce this computation
temp2=exp((-i*2*pi/L)*(0:L-1));

% Optimization note : Here we are computing indexes for all the
% exponential terms, which leads to a highly structured matrix
% which strcture can be formalized (notably it is symetric) and
% used to reduce the computation cost
temp=mod((1:L-1)'*(1:L-1),L)+1;

% Optimization note : Finaly we construct the matrix containing all
% the needed exponential terms.
% This matrix is known as the DFT matrix and appears in the matrix
% formulation of the dft, but in our case it is used for pointwise
% multiplication instead of matrix mulitplication.
% There might be optimized algorithms for the computation of this
% matrix described in the context of fft.
temp=temp2(temp);

cadj(2:L,2:L)=conj(coef(L:-1:2,L:-1:2)).*temp;


function pdf = dipole_pdf ( x, a, b )

%*****************************************************************************80
%
%% DIPOLE_PDF evaluates the Dipole PDF.
%
%  Discussion:
%
%    PDF(X)(A,B) =
%        1 / ( PI * ( 1 + X**2 ) )
%      + B**2 * ( ( 1 - X**2 ) * cos ( 2 * A ) + 2.0D+00 * X * sin ( 2 * A ) )
%      / ( PI * ( 1 + X )**2 )
%
%    Densities of this kind commonly occur in the analysis of resonant
%    scattering of elementary particles.
%
%    DIPOLE_PDF(X)(A,0) = CAUCHY_PDF(X)(A)
%    A = 0, B = 1 yields the single channel dipole distribution.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Robert Knop,
%    Algorithm 441,
%    ACM Transactions on Mathematical Software.
%
%  Parameters:
%
%    Input, real X, the argument of the PDF.
%
%    Input, real A, B, the parameters of the PDF.
%    A is arbitrary, but represents an angle, so only 0 <= A <= 2 * PI
%      is interesting,
%    and -1.0D+00 <= B <= 1.0.
%
%    Output, real PDF, the value of the PDF.
%
  pdf = 1.0 / ( pi * ( 1.0 + x * x ) ) ...
    + b * b * ( ( 1.0 - x * x ) * cos ( 2.0 * a ) ...
    + 2.0 * x * sin ( 2.0 * x ) ) / ( pi * ( 1.0 + x * x ) * ( 1.0 + x * x ) );

  return
end


function [ vX ] = ProxHuberLossBoyd( vY, paramDelta, paramLambda )
% ----------------------------------------------------------------------------------------------- %
% [ vX ] = ProxHuberLossBoyd( vY, paramDelta, paramLambda )
%   Solves the Proximal Operator of the Huber Loss Function:
%   $$ \arg \min_{x} \frac{1}{2} {\left| x - y \right\|}_{2}^{2} + \lambda {H}_{\delta} \left( x \right) $$
%   Where {H}_{\delta} \left( x \right) is the Huber Loss Function.
% Input:
%   - vY            -   Input Vector.
%                       Structure: Vector (Column).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
%   - paramDelta    -   Parameter Delta.
%                       The Delta Parameter of the Huber Loss Function.
%                       This is the value the Huber Loss changes from
%                       L2 Norm to the L1 Norm.
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range: (0, inf).
%   - paramLambda   -   Parameter Lambda.
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range: (0, inf).
% Output:
%   - vX            -   Output Vector.
%                       The solution of the Proximal Operator.
%                       Structure: Vector (Column).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
% References
%   1.  Huber Loss (Wikipedia) - https://en.wikipedia.org/wiki/Huber_loss.
%   2.  Distributed Optimization and Statistical Learning via the Alternating Direction Method of Multipliers (See Huber Fitting).
%   3.  Proximal Operator of the Huber Loss Function - https://math.stackexchange.com/questions/3589025.
% Remarks:
%   1.  In the reference by Boyd they use the $ \rho = 1 / \lambda $ form of
%       the Proximal Operator. Hence the adoption of the scaling. Also the
%       book use Huber Loss Function with $ \delta = 1 $.
% TODO:
%   1.  U.
% Release Notes:
%   -   1.0.000     21/03/2020  Royi Avital
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

FALSE   = 0;
TRUE    = 1;

OFF     = 0;
ON      = 1;

hProxL1 = @(vX, paramLambda) sign(vX) .* max(abs(vX) - paramLambda, 0);
vX = ((1 / (1 + paramLambda)) * vY) + (paramDelta * (paramLambda / (1 + paramLambda)) * hProxL1((vY / paramDelta), 1 + paramLambda));

end


function E = keplerEq(M,e,eps)
% Function solves Kepler's equation M = E-e*sin(E)
% Input - Mean anomaly M [rad] , Eccentricity e and Epsilon
% Output  eccentric anomaly E [rad].
   	En  = M;
	Ens = En - (En-e*sin(En)- M)/(1 - e*cos(En));
	while ( abs(Ens-En) > eps )
		En = Ens;
		Ens = En - (En - e*sin(En) - M)/(1 - e*cos(En));
	end;
	E = Ens;


function [J, grad] = cofiCostFunc(params, Y, R, num_users, num_movies, ...
                                  num_features, lambda)
%COFICOSTFUNC Collaborative filtering cost function
%   [J, grad] = COFICOSTFUNC(params, Y, R, num_users, num_movies, ...
%   num_features, lambda) returns the cost and gradient for the
%   collaborative filtering problem.
%

% Unfold the U and W matrices from params
X = reshape(params(1:num_movies*num_features), num_movies, num_features);
Theta = reshape(params(num_movies*num_features+1:end), ...
                num_users, num_features);

% You need to return the following values correctly
J = 0;
X_grad = zeros(size(X));
Theta_grad = zeros(size(Theta));

% ====================== YOUR CODE HERE ======================
% Instructions: Compute the cost function and gradient for collaborative
%               filtering. Concretely, you should first implement the cost
%               function (without regularization) and make sure it is
%               matches our costs. After that, you should implement the
%               gradient and use the checkCostFunction routine to check
%               that the gradient is correct. Finally, you should implement
%               regularization.
%
% Notes: X - num_movies  x num_features matrix of movie features
%        Theta - num_users  x num_features matrix of user features
%        Y - num_movies x num_users matrix of user ratings of movies
%        R - num_movies x num_users matrix, where R(i, j) = 1 if the
%            i-th movie was rated by the j-th user
%
% You should set the following variables correctly:
%
%        X_grad - num_movies x num_features matrix, containing the
%                 partial derivatives w.r.t. to each element of X
%        Theta_grad - num_users x num_features matrix, containing the
%                     partial derivatives w.r.t. to each element of Theta
%

diff = R .* (X * Theta' - Y);
sum_squared = @(A) sum(sum(A .^ 2));

J = (sum_squared(diff) + lambda * (sum_squared(X)  + sum_squared(Theta))) /2;
X_grad = diff * Theta + lambda * X;
Theta_grad = diff' * X + lambda * Theta;

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

grad = [X_grad(:); Theta_grad(:)];

end


function geometry_test015 ( )

%*****************************************************************************80
%
%% TEST015 tests CIRCLE_EXP2IMP_2D, TRIANGLE_CIRCUMCIRCLE_2D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
  n = 3;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST015\n' );
  fprintf ( 1, '  CIRCLE_EXP2IMP_2D computes the radius and \n' );
  fprintf ( 1, '  center of the circle through three points.\n' );
  fprintf ( 1, '  TRIANGLE_CIRCUMCIRCLE_2D computes the radius and \n' );
  fprintf ( 1, '  center of the circle through the vertices of\n' );
  fprintf ( 1, '  a triangle.\n' );

  p1test(1:2,1:n) = [ ...
    4.0, 2.0; ...
    1.0, 5.0; ...
   -2.0, 2.0 ]';

  p2test(1:2,1:n) = [ ...
    4.0, 2.0; ...
    5.0, 4.0; ...
    6.0, 6.0 ]';

  p3test(1:2,1:n) = [ ...
    4.0, 2.0; ...
    1.0, 5.0; ...
    4.0, 2.0 ]';

  for i = 1 : n

    p1(1:2,1) = p1test(1:2,i);
    p2(1:2,1) = p2test(1:2,i);
    p3(1:2,1) = p3test(1:2,i);

    r8vec_print ( 2, p1, '  P1:' );
    r8vec_print ( 2, p2, '  P2:' );
    r8vec_print ( 2, p3, '  P3:' );

    [ r, center ] = circle_exp2imp_2d ( p1, p2, p3 );

    circle_imp_print_2d ( r, center, '  The implicit circle:' )

    t(1:2,1:3) = [ p1(1:2,1)'; p2(1:2,1)'; p3(1:2,1)' ]';

    [ r, center ] = triangle_circumcircle_2d ( t );

    circle_imp_print_2d ( r, center, '  The triangle''s circumcircle:' )

  end

  return
end


function [ row, col, a ] = r8sp_dif2 ( m, n, nz_num )

%*****************************************************************************80
%
%% R8SP_DIF2 returns the DIF2 matrix in R8SP format.
%
%  Example:
%
%    N = 5
%
%    2 -1  .  .  .
%   -1  2 -1  .  .
%    . -1  2 -1  .
%    .  . -1  2 -1
%    .  .  . -1  2
%
%  Properties:
%
%    A is banded, with bandwidth 3.
%
%    A is tridiagonal.
%
%    Because A is tridiagonal, it has property A (bipartite).
%
%    A is a special case of the TRIS or tridiagonal scalar matrix.
%
%    A is integral, therefore det ( A ) is integral, and
%    det ( A ) * inverse ( A ) is integral.
%
%    A is Toeplitz: constant along diagonals.
%
%    A is symmetric: A' = A.
%
%    Because A is symmetric, it is normal.
%
%    Because A is normal, it is diagonalizable.
%
%    A is persymmetric: A(I,J) = A(N+1-J,N+1-I).
%
%    A is positive definite.
%
%    A is an M matrix.
%
%    A is weakly diagonally dominant, but not strictly diagonally dominant.
%
%    A has an LU factorization A = L * U, without pivoting.
%
%      The matrix L is lower bidiagonal with subdiagonal elements:
%
%        L(I+1,I) = -I/(I+1)
%
%      The matrix U is upper bidiagonal, with diagonal elements
%
%        U(I,I) = (I+1)/I
%
%      and superdiagonal elements which are all -1.
%
%    A has a Cholesky factorization A = L * L', with L lower bidiagonal.
%
%      L(I,I) =    sqrt ( (I+1) / I )
%      L(I,I-1) = -sqrt ( (I-1) / I )
%
%    The eigenvalues are
%
%      LAMBDA(I) = 2 + 2 * COS(I*PI/(N+1))
%                = 4 SIN^2(I*PI/(2*N+2))
%
%    The corresponding eigenvector X(I) has entries
%
%       X(I)(J) = sqrt(2/(N+1)) * sin ( I*J*PI/(N+1) ).
%
%    Simple linear systems:
%
%      x = (1,1,1,...,1,1),   A*x=(1,0,0,...,0,1)
%
%      x = (1,2,3,...,n-1,n), A*x=(0,0,0,...,0,n+1)
%
%    det ( A ) = N + 1.
%
%    The value of the determinant can be seen by induction,
%    and expanding the determinant across the first row:
%
%      det ( A(N) ) = 2 * det ( A(N-1) ) - (-1) * (-1) * det ( A(N-2) )
%                = 2 * N - (N-1)
%                = N + 1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 July 2000
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Robert Gregory, David Karney,
%    A Collection of Matrices for Testing Computational Algorithms,
%    Wiley, 1969,
%    ISBN: 0882756494,
%    LC: QA263.68
%
%    Morris Newman, John Todd,
%    Example A8,
%    The evaluation of matrix inversion programs,
%    Journal of the Society for Industrial and Applied Mathematics,
%    Volume 6, Number 4, pages 466-476, 1958.
%
%    John Todd,
%    Basic Numerical Mathematics,
%    Volume 2: Numerical Algebra,
%    Birkhauser, 1980,
%    ISBN: 0817608117,
%    LC: QA297.T58.
%
%    Joan Westlake,
%    A Handbook of Numerical Matrix Inversion and Solution of
%    Linear Equations,
%    John Wiley, 1968,
%    ISBN13: 978-0471936756,
%    LC: QA263.W47.
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, integer NZ_NUM, the number of nonzero elements in
%    the matrix.
%
%    Output, integer ROW(NZ_NUM), COL(NZ_NUM), the row and
%    column indices of the nonzero elements.
%
%    Output, real A(NZ_NUM), the nonzero elements of the matrix.
%
  row = zeros(nz_num,1);
  col = zeros(nz_num,1);
  a = zeros(nz_num,1);

  k = 0;
  for i = 1 : m

    if ( 0 < i - 1 )
      k = k + 1;
      row(k) = i;
      col(k) = i - 1;
      a(k) = -1.0;
    end

    k = k + 1;
    row(k) = i;
    col(k) = i;
    a(k) = 2.0;

    if ( i < n )
      k = k + 1;
      row(k) = i;
      col(k) = i + 1;
      a(k) = -1.0;
    end

  end

  return
end


function linpack_d_test06 ( )

%*****************************************************************************80
%
%% TEST06 tests DGBFA and DGBDI.
%
%  Discussion:
%
%    Matrix A is ( 2 -1  0  0  0)
%                (-1  2 -1  0  0)
%                ( 0 -1  2 -1  0)
%                ( 0  0 -1  2 -1)
%                ( 0  0  0 -1  2)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2009
%
%  Author:
%
%    John Burkardt
%
%  Local Parameters:
%
%    N is the number of equations.
%
%    ML is the number of subdiagonals,
%    MU the number of superdiagonals.
%
%    LDA is the leading dimension of the array used to store the
%    matrix, which must be at least 2*ML+MU+1.
%
  n_max = 128;
  ml = 1;
  mu = 1;
  lda = 2*ml+mu+1;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST06\n' );
  fprintf ( 1, '  For a general banded matrix,\n' );
  fprintf ( 1, '  DGBFA factors the matrix,\n' );
  fprintf ( 1, '  DGBDI computes the determinant as\n' );
  fprintf ( 1, '    det = MANTISSA * 10^EXPONENT\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Find the determinant of the -1,2,-1 matrix\n' );
  fprintf ( 1, '  for N = 2, 4, 8, 16, 32, 64, 128.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  (For this matrix, det ( A ) = N + 1.)\n' );
%
%  Set the matrix A.
%
  m = ml + mu + 1;
  fprintf ( 1, '  The bandwidth of the matrix is %d\n', m );
  fprintf ( 1, '\n' );
  fprintf ( 1, '       N    Mantissa       Exponent\n' );
  fprintf ( 1, '\n' );

  n = 1;

  for n_log = 1 : 7

    n = 2 * n;

    a(1:lda,1:n) = 0.0;

    for j = 1 : n
      a(m-1,j) = -1.0;
      a(m,j) =    2.0;
      a(m+1,j) = -1.0;
    end

    [ a, ipivot, info ] = dgbfa ( a, lda, n, ml, mu );

    if ( info ~= 0 )
      fprintf ( 1, '  Error!  DGBFA returns INFO = %d\n', info );
      return
    end

    det = dgbdi ( a, lda, n, ml, mu, ipivot );

    fprintf ( 1, '  %6d  %14f  %14f\n', n, det(1), det(2) );

  end

  return
end


function g = p16_g ( n, x )

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

  f1 = 1.5   - x(1) * ( 1.0 - x(2)    );
  f2 = 2.25  - x(1) * ( 1.0 - x(2) * x(2) );
  f3 = 2.625 - x(1) * ( 1.0 - x(2) * x(2) * x(2) );

  df1dx1 = - ( 1.0 - x(2) );
  df1dx2 = x(1);
  df2dx1 = - ( 1.0 - x(2) * x(2) );
  df2dx2 = 2.0 * x(1) * x(2);
  df3dx1 = - ( 1.0 - x(2) * x(2) * x(2) );
  df3dx2 = 3.0 * x(1) * x(2) * x(2);

  g(1) = 2.0 * ( f1 * df1dx1 + f2 * df2dx1 + f3 * df3dx1 );
  g(2) = 2.0 * ( f1 * df1dx2 + f2 * df2dx2 + f3 * df3dx2 );

  return
end


function w = simplex_projection(v, b)
% Simplex_Projection Projects point onto simplex of specified radius.
%
% w = simplex_projection(v, b) returns the vector w which is the solution
%   to the following constrained minimization problem:
%
%    min   ||w - v||_2
%    s.t.  sum(w) <= b, w >= 0.
%
%   That is, performs Euclidean projection of v to the positive simplex of
%   radius b.
%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This file is part of OLPS: http://OLPS.stevenhoi.org/
% Original authors: John Duchi (jduchi@cs.berkeley.edu)
% Contributors: Bin LI, Steven C.H. Hoi
% Change log:
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if (b < 0)
  error('Radius of simplex is negative: %2.3f\n', b);
end
v = (v > 0) .* v;
u = sort(v,'descend');
sv = cumsum(u);
rho = find(u > (sv - b) ./ (1:length(u))', 1, 'last');
theta = max(0, (sv(rho) - b) / rho);
w = max(v - theta, 0);


function R = randrot(n, N)
% Generates uniformly random rotation matrices.
%
% function R = randrot(n, N)
%
% R is a n-by-n-by-N matrix such that each slice R(:, :, i) is an
% orthogonal matrix of size n of determinant +1 (i.e., a matrix in SO(n)).
% By default, N = 1.
% Complexity: N times O(n^3).
% Theory in Diaconis and Shahshahani 1987 for the uniformity on O(n);
% With details in Mezzadri 2007,
% "How to generate random matrices from the classical compact groups."
% To ensure matrices in SO(n), we permute the two first columns when
% the determinant is -1.
%
% See also: randskew

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

    if nargin < 2
        N = 1;
    end

    if n == 1
        R = ones(1, 1, N);
        return;
    end

    R = zeros(n, n, N);

    for i = 1 : N

        % Generated as such, Q is uniformly distributed over O(n), the set
        % of orthogonal matrices.
        A = randn(n);
        [Q, RR] = qr(A);
        Q = Q * diag(sign(diag(RR))); %% Mezzadri 2007

        % If Q is in O(n) but not in SO(n), we permute the two first
        % columns of Q such that det(new Q) = -det(Q), hence the new Q will
        % be in SO(n), uniformly distributed.
        if det(Q) < 0
            Q(:, [1 2]) = Q(:, [2 1]);
        end

        R(:, :, i) = Q;

    end

end


function a = carry ( n, alpha )

%*****************************************************************************80
%
%% CARRY returns the CARRY matrix.
%
%  Discussion:
%
%    We assume that arithmetic is being done in base ALPHA.  We are adding
%    a column of N digits base ALPHA, as part of adding N random numbers.
%    We know the carry digit, between 0 and N-1, that is being carried into the
%    column sum (the incarry digit), and we want to know the probability of
%    the various carry digits 0 through N-1 (the outcarry digit) that could
%    be carried out of the column sum.
%
%    The carry matrix summarizes this data.  The entry A(I,J) represents
%    the probability that, given that the incarry digit is I-1, the
%    outcarry digit will be J-1.
%
%  Formula:
%
%    A(I,J) = ( 1 / ALPHA )^N * sum ( 0 <= K <= J-1 - floor ( I-1 / ALPHA ) )
%      (-1)^K * C(N+1,K) * C(N-I+(J-K)*ALPHA, N )
%
%  Example:
%
%    ALPHA = 10, N = 4
%
%    0.0715 0.5280 0.3795 0.0210
%    0.0495 0.4840 0.4335 0.0330
%    0.0330 0.4335 0.4840 0.0495
%    0.0210 0.3795 0.5280 0.0715
%
%  Square Properties:
%
%    A is generally not symmetric: A' /= A.
%
%    A is a Markov matrix.
%
%    A is centrosymmetric: A(I,J) = A(N+1-I,N+1-J).
%
%    LAMBDA(I) = 1 / ALPHA^(I-1)
%
%    det ( A ) = 1 / ALPHA^((N*(N-1))/2)
%
%    The eigenvectors do not depend on ALPHA.
%
%    A is generally not normal: A' * A /= A * A'.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 September 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    John Holte,
%    Carries, Combinatorics, and an Amazing Matrix,
%    The American Mathematical Monthly,
%    February 1997, pages 138-149.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer ALPHA, the numeric base being used in the addition.
%
%    Output, real A(N,N), the matrix.
%
  alpha = floor ( alpha );

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

      temp = 0.0;
      s = -1.0;

      for k = 0 : j - 1 - floor ( ( i - 1 ) / alpha )
        s = - s;
        c1 = r8_choose ( n + 1, k );
        c2 = r8_choose ( n - i + ( j - k ) * alpha, n );
        temp = temp + s * c1 * c2;
      end

      a(i,j) = temp / alpha^n;

    end

  end

  return
end


function S=tria(A)
%%TRIA Square root matrix triangularization. Given a rectangular square
%      root matrix, obtain a lower-triangular square root matrix that is
%      square.
%
%INPUTS: A A numRowXnumCol matrix that is generally not square.
%
%OUTPUTS: S A lower-triangular matrix such that S*S'=A*A'. If
%           numCol>=numRow, then S is a square numRowXnumRow matrix.
%           Otherwise, S is a numRowXnumCol matrix.
%
%This is the tria function needed for various steps in the cubature Kalman
%filter and the square root Kalman filter. It is described in [1]. It has
%been slightly modified from the paper so that the diagonal elements remain
%positive.
%
%REFERENCES:
%[1] D. F. Crouse, "Basic tracking using nonlinear 3D monostatic and
%    bistatic measurements," IEEE Aerospace and Electronic Systems
%    Magazine, vol. 29, no. 8, Part II, pp. 4-53, Aug. 2014.
%
%July 2012 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    [~,R]=qr(A',0);
    S=R';

    %Make the diagonal elements all positive.
    sel=diag(S)<0;
    S(:,sel)=-S(:,sel);
end

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


function r=rotpl2ro(u,v,t)
%ROTPL2RO find matrix to rotate in the plane containing u and v r=[u,v,t]
% Inputs:
%
%     U(n,1) and V(n,1) define a plane in n-dimensional space
%     T is the rotation angle in radians from U towards V. If T
%       is omitted it will default to the angle between U and V
%
% Outputs:
%
%     R(n,n)   Rotation matrix

%
%      Copyright (C) Mike Brookes 2007
%      Version: $Id: rotpl2ro.m,v 1.1 2007/11/21 16:50:32 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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

u=u(:);
    n=length(u);
v=v(:);
l=sqrt(u'*u);
if l==0, error('input u is a zero vector'); end
u=u/l;      % normalize
q=v-v'*u*u;        % q is orthogonal to x
l=sqrt(q'*q);
if l==0          % u and v are colinear or v=zero
    [m,i]=max(abs(u));
    q=zeros(n,1);
    q(1+mod(i(1),n))=1;  % choose next available dimension
    q=q-q'*u*u;  % q is orthogonal to x
    l=sqrt(q'*q);
end
q=q/l;          % normalize
if nargin<3
    [s,c]=atan2sc(v'*q,v'*u);
    r=eye(n)+(c-1)*(u*u'+q*q')+s*(q*u'-u*q');
else
    r=eye(n)+(cos(t)-1)*(u*u'+q*q')+sin(t)*(q*u'-u*q');
end


function [theta,alpha,e]=tls(Z)
% tls
% Computes tls estimate of hyperplane parameters
% [theta, alpha] = tls ( Z)
% Z is of form (p1'; p2'; ..) where pi is p-by-1 (dimension p)
% Computed hyperplane is of form theta' * p = alpha with alpha>0

%* Author: Ranjith Unnikrishnan                                          *
%* Carnegie Mellon University, Vision and Mobile Robotics Laboratory       *
%* THE MATERIAL EMBODIED IN THIS SOFTWARE IS PROVIDED TO YOU "AS-IS"     *
%* AND WITHOUT WARRANTY OF ANY KIND, EXPRESS, IMPLIED OR OTHERWISE,      *
%* INCLUDING WITHOUT LIMITATION, ANY WARRANTY OF MERCHANTABILITY OR      *
%* FITNESS FOR A PARTICULAR PURPOSE.  IN NO EVENT SHALL CARNEGIE MELLON  *
%* UNIVERSITY BE LIABLE TO YOU OR ANYONE ELSE FOR ANY DIRECT,            *
%* SPECIAL, INCIDENTAL, INDIRECT OR CONSEQUENTIAL DAMAGES OF ANY         *
%* KIND, OR ANY DAMAGES WHATSOEVER, INCLUDING WITHOUT LIMITATION,        *
%* LOSS OF PROFIT, LOSS OF USE, SAVINGS OR REVENUE, OR THE CLAIMS OF     *
%* THIRD PARTIES, WHETHER OR NOT CARNEGIE MELLON UNIVERSITY HAS BEEN     *
%* ADVISED OF THE POSSIBILITY OF SUCH LOSS, HOWEVER CAUSED AND ON        *
%* ANY THEORY OF LIABILITY, ARISING OUT OF OR IN CONNECTION WITH THE     *
%* POSSESSION, USE OR PERFORMANCE OF THIS SOFTWARE.                      *
%

p=size(Z,2);
n=size(Z,1);
centroid=mean(Z,1);
Z=Z-repmat(centroid,n,1);
[U,S,V]=svd(Z'*Z);
theta=U(:,p);
alpha=theta'*centroid';

if(alpha<0)
    theta=-theta;
    alpha=-alpha;
end

e=median( abs(Z*theta - alpha +centroid*theta));
return;


function geometry_test200 ( )

%*****************************************************************************80
%
%% TEST200 tests SPHERE_TRIANGLE_SIDES_TO_ANGLES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  r = 10.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST200\n' );
  fprintf ( 1, '  SPHERE_TRIANGLE_SIDES_TO_ANGLES takes the sides of a\n' );
  fprintf ( 1, '  spherical triangle and determines the angles.\n' );

  as = 121.0 + ( 15.4 / 60.0 );
  bs = 104.0 + ( 54.7 / 60.0 );
  cs =  65.0 + ( 42.5 / 60.0 );

  as = degrees_to_radians ( as );
  bs = degrees_to_radians ( bs );
  cs = degrees_to_radians ( cs );

  as = r * as;
  bs = r * bs;
  cs = r * cs;
%
%  Get the spherical angles.
%
  [ a, b, c ] = sphere_triangle_sides_to_angles ( r, as, bs, cs );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  A       = %8f (radians)\n', a );
  a = radians_to_degrees ( a );
  fprintf ( 1, '  A       = %8f (degrees)\n', a );
  a = 117.0 + ( 58.0 / 60.0 );
  fprintf ( 1, '  Correct = %8f (radians)\n', a );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  B       = %8f (radians)\n', b );
  b = radians_to_degrees ( b );
  fprintf ( 1, '  B       = %8f (degrees)\n', b );
  b = 93.0 + ( 13.8 / 60.0 );
  fprintf ( 1, '  Correct = %8f (radians)\n', b );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  C       = %8f (radians)\n', c );
  c = radians_to_degrees ( c );
  fprintf ( 1, '  C       = %8f (degrees)\n', c );
  c = 70.0 + ( 20.6 / 60.0 );
  fprintf ( 1, '  Correct = %8f (radians)\n', c );

  return
end


% Maximum volume inscribed ellipsoid in a polyhedron
% Section 8.4.1, Boyd & Vandenberghe "Convex Optimization"
% Original version by Lieven Vandenberghe
% Updated for CVX by Almir Mutapcic - Jan 2006
% (a figure is generated)
%
% We find the ellipsoid E of maximum volume that lies inside of
% a polyhedra C described by a set of linear inequalities.
%
% C = { x | a_i^T x <= b_i, i = 1,...,m } (polyhedra)
% E = { Bu + d | || u || <= 1 } (ellipsoid)
%
% This problem can be formulated as a log det maximization
% which can then be computed using the det_rootn function, ie,
%     maximize     log det B
%     subject to   || B a_i || + a_i^T d <= b,  for i = 1,...,m

% problem data
n = 2;
px = [0 .5 2 3 1];
py = [0 1 1.5 .5 -.5];
m = size(px,2);
pxint = sum(px)/m; pyint = sum(py)/m;
px = [px px(1)];
py = [py py(1)];

% generate A,b
A = zeros(m,n); b = zeros(m,1);
for i=1:m
  A(i,:) = null([px(i+1)-px(i) py(i+1)-py(i)])';
  b(i) = A(i,:)*.5*[px(i+1)+px(i); py(i+1)+py(i)];
  if A(i,:)*[pxint; pyint]-b(i)>0
    A(i,:) = -A(i,:);
    b(i) = -b(i);
  end
end

% formulate and solve the problem
cvx_begin
    variable B(n,n) symmetric
    variable d(n)
    maximize( det_rootn( B ) )
    subject to
       for i = 1:m
           norm( B*A(i,:)', 2 ) + A(i,:)*d <= b(i);
       end
cvx_end

% make the plots
noangles = 200;
angles   = linspace( 0, 2 * pi, noangles );
ellipse_inner  = B * [ cos(angles) ; sin(angles) ] + d * ones( 1, noangles );
ellipse_outer  = 2*B * [ cos(angles) ; sin(angles) ] + d * ones( 1, noangles );

clf
plot(px,py)
hold on
plot( ellipse_inner(1,:), ellipse_inner(2,:), 'r--' );
plot( ellipse_outer(1,:), ellipse_outer(2,:), 'r--' );
axis square
axis off
hold off


function a = sylvester_kac ( n )

%*****************************************************************************80
%
%% SYLVESTER_KAC returns the SYLVESTER_KAC matrix.
%
%  Formula:
%
%    If J = I - 1
%      A(I,J) = N + 1 - I
%    If J = I + 1
%      A(I,J) = I
%
%  Example:
%
%    N = 5,
%
%    0 1 0 0 0
%    4 0 2 0 0
%    0 3 0 3 0
%    0 0 2 0 4
%    0 0 0 1 0
%
%  Properties:
%
%    A is generally not symmetric: A' /= A.
%
%    A is tridiagonal.
%
%    If N is odd, the eigenvalues are:
%      -(N-1), -(N-3), ..., -2, 0, 2, ... (N-3), (N-1).
%
%    If N is even, the eigenvalues are:
%      -(N-1), -(N-3), ..., -1, +1, ..., (N-3), (N-1).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 April 2015
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Paul Clement,
%    A class of triple-diagonal matrices for test purposes,
%    SIAM Review,
%    Volume 1, 1959, pages 50-52.
%
%    Olga Taussky, John Todd,
%    Another Look at a Matrix of Mark Kac,
%    Linear Algebra and its Applications,
%    Volume 150, 1991, pages 341-360.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  for i = 1 : n - 1
    a(i,i+1) = i;
    a(i+1,i) = n - i;
  end

  return
end


function kl = cross_entropy(p, q, symmetric)
% CROSS_ENTROPY Compute the Kullback-Leibler divergence between two discrete prob. distributions
% kl = cross_entropy(p, q, symmetric)
%
% If symmetric = 1, we compute the symmetric version. Default: symmetric = 0;

tiny = exp(-700);
if nargin < 3, symmetric = 0; end
p = p(:);
q = q(:);
if symmetric
  kl  = (sum(p .* log((p+tiny)./(q+tiny))) + sum(q .* log((q+tiny)./(p+tiny))))/2;
else
  kl  = sum(p .* log((p+tiny)./(q+tiny)));
end


function [ c, sigma ] = covariance_to_correlation ( n, k )

%*****************************************************************************80
%
%% COVARIANCE_TO_CORRELATION: correlation matrix from a covariance matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real K(N,N), the covariance matrix.
%
%    Output, real C(N,N), the correlation matrix.
%
%    Output, real SIGMA(N), the standard deviations.
%
  tol = sqrt ( eps );
%
%  K must be symmetric.
%
  error_frobenius = r8mat_is_symmetric ( n, n, k );

  if ( tol < error_frobenius )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'COVARIANCE_TO_CORRELATION - Fatal error!\n' );
    fprintf ( 1, '  Input matrix K fails symmetry test with error %g\n', error_frobenius );
    error ( 'COVARIANCE_TO_CORRELATION - Fatal error!' );
  end
%
%  It must be the case that K(I,J)^2 <= K(I,I) * K(J,J).
%
  error_max = 0.0;
  for i = 1 : n
    for j = i + 1 : n
      error_max = max ( error_max, k(i,j)^2 - k(i,i) * k(j,j) );
    end
  end

  if ( tol < error_max )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'COVARIANCE_TO_CORRELATION - Fatal error!\n' );
    fprintf ( 1, '  Input matrix K fails K(I,J)^2 <= K(I,I)*K(J,J)\n' );
    error ( 'COVARIANCE_TO_CORRELATION - Fatal error!' );
  end
%
%  Get the diagonal.
%
  sigma = zeros ( n, 1 );
  for i = 1 : n
    sigma(i) = k(i,i);
  end
%
%  Ensure the diagonal is positive.
%
  sigma_min = min ( sigma(1:n) );

  if ( sigma_min <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'COVARIANCE_TO_CORRELATION - Fatal error!\n' );
    fprintf ( 1, '  Input matrix K has nonpositive diagonal entry = %g\n', sigma_min );
    error ( 'COVARIANCE_TO_CORRELATION - Fatal error!' );
  end
%
%  Convert from variance to standard deviation.
%
  sigma(1:n) = sqrt ( sigma(1:n) );
%
%  Form C.
%
  c(1:n,1:n) = diag ( 1.0 ./ sigma(1:n) ) * k(1:n,1:n) * diag ( 1.0 ./ sigma(1:n) );

  return
end


function varargout = quantumstates(varargin)
%QUANTUMSTATES    Compute and plot Schroedinger eigenstates.
%   This program computes and plots eigenvalues lambda and eigenfunctions u
%   for the equation Lu = lambda*u, where L is the Schroedinger operator
%   defined by Lu(x) = -h^2*u"(x) + V(x)*u(x).  Here h is a small parameter
%   and the potential function V is given as a Chebfun. The domain of the
%   problem is the domain of V, with boundary conditions u=0 at both ends.
%
%   Inputs:
%
%       QUANTUMSTATES(V) plots 10 eigenstates for h=0.1
%       QUANTUMSTATES(V, n) plots n eigenstates for h=0.1
%       QUANTUMSTATES(V, h), h noninteger, plots 10 eigenstates for given h
%       QUANTUMSTATES(V, n, h) plots n eigenstates for given h
%       QUANTUMSTATES(..., 'noplot') produces no plot
%
%   Outputs:
%
%       D = QUANTUMSTATES(...) returns a vector D of eigenvalues
%       [U, D] = QUANTUMSTATES(...) returns a quasimatrix U of eigenfunctions
%       and a diagonal matrix of eigenvalues
%
%   Examples:
%
%       x = chebfun('x', [-3, 3]);
%       V = x.^2;                 % harmonic oscillator, or
%       V = abs(x);               % absolute value, or
%       V = (x.^2-1).^4;          % double well

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

%% Parsing of inputs:
noplot = strcmpi(varargin{nargin}, 'noplot');      % check for no plot
nargin1 = nargin;                                  % no of input args
if ( noplot )
    nargin1 = nargin1 - 1;
end
V = varargin{1};                                   % potential function
n = 10;                                            % default no of states
h = 0.1;                                           % default constant
if ( nargin1 == 3 )
    n = varargin{2};
    h = varargin{3};
end
if ( nargin1 == 2 )
    v2 = varargin{2};
    if ( v2 == round(v2) )
        n = v2;
    else
        h = v2;
    end
end

%% Eigenvalue computation:
[xmin, xmax] = domain(V);                          % domain of problem
% Create a CHEBOP with Dirichlet BCs
L = chebop([xmin, xmax]);
L.lbc = 0; L.rbc = 0;
L.op = @(x,u) -h^2*diff(u,2) + V.*u;               % Schroedinger operator
[U, D] = eigs(L, n, 'sr');                         % compute evals/efuns
d = diag(D);                                       % vector of evals
[d, ii] = sort(d);                                 % sort them
U = U(:,ii);

%% Outputs:
if ( nargout == 2 )
    varargout = {U, diag(d)};
else
    varargout = {d};
end

if ( noplot )
    % If we're not plotting, then we return.
    return
end

%% Plot:

holdState = ishold;

% Plot the potential function:
LW = 'linewidth'; lw = 1;
plot(V, 'k', LW, lw, 'jumpline', '-k'), hold on
s = sprintf('h = %4g      %d eigenstates', h, n);
title(s, 'fontsize', 12)

% Vertical limits:
ymax = max(d);
ymin = min(V);
ydiff = ymax - ymin;
ymax = ymax + .2*ydiff;
ymin = ymin - 0*ydiff;

% V values at endpoints:
Vxmin = feval(V, xmin);
Vxmax = feval(V, xmax);
if ( ymax > Vxmin )                              % The potential
    plot(xmin*[1 1], [ymax, Vxmin], 'k', LW, lw)  %   V(x) effectively
end                                              %   goes to infinity
if ( ymax > Vxmax )                              %   at the endpoints,
    plot(xmax*[1 1], [ymax Vxmax], 'k', LW, lw)   %   so we make the
end                                              %   plot show this.
dx = .05*(xmax - xmin);
dy = .25*ydiff/max(5, n);

% Plot the eigenfunction, lifted by the corresponding eigenvalue:
W = dy*U;
W = num2cell(W);
for j = 1:n
    umm = minandmax(W{j});
    if ( umm(2) < -umm(1) )
        W{j} = -W{j};
    end
    W{j} = W{j} + d(j);
end
W = horzcat(W{:});
plot(W, LW, lw)

% Plot V(x) again (so that black ends up on top):
plot(V, 'k', LW, lw, 'jumpline', '-k')
if ( ymax > Vxmin )
    plot(xmin*[1, 1], [ymax, Vxmin], 'k', LW, lw)
end
if ( ymax > Vxmax )
    plot(xmax*[1, 1], [ymax Vxmax], 'k', LW, lw)
end

% Set axis:
axis([xmin - dx, xmax + dx, ymin - dy, ymax]), drawnow

if ( ~holdState )
    % Stop holding:
    hold off
end

end


function [xc,yc,radius] = fit_circle_to_points(x,y)

x = x(:);
y = y(:);

% solve for parameters a, b, and c in the least-squares sense by
% using the backslash operator
abc = [x y ones(numel(x),1)] \ -(x.^2+y.^2);
a = abc(1); b = abc(2); c = abc(3);

% calculate the location of the center and the radius
xc = -a/2;
yc = -b/2;
radius  =  sqrt((xc^2+yc^2)-c);


classdef matrix < handle

    methods(Static)
        function result = is_square(A)
            [m, n]  = size(A);
            result = (m == n);
        end

        function result = is_symmetric(A)
            result = spx.matrix.is_square(A);
            if ~result
                return;
            end
            result =  all(all(A==A.'));
        end

        function result = is_hermitian(A)
            result = spx.matrix.is_square(A);
            if ~result
                return;
            end
            result =  all(all(A==A'));
        end

        function result = is_positive_definite(A)
            result = spx.matrix.is_hermitian(A);
            if ~result
                return;
            end
            % Perform Cholesky decomposition of A
            [R, p] = chol(A);
            result = (p == 0);
        end

        function result = is_orthogonal(A)
            G = A' * A;
            [m, n] = size(G);
            idx = eye(m, n);
            G = abs(G(~idx));
            result = all(G <= 1e-12);
        end

        function result = is_identity(A)
            [m, n]  = size(A);
            if m ~= n
                result = false;
                return;
            end
            I = eye(m);
            diff = abs(A - I);
            result = all(all(diff <= 1e-12));
        end

        function result = is_orthonormal(A)
            if ~isreal(A)
                result = false;
                return;
            end
            G = A' * A;
            m = size(G, 1);
            I = eye(m);
            diff = abs(G - I);
            result = all(all(diff <= 1e-12));
        end

        function X = off_diagonal_elements(X)
            % Returns a column vector of off diagonal elements
            [m, n] = size(X);
            idx = eye(m, n);
            X = X(~idx);
        end

        function X = off_diagonal_matrix(X)
            % Returns the matrix of off diagonal elements
            [m, n] = size(X);
            idx = eye(m, n);
            X = (1 - idx).* X;
        end

        function X = off_diag_upper_tri_elements(X)
            % Returns the upper triangular off diagonal elements in a vector
            [m, n] = size(X);
            idx = tril(ones(m, n));
            X = X(~idx);
        end

        function X = off_diag_upper_tri_matrix(X)
            % Returns the upper triangular off diagonal elements in a matrix
            [m, n] = size(X);
            idx = tril(ones(m, n));
            X = (1 - idx).* X;
        end

        function result = nonzero_density(X)
            % Density of nonzero entries in the matrix
            result = nnz(X) / numel(X);
        end

        function [ result ] = is_diagonally_dominant( A, strict)
        %ISDIAGONALLYDOMINANT Returns if A is a diagonally dominant
        %matrix
        if ~exist('strict', 'var')
            strict = true;
        end
        A = abs(A);
        % Extract the diagonal
        d = diag(A);
        % Set the diagonal elements to 0
        A(logical(eye(size(A)))) = 0;
        % Now sum over the rows
        s = sum(A, 2);
        % We now check whether
        if strict
            result = all(d > s);
        else
            result = all(d >= s);
        end
        end

        function [ A ] = make_diagonally_dominant( A, strict )
        %MAKEDIAGONALLYDOMINANT Makes a matrix diagonally dominant
        if ~exist('strict', 'var')
            strict = true;
        end
        B = abs(A);
        % Set the diagonal elements to 0
        B(logical(eye(size(B)))) = 0;
        % Now sum over the rows
        s = sum(B, 2);
        if strict
            d = s + 1;
        else
            d = s;
        end
        % Extract the diagonal elements of A
        dd = diag(A);
        % Identify the ones which are not dominant
        requiredChanges = abs(dd) < d;
        % Assign the updated values
        dd(requiredChanges) = d(requiredChanges);
        % Update the matrix
        A(logical(eye(size(A)))) = dd;
        end

        function compare(A, B)
            % Compares two matrices
            C = abs(A - B);
            max_diff = max(max(abs(C)));
            fprintf('Maximum difference: %.4e\n', max_diff);
        end

        function df = dof_lowrank(m, n, r)
            % Returns the degrees of freedom of a low rank matrix
            df = r * (m + n - r);
        end

    end

end


function out = proj_psd(X)
%PROJ_PSD computes the orthogonal projection onto the cone of positive semidefinite matrics
%                                     (psd cone)   {X: X psd }
%
%  Usage:
%  out = PROJ_PSD(X)
%  ===========================================
%  INPUT:
%  X - matrix to be projected
%  ===========================================
%  Assumptions:
%  X is symmetric
%  ===========================================
%  Output:
%  out - projection matrix

% This file is part of the FOM package - a collection of first order methods for solving convex optimization problems
% Copyright (C) 2017 Amir and Nili Beck
%
% 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/>.

if (nargin < 1)
    error ('usage: proj_psd(X)') ;
end

eps = 1e-10 ;   % defalut value for eps : 1e-10
if ((size(X,1) ~= size(X,2)) || (norm( X - X') > eps))
    error('usage: proj_psd(X) - X should be a symmetric matrix') ;
end

X = 0.5 * (X + X');

[V,D] = eig(X) ;

out = V * max(D,0) *V' ;


function node_xyz = sphere01_triangle_project ( a_xyz, b_xyz, c_xyz, f1, ...
  f2, f3 )

%*****************************************************************************80
%
%% SPHERE01_TRIANGLE_PROJECT projects from plane to spherical triangle.
%
%  Discussion:
%
%    We assume that points A, B and C lie on the unit sphere, and they
%    thus define a spherical triangle.
%
%    They also, of course, define a planar triangle.
%
%    Let (F1,F2,F3) be the barycentric coordinates of a point in this
%    planar triangle.
%
%    This function determines the coordinates of the point in the planar
%    triangle identified by the barycentric coordinates, and returns the
%    coordinates of the projection of that point onto the unit sphere.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 April 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A_XYZ(3), B_XYZ(3), C_XYZ(3), the coordinates
%    of the points A, B, and C.
%
%    Input, integer F1, F2, F3, the barycentric coordinates
%    of a point in the triangle ABC.  Normally, these coordinates would
%    be real numbers, and would sum to 1.  For convenience, we allow these
%    to be integers which must be divided by F1+F2+F3.
%
%    Output, real NODE_XYZ(3), the coordinates of the
%    point on the unit sphere which is the projection of the point on the plane
%    whose barycentric coordinates with respect to A, B, and C is
%    (F1,F2,F3)/(F1+F2+F3).
%

%
%  Destroy all row vectors!
%
  a_xyz = a_xyz(:);
  b_xyz = b_xyz(:);
  c_xyz = c_xyz(:);

  node_xyz(1:3,1) = ...
    ( ( f1           ) * a_xyz(1:3)   ...
    + (      f2      ) * b_xyz(1:3)   ...
    + (           f3 ) * c_xyz(1:3) ) ...
    / ( f1 + f2 + f3 );

  node_norm = r8vec_norm ( 3, node_xyz(1:3,1) );

  node_xyz(1:3,1) = node_xyz(1:3,1) / node_norm;

  return
end


%% Mesh Smoothing and Optimization
%
%% Improve geometric mesh quality
%
% The function [node,elem] = optmesh(node,elem) will optimize the shape
% regularity of triangles in the input mesh (node,elem) and outputs a
% better mesh (node,elem).

clear all; close all;
load airfoilperturb
figure(1); subplot(1,2,1);
showmesh(node,elem); title('original mesh');
figure(2); subplot(1,2,1);
showmeshquality(node,elem); axis([0 1 0 2700]);
[node,elem] = optmesh(node,elem);
figure(1); subplot(1,2,2);
showmesh(node,elem); title('smoothed mesh');
figure(2); subplot(1,2,2);
showmeshquality(node,elem); axis([0 1 0 2700]);
%%
% We explain algorithms implemented in optimesh.m in the following.

%% ODT-based mesh smoothing
%
% In the function <../../../mesh/html/meshsmoothing.html meshsmoothing>, we move one node at a time inside its
% patch, which consists of all triangles surrounding this node, such that
% the interpolation error to a quadratic function is minimized. The
% function |meshsmoothing| will keep the topology of the input mesh, i.e.,
% the node index and connectivity of nodes are unchanged.
%
% In the simplest case, the scheme is to move the node to the average of
% circumenters of triangles in the local patch. Details can be found in the
% paper <http://math.uci.edu/~chenlong/CH2008.html ODTmesh>.
theta = [-2*pi/3 -pi/3 0 pi/3 2*pi/3 pi]';
node = [cos(theta), sin(theta)];
node(end+1,:) = 0;
elem = delaunayn(node);
node(end,:) = rand(1,2)*0.4;
figure(1); subplot(1,2,1);
showmesh(node,elem); findnode(node,'all','noindex');
c = circumcenter(node,elem);
hold on; plot(c(:,1),c(:,2),'r.','MarkerSize',16)
node(end,:) = mean(c);
plot(node(end,1),node(end,2),'b.','MarkerSize',16)
figure(1); subplot(1,2,2);
showmesh(node,elem); findnode(node,'all','noindex');


% Create a spectrum with a linear phase, a given delay in samples
%
% Description
%  If the spectrum length is even, the Nyquist's phase is managed as follow:
%   sign(real(exp((delay*1i*pi))))
%
% Inputs
%  delay    : [samples]
%  fftlen   : length of the fft
%
% Outputs
%  shift    : the delay-spectrum
%
% Example
%  S = fft(...);
%  D = delay2spec(12, 1024);
%  S = S.*D;  % shift the time signal by 12 samples to the left
%
% Copyright (c) 2008 Ircam/CNRS-UMR9912-STMS
%
% License
%  This file is under the LGPL license,  you can
%  redistribute it and/or modify it under the terms of the GNU Lesser General
%  Public License as published by the Free Software Foundation, either version 3
%  of the License, or (at your option) any later version. This file 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 Lesser General Public License for more
%  details.
%
% This function is part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  Gilles Degottex <gilles.degottex@ircam.fr>
%

function shift = delay2spec(delay, dftlen)
    if delay==0
        shift = ones(1,dftlen);
    else
        % odd length
        if mod(dftlen,2)==1
            shift = exp((delay*2i*pi./dftlen).*(1:(dftlen-1)/2));
            shift = [1, shift(1:end), conj(shift(end:-1:1))];

        % even length
        else
            shift = exp((delay*2i*pi./dftlen).*(1:dftlen/2));
            shift = [1, shift(1:end-1), sign(real(shift(end))), conj(shift(end-1:-1:1))];
        end
    end
return


function X = kr(U,varargin)
%KR Khatri-Rao product.
%   kr(A,B) returns the Khatri-Rao product of two matrices A and B, of
%   dimensions I-by-K and J-by-K respectively. The result is an I*J-by-K
%   matrix formed by the matching columnwise Kronecker products, i.e.
%   the k-th column of the Khatri-Rao product is defined as
%   kron(A(:,k),B(:,k)).
%
%   kr(A,B,C,...) and kr({A B C ...}) compute a string of Khatri-Rao
%   products A o B o C o ..., where o denotes the Khatri-Rao product.
%
%   See also kron.

%   Version: 21/10/10
%   Authors: Laurent Sorber (Laurent.Sorber@cs.kuleuven.be)

if ~iscell(U), U = [U varargin]; end
K = size(U{1},2);
if any(cellfun('size',U,2)-K)
    error('kr:ColumnMismatch', ...
          'Input matrices must have the same number of columns.');
end
J = size(U{end},1);
X = reshape(U{end},[J 1 K]);
for n = length(U)-1:-1:1
    I = size(U{n},1);
    A = reshape(U{n},[1 I K]);
    X = reshape(bsxfun(@times,A,X),[I*J 1 K]);
    J = I*J;
end
X = reshape(X,[size(X,1) K]);


function [ u, it_num ] = multigrid_poisson_1d ( n, a, b, ua, ub, force, exact )

%*****************************************************************************80
%
%% MULTIGRID_POISSON_1D solves a 1D PDE using the multigrid method.
%
%  Discussion:
%
%    This routine solves a 1D boundary value problem of the form
%
%      - U''(X) = F(X) for A < X < B,
%
%    with boundary conditions U(A) = UA, U(B) = UB.
%
%    The multigrid method is used.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 July 2014
%
%  Author:
%
%    Original FORTRAN77 version by William Hager.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    William Hager,
%    Applied Numerical Linear Algebra,
%    Prentice-Hall, 1988,
%    ISBN13: 978-0130412942,
%    LC: QA184.H33.
%
%  Parameters:
%
%    Input, integer N, the number of intervals.
%    N must be a power of 2.
%
%    Input, real A, B, the left and right endpoints of the region.
%
%    Input, real UA, UB, the left and right boundary values.
%
%    Input, function value = FORCE ( x ), the name of the function
%    which evaluates the right hand side.
%
%    Input, function value = EXACT ( x ), the name of the function
%    which evaluates the exact solution.
%
%    Output, integer IT_NUM, the number of iterations.
%
%    Output, real U(N+1), the computed solution.
%

%
%  Determine if we have enough storage.
%
  k = i4_log_2 ( n );

  if ( n ~= 2^k )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'MULTIGRID_POISSON_1D - Fatal error!\n' );
    fprintf ( 1, '  N is not a power of 2.\n' );
    error ( 'MULTIGRID_POISSON_1D - Fatal error!' );
  end

  nl = n + n + k - 2;

  uu = zeros ( nl, 1 );
  r = zeros ( nl, 1 );
%
%  Initialization.
%
  it = 4;
  it_num = 0;
  tol = 0.0001;
  utol = 0.7;
  m = n;
%
%  Set the nodes.
%
  x = ( linspace ( a, b, n + 1 ) )';
%
%  Set the right hand side.
%
  r = zeros ( n + 1, 1 );
  r(1) = ua;
  r(2:n) = force ( x(2:n) ) / n / n;
  r(n+1) = ub;
%
%  L points to first entry of solution.
%  LL points to penultimate entry.
%
  l = 1;
  ll = n;
%
%  Gauss-Seidel iteration
%
  d1 = 0.0;
  j = 0;

  while ( 1 )

    d0 = d1;
    j = j + 1;
    [ uu(l:ll+1), d1 ] = gauss_seidel ( n + 1, r(l:ll+1), uu(l:ll+1) );
    it_num = it_num + 1;
%
%  Do at least 4 iterations at each level.
%
    if ( j < it )

      continue
%
%  Enough iterations, satisfactory decrease, on finest grid, exit.
%
    elseif ( d1 < tol && n == m )

      break
%
%  Enough iterations, satisfactory convergence, go finer.
%
    elseif ( d1 < tol )

      uu(l-1-n-n:l-1) = ctof ( n + 1, uu(l:ll+1), n + n + 1, uu(l-1-n-n:l-1) );

      n = n + n;
      ll = l - 2;
      l = l - 1 - n;
      j = 0;
%
%  Enough iterations, slow convergence, 2 < N, go coarser.
%
    elseif ( utol * d0 <= d1 && 2 < n )

      [ uu(l+n+1:l+n+1+(n/2)), r(l+n+1:l+n+1+(n/2)) ] = ...
        ftoc ( n + 1, uu(l:ll+1), r(l:ll+1), (n/2)+1 );

      n = n / 2;
      l = ll + 2;
      ll = ll + n + 1;
      j = 0;

    end

  end

  u(1:n+1) = uu(1:n+1);

  return
end


function [xi,w]=seventhOrder2DCubPoints()
%%SEVENTHORDER2DCUBPOINTS Generate seventh-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] (12 points).
%
%EXAMPLE:
%We compare a 6th-order moment computed using these cubature points
%to one computed using monomialIntCube (a 7th order moment would have just
%been 0). The results are the same within typical finite precision limits.
% [xi,w]=seventhOrder2DCubPoints();
% alpha=[2;4];
% 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.92582009977255146156656677658399952253,                                          0,   0.24197530864197530864197530864197530864;
                                        0,   0.92582009977255146156656677658399952253,   0.24197530864197530864197530864197530864;
-0.92582009977255146156656677658399952253,                                          0,   0.24197530864197530864197530864197530864;
                                        0,  -0.92582009977255146156656677658399952253,   0.24197530864197530864197530864197530864;
  0.8059797829185987437078561813507442463,    0.8059797829185987437078561813507442463,   0.23743177469063023421810525931129352533;
  0.8059797829185987437078561813507442463,   -0.8059797829185987437078561813507442463,   0.23743177469063023421810525931129352533;
 -0.8059797829185987437078561813507442463,    0.8059797829185987437078561813507442463,   0.23743177469063023421810525931129352533;
 -0.8059797829185987437078561813507442463,   -0.8059797829185987437078561813507442463,   0.23743177469063023421810525931129352533;
  0.3805544332083156563791063590863941355,    0.3805544332083156563791063590863941355,   0.52059291666739445713991943204673116603;
  0.3805544332083156563791063590863941355,   -0.3805544332083156563791063590863941355,   0.52059291666739445713991943204673116603;
 -0.3805544332083156563791063590863941355,    0.3805544332083156563791063590863941355,   0.52059291666739445713991943204673116603;
 -0.3805544332083156563791063590863941355,   -0.3805544332083156563791063590863941355,   0.52059291666739445713991943204673116603];
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.


function value = parallelipiped_volume_3d ( x, y, z )

%*****************************************************************************80
%
%% PARALLELIPIPED_VOLUME_3D returns the volume of a parallelipiped in 3D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 November 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X(4), Y(4), Z(4), the coordinates of one corner
%    of the parallelipiped, and its 3 immediate neighbors.
%
%    Output, real PARALLELIPIPED_VOLUME_3D, the volume of
%    the parallelipiped.
%
  value = abs ( ...
    ( z(2) - z(1) ) * ( y(4) * x(3) - y(3) * x(4) ) + ...
    ( z(3) - z(1) ) * ( x(4) * y(2) - x(2) * y(4) ) + ...
    ( z(4) - z(1) ) * ( x(2) * y(3) - x(3) * y(2) ) + ...
    ( z(3) - z(2) ) * ( y(4) * x(1) - y(1) * x(4) ) + ...
    ( z(4) - z(2) ) * ( x(3) * y(1) - x(1) * y(3) ) + ...
    ( z(4) - z(3) ) * ( x(1) * y(2) - x(2) * y(1) ) );

  return
end


function phi = basis_mn_tet10 ( t, n, p )

%*****************************************************************************80
%
%% BASIS_MN_TET10: all bases at N points for a TET10 element.
%
%  Discussion:
%
%    The routine is given the coordinates of the vertices of a tetrahedron.
%
%    It works directly with these coordinates, and does not refer to a
%    reference element.
%
%    P1 through P4 are vertices.
%
%    P1 <= P5  <= P2
%    P2 <= P6  <= P3
%    P1 <= P7  <= P3
%    P1 <= P8  <= P4
%    P2 <= P9  <= P4
%    P3 <= P10 <= P4
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 August 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Olgierd Zienkiewicz,
%    The Finite Element Method,
%    Sixth Edition,
%    Butterworth-Heinemann, 2005,
%    ISBN: 0750663200,
%    LC: TA640.2.Z54.
%
%  Parameters:
%
%    Input, real T(3,4), the coordinates of the vertices.
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real P(3,N), the points where the basis functions
%    are to be evaluated.
%
%    Output, real PHI(10,N), the value of the basis functions
%    at the evaluation points.
%
  phi_linear(1:4,1:n) = basis_mn_tet4 ( t, n, p );

  phi( 1,1:n) = ( 2.0 * phi_linear(1,1:n)  - 1.0 ) .* phi_linear(1,1:n);
  phi( 2,1:n) = ( 2.0 * phi_linear(2,1:n)  - 1.0 ) .* phi_linear(2,1:n);
  phi( 3,1:n) = ( 2.0 * phi_linear(3,1:n)  - 1.0 ) .* phi_linear(3,1:n);
  phi( 4,1:n) = ( 2.0 * phi_linear(4,1:n)  - 1.0 ) .* phi_linear(4,1:n);
  phi( 5,1:n) =   4.0 * phi_linear(1,1:n)          .* phi_linear(2,1:n);
  phi( 6,1:n) =   4.0 * phi_linear(2,1:n)          .* phi_linear(3,1:n);
  phi( 7,1:n) =   4.0 * phi_linear(1,1:n)          .* phi_linear(3,1:n);
  phi( 8,1:n) =   4.0 * phi_linear(1,1:n)          .* phi_linear(4,1:n);
  phi( 9,1:n) =   4.0 * phi_linear(2,1:n)          .* phi_linear(4,1:n);
  phi(10,1:n) =   4.0 * phi_linear(3,1:n)          .* phi_linear(4,1:n);

  return
end


function price = CrankNicolsonFD_BlackScholes_func( S_0, K, r, T, sigma, call, dS, dt, Smax, Smin)
% Description: Crank-Nicolson PDE Finite Difference method to price European Option in Black Scholes Model
% Author: Justin Kirkby
M = round((Smax - Smin)/dS); % grid points
N = round(T/dt); % time steps

dS = (Smax - Smin) / M; % readjust
dt = T / N;  % readjust

vals = zeros(M+1, N+1);
vS = linspace(Smin, Smax, M+1)';
vI = vS / dS;
vJ = 0:N;

% Boundary Conditions
if call == 1  % call option
    vals(:, N+1) = max(vS - K, 0);
    vals(1, :) = 0;
    vals(M+1, :) = Smax - K*exp(-r*dt*(N - vJ));
else % put option
    vals(:, N+1) = max(K - vS, 0);
    vals(1, :) = K*exp(-r*dt*(N - vJ));
    vals(M+1, :) = 0;
end

% Tridiagonal Coefficients
a = 0.25 * dt * (sigma^2 *(vI.^2) - r*vI);
b = -dt*0.5*(sigma^2*(vI.^2) + r);
c = 0.25*dt*(sigma^2*(vI.^2) + r*vI);

M1 = -diag(a(3:M), -1) + diag(1 - b(2:M)) - diag(c(2:M-1),1);
[L,U] = lu(M1);
M2 = diag(a(3:M), -1) + diag(1 + b(2:M)) + diag(c(2:M-1),1);

% Solve systems (backward in time)
for j = N:-1:1
    vals(2:M,j) = U \ (L \ (M2*vals(2:M,j+1)));
end

price = interp1(vS, vals(:,1), S_0);

end


function logistic
% Logistic growth
%    using MATLAB for analytical solution
%
%   $Ekkehard Holzbecher  $Date: 2006/04/20 $
%--------------------------------------------------------------------------
T = 10;                  % maximum time
r = 1;                   % rate
kappa = 1;               % capacity
c0 = 0.01;               % initial value

%----------------------execution-------------------------------------------

t = linspace (0,T,100);
e = exp(r*t);
c = c0*kappa*e./(kappa+c0*(e-1));

%---------------------- graphical output ----------------------------------

plot (t,c); grid;
xlabel ('time'); legend ('population');
title ('logistic growth');


function [lat,lon,h]=xyz2ell3(X,Y,Z,a,b,e2)
% XYZ2ELL3  Converts cartesian coordinates to ellipsoidal.
%   Uses direct algorithm in B.R. Bowring, "The accuracy of
%   geodetic latitude and height equations", Survey
%   Review, v28 #218, October 1985, pp.202-206.  Vectorized.
%   See also XYZ2ELL, XYZ2ELL2.
% Version: 2011-02-19
% Useage:  [lat,lon,h]=xyz2ell3(X,Y,Z,a,b,e2)
%          [lat,lon,h]=xyz2ell3(X,Y,Z)
% Input:   X \
%          Y  > vectors of cartesian coordinates in CT system (m)
%          Z /
%          a   - ref. ellipsoid major semi-axis (m); default GRS80
%          b   - ref. ellipsoid minor semi-axis (m); default GRS80
%          e2  - ref. ellipsoid eccentricity squared; default GRS80
% Output:  lat - vector of ellipsoidal latitudes (radians)
%          lon - vector of ellipsoidal longitudes (radians)
%          h   - vector of ellipsoidal heights (m)

% Copyright (c) 2011, Michael R. Craymer
% All rights reserved.
% Email: mike@craymer.com

if nargin ~= 3 & nargin ~= 6
  warning('Incorrect number of input arguments');
  return
end
if nargin == 3
  [a,b,e2]=refell('grs80');
end

lon=atan2(Y,X);
e=e2*(a/b)^2;
p=sqrt(X.*X+Y.*Y);
r=sqrt(p.*p+Z.*Z);
u=atan(b.*Z.*(1+e.*b./r)./(a.*p));
lat=atan((Z+e.*b.*sin(u).^3)./(p-e2.*a.*cos(u).^3));
v=a./sqrt(1-e2.*sin(lat).^2);
h=p.*cos(lat)+Z.*sin(lat)-a*a./v;


% Exercise 4.47: Maximum determinant PSD matrix completion
% Boyd & Vandenberghe "Convex Optimization"
% Almir Mutapcic - Jan 2006
%
% Given a symmetric matrix A in R^(n-by-n) with some entries unspecified
% we find its completion such that A is positive semidefinite and
% it has a maximum determinant out of all possible completions.
% This problem can be formulated as a log det (and det_rootn) problem.
%
% This is a numerical instance of the specified book exercise.

% problem size
n = 4;

% create and solve the problem
cvx_begin
  % A is a PSD symmetric matrix (n-by-n)
  variable A(n,n) semidefinite;

  % constrained matrix entries.
  A(1,1) == 3; %#ok
  A(2,2) == 2; %#ok
  A(3,3) == 1; %#ok
  A(4,4) == 5; %#ok
  % Note that because A is symmetric, these off-diagonal
  % constraints affect the corresponding element on the
  % opposite side of the diagonal.
  A(1,2) == .5; %#ok
  A(1,4) == .25; %#ok
  A(2,3) == .75; %#ok

  % find the solution to the problem
  maximize( log_det( A ) )
  % maximize( det_rootn( A ) )
cvx_end

% display solution
fprintf('Matrix A with maximum determinant (%g) is:\n', det(A));
disp(A)
disp('Its eigenvalues are:')
disp(eig(A))


function [bestEpsilon bestF1] = selectThreshold(yval, pval)
%SELECTTHRESHOLD Find the best threshold (epsilon) to use for selecting
%outliers
%   [bestEpsilon bestF1] = SELECTTHRESHOLD(yval, pval) finds the best
%   threshold to use for selecting outliers based on the results from a
%   validation set (pval) and the ground truth (yval).
%

bestEpsilon = 0;
bestF1 = 0;
F1 = 0;

stepsize = (max(pval) - min(pval)) / 1000;
for epsilon = min(pval):stepsize:max(pval)

    % ====================== YOUR CODE HERE ======================
    % Instructions: Compute the F1 score of choosing epsilon as the
    %               threshold and place the value in F1. The code at the
    %               end of the loop will compare the F1 score for this
    %               choice of epsilon and set it to be the best epsilon if
    %               it is better than the current choice of epsilon.
    %
    % Note: You can use predictions = (pval < epsilon) to get a binary vector
    %       of 0's and 1's of the outlier predictions

    tp = sum( ((pval<epsilon)==1) & (yval==1) );
    fp = sum( ((pval<epsilon)==1) & (yval==0) );
    fn = sum( ((pval<epsilon)==0) & (yval==1) );
    prec = tp / (tp+fp);
    rec  = tp / (tp+fn);
    F1 = 2 * prec * rec / (prec+rec);

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

    if F1 > bestF1
       bestF1 = F1;
       bestEpsilon = epsilon;
    end
end

end


function result = part_sf_majorize ( n, nparta, a, npartb, b )

%*****************************************************************************80
%
%% PART_SF_MAJORIZE determines if partition A majorizes partition B.
%
%  Discussion:
%
%    The partitions must be in standard form.
%
%    If A, with NPARTA parts, and B, with NPARTB parts, are both partitions
%    of the same positive integer N, then we say that A majorizes B if,
%    for every index K from 1 to N, it is true that
%
%      sum ( 1 <= I <= K ) B(I) <= sum ( 1 <= I <= K ) A(I)
%
%    where entries of A beyond index NPARTA, and of B beyond BPARTB
%    are assumed to be 0.  We say that A strictly majorizes B if
%    A majorizes B, and for at least one index K the inequality is strict.
%
%    For any two partitions of N, it is possible that A majorizes B,
%    B majorizes A, both partitions majorize each other (in which case
%    they are equal), or that neither majorizes the other.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 July 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Jack vanLint, Richard Wilson,
%    A Course in Combinatorics,
%    Cambridge, 1992,
%    ISBN: 0-521-42260-4,
%    LC: QA164.L56.
%
%  Parameters:
%
%    Input, integer N, the integer to be partitioned.
%    N must be positive.
%
%    Input, integer NPARTA, the number of parts in partition A.
%    1 <= NPARTA <= N.
%
%    Input, integer A(NPARTA), contains partition A in standard
%    form.  A(1) through A(NPARTA) contain nonzero integers which sum to N.
%
%    Input, integer NPARTB, the number of parts in partition B.
%    1 <= NPARTB <= N.
%
%    Input, integer B(NPARTB), contains partition B in standard
%    form.  B(1) through B(NPARTB) contain nonzero integers which sum to N.
%
%    Output, integer RESULT, the result of the comparison.
%    -2, A and B are incomparable, would have been -1.
%    -1, A < B, (A is strictly majorized by B),
%     0, A = B, (A and B are identical),
%    +1, A > B, (A strictly majorizes B),
%    +2, A and B are incomparable, would have been +1.
%

%
%  Check.
%
  ierror = part_sf_check ( n, nparta, a );

  if ( ierror ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'PART_SF_MAJORIZE - Fatal error!\n' );
    fprintf ( 1, '  The input array A is illegal.\n' );
    error ( 'PART_SF_MAJORIZE - Fatal error!' );
  end

  ierror = part_sf_check ( n, npartb, b );

  if ( ierror ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'PART_SF_MAJORIZE - Fatal error!\n' );
    fprintf ( 1, '  The input array B is illegal.\n' );
    error ( 'PART_SF_MAJORIZE - Fatal error!' );
  end

  result = 0;
  suma = 0;
  sumb = 0;

  for i = 1 : min ( nparta, npartb )

    if ( i <= nparta )
      suma = suma + a(i);
    end

    if ( i <= npartb )
      sumb = sumb + b(i);
    end

    if ( result == -1 )

      if ( sumb < suma )
        result = -2;
        return
      end

    elseif ( result == 0 )

      if ( suma < sumb )
        result = -1;
      elseif ( sumb < suma )
        result = +1;
      end

    elseif ( result == + 1 )

      if ( suma < sumb )
        result = +2;
        return
      end

    end

  end

  return
end


% Newton下山法 当初值选择不当时 调整下山因子lamda
clear;
format long;
tol = 1e-5;
N = 100;
x0 = 0.6;
lamda = 1;
f = @(x) x^3 - x - 1;  %f(x)表达式
df = @(x) 3*x^2 - 1;
fprintf('f(x)的初值: %d\n', abs(f(x0)));
for k = 1 : N
    x1 = x0 - f(x0)/ df(x0);
     while abs(f(x1)) > abs(f(x0))
         lamda = lamda / 2;   % 下山因子减半
         fprintf('下山因子减半后的值: %d\n', lamda);
         x1 = x0 - lamda * f(x0) / df(x0);
     end
    fprintf('本次迭代f(x)的值: %d\n', abs(f(x1)));
    if abs(x1 - x0) < tol
        fprintf('迭代次数: %d\n', k);
        fprintf('方程的正根: %10.8f\n', x1);
        break;
    end
    x0 = x1;
end
if k == N
    fprintf('迭代方法失败\n');
end


function value = angle_deg_2d ( p1, p2, p3 )

%*****************************************************************************80
%
%% ANGLE_DEG_2D returns the angle swept out between two rays in 2D.
%
%  Discussion:
%
%    Except for the zero angle case, it should be true that
%
%      ANGLE_DEG_2D(P1,P2,P3) + ANGLE_DEG_2D(P3,P2,P1) = 360.0
%
%        P1
%        /
%       /
%      /
%     /
%    P2--------->P3
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real P1(2,1), P2(2,1), P3(2,1), define the rays
%    P1 - P2 and P3 - P2 which in turn define the angle.
%
%    Output, real VALUE, the angle swept out by the rays, measured
%    in degrees.  0 <= VALUE < 360.  If either ray has zero length,
%    then VALUE is set to 0.
%
  p(1,1) = ( p3(1,1) - p2(1,1) ) * ( p1(1,1) - p2(1,1) ) ...
         + ( p3(2,1) - p2(2,1) ) * ( p1(2,1) - p2(2,1) );

  p(2,1) = ( p3(1,1) - p2(1,1) ) * ( p1(2,1) - p2(2,1) ) ...
         - ( p3(2,1) - p2(2,1) ) * ( p1(1,1) - p2(1,1) );

  if ( p(1,1) == 0.0 & p(2,1) == 0.0 )
    value = 0.0;
    return
  end

  value = atan2 ( p(2,1), p(1,1) );

  if ( value < 0.0 )
    value = value + 2.0 * pi;
  end

  value = radians_to_degrees ( value );

  return
end


function phy = simplex_unit_to_general ( dim_num, point_num, t, ref )

%*****************************************************************************80
%
%% SIMPLEX_UNIT_TO_GENERAL maps the unit simplex to a general simplex.
%
%  Discussion:
%
%    Given that the unit simplex has been mapped to a general simplex
%    with vertices T, compute the images in T, under the same linear
%    mapping, of points whose coordinates in the unit simplex are REF.
%
%    The vertices of the unit simplex are listed as suggested in the
%    following:
%
%      (0,0,0,...,0)
%      (1,0,0,...,0)
%      (0,1,0,...,0)
%      (0,0,1,...,0)
%      (...........)
%      (0,0,0,...,1)
%
%    Thanks to Andrei ("spiritualworlds") for pointing out a mistake in the
%    previous implementation of this routine, 02 March 2008.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 March 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer POINT_NUM, the number of points to transform.
%
%    Input, real T(DIM_NUM,DIM_NUM+1), the vertices of the
%    general simplex.
%
%    Input, real REF(DIM_NUM,POINT_NUM), points in the
%    reference triangle.
%
%    Output, real PHY(DIM_NUM,POINT_NUM), corresponding points
%    in the physical triangle.
%

%
%  The image of each point is initially the image of the origin.
%
%  Insofar as the pre-image differs from the origin in a given vertex
%  direction, add that proportion of the difference between the images
%  of the origin and the vertex.
%
  for dim = 1 : dim_num

    phy(dim,1:point_num) = t(dim,1);

    for vertex = 2 : dim_num + 1

      phy(dim,1:point_num) = phy(dim,1:point_num) ...
        + ( t(dim,vertex) - t(dim,1) ) * ref(vertex-1,1:point_num);

    end

  end

  return
end


function s = polyline_arclength_nd ( dim_num, n, p )

%*****************************************************************************80
%
%% POLYLINE_LENGTH_ND computes the length of a polyline in ND.
%
%  Discussion:
%
%    A polyline of order M is the geometric structure consisting of
%    the M-1 line segments that lie between successive elements of a list
%    of M points.
%
%    An ordinary line segment is a polyline of order 2.
%    The letter "V" is a polyline of order 3.
%    The letter "N" is a polyline of order 4, and so on.
%
%    DIST(I+1,I) = sqrt ( sum ( 1 <= J <= DIM_NUM ) ( X(I+1) - X(I) )**2 )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 February 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer N, the number of points.
%
%    Input, real P(DIM_NUM,N), the points.
%
%    Output, real S(N), the arclength coordinates
%    of each point.  The first point has S(1) = 0 and the
%    last point has S(N) = arclength of the entire polyline.
%
  s(1) = 0.0;

  for i = 2 : n

    s(i) = s(i-1) + sqrt ( sum ( ( p(1:dim_num,i) - p(1:dim_num,i-1) ).^2 ) );

  end

  return
end