function [xInt,t]=findEllipsLineIntersect(xc,A,x0,a,aType)
%%FINDELLIPSLINEINTERSECT Given the parameters for an ellipsoid (in 2D an
%       ellipse) of the form (x-xc)'*A*(x-xc)=1 and a line either given
%       parameterically as x=x0+a*t or whether x0 and a are two points on
%       the line, find the points of intersection between the ellipsoid and
%       the line.
%
%INPUTS: xc The xDimX1 center of the ellipsoid.
%         A The xDimXxDim symmetric matrix defining the above equation for
%           an ellipsoid.
%     x0, a These xDimX1 values define a line and the type of line is given
%           by aType. If aType=0, then the line is parameteric as x=x0+a*t
%           where t is a scalar value. If aType=1, then x0 and a are both
%           points on the line.
%     aType As mentioned above, this specifies how the line is
%           parameterized. The default if omitted or an empty matrix is
%           passed is 0.
%
%OUTPUTS: xInt Either xDimX2 (or rarely xDimX1) matrix holding the vector
%              points of intersection of the line and the ellipsoid or an
%              empty matrix if there are no real solutions (the line does
%              not intersect the ellipsoid).
%            t If solutions exist, this is the 2X1 (or rarely 1X1) set of
%              parametric parameters. If aType=0, then these are the t
%              values in the parameteric equation. If aType=1, then these
%              are the t values in the parameteric equation x=x0+(a-x0)*t,
%              in which case values between 0 and 1 indicate that the
%              intersection occured between the two specified points.
%
%Start with the equations x=x0+a*t and (x-xc)'*A*(x-xc)=1. Substitite the
%first into the second and one ends up with a quadratic equation
%c1*t^2+c2*t+c3=0 with c1=a'*A*a, c2=2*xTilde'*A*a, c3=xTilde'*A*xTile-1
%where xTilde=x0-xc.
%
%EXAMPLE:
%Find and plot the intersection points of a line an an ellipse. Also
%consider a line that does not intersect and one sees that nothing is
%plotted from that, because no solution exists.
% %The ellipse.
% xc=[1;2];
% A=[1,0.5;
%    0.5,2];
% %Two points defining a line that does intersect.
% x01=[0;3];
% x02=[2;1];
% %Two points defining a line that does not intersect.
% x11=[3;3];
% x12=[3;1];
%
% aType=1;
% xInt0=findEllipsLineIntersect(xc,A,x01,x02,aType);
% xInt1=findEllipsLineIntersect(xc,A,x11,x12,aType);%This is empty.
%
% figure(1)
% clf
% hold on
% drawEllipse(xc,A,1,'linewidth',2)
% plot([x01(1);x02(1)],[x01(2);x02(2)],'-k','linewidth',2)
% plot([x11(1);x12(1)],[x11(2);x12(2)],'-k','linewidth',2)
%
% numSol0=size(xInt0,2);
% numSol1=size(xInt1,2);%Should be 0.
% for k=1:numSol0
%     scatter(xInt0(1,k),xInt0(2,k),400,'.r')
% end
% for k=1:numSol1
%     scatter(xInt1(1,k),xInt1(2,k),400,'.g')
% end
%
%July 2021 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<5||isempty(aType))
    aType=0;
end

if(aType==1)
    %Make it parameteric.
    a=a-x0;
end

xTilde=x0-xc;

c1=a'*A*a;
c2=2*xTilde'*A*a;
c3=xTilde'*A*xTilde-1;

t=roots([c1;c2;c3]);

if(isempty(t)||any(~isreal(t)))
    t=[];
    xInt=[];
    return
end

xDim=size(x0,1);
numSol=length(t);

xInt=zeros(xDim,numSol,1);
for curSol=1:numSol
    xInt(:,curSol)=x0+a*t(curSol);
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 value = r8_si ( x )

%*****************************************************************************80
%
%% R8_SI evaluates the sine integral Si of an R8 argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the sine integral Si evaluated at X.
%
  persistent nsi
  persistent sics
  persistent xsml

  if ( isempty ( nsi ) )

    sics = [ ...
      -0.1315646598184841928904275173000457, ...
      -0.2776578526973601892048287660157299, ...
       0.0354414054866659179749135464710086, ...
      -0.0025631631447933977658752788361530, ...
       0.0001162365390497009281264921482985, ...
      -0.0000035904327241606042670004347148, ...
       0.0000000802342123705710162308652976, ...
      -0.0000000013562997692540250649931846, ...
       0.0000000000179440721599736775567759, ...
      -0.0000000000001908387343087145490737, ...
       0.0000000000000016669989586824330853, ...
      -0.0000000000000000121730988368503042, ...
       0.0000000000000000000754181866993865, ...
      -0.0000000000000000000004014178842446, ...
       0.0000000000000000000000018553690716, ...
      -0.0000000000000000000000000075166966, ...
       0.0000000000000000000000000000269113, ...
      -0.0000000000000000000000000000000858 ]';

    nsi = r8_inits ( sics, 18, 0.1 * r8_mach ( 3 ) );
    xsml = sqrt ( r8_mach ( 3 ) );

  end

  absx = abs ( x );

  if ( absx < xsml )
    value = x;
  elseif ( absx <= 4.0 )
    value = x * ( 0.75 + r8_csevl ( ( x * x - 8.0 ) * 0.125, sics, nsi ) );
  else
    [ f, g ] = r8_sifg ( absx );
    cosx = cos ( absx );
    value = 0.5 * pi - f * cosx - g * sin ( x );
    if ( x < 0.0 )
      value = - value;
    end
  end

  return
end


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


function pass = test_bvp4c(pref)

if ( nargin == 0 )
    pref = chebfunpref();
end

%% Test using TWOODE():
d = [0, 4];
y0 = chebfun([1, 0], d, pref);
solinit = bvpinit([0, 1, 2, 3, 4], [1, 0]);
% Test bvp4c using default tolerance (RelTol = 1e-3)
y = bvp4c(@twoode, @twobc, y0);         % Chebfun solution
sol = bvp4c(@twoode, @twobc, solinit);  % Matlab's solution
pass(1) = max(max(abs(sol.y' - feval(y,sol.x')))) < 2e-2;

%% Test using MAT4BC(): (A problem with a parameter)

tol = 1e-4;

p_true = 17.096591689705100;
sol1_true = [-0.703689352093852, 1.033088348257337];

% Problem parameter, shared with nested functions.
q = 5;
lambda = 15;

% Derivative function. q is provided by the outer function.
mat4ode = @(x, y, lambda) [ y(2) ; -(lambda - 2*q*cos(2*x))*y(1) ];
% Boundary conditions. lambda is a required argument.
mat4bc = @(ya, yb, lambda) [ ya(2) ; yb(2) ; ya(1)-1 ];
% Auxiliary function -- initial guess for the solution
mat4init = @(x) [ cos(4*x) , -4*sin(4*x) ];

opts = odeset('AbsTol', 1e-5, 'RelTol', 1e-5);
solinit = bvpinit(linspace(0, pi, 10), mat4init, lambda);
solinit = chebfun(mat4init, [0, pi]);

[sol, p] = bvp4c(mat4ode, mat4bc, solinit, lambda, opts);
sol1 = feval(sol, 1);
pass(2) = norm(sol1 - sol1_true, inf) < tol;
pass(3) = abs(p - p_true) < tol;

end


function ch=rayleigh(n_samp,fd,fs)

% ch=rayleigh(n_samp,fd,fs)
%
% Create n_samp samples of a Rayleigh channel whose doppler spread is fd
% and which is sampled at frequency fs.

% Copyright 2012 Evrytania LLC (http://www.evrytania.com)
%
% Written by James Peroulas <james@evrytania.com>
%
% 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/>.

error(nargchk(3,3,nargin));
error(chk_param(n_samp,'n_samp','scalar','real','integer','>=',1));
error(chk_param(fd,'fd','scalar','real','>=',0));
error(chk_param(fs,'fs','scalar','real','>',0));
if (fd>fs/2)
  error('Doppler spread must be smaller than fs/2');
end

% Special case when requested doppler spread is zero.
if (fd==0)
  ch=rayleigh(1,0.5,2);
  ch=repmat(ch,1,n_samp);
  return;
end

% First, a rayleigh channel is created with a doppler frequency of 0.5
% and a sampling frequency of 2. Then, this channel is interpolated
% up to the desired sampling frequency.

% Calculate the interpolation factor.
intp=fs/fd/4;

% Rayleigh transfer function
rtf=@(f)(abs(f)<.5).*sqrt(1./((pi*0.5)*sqrt(1-(f/(0.5+eps(0.5))).^2)));

% Create the source Rayleigh signal
n_samp_source=max(2048,ceil(n_samp/intp));
bb=sqrt(2)*cyc_filt(blnoise(n_samp_source),rtf);

% Now, interpolate up to the desired sampling rate.
ch=interpft(bb,round(n_samp_source*intp));
ch=ch(1:n_samp);


%% RATE OF CONVERGENCE OF WEAK GALERKIN FINITE ELEMENT METHOD
%
% This example is to show the rate of convergence of linear finite element
% approximation of the Poisson equation on the unit square with the
% following boundary conditions:
%
% - Non-empty Dirichlet boundary condition.
% - Pure Neumann boundary condition.
% - Robin boundary condition.
%
% The basis, data structure and numerical test is summarized in <a
% href="matlab:ifem Poissonfemrate">Poissonfemrate</a>.
%
% See also PoissonP2femrate, Poissonafemrate
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

clear variables
%% Setting
[node,elem] = squaremesh([0,1,0,1],0.25);
mesh = struct('node',node,'elem',elem);
option.L0 = 2;
option.maxIt = 4;
option.printlevel = 1;
option.elemType = 'WG';

%% Non-empty Dirichlet boundary condition.
option.plotflag = 1;
pde = sincosdata;
mesh.bdFlag = setboundary(node,elem,'Dirichlet','~(x==0)','Neumann','x==0');
femPoisson(mesh,pde,option);

%% Pure Neumann boundary condition.
option.plotflag = 0;
pde = sincosNeumanndata;
% pde = sincosdata;
mesh.bdFlag = setboundary(node,elem,'Neumann');
femPoisson(mesh,pde,option);

%% Pure Robin boundary condition.
option.plotflag = 0;
pde = sincosRobindata;
mesh.bdFlag = setboundary(node,elem,'Robin');
femPoisson(mesh,pde,option);

%% Conclusion
%
% The optimal rate of convergence of the H1-norm (1st order) and L2-norm
% (2nd order) is observed. No superconvergence for ||DuI-Duh||.
%
% MGCG converges uniformly in all cases.


function [ x, w ] = legendre_set_sqrtx_01 ( n )

%*****************************************************************************80
%
%% LEGENDRE_SET_SQRTX_01 sets a Gauss-Legendre rule for SQRT(X) * F(X) on [0,1].
%
%  Discussion:
%
%    The integral:
%
%      Integral ( 0 <= X <= 1 ) SQRT ( X ) * F(X) dX =
%      Integral ( 0 <= Y <= 1 ) 2 * Y**2 * F(Y**2) dY.
%      (using Y = SQRT(X) )
%
%    The quadrature rule:
%
%      Sum ( 1 <= I <= N ) W(I) * F ( X(I) )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 June 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Daniel Zwillinger, editor,
%    CRC Standard Mathematical Tables and Formulae,
%    CRC Press, 30th Edition, 2000, page 696.
%
%  Parameters:
%
%    Input, integer N, the order.
%
%    Output, real X(N), the abscissas.
%
%    Output, real W(N), the weights.
%
  n2 = 2 * n + 1;

  [ x2, w2 ] = legendre_set ( n2 );

  x = zeros ( n, 1 );
  w = zeros ( n, 1 );

  x(1:n,1) = x2(n+2:2*n+1,1).^2;

  w(1:n,1) = 2.0 * w2(n+2:2*n+1,1) .* x(1:n,1);

  return
end


% function H = histo2D(D,[Xlo Xhi],Xn,[Ylo Yhi],Yn,Xlab,Ylab,Title)
%
% 2 Dimensional Histogram (size(H) == [Yn Xn])
% Counts number of points in the bins defined by
% X = linspace(Xlo,Xhi,Xn) and
% Y = linspace(Ylo,Yhi,Yn)

function H = histo2D(D,Xrange,Xn,Yrange,Yn,Xlab,Ylab,Title)

Xlo = Xrange(1) ; Xhi = Xrange(2) ;
Ylo = Yrange(1) ; Yhi = Yrange(2) ;
X = linspace(Xlo,Xhi,Xn)' ;
Y = linspace(Ylo,Yhi,Yn)' ;

Dx = D(:,1) ; Dy = D(:,2) ;
n = length(D) ;

H = zeros(Yn,Xn) ;

for i = 1:n
    x = dsearchn(X,Dx(i)) ;
    y = dsearchn(Y,Dy(i)) ;
    H(y,x) = H(y,x) + 1 ;
end ;

figure , pcolor(X,Y,H) ;
% Xmid = 0.5*(X(1:end-1)+X(2:end)) ;
% Ymid = 0.5*(Y(1:end-1)+Y(2:end)) ;
% figure , pcolor(Xmid,Ymid,H) ;
colorbar ; shading flat ; axis square tight ; grid on ;
xlabel(Xlab) ; xlabel(Ylab) ; title(Title) ;


function [ vX, paramLambda ] = SolveLsNormSquaredConst( mA, vB, normSquaredConst, numIterations )
% ----------------------------------------------------------------------------------------------- %
%[ vX ] = SolveLsNormConst( mA, vB, normConst )
% Solves norm constrained Least Squares problem by finding the optimal Dual
% Variable (paramLambda) of the KKT Conditions by succesively solving
% Tikhonov Regularized Least Squares problems.
% The objective Funciton is given by:
% \arg \min_{x} \frac{1}{2} {\left\| A x - b \right\|}_{2}^{2}
% subject to {\left\| x \right\|}_{2}^{2} \leq normSquaredConst
% Input:
%   - mA                -   Input Matrix.
%                           The given matrix of the problem 0.5 || A x - b|| ^ 2.
%                           Structure: Vector (Column Vector).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - vB                -   Input Vector.
%                           The given vector of the problem 0.5 || A x - b|| ^ 2.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: {1, 2, ...}.
%   - normConst         -   Norm Constraint.
%                           The solution must obey || x || <= normConst.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: (0, inf).
% Output:
%   - vX                -   Solution Vector.
%                           The optimal solution of the problem 0.5 || A x
%                           - b || ^ 2 subject to || x || <= normConst.
%                           Structure: Vector (Column Vector).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
% References
%   1.  See - https://stats.stackexchange.com/questions/401212.
% Remarks:
%   1.  T
% TODO:
%   1.  U
% Release Notes:
%   -   1.0.000     14/04/2019
%       *   First realease version.
% ----------------------------------------------------------------------------------------------- %

paramLambda = 0; %<! Initialization

vX = pinv(mA) * vB;
mI = eye(size(vX, 1));

mAA = mA.' * mA;
mAb = mA.' * vB;

if((norm(vX, 2) ^ 2) <= normSquaredConst)
    return;
end

hObjFun     = @(paramLambda) (norm((mAA + (2 * paramLambda * mI)) \ mAb, 2) ^ 2) - normSquaredConst;
hObjFunGrad = @(paramLambda) -4 * vB.' * mA * inv(mAA + (2 * paramLambda * mI)) * inv(mAA + (2 * paramLambda * mI)) * inv(mAA + (2 * paramLambda * mI)) * mA.' * vB;

for ii = 1:numIterations
    paramLambda = paramLambda - (hObjFun(paramLambda) / hObjFunGrad(paramLambda));
end

% paramLambda = fzero(hObjFun, 1e-6); %<! Doesn't work for some reason
% paramLambda = FindZero(hObjFun, paramLambda, 1e6);

% hObjFun     = @(paramLambda) (((norm((mAA + (2 * paramLambda * mI)) \ mAb, 2) ^ 2) - normSquaredConst) ^ 2);
% % paramLambda = fminbnd(hObjFun, 0, 2000);
% paramLambda = fminsearch(hObjFun, paramLambda);

vX = (mAA + (2 * paramLambda * mI)) \ mAb;

end


function [U,S,V]=BL_SVD(A,u0,v0,k)

% Block lanczos method to compute SVD of matrix A with k steps;
% Suppose A is m*n, where m>n; A0=U0*S0*V0', where U0 is m*r, V0 is n*r;
% This is equivalent to compute block lanczos on C=[0,A;A',0];
% The initial lanczos block should set to be uv0=[U0;V0]/sqrt(2);
% where r is the rank of A0 and c is the slack positive interger variable;
%
% A - m * n matrix (required input)
% [u0; v0]/sqrt(2) - Initial subspace (required input)
% k - the number of blocks
%
% U S V - partial singular value decomposition
%
% Reference: Zhouchen Lin and Siming Wei, A Block Lanczos with Warm Start
% Technique for Accelerating Nuclear Norm Minimization Algorithms,
% arxiv: 1012.0365.
%
% Bug report: zclin2000@hotmail.com
%

if nargin < 3
    error('Too few arguments') ;
end

if nargin < 4
    k = 1;
elseif k == -1
    k = 1;
end

[m,n]=size(A);
mark=0;
if m<n % Let m>=n;
    mark=1;
    A=A';
    t=m;
    m=n;
    n=t;
    uv0=[v0; u0]/sqrt(2);
else
    uv0=[u0; v0]/sqrt(2);
end
r=size(uv0,2);
% Initialization;

% tridiagolization;
X(:,:,1)=uv0;
% AX=multiC(A,uv0,m,n);
AX = [A*uv0(m+1:m+n,:);A'*uv0(1:m,:)];
M(:,:,1)=uv0'*AX;
Q=X(:,:,1);
Bdiag=[];
Mdiag=M(:,:,1);
l=1;
stop=0;
tol=1.e-3;

while stop~=1
    if l==1
    R(:,:,l)=AX-uv0*M(:,:,1);
    else
    R(:,:,l)=AX-X(:,:,l)*M(:,:,l)-X(:,:,l-1)*B(:,:,l-1)';
    end
    R_max=max(max(abs(R(:,:,l))));

    if R_max<= tol
        stop=1;
    else
    [X(:,:,l+1),B(:,:,l)]=qr(R(:,:,l),0);
%     AX=multiC(A,X(:,:,l+1),m,n);
%     Y(:,:) = X(:,:,l+1);
    AX = [A*X(m+1:m+n,:,l+1);A'*X(1:m,:,l+1)];
    M(:,:,l+1)=X(:,:,l+1)'*AX;
    Q=[Q,X(:,:,l+1)];

    % Record the tridiagonal elments;
    Bdiag=blkdiag(Bdiag,B(:,:,l));
    Mdiag=blkdiag(Mdiag,M(:,:,l+1));
    l=l+1;
         if l>=k
             stop=1;
         end
    end
end

% Now T=Q'CQ is block tridiagonal;
B0=[zeros(r,l*r);Bdiag,zeros(l*r-r,r)];
T=Mdiag+B0+B0';
T=(T+T')/2;

% Compute EVD of T: T=Z*S*Z';
[Z,S]=eig(T);

% Therefore (QZ)'*C*(QZ)=S
add=0;
QZ=Q*Z(:,l*r:-1:l*r-r+1-add);
U_temp=QZ(1:m,:)*sqrt(2);
V_temp=QZ(m+1:m+n,:)*sqrt(2);
dS=diag(S);
S=diag(dS(l*r:-1:l*r+1-r-add));
if mark==1
    U=V_temp;
    V=U_temp;
else
    U=U_temp;
    V=V_temp;
end


function [fdr,x] = fdrCurve(fit, MODE, sgn)

% [fdr,x] = fdrCurve(fit, [MODE], [sgn])
%
% Computes the false discovery rate curve.
%
% Input:
%   fit     Poisson regression structure from function empNull.
%   MODE    Type of FDR
%               'tail' for tail FDR (default)
%               'local' for local FDR
%   sgn     1 (default) or -1, for right or left tail in 'FDR' case.
%           Irrelevant in 'fdr' case.
%
% Output:
%   fdr     3 column matrix, containing the fdr curve estimate and
%           the upper and lower limits corresponding to the 95% pointwise
%           confidence bands on the empirical distribution of T.
%   x       values at which fdr is evaluated
%
% See also:
%   fdrEmpNull.m

% HISTORY:
%   2006.05.08 ASH (armins@hsph.harvard.edu) wrote it.
%

if ~exist('fit'),
    error('Not enough arguments')
end
if ~exist('MODE'),
    MODE = 'tail';
end
if ~exist('sgn'),
    sgn = 1;
end

[x, X, W, y, yhat] = deal(fit.x, fit.X, fit.W, fit.y, fit.yhat);
K = length(fit.x);

if strcmp(MODE,'tail'),
    S = triu(ones(K,K),1) + diag(ones(1,K))/2;
    if (sgn == -1), S = S'; end
    fdr(:,1) = (S*yhat)./(S*y);
    V = X * inv(X'*W*diag(y)*W*X) * X' * W;
    V = diag(1./(S*yhat)) * S * diag(yhat) * V - diag(1./(S*y));
else % local
    fdr(:,1) = yhat./y;
    V = X * inv(X'*W*diag(y)*W*X) * X' * W - diag(1./y);
end

% Confidence intervals
cov = V * diag(y) * V';
v = diag(cov);
alpha = 0.05;
fdr(:,2) = fdr(:,1) .* exp(norminv(1-alpha/2) * sqrt(v));
fdr(:,3) = fdr(:,1) .* exp(-norminv(1-alpha/2) * sqrt(v));


function rank = subset_colex_rank ( n, t )

%*****************************************************************************80
%
%% SUBSET_COLEX_RANK computes the colexicographic rank of a subset.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 August 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Donald Kreher, Douglas Simpson,
%    Combinatorial Algorithms,
%    CRC Press, 1998,
%    ISBN: 0-8493-3988-X,
%    LC: QA164.K73.
%
%  Parameters:
%
%    Input, integer N, the number of items in the master set.
%    N must be positive.
%
%    Input, integer T(N), the subset.  If T(I) = 0, item I is
%    not in the subset; if T(I) = 1, item I is in the subset.
%
%    Output, integer RANK, the rank of the subset.
%

%
%  Check.
%
  subset_check ( n, t );

  rank = 0;

  for i = 1 : n

    if ( t(i) == 1 )
      rank = rank + 2^( i - 1 );
    end

  end

  return
end


function [ pn, dist ] = shape_point_near_2d ( center, p1, nside, p, pn, dist )

%*****************************************************************************80
%
%% SHAPE_POINT_NEAR_2D: nearest point ( regular shape, point ) in 2D.
%
%  Discussion:
%
%    The "regular shape" is assumed to be an equilateral and equiangular
%    polygon, such as the standard square, pentagon, hexagon, and so on.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real CENTER(2,1), the center of the shape.
%
%    Input, real P1(2,1), the first vertex of the shape.
%
%    Input, integer NSIDE, the number of sides in the shape.
%
%    Input, real P(2,1), the point to be checked.
%
%    Output, real PN(2,1), the point on the shape that is nearest
%    to the given point.
%
%    Output, real DIST, the distance between the points.
%

%
%  Determine the angle subtended by a single side.
%
  sector_angle = 360.0 / nside;
%
%  How long is the half-diagonal?
%
  radius = sqrt ( sum ( ( p1(1:2,1) - center(1:2,1) ).^2 ) );
%
%  If the radius is zero, then the shape is a point and the computation is easy.
%
  if ( radius == 0.0 )
    pn(1:2,1) = center(1:2,1);
    dist = sqrt ( sum ( ( p(1:2,1) - pn(1:2,1) ).^2 ) );
    return
  end
%
%  If the test point is at the center, then the computation is easy.
%  The angle subtended by any side is ( 2 * PI / NSIDE ) and the
%  nearest distance is the midpoint of any such side.
%
  if ( p(1:2,1) == center(1:2,1) )
    angle = pi / nside;
    pd(1,1) =   ( p(1,1) - center(1,1) ) * cos ( angle ) ...
              + ( p(2,1) - center(2,1) ) * sin ( angle );
    pd(2,1) = - ( p(1,1) - center(1,1) ) * sin ( angle ) ...
              + ( p(2,1) - center(2,1) ) * cos ( angle );
    pn(1,1) = center(1,1) + pd(1,1) * cos ( angle );
    pn(2,1) = center(2,1) + pd(2,1) * sin ( angle );
    dist = radius * cos ( angle );
    return
  end
%
%  Determine the angle between the ray to the first corner,
%  and the ray to the test point.
%
  angle = angle_deg_2d ( p1(1:2,1), center(1:2,1), p(1:2,1) );
%
%  Determine the sector of the point.
%
  sector_index = floor ( angle / sector_angle ) + 1;
%
%  Generate the two corner points that terminate the SECTOR-th side.
%
  angle2 = ( sector_index - 1 ) * sector_angle;
  angle2 = degrees_to_radians ( angle2 );

  pa = vector_rotate_base_2d ( p1, center, angle2 );

  angle2 = ( sector_index ) * sector_angle;
  angle2 = degrees_to_radians ( angle2 );

  pb = vector_rotate_base_2d ( p1, center, angle2 );
%
%  Determine the point on the SECTOR-th side of the shape which is
%  nearest.
%
  [ pn, dist, t ] = segment_point_near_2d ( pa, pb, p );

  return
end


function demoMertonModel()
%%DEMOMERTONMODEL Demonstrates scenario simulation of two independent
%             Merton processes using the strongStochTaylorStepJump
%             function. A plot comparing the approximated paths to the true
%             path is generated. In order to demonstrate ideal results, the
%             same collection of driving processes are used for both the
%             true path and the simulations. The first line may be
%             uncommented and modified if reproducible plots are desired.
%
%The Merton model is discussed in detail in Chapter 1.7, pages 50-52 and
%Chapter 9.6, page 414 of [1]. See equations 9.6.3 and 9.6.4 and the
%discussion on page 414. Note we simplified the jump process term's
%coefficient to c(y)=psi*y as suggested in Chapter 7.1, page 313.
%
%REFERENCES:
%[1] Platen, Eckhard, and Nicola Bruti-Liberati. Numerical solution of
%    stochastic differential equations with jumps in finance. Vol. 64.
%    Springer Science & Business Media, 2010.
%
%July 2019 Codie T. Lewis, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    %rng(16,'twister'); % Make the example reproducible
    close all;

    %Define Parameters
    X0=[1;1]; % Initial path value
    mue=[0.05;0.05]; % Drift mean
    sig=[0.2;0.2]; % Wiener process standard deviations
    psi=[-0.25;-0.25]; % Jump intensity coefficients
    T=10; % End time starting from 0
    lambda=0.3; % Poisson jump frequency for unit time interval

    %Define Discretization Coefficients and Derivatives
    % Assume a time invariant SDE of the form:
    % dY_{n+1} = a(Y_n) dt + b(Y_n) dW + c(Y_n) dP
    a = @(x) diag(mue)*x;
    B = @(x) diag(sig)*diag(x);
    c = @(x) diag(psi)*x;
    pBpy = cat(3,sig(1)*[1 0; 0 0],sig(2)*[0 0; 0 1]);
    pcpy = diag(psi);

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    %%%%%%%%%%%%%%%%%%%% Exact Solution %%%%%%%%%%%%%%%%%%%%%
    %The Merton model is discussed in detail on p.50-52 and p.414 of [1].
    % We simplified the compound jump process to have constant jump size.
    % See equations 9.6.3 and 9.6.4.

    dt = 0.001; % True path time step
    t = 0:dt:T;
    X = zeros([2,length(t)]); % A vector to contain the true path values
    X(:,1) = X0; % Initial condition

    %Generate Poisson samples as 2 independent compound Poisson processes
    CP = [0,genMarkedPointProc(lambda,dt,0+dt,T,0)];

    %Simulate 2 independent Wiener Processes
    W = [[0;0],sqrt(dt).*randn([2,length(t)-1])];
    W = cumsum(W,2);

    %Compute Merton Process Values using Exact Solution from [1]
    for i = 2:length(X)
        X(:,i) = X0.*exp((mue-0.5*sig.^2)*t(i)+sig.*W(:,i)+psi*CP(i));
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    %%%%%%%%%%%%%%%%%%%% Approximations %%%%%%%%%%%%%%%%%%%%%

    %Define Approximation Parameters
    dtapprox = 0.5; % Approximation time step.
    tapprox = 0:dtapprox:T;
    numruns = 50; % Number of times to perform the random simulation steps.

    XEuler = zeros([2,length(tapprox)]);
    XEuler(:,1) = X0;
    XTaylor = zeros([2,length(tapprox)]);
    XTaylor(:,1) = X0;
    XDerFree = zeros([2,length(tapprox)]);
    XDerFree(:,1) = X0;

    for i = 2:length(tapprox)
        [deltaP,Wj] = getDrivers(W,CP,dtapprox,dt,i);

        %Get Euler Approximation
        for run = 1:numruns
            XEuler(:,i) = XEuler(:,i) + strongStochTaylorStepJump(...
                XEuler(:,i-1),a,B,@(x,k,idx)c(x),lambda,dtapprox,1,[],[],deltaP,Wj);
        end
        XEuler(:,i) = XEuler(:,i)/numruns;

        %Get Taylor Approximation
        for run = 1:numruns
            XTaylor(:,i) = XTaylor(:,i) + strongStochTaylorStepJump(...
                XTaylor(:,i-1),a,B,c,lambda,dtapprox,3,pBpy,pcpy,deltaP,Wj,tapprox(i));
        end
        XTaylor(:,i) = XTaylor(:,i)/numruns;

        %Get Derivative Free Taylor Approximation
        for run = 1:numruns
            XDerFree(:,i) = XDerFree(:,i) + strongRungeKStepJump(...
                XDerFree(:,i-1),a,B,c,lambda,dtapprox,3,deltaP,Wj,tapprox(i));
        end
        XDerFree(:,i) = XDerFree(:,i)/numruns;
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    %%%%%%%%%%%%%%%%%%%%% Plotting %%%%%%%%%%%%%%%%%%%%%%%%

    %Plot the Paths
    figure('Position',[10 10 900 600])
    plot3(t,X(1,:),X(2,:)); label11='Exact';
    hold on
    plot3(tapprox,XEuler(1,:),XEuler(2,:)); label12='Euler';
    plot3(tapprox,XTaylor(1,:),XTaylor(2,:)); label13='Taylor';
    plot3(tapprox,XDerFree(1,:),XDerFree(2,:)); label14='DF';
    legend(label11,label12,label13,label14)
    xlabel('Time (t)','FontSize',16)
    ylabel('Position ($X_t^{(1)}$ or $\tilde{X}_t^{(1)})$','Interpreter','latex','FontSize',16)
    zlabel('Position ($X_t^{(2)}$ or $\tilde{X}_t^{(2)})$','Interpreter','latex','FontSize',16)
    title('A Simulated Merton Model','FontSize',16)
    hold off

    %Plot the Errors
    figure('Position',[10 10 900 600])
    errorEuler = sum((X(:,1:floor(dtapprox/dt):end)-XEuler).^2)/2;
    errorTaylor = sum((X(:,1:floor(dtapprox/dt):end)-XTaylor).^2)/2;
    errorDerFree = sum((X(:,1:floor(dtapprox/dt):end)-XDerFree).^2)/2;
    plot(tapprox,errorEuler); label21='Euler';
    hold on
    plot(tapprox,errorTaylor); label22='Taylor';
    plot(tapprox,errorDerFree); label23='DF';
    legend(label21,label22,label23)
    xlabel('Time (t)','FontSize',16)
    ylabel('$\frac{1}{2}\left[(X_t^{(1)}-\tilde{X}_t^{(1)})^2+(X_t^{(2)}-\tilde{X}_t^{(2)})^2\right]$','Interpreter','latex','FontSize',16)
    title('Mean Squared Error for Each Time Step','FontSize',16)
    hold off
end

function [deltaP,Wj]=getDrivers(W,CP,dtapprox,dt,iter)
%%GETDRIVERS A helper function to automate retrieval of the correct
%            interval of values from the driving processes used for the
%            generation of the true path.
%
%INPUTS: W An mXlength(t) vector containing realized Wiener processes in
%          each row.
%       CP A 1Xlength(t) row vector containing a realized Poisson process.
% dtapprox The coarse approximation time step (assumed to be constant).
%       dt The fine true path time step (assumed to be constant).
%     iter The current iteration index (assumed indices are positive and
%          begin at 1).
%
%OUTPUTS: deltaP The number of jumps which occurred in CP during the time
%                interval.
%             Wj An mXdeltaP+2 vector which contains the values from W
%                which most closely occured at the same time as each jump
%                counted in deltaP. This assumes the distribution of jumps
%                over the time interval are uniformly random.
%
%July 2019 Codie T. Lewis, Naval Research Laboratory, Washington D.C.

    if(iter>1)
        currentIdx=floor(dtapprox/dt)*(iter-1)+1;
        prevIdx=floor(dtapprox/dt)*(iter-2)+1;
        deltaP=CP((iter-1)*floor(dtapprox/dt)+1)-CP((iter-2)*floor(dtapprox/dt)+1);
        if(deltaP~=0)
            jumpIdx=floor((prevIdx+rand([deltaP,1])*dtapprox/dt));
            Wj=W(:,[prevIdx;jumpIdx;currentIdx]);
        else
            Wj=W(:,[prevIdx;currentIdx]);
        end
    else
        error('Iter must be >1.')
    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 [Rchordal, tchordal] = chordal_initialization(measurements)
% function [Rchordal, tchordal] = chordal_initialization(measurements)
%
% This helper function accepts a MATLAB struct containing the raw
% measurements specifying a special Euclidean synchronization problem, and
% returns the corresponding chordal initialization for this problem.
%
% INPUT: A MATLAB struct 'measurements' containing the following fields
% (see eq. (11) in the long-form version of the paper for details):
% edges:  An (mx2)-dimension encoding the edges in the measurement network;
%     edges(k, :) = [i,j] means that the kth measurement is of the
%     relative transform from pose i to pose j.  NB:  This indexing scheme
%     requires that the states x_i are numbered sequentially as
%     x_1, ... x_n.
% R:  An m-dimensional cell array whose kth element is the rotational part
%     of the kth measurement
% t:  An m-dimensional cell array whose kth element is the translational
%     part of the kth measurement
% kappa:  An m-dimensional cell array whose kth element gives the precision
%     of the rotational part of the kth measurement.
% tau:  An m-dimensional cell array whose kth element gives the precision
%     of the translational part of the kth measurement.

% OUTPUT:
%  -Rchordal:  A d x dn block matrix Rchordal containing the estimating
%     orientations.
%  -tchordal [optional]:  A d x n block matrix containing the estimated
%     positions.

% Copyright (C) 2016 by David M. Rosen

d = length(measurements.t{1});
n = max(max(measurements.edges));
m = size(measurements.edges, 1);

if nargout > 1
    [B3, B2, B1] = construct_B_matrices(measurements);
else
    B3 = construct_B_matrices(measurements);
end

% First, estimate the rotations using *only* the rotational observations
Id = eye(d);
Id_vec = Id(:);

% Compute the constant vector cR induced by fixing the first orientation
% estimate to be the identity Id.
cR = B3(:, 1:d^2) * Id_vec;

% Compute an estimate of the remaining rotations by solving the resulting
% least squares problem *without* enforcing the constraint the the
% estimates lie in SO(d)
r2vec = - B3(:, d^2 + 1 : end) \ cR;
rvec = vertcat(Id_vec, r2vec);
R_LS = reshape(rvec, d, d*n);

% Now reproject these estimates onto SO(d)
Rchordal = zeros(d, d*n);
for i = 1:n
    Rchordal(:, d*(i-1) + 1 : d*i) = project_to_SOd(R_LS(:, d*(i-1) + 1 : d*i));
end

if nargout > 1

    % Solve for the translations in terms of the rotations

    % Constant vector induced by fixing R
    cT = B2 * Rchordal(:);

    % Solve for t_2, ... t_n assuming that t_1 = 0.

    t2 = - B1(:, d + 1 : end) \ cT;
    tvec = vertcat(zeros(d, 1), t2);
    tchordal = reshape(tvec, d, n);
end

end


function type = r82poly2_type ( a, b, c, d, e, f )

%*****************************************************************************80
%
%% R82POLY2_TYPE analyzes a second order polynomial in two variables.
%
%  Discussion:
%
%    The polynomial has the form
%
%      A x^2 + B y^2 + C xy + Dx + Ey + F = 0
%
%    The possible types of the solution set are:
%
%     1: a hyperbola;
%        9x^2 -  4y^2       -36x - 24y -  36 = 0
%     2: a parabola;
%        4x^2 +  1y^2 - 4xy + 3x -  4y +   1 = 0;
%     3: an ellipse;
%        9x^2 + 16y^2       +36x - 32y -  92 = 0;
%     4: an imaginary ellipse (no real solutions);
%         x^2 +   y^2       - 6x - 10y + 115 = 0;
%     5: a pair of intersecting lines;
%                        xy + 3x -   y -   3 = 0
%     6: one point;
%         x^2 +  2y^2       - 2x + 16y +  33 = 0;
%     7: a pair of distinct parallel lines;
%                 y^2            -  6y +   8 = 0
%     8: a pair of imaginary parallel lines (no real solutions);
%                 y^2            -  6y +  10 = 0
%     9: a pair of coincident lines.
%                 y^2            -  2y +   1 = 0
%    10: a single line;
%                             2x -   y +   1 = 0;
%    11; all space;
%                                          0 = 0;
%    12; no solutions;
%                                          1 = 0;
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 October 2005
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Daniel Zwillinger, editor,
%    CRC Standard Mathematical Tables and Formulae,
%    CRC Press, 30th Edition, 1996, pages 282-284.
%
%  Parameters:
%
%    Input, real A, B, C, D, E, F, the coefficients.
%
%    Output, integer TYPE, indicates the type of the solution set.
%

%
%  Handle the degenerate case.
%
  if ( a == 0.0 && b == 0.0 && c == 0.0 )
    if ( d == 0.0 && e == 0.0 )
      if ( f == 0.0 )
        type = 11;
      else
        type = 12;
      end
    else
      type = 10;
    end
    return
  end

  delta = 8.0 * a * b * f ...
        + 2.0 * c * e * d ...
        - 2.0 * a * e * e ...
        - 2.0 * b * d * d ...
        - 2.0 * f * c * c;

  j = 4.0 * a * b - c * c;

  if ( delta ~= 0.0 )
    if ( j < 0.0 )
      type = 1;
    elseif ( j == 0.0 )
      type = 2;
    elseif ( 0.0 < j )
      if ( r8_sign ( delta ) ~= r8_sign ( a + b ) )
        type = 3;
      elseif ( r8_sign ( delta ) == r8_sign ( a + b ) )
        type = 4;
      end
    end
  elseif ( delta == 0.0 )
    if ( j < 0.0 )
      type = 5;
    elseif ( 0.0 < j )
      type = 6;
    elseif ( j == 0.0 )

      k = 4.0 * ( a + b ) * f - d * d - e * e;

      if ( k < 0.0 )
        type = 7;
      elseif ( 0.0 < k )
        type = 8;
      elseif ( k == 0.0 )
        type = 9;
      end

    end
  end

  return
end


function [ cluster_center, seed ] = cluster_initialize_5 ( dim_num, ...
  point_num, cluster_num, point, seed )

%*****************************************************************************80
%
%% CLUSTER_INITIALIZE_5 initializes the cluster centers to random values.
%
%  Discussion:
%
%    In this case, each cluster center is a random convex combination
%    of the data points.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 October 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the number of spatial dimensions.
%
%    Input, integer POINT_NUM, the number of points.
%
%    Input, integer CLUSTER_NUM, the number of clusters.
%
%    Input, real POINT(DIM_NUM,POINT_NUM), the coordinates
%    of the points.
%
%    Input, integer SEED, a seed for the random
%    number generator.
%
%    Output, real CLUSTER_CENTER(DIM_NUM,CLUSTER_NUM),
%    the coordinates of the cluster centers.
%
%    Output, integer SEED, a seed for the random
%    number generator.
%

%
%  Get a PxC block of random factors.
%
  [ factor, seed ] = r8mat_uniform_01 ( point_num, cluster_num, seed );
%
%  Make each column of factors have unit sum.
%
  for j = 1 : cluster_num
    column_sum = sum ( factor(1:point_num,j) );
    factor(1:point_num,j) = factor(1:point_num,j) / column_sum;
  end
%
%  Set centers = points * factors.
%
  cluster_center(1:dim_num,1:cluster_num) = ...
    point(1:dim_num,1:point_num) * factor(1:point_num,1:cluster_num);

  return
end


clear all; close all; clc

mtime=21
L=40; n=512; x2=linspace(-L/2,L/2,n+1); x=x2(1:n); % spatial discretization
k=(2*pi/L)*[0:n/2-1 -n/2:-1].';                    % wavenumbers for FFT
t=linspace(0,2*pi,mtime);                      % time domain collection points

q=2*ones(mtime,1);
for j=2:2:mtime
  q(j)=4;
end
q(1)=1; q(mtime)=1;

N=2;
u=N*sech(x);
ut=fft(u);
[t,utsol]=ode45('ch_pod_sol_rhs',t,ut,[],k);
for j=1:length(t)
  usol(j,:)=ifft(utsol(j,:));
end

figure(1)
subplot(2,2,2)
waterfall(x,t,abs(usol)), shading interp, colormap([0 0 0])
%hold on
%pcolor(x,t,abs(usol2)), shading interp, %colormap(gray)
view(20,25)
set(gca,'Xlim',[-20 20],'Ylim',[0 2*pi],'Zlim',[0 4],'Xtick',[-20 -10 0 10 20],'Ytick',[0 3 6],'Ztick',[0 2 4], ...
    'Fontsize',[15]);
subplot(2,2,4)
waterfall(fftshift(k),t,abs(fftshift(utsol))), shading interp, colormap([0 0 0])
view(20,25)
set(gca,'Xlim',[-40 40],'Ylim',[0 2*pi],'Zlim',[0 80],'Xtick',[-40 -20 0 20 40],'Ytick',[0 3 6],'Ztick',[0 40 80], ...
    'Fontsize',[15]);

X=usol.';
for j=1:mtime
  X2(:,j)=X(:,j)*q(j);
end

[U,S,V]=svd(X);
[U2,S2,V2]=svd(X2);

%%

figure(2)
subplot(3,2,1)
semilogy(100*diag(S)/sum(diag(S)),'ko','Linewidth',[2]), grid on, hold on
semilogy(100*S(1,1)/sum(diag(S)),'bo','Linewidth',[2]),
semilogy(2,100*S(2,2)/sum(diag(S)),'go','Linewidth',[2]),
semilogy(3,100*S(3,3)/sum(diag(S)),'ro','Linewidth',[2]),

set(gca,'Xlim',[0 21],'Xtick',[1 6 11 16 21],'Ylim',[10^(-8) 10^3],'Ytick',[10^(-6) 10^(-3) 10^0 10^3],'Fontsize',[15])
xlabel('')

subplot(3,2,2)
semilogy(100*diag(S2)/sum(diag(S2)),'ko','Linewidth',[2]), grid on, hold on
semilogy(100*S2(1,1)/sum(diag(S2)),'bo','Linewidth',[2]),
semilogy(2,100*S2(2,2)/sum(diag(S2)),'go','Linewidth',[2]),
semilogy(3,100*S2(3,3)/sum(diag(S2)),'ro','Linewidth',[2]),

set(gca,'Xlim',[0 21],'Xtick',[1 6 11 16 21],'Ylim',[10^(-8) 10^3],'Ytick',[10^(-6) 10^(-3) 10^0 10^3],'Fontsize',[15])
xlabel('')

subplot(3,1,2)
sn=[-1 -1 1 -1 -1];
for j=1:5
  nrm=1;
  Up(:,j)=real(U(:,j))*sn(j)/nrm;
end
plot(x,real(Up(:,2)),'g','Linewidth',[2]), hold on
plot(x,real(Up(:,3)),'r','Linewidth',[2])
plot(x,real(Up(:,1)),'b','Linewidth',[2])
set(gca,'Xlim',[-10 10],'Xtick',[-10 -5 0 5 10],'Ylim',[-0.3 0.3],'Ytick',[-0.3 0 0.3],'Fontsize',[15])
subplot(3,1,3)
sn=[-1 -1 1 -1 -1];
for j=1:5
  nrm=sqrt(trapz(x,U2(:,j).^2));
  nrm=1;
  Up2(:,j)=real(U2(:,j))*sn(j)/nrm;
end
plot(x,real(Up2(:,3)),'r','Linewidth',[2]), hold on
plot(x,real(Up2(:,2)),'g','Linewidth',[2])
plot(x,real(Up2(:,1)),'b','Linewidth',[2])
set(gca,'Xlim',[-10 10],'Xtick',[-10 -5 0 5 10],'Ylim',[-0.3 0.3],'Ytick',[-0.3 0 0.3],'Fontsize',[15])
legend('mode 3','mode 2','mode 1')

figure(3)
plot(100*diag(S)/sum(diag(S)),'ro','Linewidth',[2])
hold on
plot(100*diag(S2)/sum(diag(S2)),'ko','Linewidth',[2])

figure(4)
sig1=100*diag(S)/sum(diag(S));
sig2=100*diag(S2)/sum(diag(S2));
diff=abs(sig1-sig2);
bar(diff)

%% low-rank reconstructions

X1=U(:,1:3)*S(1:3,1:3)*(V(:,1:3).');
X22=U2(:,1:3)*S2(1:3,1:3)*(V2(:,1:3).');

for j=1:mtime
  X222(:,j)=X22(:,j)/q(j);
end

figure(6)
subplot(2,2,1)
waterfall(x,t,abs(X1).'), shading interp, colormap([0 0 0])
subplot(2,2,2)
waterfall(x,t,abs(X222).'), shading interp, colormap([0 0 0])
E1=norm(X1-X);
E2=norm(X222-X);


function y = vector_norm(x,dim)
%VECTOR_NORM norm for the rows/columns of a matrix
%
%   Usage: y = vector_norm(x,dim)
%
%   Input parameters:
%       x   - matrix [n x m]
%       dim - dimension along the norm should be calculated
%
%   Output parameter:
%       y   - norm of the vectors [1 x m] or [n x 1]
%
%   VECTOR_NORM(x,dim) calculates the p-norm (with p=2) for the vectors
%   given within the matrix along the dimension dim.
%
%   See also: norm, vector_product

%*****************************************************************************
% 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 ==================================
% NOTE: this is disabled due to performance issues in HRTF extrapolation, where
% this function is called multiple times.
%nargmin = 2;
%nargmax = 2;
%narginchk(nargmin,nargmax)
%isargmatrix(x)
%isargpositivescalar(dim)

%% ===== Computation =====================================================
y = sum(abs(x).^2,dim).^(1/2);


function x = f1_abscissas_ab ( a, b, n )

%*****************************************************************************80
%
%% F1_ABSCISSAS_AB computes Fejer type 1 abscissas for the interval [A,B].
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 July 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Philip Davis, Philip Rabinowitz,
%    Methods of Numerical Integration,
%    Second Edition,
%    Dover, 2007,
%    ISBN: 0486453391,
%    LC: QA299.3.D28.
%
%    Walter Gautschi,
%    Numerical Quadrature in the Presence of a Singularity,
%    SIAM Journal on Numerical Analysis,
%    Volume 4, Number 3, 1967, pages 357-362.
%
%    Joerg Waldvogel,
%    Fast Construction of the Fejer and Clenshaw-Curtis Quadrature Rules,
%    BIT Numerical Mathematics,
%    Volume 43, Number 1, 2003, pages 1-18.
%
%  Parameters:
%
%    Input, real A, B, the endpoints of the interval.
%
%    Input, integer N, the order of the rule.
%
%    Output, real X(N), the abscissas.
%
  if ( n == 1 )
    x(1) = 0.5 * ( b + a );
    return
  end

  for i = 1 : n
    theta(i) = ( 2 * n - 2 * i + 1 ) * pi ...
             / ( 2 * n             );
  end

  x(1:n) = 0.5 * ( ( b + a ) + ( b - a ) * cos ( theta(1:n) ) );

  return
end


function ex4bvp
%EX4BVP  Example 4 of the BVP tutorial.
%   This is a nerve impulse model considered in Example 7.1 of
%   R. Seydel, From Equilibrium to Chaos, Elsevier, 1988.  The
%   differential equations
%
%      y1' = 3*(y1 + y2 - 1/3*y1^3 + lambda)
%      y2' = -(y1 - 0.7 + 0.8*y2)/3
%
%   are to be solved subject to periodic boundary conditions
%
%      y1(0) = y1(T)
%      y2(0) = y2(T)
%
%   This is an example of non-separated boundary conditions, meaning
%   that some conditions involve values of the solution at both ends
%   of the interval.  Periodic boundary conditions are a common source
%   of such problems.  BVP4C is unusual in that it accepts problems
%   with non-separated boundary conditions.
%
%   The parameter lambda has the value -1.3 in the text cited.  The
%   period T is unknown, which is to say that the length of the
%   interval is not known. Such problems require some preparation.
%   By scaling the independent variable to tau = t/T the problem is
%   posed on the fixed interval [0,1]. When this is done, T becomes
%   an unknown parameter, but BVP4C provides for unknown parameters.

% Copyright 1999, The MathWorks, Inc.

%  Periodic functions evaluated in EX4INIT are used as a guess for the
%  solution on a crude mesh of 5 equally spaced points.  A guess of 2*pi
%  is provided for the unknown parameter T.
solinit = bvpinit(linspace(0,1,5),@ex4init,2*pi);

options = bvpset('stats','on');

sol = bvp4c(@ex4ode,@ex4bc,solinit,options);
T = sol.parameters;

%  The independent variable needs to be rescaled to its original value
%  before plotting the solution.  The initial guess is also plotted.
clf reset
plot(T*sol.x,sol.y(1,:),T*solinit.x,solinit.y(1,:),'o')
legend('solution found','initial guess');
axis([0 T -2.2 2]);
title('Nerve impulse model');
ylabel('solution y_1(t)');
xlabel(['Periodic solution with period ',num2str(sol.parameters,4)]);
shg

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

function v = ex4init(x)
%EX4INIT  Guess function for Example 4 of the BVP tutorial.
%   V = EX4INIT(X) returns a column vector V that is a guess for y(x).
v = [ sin(2*pi*x)
      cos(2*pi*x)];

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

function dydt = ex4ode(t,y,T);
%EX4ODE  ODE funcion for Example 4 of the BVP tutorial.
dydt = [ 3*T*(y(1) + y(2) - 1/3*(y(1)^3) - 1.3);
         (-1/3)*T*(y(1) - 0.7 + 0.8*y(2)) ];

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

function res = ex4bc(ya,yb,T)
%EX4BC  Boundary conditions for Example 4 of the BVP tutorial.
res = [ya(1) - yb(1)
       ya(2) - yb(2)
       T*(-1/3)*(ya(1) - 0.7 + 0.8*ya(2)) - 1];


function FPDF = GaussPDF
% Return function handles to routines to calculate the area, mean, and
% second moment of a unit-variance, zero-mean, Gaussian probability
% density function.
%                 b
%   Farea(a,b) = Int p(x) dx
%                 a
%                 b
%   Fmean(a,b) = Int x p(x) dx
%                 a
%                b
%   Fvar(a,b) = Int x^2 p(x) dx
%                a
% where p(x) = 1/sqrt(2 pi) e^(-x^2/2)

FPDF = {@Garea, @Gmean, @Gvar};

% ----- ------
function v = Garea (a, b)

% Evaluate the function so as to avoid taking differences
% between nearly equal quantities (e.g. when a<0 and b<0)
if (b >= 0)
  if (a >= 0)
    v = F0(b) - F0(a);                    % Both a and b positive
  else
    v = (F0(b) + 0.5) + (F0(-a) + 0.5);   % a negative, b positive
  end
else
  if (a < 0)
    v = F0(-a) - F0(-b);                  % Both a and b negative
  else
    v = (-0.5 - F0(a)) + (-0.5 - F0(-b)); % a positive, b negative
  end
end

return

% -----
% Evaluate the indefinite integral
% The definite integral between a and b is F0(b) - F0(a)
%
% F0(x) = c Int e^(-x^2/2) dx  [c = 1/sqrt(2 pi)]
%       = -Q(x)
%       = -1/2 erfc(x/sqrt(2))

function v = F0(x)

v = -0.5*erfc(x/sqrt(2));

return

% ----- ------
function v = Gmean (a, b)

% Since the integrand x*p(x) is odd, Gmean(A,B)=Gmean(abs(A),abs(B))
v = F1(abs(b)) - F1(abs(a));

return

% -----
% Evaluate the indefinite integral
% The definite integral between a and b is F1(b) - F1(a)
%
% F1(x) = c Int x e^(-x^2/2) du,
%
% A change of variables v=x^2/2, dv=x dx, gives
%
% F1(x) = c Int e^(-v) dv
%       = -c e^(-v)
%       = -c e^(-x^2/2)

function v = F1 (x)

v = -exp(-0.5*x^2)/sqrt(2*pi);

return

% ----- ------
function v = Gvar (a, b)

% Evaluate the function so as to avoid taking differences
% between nearly equal quantities (e.g. when a<0 and b<0)
if (b >= 0)
  if (a >= 0)
    v = F2(b) - F2(a);                    % Both a and b positive
  else
    v = (F2(b) + 0.5) + (F2(-a) + 0.5);   % a negative, b positive
  end
else
  if (a < 0)
    v = F2(-a) - F2(-b);                  % Both a and b negative
  else
    v = (-0.5 - F2(a)) + (-0.5 - F2(-b)); % a positive, b negative
  end
end

return

% -----
% Evaluate the indefinite integral
% The definite integral between a and b is F2(b) - F2(a)
%
% F2(x) = c Int x^2 e^(-x^2/2) dx
%
% Using integration by parts, identify
%    f(x) = x,           f'(x) = 1,
%   g'(u) = x e^(-x2/2),  g(x) = -e^(-x^2/2)
%
%  Int f(x) g'(x) dx = f(x) g(x) - Int f'(x) g(x) dx
%
% Then
% F2(x) = -c x e^(-x^2/2) + c Int e^(-x^2/2) dx
%       = -c x e^(-x^2/2) - Q(x)

function v = F2 (x)

% The function evaluation fails for x = Inf because the term x e^(-x^2/2)
% in the expression returns NaN. For non-infinite x, the expression
% evaluates to 0 for large x.
xmax = 40;
if (x > xmax)
  v = 0;
else
  v = -x*exp(-0.5*x^2)/sqrt(2*pi) - 0.5*erfc(x/sqrt(2));
end

return


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni

%problem 1

%x(t) =t*exp(-t), 0<t<T , T=6

%x(t),0<t<4T
t=0:.1:6;
x=t.*exp(-t);
xp=repmat(x,1,4);
tp=linspace(-6,18,length(xp));
plot(tp,xp)
legend('x(t) in 4 periods')

%'Approximations
figure %exponential
t0=0;
T=6;
w=2*pi/T;
syms t
x=t.*exp(-t);
k=-40:40;
a=(1/T)*int(x*exp(-j*k*w*t), t,t0,t0+T);
xx=sum(a.*exp(j*k*w*t));
ezplot(xx,[-6 18]);
legend('approximate signal in 4 periods using a_k')

figure %trigonometric
a0=(1/T)*int(x,t0,t0+T);
n=1:81;
b=(2/T)*int(x*cos(n*w*t),t,t0,t0+T);
c=(2/T)*int(x*sin(n*w*t),t,t0,t0+T);
xx=sum(a.*exp(j*k*w*t));
ezplot(xx,[-6 18]);
legend('approximate signal in 4 periods using b_k ,c_k' )


function [v]=arsim(w,A,C,n,ndisc)
%ARSIM	Simulation of AR process.
%
%  v=ARSIM(w,A,C,n) simulates n time steps of the AR(p) process
%
%     v(k,:)' = w' + A1*v(k-1,:)' +...+ Ap*v(k-p,:)' + eta(k,:)',
%
%  where A=[A1 ... Ap] is the coefficient matrix, and w is a vector of
%  intercept terms that is included to allow for a nonzero mean of the
%  process. The vectors eta(k,:) are independent Gaussian noise
%  vectors with mean zero and covariance matrix C.
%
%  The p vectors of initial values for the simulation are taken to
%  be equal to the mean value of the process. (The process mean is
%  calculated from the parameters A and w.) To avoid spin-up effects,
%  the first 10^3 time steps are discarded. Alternatively,
%  ARSIM(w,A,C,n,ndisc) discards the first ndisc time steps.

%  Modified 13-Oct-00
%  Author: Tapio Schneider
%          tapio@gps.caltech.edu

  m       = size(C,1);                  % dimension of state vectors
  p       = size(A,2)/m;                % order of process

  if (p ~= round(p))
    error('Bad arguments.');
  end

  if (length(w) ~= m | min(size(w)) ~= 1)
    error('Dimensions of arguments are mutually incompatible.')
  end
  w       = w(:)';                      % force w to be row vector

  % Check whether specified model is stable
  A1 	  = [A; eye((p-1)*m) zeros((p-1)*m,m)];
  lambda  = eig(A1);
  if any(abs(lambda) > 1)
    warning('The specified AR model is unstable.')
  end

  % Discard the first ndisc time steps; if ndisc is not given as input
  % argument, use default
  if (nargin < 5)
    ndisc = 10^3;
  end

  % Compute Cholesky factor of covariance matrix C
  [R, err]= chol(C);                    % R is upper triangular
  if err ~= 0
    error('Covariance matrix not positive definite.')
  end

  % Get ndisc+n independent Gaussian pseudo-random vectors with
  % covariance matrix C=R'*R
  randvec = randn([ndisc+n,m])*R;

  % Add intercept vector to random vectors
  randvec = randvec + ones(ndisc+n,1)*w;

  % Get transpose of system matrix A (use transpose in simulation because
  % we want to obtain the states as row vectors)
  AT      = A';

  % Take the p initial values of the simulation to equal the process mean,
  % which is calculated from the parameters A and w
  if any(w)
    %  Process has nonzero mean    mval = inv(B)*w'    where
    %             B = eye(m) - A1 -... - Ap;
    %  Assemble B
    B 	 = eye(m);
    for j=1:p
      B = B - A(:, (j-1)*m+1:j*m);
    end
    %  Get mean value of process
    mval = w / B';

    %  The optimal forecast of the next state given the p previous
    %  states is stored in the vector x. The vector x is initialized
    %  with the process mean.
    x    = ones(p,1)*mval;
  else
    %  Process has zero mean
    x    = zeros(p,m);
  end

  % Initialize state vectors
  u      = [x; zeros(ndisc+n,m)];

  % Simulate n+ndisc observations. In order to be able to make use of
  % Matlab's vectorization capabilities, the cases p=1 and p>1 must be
  % treated separately.
  if p==1
    for k=2:ndisc+n+1;
      x(1,:) = u(k-1,:)*AT;
      u(k,:) = x + randvec(k-1,:);
    end
  else
    for k=p+1:ndisc+n+p;
      for j=1:p;
	x(j,:) = u(k-j,:)*AT((j-1)*m+1:j*m,:);
      end
      u(k,:) = sum(x)+randvec(k-p,:);
    end
  end

  % return only the last n simulated state vectors
  v = u(ndisc+p+1:ndisc+n+p,:);


function qwgw_test01 ( )

%*****************************************************************************80
%
%% TEST01 tests QWGW for the Chebyshev Type 1 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;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST01:\n' );
  fprintf ( 1, '  Compute points and weights for Gauss quadrature\n' );
  fprintf ( 1, '  with the Chebyshev Type 1 weight w(x) = 1/sqrt(1-x^2).\n' );
  fprintf ( 1, '  Order N = %d\n', n );
  fprintf ( 1, '  Interval = [%g,%g]\n', a, b );
%
%  Set the recursion coefficients.
%
  aj = zeros ( n, 1 );
  bj = zeros ( n, 1 );

  aj(1:n) = 0.0;

  bj(1) = 1.0 / 2.0;
  for j = 2 : n - 1
    bj(j) = 1.0 / 4.0;
  end
  bj(n) = 0.0;

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

  mu0 = pi;
%
%  Compute the points and weights.
%
  [ x, w ] = sgqf ( n, aj, bj, mu0 );

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

  return
end


function value = r4_acos ( x )

%*****************************************************************************80
%
%% R4_ACOS evaluates the arc-cosine of an R4 argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the arc-cosine of X.
%
  value = 0.5 * pi - r4_asin ( x );

  return
end


function a = complex_i ( )

%*****************************************************************************80
%
%% COMPLEX_I returns the COMPLEX_I matrix.
%
%  Formula:
%
%    0 1
%   -1 0
%
%  Properties:
%
%    A is integral: int ( A ) = A.
%
%    A is anti-involutional: A * A = - I
%
%    A * A * A * A = I
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real A(2,2), the matrix.
%
  a(1,1) =  0.0;
  a(1,2) =  1.0;
  a(2,1) = -1.0;
  a(2,2) =  0.0;

  return
end


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni
%
%
%
%
% problem 1- Unilateral Laplace Transform computation

syms s t
f=-1.25+3.5*t*exp(-2*t) +1.25*exp(-2*t);

F=laplace(f,s);
simplify (F);
pretty(ans)

%verification
f=ilaplace(F,t);
simplify(f);
pretty(ans)


%  <Purpose>
%     Computing the numerical rank of a updated matrix.
%
%  <Syntax>
%     [r,Basis,C] = NumericalRankUpdate(A,pth,vec,C,RC)
%
%  <Input Parameters>
%   1.   A -- the target matrix;
%   2. pth -- the index of row/column to be inserted;
%   3. vec -- the row/column vector to be inserted;
%   4.   C -- cell array contains information required by updating/downdating;
%   5.  RC -- Set to 'row', then the pth row will be inserted.
%             Set to 'column', then the pth column will be inserted.
%
%  <Output Parameters>
%   1. r     -- the numerical rank of the updated matrix;
%   2. Basis --
%          For high rank cases...
%              a matrix whose columns form an orthonormal basis of
%              the numerical kernel;
%
%          For low rank cases...
%              a matrix whose columns form an orthonormal basis of
%              the numerical range;
%
%   3.     C -- Matlab cell array
%
%      For high rank cases...
%          C{1,1} = rank : the numerical rank of the updated matrix;
%          C{2,1} = Basis : matrix whose columns form an orthonormal kernel basis;
%          C{3,1} = Q : the Q in the QR decomposition of the kernel stacked matrix;
%          C{4,1} = R : the R in the QR decomposition of the kernel stacked matrix;
%          C{5,1} = tau : scaling factor in the kernel stacked matrix;
%          C{6,1} = tol : the rank decision threshold;
%
%      For low rank cases...
%          C{1,1} = rank : the numerical rank of the updated matrix;
%          C{2,1} = U : the U in the USV+E decomposition of the updated matrix;
%          C{3,1} = V : the V in the USV+E decomposition of the updated matrix;
%          C{4,1} = S : the S in the USV+E decomposition of the updated matrix;
%          C{5,1} = tol : the rank decision threshold;
%
%  <Reference>
%    [1] T.Y. Li and Z. Zeng, "A Rank-Revealing Method with Updating, Downdating
%        and Applications", SIAM J. Matrix Anal. and Appl., 26 (2005), pp. 918--946.
%
%    [2] T.L. Lee, T.Y. Li and Z. Zeng, "A Rank-Revealing Method with Updating,
%        Downdating and Applications, Part II", SIAM J. Matrix Anal. and Appl. (2009).


function [x]=bias_nodes1d(x,f)

% function [x]=bias_nodes1d(x,f)
% ------------------------------------------------------------------------
% This function biases the spacing for the entries in x using the factor f
% such that the output array goes from x(1) to x(end) but biased using x^f
% (i.e. the spacing decreasing depending on the magnitude of x).
%
%
% Kevin Mattheus Moerman
% gibbon.toolbox@gmail.com
%
% 2014/09/25
%------------------------------------------------------------------------

min_x=min(x,[],2)*ones(1,size(x,2));
x=x-min_x;
max_x=max(x,[],2)*ones(1,size(x,2));
x=max_x.*((x.^f)./(max_x.^f));
x=x+min_x;

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


function ilt = euler_inversion_sym(f_s, t, M, P)

% ilt = euler_inversion_sym(f_s, t, [M], [P])
%
% Returns an approximation to the inverse Laplace transform of function
% handle f_s evaluated at each value in t (1xn) using the Euler method as
% summarized in the source below. This is a symbolic implementation capable
% of much greater accuracy than euler_inversion. For this reason, it takes
% an additional argument, P, the number of significant digits required by
% the calculation. In general, P should be about 0.6*M.
%
% f_s: Handle to function of s
% t:   Times at which to evaluate the inverse Laplace transformation of f_s
% M:   Optional number of terms to sum for each t (64 is a good guess);
%      highly oscillatory functions require higher M, but this can grow
%      unstable; see example_inversions.m for an example of stability [64]
% P:   Optional precision of calculation in significant digits [default 32]
%
% Requires the Symbolic Toolbox(TM).
%
% Abate, Joseph, and Ward Whitt. "A Unified Framework for Numerically
% Inverting Laplace Transforms." INFORMS Journal of Computing, vol. 18.4
% (2006): 408-421. Print.
%
% The paper is also online: http://www.columbia.edu/~ww2040/allpapers.html.
%
% Tucker McClure
% Copyright 2012, The MathWorks, Inc.

    % Make sure t is n-by-1.
    if size(t, 1) == 1
        t = t';
    elseif size(t, 2) > 1
        error('Input times, t, must be a vector.');
    end

    % Set M to 64 if user didn't specify an M.
    if nargin < 3, M = 32; end

    % Set P to the greater of the default from digits() or 0.6M.
    if nargin < 4, P = max(floor(0.6*M), digits()); end

    % Vectorized Talbot's algorithm

    % Binominal function
    bnml = @(n, z) factorial(n)/(factorial(z)*factorial(n-z));

    xi = sym([0.5, ones(1, M), zeros(1, M-1), 2^-sym(M)]);
    for k = 1:M-1
        xi(2*M-k + 1) = xi(2*M-k + 2) + 2^-sym(M) * bnml(sym(M), sym(k));
    end
    k = sym(0:2*M); % Iteration index
    beta = vpa(sym(M)*log(sym(10))/3 + 1i*pi*k, P);
    eta  = vpa((1-mod(k, 2)*2) .* xi, P);

    % Make a mesh so we can do this entire calculation across all k for all
    % given times without a single loop (it's faster this way).
    [beta_mesh, t_mesh] = meshgrid(beta, sym(t));
    eta_mesh = meshgrid(eta, t);

    % Finally, calculate the inverse Laplace transform for each given time.
    f_s_evals = arrayfun(f_s, beta_mesh./t_mesh, 'UniformOutput', false);
    f_s_evals = vpa(reshape([f_s_evals{:}], size(beta_mesh)), P);
    ilt = vpa(10^(sym(M)/3)./sym(t).*sum(eta_mesh.*real(f_s_evals), 2), P);

end


function [ n_data, n, m, x, fx ] = legendre_associated_normalized_values ...
  ( n_data )

%*****************************************************************************80
%
%% LEGENDRE_ASSOCIATED_NORMALIZED_VALUES: normalized associated Legendre.
%
%  Discussion:
%
%    The function considered is the associated Legendre polynomial P^M_N(X).
%
%    In Mathematica, the function can be evaluated by:
%
%      LegendreP [ n, m, x ]
%
%    The function is normalized by dividing by
%
%      sqrt ( 4 * pi * ( n + m )! / ( 2 * n + 1 ) / ( n - m )! )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 September 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz, Irene Stegun,
%    Handbook of Mathematical Functions,
%    National Bureau of Standards, 1964,
%    ISBN: 0-486-61272-4,
%    LC: QA47.A34.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Cambridge University Press, 1999,
%    ISBN: 0-521-64314-7,
%    LC: QA76.95.W65.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0
%    before the first call.  On each call, the routine increments N_DATA by 1,
%    and returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, integer N, integer M, real X,
%    the arguments of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 21;

  fx_vec = [ ...
     0.2820947917738781, ...
     0.2443012559514600, ...
    -0.2992067103010745, ...
    -0.07884789131313000, ...
    -0.3345232717786446, ...
     0.2897056515173922, ...
    -0.3265292910163510, ...
    -0.06997056236064664, ...
     0.3832445536624809, ...
    -0.2709948227475519, ...
    -0.2446290772414100, ...
     0.2560660384200185, ...
     0.1881693403754876, ...
    -0.4064922341213279, ...
     0.2489246395003027, ...
     0.08405804426339821, ...
     0.3293793022891428, ...
    -0.1588847984307093, ...
    -0.2808712959945307, ...
     0.4127948151484925, ...
    -0.2260970318780046 ]';
  m_vec = [ ...
    0, 0, 1, 0, ...
    1, 2, 0, 1, ...
    2, 3, 0, 1, ...
    2, 3, 4, 0, ...
    1, 2, 3, 4, ...
    5 ]';
  n_vec = [ ...
    0,  1,  1,  2, ...
    2,  2,  3,  3, ...
    3,  3,  4,  4, ...
    4,  4,  4,  5, ...
    5,  5,  5,  5, ...
    5 ]';
  x_vec = [ ...
    0.50, ...
    0.50, ...
    0.50, ...
    0.50, ...
    0.50, ...
    0.50, ...
    0.50, ...
    0.50, ...
    0.50, ...
    0.50, ...
    0.50, ...
    0.50, ...
    0.50, ...
    0.50, ...
    0.50, ...
    0.50, ...
    0.50, ...
    0.50, ...
    0.50, ...
    0.50, ...
    0.50 ]';

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

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

  return
end


function variance = weibull_variance ( a, b, c )

%*****************************************************************************80
%
%% WEIBULL_VARIANCE returns the variance of the Weibull PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, C, the parameters of the PDF.
%    0.0 < B,
%    0.0 < C.
%
%    Output, real VARIANCE, the variance of the PDF.
%
  g1 = gamma ( ( c + 2.0 ) / c );
  g2 = gamma ( ( c + 1.0 ) / c );

  variance = b * b * ( g1 - g2 * g2 );

  return
end


%% Example
%
% Copyright:
%   2018 - Simo Särkkä and Arno Solin
%
% License:
%   This software is provided under the MIT License. See the accompanying
%   LICENSE file for details.

%% Leapfrog / Verlet integration

dts = logspace(-2,0,10);
err = nan(10,numel(dts));

for j=1:numel(dts)
    fprintf('Running step %d/%d\n',j,numel(dts));

% Leapfrog / Verlet integration

  % Lock seed
  if exist('rng') % Octave doesn't have rng
    rng(j,'twister')
  else
    randn('state',j);
  end

  % Parameters
  n = 25000;
  g = 1; % g>0
  eta = 1;
  q = 1;
  x0 = 1;
  v0 = 0;

  % Time discretization
  dt = dts(j); %0.1;
  t = 0:dt:10;

  % Specify the model
  f = @(x) -g*x;
  s = @(x) 1;

  % Allocate space and set initial conditions
  x = zeros(n,numel(t)); x(:,1) = x0;
  v = zeros(n,numel(t)); v(:,1) = v0;

  % For each trajectory
  for i=1:n

    % Simulate one trajectory from the leapfrog method
    z = leapfrog(f,s,eta,t,[x0; v0],q);

    x(i,:) = z(1,:);
    v(i,:) = z(2,:);

  end

  figure(1); clf
  subplot(211)
    plot(t,x(1:5,:), ...
         t,mean(x), ...
         t,mean(x)+1.96*std(x),'--', ...
         t,mean(x)-1.96*std(x),'--')
    xlabel('Time, t'), ylabel('Position, x(t)')
  subplot(212)
    plot(t,v(1:5,:), ...
         t,mean(v), ...
         t,mean(v)+1.96*std(v),'--', ...
         t,mean(v)-1.96*std(v),'--')
    xlabel('Time, t'), ylabel('Velocity, v(t)')

% Euler-Maruyama

  % Same random seed
  % Lock seed
  if exist('rng') % Octave doesn't have rng
    rng(j,'twister')
  else
    randn('state',j);
  end

  % Specify the model
  f = @(x,t) [0 1; -g -eta]*x;
  L = @(x,t) [0; 1];

  % Allocate space and set initial conditions
  x_em = zeros(n,numel(t)); x_em(:,1) = x0;
  v_em = zeros(n,numel(t)); v_em(:,1) = v0;

  % For each trajectory
  for i=1:n

    % Simulate one trajectory from the leapfrog method
    z = eulermaruyama(f,L,t,[x0; v0],q);

    x_em(i,:) = z(1,:);
    v_em(i,:) = z(2,:);

  end

  figure(1); clf
  subplot(211)
    plot(t,x_em(1:5,:), ...
         t,mean(x_em), ...
         t,mean(x_em)+1.96*std(x_em),'--', ...
         t,mean(x_em)-1.96*std(x_em),'--')
    xlabel('Time, t'), ylabel('Position, x(t)')
  subplot(212)
    plot(t,v(1:5,:), ...
         t,mean(v_em), ...
         t,mean(v_em)+1.96*std(v_em),'--', ...
         t,mean(v_em)-1.96*std(v_em),'--')
    xlabel('Time, t'), ylabel('Velocity, v(t)')

% Closed-form solution

  % This is a LTI SDE of the form
  F = [0 1; -g -eta];
  L = [0; 1];
  Qc = q;

  % Discretize
  [A,Q] = lti_disc(F,L,Qc,dt);

  % Evaluate
  M = zeros(2,numel(t)); M(:,1) = [x0; v0];
  P = zeros(2,2,numel(t));

  % Loop through
  for k=1:numel(t)-1
    M(:,k+1) = A*M(:,k);
    P(:,:,k+1) = A*P(:,:,k)*A'+Q;
  end

  figure(2); clf
  subplot(211)
    plot(t,M(1,:), ...
         t,M(1,:)'+1.96*sqrt(squeeze(P(1,1,:))),'--', ...
         t,M(1,:)'-1.96*sqrt(squeeze(P(1,1,:))),'--')
  subplot(212)
    plot(t,M(2,:), ...
         t,M(2,:)'+1.96*sqrt(squeeze(P(2,2,:))),'--', ...
         t,M(2,:)'-1.96*sqrt(squeeze(P(2,2,:))),'--')

% Calculate errors

  err(1,j) = P(1,1,end)-std(x(:,end))^2;
  err(2,j) = P(2,2,end)-std(v(:,end))^2;

  err(3,j) = P(1,1,end)-std(x_em(:,end))^2;
  err(4,j) = P(2,2,end)-std(v_em(:,end))^2;

  err(5,j) = M(1,end)-mean(x(:,end));
  err(6,j) = M(2,end)-mean(v(:,end));

  err(7,j) = M(1,end)-mean(x_em(:,end));
  err(8,j) = M(2,end)-mean(v_em(:,end));

  err(9,j)  = mean(abs(squeeze(P(1,1,:))'-std(x).^2));
  err(10,j) = mean(abs(squeeze(P(1,1,:))'-std(x_em).^2));

end

%% Compose results

  figure(1); clf
    loglog(dts,abs(err(9,:)),'-k', ...
           dts,abs(err(10,:)),'--k')
    xlabel('Time step length, $\Delta t$')
    ylabel('Mean absolute error in $\sigma_x^2$')
    legend('Leapfrog Verlet', 'Euler--Maruyama')
    %ylim([0.4 10])
    xlim([min(dts) max(dts)])
    set(gca,'XTick',[0.01 0.1 1],'XTickLabel',[0.01 0.1 1])
    set(gca,'YTick',[1e-3 1e-2 1e-1 1e0 1e1], ...
            'YTickLabel',{'','$10^{-2}$','$10^{-1}$','$10^{0}$','$10^1$'})


% Small technical example.
% Shows how to retrieve the details on a quincunx grid, after a multi-
% resolution decomposition has been performed.
%
disp('Small technical example.');
disp('Shows how to retrieve the details on a quincunx grid, after a multi-');
disp('resolution decomposition has been performed.');
disp('FOR MORE INFORMATION:  help retrieveQ1001');
disp(' ');
disp('See also the report http://repository.cwi.nl:8888/cwi_repository/docs/IV/04/04178D.pdf');
disp('Dr. Paul M. de Zeeuw <Paul.de.Zeeuw@cwi.nl>');
disp(' (C) 1998-2006 Stichting CWI, Amsterdam, The Netherlands');
disp(' ');
%---PARAMETERS-----------------------------------------------------------------
% How to execute, set parameters
N = 6;                   %  maximum level (even number) in lifting scheme
filtername = 'Neville4';
%
%---INSERT YOUR IMAGE HERE-----------------------------------------------------
if exist('imread','file') == 2
  Orig = double(imread('zenithgray.TIF','tiff'));
else
  load zenithgray; Orig = zenithgray; clear zenithgray;
end
%
disp([' Dimensions of original      ' int2str( size(Orig) )]);
%---DECOMPOSITION--------------------------------------------------------------
disp([' Filter type is ' filtername]);
[C,S] = QLiftDec2(Orig,N,filtername);
%
%---RETRIEVE DETAIL AT ODD LEVEL-----------------------------------------------
level = N-1;
disp([' Level ' int2str(level)]);
[F10, F01] = retrieveQ1001(level, 'd', C, S);
%
disp('The dimensions of the detail function read as follows');
disp([int2str(size(F10)) ' and ' int2str(size(F01))]);
%


function [M,N,G] = logfmap(I,L,H)
% [M,N] = logfmap(I,L,H)
%     Return a maxtrix for premultiplying spectrograms to map
%     the rows into a log frequency space.
%     Output map covers bins L to H of input
%     L must be larger than 1, since the lowest bin of the FFT
%     (corresponding to 0 Hz) cannot be represented on a
%     log frequency axis.  Including bins close to 1 makes
%     the number of output rows exponentially larger.
%     N returns the recovery matrix such that N*M is approximately I
%     (for dimensions L to H).
%
% 2004-05-21 dpwe@ee.columbia.edu

% Convert base-1 indexing to base-0
L = L-1;
H = H-1;

ratio = (H-1)/H;
opr = round(log(L/H)/log(ratio));
%ibin = H*exp((opr-[1:opr])*log((H-1)/H));
ibin = L*exp([0:(opr-1)]*-log(ratio));

M = zeros(opr,I);

for i = 1:opr
  % Where do we sample this output bin?
  % Idea is to make them 1:1 at top, and progressively denser below
  % i.e. i = max -> bin = topbin, i = max-1 -> bin = topbin-1,
  % but general form is bin = A exp (i/B)
%  M(i,round(ibin(i))) = 1;
  tt = pi*([0:(I-1)]-ibin(i));
  M(i,:) = (sin(tt)+eps)./(tt+eps);
end

% Normalize rows, but only if they are boosted by the operation
%G = 1./max(1,diag(M'*M))';
%% Fixup gain in bottom bins
%G(1:find(G==min(G))) = min(G);

G = ones(1,I);
G(1:(H+1)) = [0:H]./H;

% Inverse is just transpose plus scaling
N = (M.*repmat(G,opr,1))';


function Ft = CIELabFunction(t, inverse)
%
%       Ft = CIELabFunction(t, inverse)
%
%
%        Input:
%           -t: image
%           -inverse: takes as values 0 or 1. If it is set to 1 the
%                     transformation from XYZ to CIE Lab is applied, otherwise
%                     the transformation from CIE Lab to XYZ
%
%        Output:
%           -Ft: application of CIE Lab f or f^{-1} (if inverse = 1) to X
%
%     Copyright (C) 2013  Francesco Banterle
%
%     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(inverse == 0) %forward function
    Ft = zeros(size(t));

    c1 = (6 / 29)^3;
    c2 = ((29 / 6)^2)/3;
    c3 = 4 / 29;

    Ft(t >  c1) = t(t >  c1).^(1 / 3);
    Ft(t <= c1) = t(t <= c1) * c2 + c3;
end

if(inverse == 1) %inverse function
    Ft = zeros(size(t));

    c1 = 6 / 29;
    c2 = ((6 / 29)^2) * 3;
    c3 = 4 / 29;

    Ft(t >  c1) =  t(t >  c1).^3;
    Ft(t <= c1) = (t(t <= c1) - c3) * c2;
end

end


function angle = lines_exp_angle_3d ( p1, p2, q1, q2 )

%*****************************************************************************80
%
%% LINES_EXP_ANGLE_3D finds the angle between two explicit lines in 3D.
%
%  Discussion:
%
%    The explicit form of a line in 3D is:
%
%      ( P1, P2 ) = ( (X1,Y1,Z1), (X2,Y2,Z3) ).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real P1(3,1), P2(3,1), two points on the first line.
%
%    Input, real Q1(3,1), Q2(3,1), two points on the second line.
%
%    Output, real ANGLE, the angle in radians between the two
%    lines.  The angle is computed using the ACOS function, and so lies between
%    0 and PI.  But if one of the lines is degenerate, the angle is
%    returned as -1.0.
%
  dim_num = 3;

  pnorm = sqrt ( sum ( ( p2(1:dim_num,1) - p1(1:dim_num,1) ).^2 ) );

  if ( pnorm == 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'LINES_EXP_ANGLE_3D - Fatal error!\n' );
    fprintf ( 1, '  The line (P1,P2) is degenerate!\n' );
    angle = -1.0;
    return
  end

  qnorm = sqrt ( sum ( ( q2(1:dim_num,1) - q1(1:dim_num,1) ).^2 ) );

  if ( qnorm == 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'LINES_EXP_ANGLE_3D - Fatal error!\n' );
    fprintf ( 1, '  The line (Q1,Q2) is degenerate!\n' );
    angle = -1.0;
    return
  end

  pdotq = ( p2(1:dim_num,1) - p1(1:dim_num,1) )' ...
        * ( q2(1:dim_num,1) - q1(1:dim_num,1) );

  ctheta = pdotq / ( pnorm * qnorm );

  angle = r8_acos ( ctheta );

  return
end


function taun = tau ( n, taun )

%*****************************************************************************80
%
%% TAU returns the value of TAU(N), the number of distinct divisors of N.
%
%  Discussion:
%
%    TAU(N) is the number of divisors of N, including 1 and N.
%
%  First values:
%
%     N   TAU(N)
%
%     1    1
%     2    2
%     3    2
%     4    3
%     5    2
%     6    4
%     7    2
%     8    4
%     9    3
%    10    4
%    11    2
%    12    6
%    13    2
%    14    4
%    15    4
%    16    5
%    17    2
%    18    6
%    19    2
%    20    6
%
%  Formula:
%
%    If the prime factorization of N is
%
%      N = P1^E1 * P2^E2 * ... * PM^EM,
%
%    then
%
%      TAU(N) = ( E1 + 1 ) * ( E2 + 1 ) * ... * ( EM + 1 ).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 July 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the value to be analyzed.  N must be 1 or
%    greater.
%
%    Output, integer TAUN, the value of TAU(N).  But if N is 0 or
%    less, TAUN is returned as 0, a nonsense value.  If there is
%    not enough room for factoring, TAUN is returned as -1.
%
  if ( n <= 0 )
    taun = 0;
    return
  end

  if ( n == 1 )
    taun = 1;
    return
  end
%
%  Factor N.
%
  [ nfactor, factor, power, nleft ] = i4_factor ( n );

  if ( nleft ~= 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TAU - Fatal error!\n' );
    fprintf ( 1, '  Not enough factorization space.\n' );
    taun = -1;
    error ( 'TAU - Fatal error!' );
  end

  taun = 1;
  for i = 1 : nfactor
    taun = taun * ( power(i) + 1 );
  end

  return
end


function value = fall ( x, n )

%*****************************************************************************80
%
%% FALL computes the falling factorial function [X]_N.
%
%  Discussion:
%
%    The number of "injections" or 1-to-1 mappings from
%    a set of N elements to a set of M elements is [M]_N.
%
%    The number of permutations of N objects out of M is [M}_N.
%
%    The Stirling numbers of the first kind can be used
%    to convert a falling factorial into a polynomial, as follows:
%
%      [X]_N = S^0_N + S^1_N * X + S^2_N * X^2 + ... + S^N_N X^N.
%
%  Formula:
%
%    [X]_N = X * ( X - 1 ) * ( X - 2 ) * ... * ( X - N + 1 ).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 January 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer X, the argument of the falling factorial
%    function.
%
%    Input, integer N, the order of the falling factorial function.
%    If N = 0, FALL = 1, if N = 1, FALL = X.  Note that if N is
%    negative, a "rising" factorial will be computed.
%
%    Output, integer VALUE, the falling factorial function.
%
  value = 1;

  arg = x;

  if ( 0 < n )

    for i = 1 : n
      value = value * arg;
      arg = arg - 1;
    end

  elseif ( n < 0 )

    for i = -1 : - 1 : - n
      value = value * arg;
      arg = arg + 1;
    end

  end

  return
end


function prob_test0276 ( )

%*****************************************************************************80
%
%% TEST0276 tests CARDIOID_MEAN, CARDIOID_SAMPLE, CARDIOID_VARIANCE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 April 2009
%
%  Author:
%
%    John Burkardt
%
  sample_num = 1000;

  a = 0.0;
  b = 0.25;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST0276\n' );
  fprintf ( 1, '  For the Cardioid PDF:\n' );
  fprintf ( 1, '  CARDIOID_MEAN computes the mean;\n' );
  fprintf ( 1, '  CARDIOID_SAMPLE samples;\n' );
  fprintf ( 1, '  CARDIOID_VARIANCE computes the variance.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  PDF parameter A = %f\n', a );
  fprintf ( 1, '  PDF parameter B = %f\n', b );

  if ( ~cardioid_check ( a, b ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'Fatal error!\n' );
    fprintf ( 1, '  The parameters are not legal.\n' );
    return;
  end

  mean = cardioid_mean ( a, b );
  variance = cardioid_variance ( a, b );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  PDF mean =                    %f\n', mean );
  fprintf ( 1, '  PDF variance =                %f\n', variance );

  for i = 1 : sample_num
    [ x(i), seed ] = cardioid_sample ( a, b, seed );
  end

  mean = r8vec_mean ( sample_num, x );
  variance = r8vec_variance ( sample_num, x );
  xmax = max ( x(1:sample_num) );
  xmin = min ( x(1:sample_num) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Sample size =     %6d\n', sample_num );
  fprintf ( 1, '  Sample mean =     %14f\n', mean );
  fprintf ( 1, '  Sample variance = %14f\n', variance );
  fprintf ( 1, '  Sample maximum =  %14f\n', xmax );
  fprintf ( 1, '  Sample minimum =  %14f\n', xmin );

  return
end


function [bp,wf]=grule(n)
%
% [bp,wf]=grule(n)
%  This function computes Gauss base points and weight factors
%  using the algorithm given by Davis and Rabinowitz in 'Methods
%  of Numerical Integration', page 365, Academic Press, 1975.
%
bp=zeros(n,1); wf=bp; iter=2; m=fix((n+1)/2); e1=n*(n+1);
mm=4*m-1; t=(pi/(4*n+2))*(3:4:mm); nn=(1-(1-1/n)/(8*n*n));
xo=nn*cos(t);
for j=1:iter
   pkm1=1; pk=xo;
   for k=2:n
      t1=xo.*pk; pkp1=t1-pkm1-(t1-pkm1)/k+t1;
      pkm1=pk; pk=pkp1;
   end
   den=1.-xo.*xo; d1=n*(pkm1-xo.*pk); dpn=d1./den;
   d2pn=(2.*xo.*dpn-e1.*pk)./den;
   d3pn=(4*xo.*d2pn+(2-e1).*dpn)./den;
   d4pn=(6*xo.*d3pn+(6-e1).*d2pn)./den;
   u=pk./dpn; v=d2pn./dpn;
   h=-u.*(1+(.5*u).*(v+u.*(v.*v-u.*d3pn./(3*dpn))));
   p=pk+h.*(dpn+(.5*h).*(d2pn+(h/3).*(d3pn+.25*h.*d4pn)));
   dp=dpn+h.*(d2pn+(.5*h).*(d3pn+h.*d4pn/3));
   h=h-p./dp; xo=xo+h;
end
bp=-xo-h;
fx=d1-h.*e1.*(pk+(h/2).*(dpn+(h/3).*(d2pn+(h/4).*(d3pn+(.2*h).*d4pn))));
wf=2*(1-bp.^2)./(fx.*fx);
if ( (m+m) > n )
	bp(m)=0;
end
if ( ~ ((m+m) == n) )
	m=m-1;
end
jj=1:m; n1j=(n+1-jj); bp(n1j)=-bp(jj); wf(n1j)=wf(jj);
bp = reshape(bp,length(bp),1);
wf = reshape(wf,length(wf),1);

end


% Copyright (C) 2000-2002 Paul Kienzle <pkienzle@users.sf.net>
%
% 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/>.
%
% usage:  w = triang (L)
%
% Returns the filter coefficients of a triangular window of length L.
% Unlike the bartlett window, triang does not go to zero at the edges
% of the window.  For odd L, triang(L) is equal to bartlett(L+2) except
% for the zeros at the edges of the window.

function w = triang(L)
  if nargin ~= 1
    help(mfilename);
  elseif ~isscalar(L) || L ~= fix (L) || L < 1
    error('triang: L has to be an integer > 0');
  end % if
  w = 1 - abs ((-(L-1):2:(L-1))' / (L+rem(L,2)));
end

%!error triang
%!error triang(1,2)
%!error triang([1,2]);
%!assert (triang(1), 1)
%!assert (triang(2), [1; 1]/2)
%!assert (triang(3), [1; 2; 1]/2);
%!assert (triang(4), [1; 3; 3; 1]/4);
%!test
%! x = bartlett(5);
%! assert (triang(3), x(2:4));

%!demo
%! subplot(221); axis([-1, 1, 0, 1.3]); grid('on');
%! title('comparison with continuous for odd n');
%! n=7; k=(n-1)/2; t=[-k:0.1:k]/(k+1);
%! plot(t,1-abs(t),';continuous;',[-k:k]/(k+1),triang(n),'g*;discrete;');
%!
%! subplot(222); axis([-1, 1, 0, 1.3]); grid('on');
%! n=8; k=(n-1)/2; t=[-k:0.1:k]/(k+1/2);
%! title('note the higher peak for even n');
%! plot(t,1+1/n-abs(t),';continuous;',[-k:k]/(k+1/2),triang(n),'g*;discrete;');
%!
%! subplot(223); axis; grid('off');
%! title('n odd, triang(n)==bartlett(n+2)');
%! n=7;
%! plot(0:n+1,bartlett(n+2),'g-*;bartlett;',triang(n),'r-+;triang;');
%!
%! subplot(224); axis; grid('off');
%! title('n even, triang(n)!=bartlett(n+2)');
%! n=8;
%! plot(0:n+1,bartlett(n+2),'g-*;bartlett;',triang(n),'r-+;triang;');
%!
%! subplot(111); title('');


function [EC,ec,degij] = edge_nei_overlap_bu(CIJ)
%EDGE_NEI_OVERLAP_BU        overlap amongst neighbors of two adjacent nodes
%
%   [EC,ec,degij] = edge_nei_bu(CIJ);
%
%   This function determines the neighbors of two nodes that are linked by
%   an edge, and then computes their overlap.  Connection matrix must be
%   binary and directed.  Entries of 'EC' that are 'inf' indicate that no
%   edge is present.  Entries of 'EC' that are 0 denote "local bridges", i.e.
%   edges that link completely non-overlapping neighborhoods.  Low values
%   of EC indicate edges that are "weak ties".
%
%   If CIJ is weighted, the weights are ignored.
%
%   Inputs:     CIJ,    undirected (binary/weighted) connection matrix
%
%   Outputs:    EC,     edge neighborhood overlap matrix
%               ec,     edge neighborhood overlap per edge, in vector format
%               degij,  degrees of node pairs connected by each edge
%
%   Reference: Easley and Kleinberg (2010) Networks, Crowds, and Markets.
%              Cambridge University Press, Chapter 3.
%
%   Olaf Sporns, Indiana University, 2012

[ik,jk,ck] = find(CIJ);
lel = length(ck);
N = size(CIJ,1);

[deg] = degrees_und(CIJ);

ec = zeros(1,lel);
degij = zeros(2,lel);
for e=1:lel
    neiik = setdiff(union(find(CIJ(ik(e),:)),find(CIJ(:,ik(e))')),[ik(e) jk(e)]);
    neijk = setdiff(union(find(CIJ(jk(e),:)),find(CIJ(:,jk(e))')),[ik(e) jk(e)]);
    ec(e) = length(intersect(neiik,neijk))/length(union(neiik,neijk));
    degij(:,e) = [deg(ik(e)) deg(jk(e))];
end;

ff = find(CIJ);
EC = 1./zeros(N);
EC(ff) = ec;                        %#ok<FNDSB>


function count = subset_sum_count ( w, t, r )

%*****************************************************************************80
%
%% SUBSET_SUM_COUNT counts the solutions to the subset sum problem in a given range.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 May 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer W(N), a set of weights.  The length of this
%    array must be no more than 31.
%
%    Input, integer T, the target value.
%
%    Input, integer R(2), the lower and upper limits to be searched.
%    If this argument is omitted, the entire range, [0, 2^N-1 ] will
%    be searched.
%
%    Output, integer COUNT, the number of solutions found in this range.
%
  c = [];
  index = [];
%
%  Using a single integer to track the subsets only works if the number
%  of objects is small enough.  MATLAB can pack no more than 31 bits of
%  information into a nonnegative integer.
%
  if ( nargin < 1 )
    w = input ( 1, '  Enter the vector of weights [ w(1), w(2), ..., w(n)]: ' );
  end

  n = length ( w );

  if ( 31 < n )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'SUBSET_SUM_FIND - Fatal error!\n' );
    fprintf ( 1, '  This function is restricted to N <= 31.\n' );
    error ( 'SUBSET_SUM_FIND - Fatal error!' );
  end

  if ( nargin < 2 )
    t = input ( '  Enter the target value T: ' );
  end
%
%  Make sure the range is reasonable.
%
  if ( nargin < 3 )
    r(1) = 0;
    r(2) = 2^n - 1;
  else
    r(1) = max ( r(1), 0 );
    r(2) = min ( r(2), 2^n - 1 );
  end

  fprintf ( 1, '\n' )
  fprintf ( 1, '  Searching indices %d through %d\n', r(1), r(2) );
%
%  Run through the range.
%
  count = 0;

  for index = r(1) : r(2)
%
%  Convert INDEX into vector of indices in W.
%
    c = find ( bitget ( index, 1:n ) );
%
%  If the sum of those weights matches the target, increment the count.
%
    if ( sum ( w(c) ) == t )
      count = count + 1;
    end

  end

  index = [];

  return
end


function h = hypot3(dx, dy, dz)
%HYPOT3 Diagonal length of a cuboidal 3D box
%
%   h = hypot3(a, b, c)
%   computes the quantity sqrt(a^2 + b^2 + c^2), by avoiding roundoff
%   errors.
%
%   Example
%     % Compute diagonal of unit cube
%     hypot3(1, 1, 1)
%     ans =
%          1.7321
%
%     % Compute more complicated diagonal
%     hypot3(3, 4, 5)
%     ans =
%         7.0711
%
%   See also
%   hypot, vectorNorm3d
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2012-04-29,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2012 INRA - Cepia Software Platform.

h = hypot(hypot(dx, dy), dz);


function pass = test_Poisson( )
% Check correctness of Poisson solver on the sphere:

% Discretization sizes:
m = 40;
n = 40;
tol = 1e3*chebfunpref().cheb2Prefs.chebfun2eps;

% Example 1:
f = spherefun(@(lam,th) -6*cos(lam).*cos(th).*sin(th));
exact = spherefun(@(lam,th) sin(th).*cos(th).*cos(lam));
u = spherefun.poisson(f, 0, m, n);
pass(1) = ( norm(u - exact, inf) < tol );

% Example 2:
f = spherefun(@(lam,th) -4*(3*cos(th)+5*cos(3*th)).*sin(lam).*sin(th));
exact = spherefun(@(lam,th) -2*sin(lam).*sin(2*th).*sin(th).^2);
u = spherefun.poisson(f, 0, m, n);
pass(2) = ( norm(u - exact, inf) < tol );

% Inline example:
f = spherefun(@(lam,th) -6*(-1+5*cos(2*th)).*sin(lam).*sin(2*th));
exact = spherefun(@(lam,th) -2*sin(lam).*sin(2*th).*sin(th).^2 -...
            sin(lam).*sin(th).*cos(th) + .5*sin(lam).*sin(2*th).*cos(2*th));
u = spherefun.poisson(f, 0, m, n);
pass(3) = ( norm(u - exact, inf) < tol );

% Check that the mean is zero.
pass(4) = ( abs(mean2(u)) < tol );

% Check that the code properly deals with a right hand side without a
% mean of zero.
f = spherefun(@(x,y,z) 1 + x);
warning('off','CHEBFUN:SPHEREFUN:POISSON:meanRHS');
u = spherefun.poisson(f, 0, 10);
warning('on','CHEBFUN:SPHEREFUN:POISSON:meanRHS');
% This f - mean2(f)
g = spherefun(@(x,y,z) x);
% Solution with the mean of f set to zero
v = spherefun.poisson(g, 0, 10);
pass(5) = ( norm(u - v) < tol );

% Check that the code allows the mean of the solution to be set to
% something other than zero.
f = spherefun(@(x,y,z) x.*y.*z );
u = spherefun.poisson(f, 1, 10);
pass(6) = ( abs(mean2(u)-1) < tol );

end


function M = cross2Matrix(x)
% CROSS2MATRIX  Antisymmetric matrix corresponding to a 3-vector
%
% Computes the antisymmetric matrix M corresponding to a 3-vector x such
% that M*y = cross(x,y) for all 3-vectors y.
%
% Input:
%   - x(3,1) : vector
%
% Output:
%   - M(3,3) : antisymmetric matrix
%

M = [0,    -x(3),  x(2); ...
     x(3),   0,   -x(1); ...
    -x(2),  x(1),   0  ];

end


function [R] = quat2mat(q)
  % QUAT2MAT convert quaternions to 3d rotation matrices
  %
  % [R] = quat2mat(q)
  %
  % Input:
  %   q is an m by 4 list of normalized quaternions, [1 i j k]
  % Output:
  %   R is a 3 by 3 by m list of rotation matrices
  %
  % See:
  %   http://en.wikipedia.org/wiki/
  %   Conversion_between_quaternions_and_Euler_angles#Rotation_matrices
  %
  % Copyright 2011, Alec Jacobson (jacobson@inf.ethz.ch)
  %

  assert(size(q,2) == 4);

  R = zeros(3,3,size(q,1));
  R(1,1,:) = q(:,1).^2 + q(:,2).^2 - q(:,3).^2 - q(:,4).^2;
  R(1,2,:) = 2*(q(:,2).*q(:,3) - q(:,1).*q(:,4));
  R(1,3,:) = 2*(q(:,1).*q(:,3) + q(:,2).*q(:,4));
  R(2,1,:) = 2*(q(:,2).*q(:,3) + q(:,1).*q(:,4));
  R(2,2,:) = q(:,1).^2 - q(:,2).^2 + q(:,3).^2 - q(:,4).^2;
  R(2,3,:) = 2*(q(:,3).*q(:,4) - q(:,1).*q(:,2));
  R(3,1,:) = 2*(q(:,2).*q(:,4) - q(:,1).*q(:,3));
  R(3,2,:) = 2*(q(:,1).*q(:,2) + q(:,3).*q(:,4));
  R(3,3,:) = q(:,1).^2 - q(:,2).^2 - q(:,3).^2 + q(:,4).^2;

  %yy2 = 2.0 .* q(:,2) .* q(:,2);
  %xy2 = 2.0 .* q(:,1) .* q(:,2);
  %xz2 = 2.0 .* q(:,1) .* q(:,3);
  %yz2 = 2.0 .* q(:,2) .* q(:,3);
  %zz2 = 2.0 .* q(:,3) .* q(:,3);
  %wz2 = 2.0 .* q(:,4) .* q(:,3);
  %wy2 = 2.0 .* q(:,4) .* q(:,2);
  %wx2 = 2.0 .* q(:,4) .* q(:,1);
  %xx2 = 2.0 .* q(:,1) .* q(:,1);
  %R(1,1,:) = - yy2 - zz2 + 1;
  %R(1,2,:) = xy2 + wz2;
  %R(1,3,:) = xz2 - wy2;
  %R(2,1,:) = xy2 - wz2;
  %R(2,2,:) = - xx2 - zz2 + 1;
  %R(2,3,:) = yz2 + wx2;
  %R(2,1,:) = xz2 + wy2;
  %R(2,2,:) = yz2 - wx2;
  %R(2,3,:) = - xx2 - yy2 + 1;
end


function prob_test0253 ( )

%*****************************************************************************80
%
%% TEST0253 tests BUFFON_PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 April 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST0253\n' );
  fprintf ( 1, '  BUFFON_PDF evaluates the Buffon PDF,\n' );
  fprintf ( 1, '  the probability that, on a grid of cells of width A,\n' );
  fprintf ( 1, '  a needle of length L, dropped at random,\n' );
  fprintf ( 1, '  will cross at least one grid line.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      A         L        PDF\n' );
  fprintf ( 1, '\n' );

  for i = 1 : 5
    a = i;

    for k = 0 : 5
      l = k * a / 5.0;
      pdf = buffon_pdf ( a, l );
      fprintf ( 1, '  %8.4f  %8.4f  %14f\n', a, l, pdf );
    end

    fprintf ( 1, '\n' );

  end

  return
end


function epsil = precise_rand(n, delta, logm, shutup)
% This function computes (almost) precise random coding bound for BSC. The only imprecision is due to
% union bound.
% epsil <= sum_k  \delta^k (1-delta)^(n-k) P[one of M-1 other codewords is inside sphere <= k]

Ks = 0:n;
bino_coeffs = (gammaln(n+1) - gammaln(Ks+1) - gammaln(n-Ks+1))/log(2);
terms_spheres = bino_coeffs + logm - n;
[tmp log_spheres] = sumlog2(terms_spheres);

% cut off stupid >1 values (this what gallager rho-trick is supposed to do also)
log_spheres = min(log_spheres, 0);

terms = bino_coeffs + log2(delta)*Ks + log2(1-delta)*(n-Ks) + log_spheres;

epsil = 2^sumlog2(terms);

if(nargin < 4) || (isempty(shutup))
	disp(sprintf('-- precise_rand(n = %d, delta = %g, logm = %.1f): epsil = %.3g', n, delta, logm, epsil));
end


%% BIHARMONICMIXEDFEM solves the biharmonic equation
%
%     laplace^2 u = f;   [0,1]^2
%               u = g_D;
%            Dn u = g_N.
%
% by writing in a mixed form
%
%       w = Lap u
%   Lap w = f
%
% The boundary condition u = g_D is imposed into the space (Dirichlet) and
% the boundary condition Dn u = g_N is imposed as a Newmann boundary
% condition.
%
% The rate of convergence for u is optimal but for w is sub-optimal. For
% linear element, optimal order for w is also observed.
%
% Created by Jie Zhou. Clean up by Long Chen. Further clean up on
% biharmonic functions are needed and multigrid solvers should be included.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

close all
clear variables;

%% Parameters
maxIt = 4;
N = zeros(maxIt,1);
h = zeros(maxIt,1);
erruL2 = zeros(maxIt,1);
erruH1 = zeros(maxIt,1);
errwL2 = zeros(maxIt,1);
errwH1 = zeros(maxIt,1);

%%  Generate an initial mesh
[node,elem] = squaremesh([0 1 0 1], 0.25);
bdFlag = setboundary(node,elem,'Dirichlet');    % Dirichlet boundary condition

for k = 1:1
    [node,elem,bdFlag] = uniformrefine(node,elem,bdFlag);
%     [node,elem,bdFlag] = uniformbisect(node,elem,bdFlag);
end

%% Set up PDE data
pde = biharmonicdata;

%% Finite Element Method
for k = 1:maxIt
    % refine mesh
   [node,elem,bdFlag] = uniformrefine(node,elem,bdFlag);
   [w,u] = biharmonicP1(node,elem,bdFlag,pde);
%    [w,u] = biharmonicP2(node,elem,bdFlag,pde);
%    [w,u] = biharmonicP3(node,elem,bdFlag,pde);
   N(k) = size(w,1)+size(u,1);
   h(k) = 1./(sqrt(size(node,1))-1);
   erruL2(k) = getL2error(node,elem,pde.exactu,u);
   erruH1(k) = getH1error(node,elem,pde.Du,u);
   errwL2(k) = getL2error(node,elem,pde.exactw,w);
   errwH1(k) = getH1error(node,elem,pde.Dw,w);
end

%% Plot convergence rates and display error table
figure;
subplot(1,2,1);
showrateh2(h,erruH1,1,'-*','|| Du - Du_h||',...
           h,erruL2,1,'k-+','|| u - u_h||');
subplot(1,2,2)
showrateh2(h,errwH1,1,'c-*','|| Dw - Dw_h||',...
           h,errwL2,1,'m-+','|| w - w_h||');

fprintf('\n');
disp('Table: Error')
colname = {'#Dof','h','||u-u_h||','||Du-Du_h||','||Dw-Dw_h||','||w - w_h||'};
disptable(colname,N,[],h,'%0.3e',erruL2,'%0.5e',erruH1,'%0.5e',errwH1,'%0.5e',errwL2,'%0.5e');