function [ x, odata, opts ] = solver_SLOPE( A, b, lambda, x0, opts )
% SOLVER_SLOPE Sorted l1-regularized least squares problem,
% [ beta, odata, opts ] = solver_SLOPE( X, y, lambda, beta0, opts )
%    Solves the l1-regularized least squares problem, using the sorted/ordered l1 norm,
%        minimize (1/2)*norm( A * x - b )^2 + norm( lasso.*sort(abs(x),'descend'), 1 )
%    using the Auslender/Teboulle variant with restart. X must be a matrix
%    or a linear operator, y must be a vector, and lambda must be a real
%    positive vector in decreasing order.
%    The initial point beta0 and option structure opts are both optional.
%
% SLOPE stands for Sorted L-One Penalized Estimation
%
% Reference:
%   "Statistical Estimation and Testing via the Ordered l1 Norm"
%   by M. Bogdan, E. van den Berg, W. Su, and E. J. Candès, 2013
%   http://www-stat.stanford.edu/~candes/OrderedL1/
%
%   See also solver_L1RLS.m, solver_LASSO.m, prox_Sl1.m

error(nargchk(3,5,nargin));
if nargin < 4, x0 = []; end
if nargin < 5, opts = []; end
if ~isfield( opts, 'restart' ),
    opts.restart = 100;
end

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

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


function r= cauchyrnd(varargin)

% USAGE:       r= cauchyrnd(a, b, n, ...)
%
% Generate random numbers from the Cauchy distribution, r= a + b*tan(pi*(rand(n)-0.5)).
%
% ARGUMENTS:
% a (default value: 0.0) must be scalars or size(x).
% b (b>0, default value: 1.0) must be scalars or size(x).
% n and onwards (default value: 1) specifies the dimension of the output.
%
% EXAMPLE:
% r= cauchyrnd(0, 1, 10); % A 10 by 10 array of random values, Cauchy distributed.
%
% SEE ALSO:    cauchycdf, cauchyfit, cauchyinv, cauchypdf.
%
% Copyright (C) Peder Axensten <peder at axensten dot se>
%
% HISTORY:
% Version 1.0, 2006-07-10.
% Version 1.1, 2006-07-26.
% - Added cauchyfit to the cauchy package.
% Version 1.2, 2006-07-31:
% - cauchyinv(0, ...) returned a large negative number but should be -Inf.
% - Size comparison in argument check didn't work.
% - Various other improvements to check list.

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

	% Default values
	a=	0.0;
	b=	1.0;
	n=	1;

	% Check the arguments
	if(nargin >= 1)
		a=	varargin{1};
		if(nargin >= 2)
			b=			varargin{2};
			b(b <= 0)=	NaN;	% Make NaN of out of range values.
			if(nargin >= 3),	n=	[varargin{3:end}];		end
		end
	end

	% Generate
	r=	cauchyinv(rand(n), a, b);
end


function [xn,dxdf,dxdc,dxdk,dxdalpha] = normalize2(x_kk,fc,cc,kc,alpha_c),

%normalize
%
%[xn] = normalize(x_kk,fc,cc,kc,alpha_c)
%
%Computes the normalized coordinates xn given the pixel coordinates x_kk
%and the intrinsic camera parameters fc, cc and kc.
%
%INPUT: x_kk: Feature locations on the images
%       fc: Camera focal length
%       cc: Principal point coordinates
%       kc: Distortion coefficients
%       alpha_c: Skew coefficient
%
%OUTPUT: xn: Normalized feature locations on the image plane (a 2XN matrix)
%
%Important functions called within that program:

k1 = kc(1);
k2 = kc(2);
k3 = kc(5);
p1 = kc(3);
p2 = kc(4);

N = size(x_kk,2);

% First: Subtract principal point, and divide by the focal length:
x_distort = [(x_kk(1,:) - cc(1))/fc(1);(x_kk(2,:) - cc(2))/fc(2)];

v1 = - x_distort(1,:) / fc(1);
v2 = - x_distort(2,:) / fc(1);

dx_distortdfc = zeros(2*N,2);
dx_distortdfc(1:2:end,1) = v1';
dx_distortdfc(2:2:end,2) = v2';

v1 = - x_distort(1,:) / fc(1);
v2 = - x_distort(2,:) / fc(1);

dx_distortdcc = zeros(2*N,2);
dx_distortdcc(1:2:end,1) = -(1/fc(1)) * ones(N,1);
dx_distortdcc(2:2:end,2) = -(1/fc(2)) * ones(N,1);

% Second: undo skew
x_distort(1,:) = x_distort(1,:) - alpha_c * x_distort(2,:);

dx_distort2dfc = [ dx_distortdfc(:,1)-alpha_c *dx_distortdfc(:,2)   dx_distortdfc(:,2)];
dx_distort2dcc = [ dx_distortdcc(:,1)-alpha_c *dx_distortdcc(:,2)   dx_distortdcc(:,2)];

dx_distort2dalpha_c = zeros(2*N,1);
dx_distort2dalpha_c(1:2:end) = -x_distort(2,:)';

x = x_distort; 				% initial guess

for kk=1:20,

    r_2 = sum(x.^2);
    k_radial =  1 + k1 * r_2 + k2 * r_2.^2 + k3 * r_2.^3;
    delta_x = [2*p1*x(1,:).*x(2,:) + p2*(r_2 + 2*x(1,:).^2); p1 * (r_2 + 2*x(2,:).^2)+2*p2*x(1,:).*x(2,:)];
    x = (x_distort - delta_x)./(ones(2,1)*k_radial);

end;

xn = x;

dxdk = zeros(2*N,5); % Approximation (no time)
dxdf = dx_distort2dfc;
dxdc = dx_distort2dcc;
dxdalpha = dx_distort2dalpha_c;


function [x,w,P]=GaussRadau(kk)
N = kk-1; % Compute for the number of points kk

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% lgrnodes.m
%
% Computes the Legendre-Gauss-Radau nodes, weights and the LGR Vandermonde
% matrix. The LGR nodes are the zeros of P_N(x)+P_{N+1}(x).
%
% References on LGR nodes and weights:
%   C. Canuto, M. Y. Hussaini, A. Quarteroni, T. A. Tang, "Spectral Methods
%   in Fluid Dynamics," Section 2.3. Springer-Verlag 1987
%
%   F. B. Hildebrand , "Introduction to Numerical Analysis," Section 8.11
%   Dover 1987
%
% Written by Greg von Winckel - 05/02/2004
% Contact: gregvw@chtm.unm.edu
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Truncation + 1
N1=N+1;

% Use Chebyshev-Gauss-Radau nodes as initial guess for LGR nodes
x=-cos(2*pi*(0:N)/(2*N+1))';

% The Legendre Vandermonde Matrix
P=zeros(N1,N1+1);

% Compute P_(N) using the recursion relation
% Compute its first and second derivatives and
% update x using the Newton-Raphson method.

xold=2;

% Free abscissae
free=2:N1;

while max(abs(x-xold))>eps

    xold=x;

    P(1,:)=(-1).^(0:N1);

    P(free,1)=1;    P(free,2)=x(free);

    for k=2:N1
        P(free,k+1)=( (2*k-1)*x(free).*P(free,k)-(k-1)*P(free,k-1) )/k;
    end

    x(free)=xold(free)-((1-xold(free))/N1).*(P(free,N1)+P(free,N1+1))...
        ./(P(free,N1)-P(free,N1+1));
end

% The Legendre-Gauss-Radau Vandermonde
P=P(1:N1,1:N1);

% Compute the weights
w=zeros(N1,1);
w(1)=2/N1^2;
w(free)=(1-x(free))./(N1*P(free,N1)).^2;


function p = polyfitweighted2(x,y,z,n,w)
% polyfitweighted2.m
% -------------------
%
% Find a least-squares fit of 2D data z(x,y) with an nth order
% polynomial, weighted by w(x,y) .
%
% By S.S. Rogers (2006)
%
% Usage
% ------
%
% P = polyfitweighted2(X,Y,Z,N,W) finds the coefficients of a polynomial
% P(X,Y) of degree N that fits the data Z best in a least-squares
% sense. P is a row vector of length (N+1)*(N+2)/2 containing the
% polynomial coefficients in ascending powers, 0th order first.
%
%   P = [p00 p10 p01 p20 p11 p02 p30 p21 p12 p03...]
%
% e.g. For a 3rd order fit,
% the regression problem is formulated in matrix format as:
%
%   wZ = V*P    or
%
%                      2       2   3   2     2      3
%   wZ = [w  wx  wy  wx  xy  wy  wx  wx y  wx y   wy ]  [p00
%                                                        p10
%                                                        p01
%                                                        p20
%                                                        p11
%                                                        p02
%                                                        p30
%                                                        p21
%                                                        p12
%                                                        p03]
%
% *Note:* P is not in the format of standard Matlab 1D polynomials. Use
% polval2.m to evaluate the polynomial in this format, at given values of
% x,y.
%
% X,Y must be vectors
% Z,W must be 2D arrays of size [length(X) length(Y)]
%
% based on polyfit.m by The Mathworks Inc. - see doc polyfit for more details
%
% Class support for inputs X,Y,Z,W:
%      float: double, single

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

lx=length(x);
ly=length(y);

if ~isequal(size(z),size(w),[ly lx])
    error('polyfitweighted2:XYSizeMismatch',...
         [' X,Y *must* be vectors' ...
          '  Z,W *must* be 2D arrays of size [length(X) length(Y)]'])
end

y=y*ones(1,lx);
x=ones(ly,1)*x';
x = x(:);
y = y(:);
z = z(:);
w = w(:);

pts=length(z);

% Construct weighted Vandermonde matrix.
V=zeros(pts,(n+1)*(n+2)/2);
V(:,1) = w;
%V(:,1) = ones(pts,1);
ordercolumn=1;
for order = 1:n
    for ordercolumn=ordercolumn+(1:order)
        V(:,ordercolumn) = x.*V(:,ordercolumn-order);
    end
    ordercolumn=ordercolumn+1;
    V(:,ordercolumn) = y.*V(:,ordercolumn-order-1);
end

% Solve least squares problem.
[Q,R] = qr(V,0);
ws = warning('off','all');
p = R\(Q'*(w.*z));    % Same as p = V\(w.*z);
warning(ws);
if size(R,2) > size(R,1)
   warning('polyfitweighted2:PolyNotUnique', ...
       'Polynomial is not unique; degree >= number of data points.')
elseif condest(R) > 1.0e10
        warning('polyfitweighted2:RepeatedPointsOrRescale', ...
            ['Polynomial is badly conditioned. Remove repeated data points\n' ...
            '         or try centering and scaling as described in HELP POLYFIT.'])
end
%r = z - (V*p)./w;
p = p.';          % Polynomial coefficients are row vectors by convention.


function[upa,upb,upc,upd,upe,upf]=ellband(varargin)
%ELLBAND  Bandwidth of modulated elliptical signals in two or three dimensions.
%
%   [A,B,C]=ELLBAND(KAPPA,LAMBDA,THETA,PHI) computes the instantaneous
%   bandwidth of the elliptical signal characterized by RMS amplitude
%   KAPPA, linearity LAMBDA, orientation THETA, and orbital phase PHI.
%
%   The three output arguments are
%
%          A  - Amplitude modulation bandwidth
%          B  - Deformation bandwidth
%          C  - Precession bandwidth.
%
%   and these satisfy UPSILON^2=A^2+B^2+C^2 where UPSILON is the joint
%   instantaneous bandwidth of the bivariate signal.
%
%   The form of these terms is as follows:
%
%         A = 1/KAPPA d/dt KAPPA
%         B = 1/2 * 1/SQRT(1-LAMBDA^2) *  d/dt LAMBDA
%         C = LAMBDA d/dt THETA
%
%   [A,B,C,UPSILON]=ELLBAND(KAPPA,LAMBDA,THETA,PHI) also returns the total
%   instantaneous bandwith UPSILON=SQRT(A^2+B^2+C^2).
%
%   ELLBAND(...,DIM) performs the analysis with time running along
%   dimension DIM, as opposed to the default behavior of DIM=1.
%
%   For details see Lilly and Olhede (2010).
%
%   ELLBAND also works if the input arguments are cell arrays of numerical
%   arrays, in which case the output will be similarly sized cell arrays.
%   __________________________________________________________________
%
%   Three dimensions
%
%   ELLBAND can also compute the instantaneous bandwidth of modulated
%   elliptical signals in three dimensions.
%
%   [A,B,C,D,E]=ELLBAND(KAPPA,LAMBDA,THETA,PHI,ALPHA,BETA) returns the
%   terms in the bandwidth from a modulated ellipical signal in a plane
%   with a normal vector having azimuth angle ALPHA and zenith angle BETA.
%
%   The five output arguments are
%
%          A   - Amplitude modulation bandwidth, as in 2D
%          B   - Deformation bandwidth, as in 2D
%          C   - Precession bandwidth, as in 2D
%          D   - Precession bandwidth with full 3D effects
%          E   - Bandwidth due to motion of the normal to the plane
%
%   and these, in principle, satisfy UPSILON^2=A^2+B^2+C^2+D^2+|E|^2 where
%   UPSILON is the joint instantaneous bandwidth of the trivariate signal.
%   See below for a caveat on this statement.
%
%   Terms A--C are just as in the bivariate case.  The new terms are:
%
%         D = LAMBDA [d/dt THETA + COS(BETA) * d/dt ALPHA]
%         E = N^T X_+ / |X_+^H X_+|
%
%   where N is the trivariate normal vector, X_+ is the trivariate analytic
%   signal vector, and "T" denotes the matrix transpose, and "H" the
%   Hermitian transpose.  Note that term E may be complex-valued.
%
%   Note that term C does not contribute to the full bandwidth, but is
%   output in order to compare the two-dimensional and three-dimensional
%   effects in the full precession bandwidth, term D.
%
%   An important point is that the trivariate ellipse parameters can be
%   ill-defined for a nearly linear signal, and the elliptical bandwidth
%   terms can give erroneously large values at isolated points.  To check
%   for this, compare with the joint bandwidth from INSTMOM.
%
%   [A,B,C,D,E,UPSILON]=ELLBAND(KAPPA,LAMBDA,THETA,PHI,ALPHA,BETA) also
%   returns the total bandwith UPSILON=SQRT(A^2+B^2+C^2+D^2+|E|^2).
%
%   For details see Lilly (2011).
%   __________________________________________________________________
%
%   ELLBAND(DT,...) sets the sample interval DT, which defaults to DT=1.
%   DT may be a scalar, or if the input fields are cell arrays having
%   length N, DT may be a numerical array of length N.
%
%   See also ANATRANS, WAVETRANS, INSTMOM.
%
%   'ellband --t' runs a test.
%   'ellband --f' generates a figure from Lilly and Olhede (2010).
%
%   Usage:  [a,b,c]=ellband(kappa,lambda,theta,phi);
%           [a,b,c]=ellband(dt,kappa,lambda,theta,phi);
%           [a,b,c]=ellband(dt,kappa,lambda,theta,phi,dim);
%           [a,b,c,upsilon]=ellband(dt,kappa,lambda,theta,phi,dim);
%           [a,b,c,d,e]=ellband(kappa,lambda,theta,phi,alpha,beta);
%           [a,b,c,d,e]=ellband(dt,kappa,lambda,theta,phi,alpha,beta);
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2006--2020 J.M. Lilly --- type 'help jlab_license' for details

if strcmpi(varargin{1}, '--t')
    ellband_test,return
end

if strcmpi(varargin{1}, '--f')
    type makefigs_ellband
    makefigs_ellband;
    return
end

if ~iscell(varargin{end})&&length(varargin{end})==1
    dim=varargin{end};
    varargin=varargin(1:end-1);
else
    dim=1;
end

if length(varargin)==5||length(varargin)==7
    dt=varargin{1};
    varargin=varargin(2:end);
else
    dt=1;
end

kappa=varargin{1};
lambda=varargin{2};
theta=varargin{3};
phi=varargin{4};
if length(varargin)>4
    alpha=varargin{5};
    beta=varargin{6};
    dims=3;
else
    alpha=[];
    beta=[];
    dims=2;
end

%Redo some initializing in case of cell arrays
if iscell(kappa)
    if isempty(alpha)
        for i=1:length(kappa)
            alpha{i,1}=[];
            beta{i,1}=[];
        end
    end
    if length(dt)==1
        dto=dt;
        for i=1:length(kappa)
            dt(i)=dto;
        end
    end
end

if ~isempty(kappa)
    if iscell(kappa)
        [upa,upb,upc,upd,upe,upf]=vempty;
        for i=1:length(kappa)
            [upa{i,1},upb{i,1},upc{i,1},upd{i,1},upe{i,1},upf{i,1}]=...
                ellband_one(dt(i),kappa{i,1},lambda{i,1},theta{i,1},phi{i,1},alpha{i,1},beta{i,1},dim,dims);
        end
    else
        [upa,upb,upc,upd,upe,upf]=ellband_one(dt,kappa,lambda,theta,phi,alpha,beta,dim,dims);
    end
else
    upa=kappa;upb=kappa;upc=kappa;
    upd=kappa;upe=kappa;upf=kappa;
end

function[upa,upb,upc,upd,upe,upf]=ellband_one(dt,kappa,lambda,theta,phi,alpha,beta,dim,dims)

omtheta=frac(vdiff(unwrap(theta,[],dim),dim),dt);
zeta=sign(lambda).*sqrt(1-lambda.^2);

%These do not seem much different
%upa=frac((vdiff(kappa,dim)),kappa*dt);
upa=frac((vdiff(log(kappa),dim)),dt);
upb=frac(sqrt(frac(1,1-lambda.^2)).*(frac(1,2)*vdiff(lambda,dim)),dt);

upb2=frac(sqrt(frac(1,1-zeta.^2)).*(frac(1,2)*vdiff(zeta,dim)),dt);
upb(abs(lambda)>sqrt(1/2))=-upb2(abs(lambda)>sqrt(1/2));
%To correct numerical instability when lambda is close to unity

%This is the same
%[a,b]=kl2ab(kappa,lambda);
%upc=abs(frac(a.*b,a.^2+b.^2).*vdiff(log(abs(b./a)),1))./dt;
upc=(lambda.*omtheta);

%Replace isolated nans that may have been lost
bool=isnan(kappa);
upa(bool)=nan;
upb(bool)=nan;
upc(bool)=nan;

if dims==3
     omalpha=frac(vdiff(unwrap(alpha,[],dim),dim),dt);
     ombeta=frac(vdiff(unwrap(beta,[],dim),dim),dt);

     upd=lambda.*(omtheta+omalpha.*cos(beta));

     [x,y,z]=ellsig(kappa,lambda,theta,phi,alpha,beta);

     %Now find x-tilde, 2-d vector
     [x,y,z]=vectmult(jmat3(-alpha,3),x,y,z);
     [x,y,z]=vectmult(jmat3(-beta,1),x,y,z);

     numer=-omalpha.*sin(beta).*x+ombeta.*y;
     denom=sqrt(abs(x).^2+abs(y).^2);
     upe=frac(numer,denom);
     upf=sqrt(upa.^2+upb.^2+upc.^2+upd.^2+abs(upe).^2);

     %Replace isolated nans that may have been lost
     upd(bool)=nan;
     upe(bool)=nan;
     upf(bool)=nan;

else
    upd=sqrt(upa.^2+upb.^2+upc.^2);
    upe=[];
    upf=[];
end

function[]=ellband_test
ellband_test1;
ellband_test2;
ellband_test3;
ellband_test4;

function[]=ellband_test1

load npg2006
use npg2006

%Decide on frequencies
fs=2*pi./(logspace(log10(10),log10(100),50)');

%Compute wavelet transforms using generalized Morse wavelets
[wx,wy]=wavetrans(real(cx),imag(cx),{1,2,4,fs,'bandpass'},'mirror');
[wxr,wyr,ir,jr]=ridgewalk(dt,wx,wy,fs,sqrt(2*4),1);

[kappa,lambda,theta,phi]=ellparams(wxr,wyr);
[ba,bd,bp]=ellband(kappa,lambda,theta,phi);
[a,om,upbar]=instmom([wxr wyr],1,2);

wxr2=permute(wxr,[3 2 1]);
wyr2=permute(wyr,[3 2 1]);
clear wr2
wr2(1,1,:)=wxr2;wr2(1,2,:)=wyr2;
[kappa2,lambda2,theta2,phi2]=ellparams(wxr2,wyr2,3);
[ba2,bd2,bp2]=ellband(kappa2,lambda2,theta2,phi2,3);
[a2,om2,upbar2]=instmom(wr2,3,2);
vcolon(kappa2,lambda2,theta2,phi2,ba2,bd2,bp2,a2,om2,upbar2);
bool=aresame([kappa2 lambda2 theta2 phi2 ba2 bd2 bp2 a2 om2 upbar2],...
    [kappa lambda theta phi ba bd bp a om upbar]);

reporttest('ELLBAND time running in pages', bool);

vindex(upbar,ba,bd,bp,2:length(upbar)-1,1);
err=vsum((upbar-sqrt(ba.^2+bd.^2+bp.^2)).^2,1)./vsum(upbar.^2,1);
reporttest('ELLBAND terms sum to bandwidth from INSTMOM', err<1e-5);

function[]=ellband_test2
t=(0:1:925)';
cxe=zeros(length(t),3);

kappa=3*exp(2*0.393*(t/1000-1));
lambda=0.4+0*t;
phi=(t/1000*5)*2*pi;
theta=pi/4+0*t;

om=vdiff(phi,1);  %Since theta is constant

[x,y]=ellsig(kappa,lambda,theta,phi);
[a,om,up]=instmom([x y],1,2);

ups=sqrt(vsum(abs(up).^2,2));
[upsa,upsb,upsc]=ellband(kappa,lambda,theta,phi);
b1=aresame(vmean([ups upsa upsb upsc]./[om om om om],1),[1 1 0 0]*0.025,1e-4);

om=vdiff(phi,1);  %Since theta is constant

kappa=2.5+0*t;
lambda=zeros(size(t));
for i=2:length(t)
    lambda(i)=real(lambda(i-1)+2*sqrt(1-lambda(i-1).^2)*0.025.*om(i));
end
lambda(1)=nan;
lambda(lambda>1)=1;

[x,y]=ellsig(kappa,lambda,theta,phi);
[a,om,ups]=instmom([x y],1,2);

[upsa,upsb,upsc]=ellband(kappa,lambda,theta,phi);
b2=aresame(vmean([ups upsa upsb upsc]./[om om om om],1),[1 0 1 0]*0.025,1e-4);

[kappa,lambda]=ab2kl(3+zeros(size(t)),2+zeros(size(t)));

theta=phi/14.45;

[x,y]=ellsig(kappa,lambda,theta,phi);
[a,om,ups]=instmom([x y],1,2);

[upsa,upsb,upsc]=ellband(kappa,lambda,theta,phi);
b3=aresame(vmean([ups upsa upsb upsc]./[om om om om],1),[1 0 0 1]*0.025,1e-4);

reporttest('ELLBAND three panels of figure each have bandwidth 0.025',b1&&b2&&b3)

function[]=ellband_test3

kappao=10;
lambdao=2/3;
omtheta=linspace(-2,2,100);
omphi=1;

dt=0.1;
t=[0:dt:100]';

zo=ellsig(kappao,lambdao,0,omphi.*t);
psi=sleptap(length(zo));

z=vzeros(length(zo),length(omtheta));
for i=1:length(omtheta)
    z(:,i)=rot(omtheta(i).*t).*zo;
end

%Taper for better computation
z=z.*vrep(psi(:,1),size(z,2),2);
[zp,zn]=anatrans(z,conj(z));

[ap,omp,upp]=instmom(dt,zp);
[an,omn,upn]=instmom(dt,zn);

kappa=sqrt(abs(zp).^2+abs(zn).^2);
om=frac(omp.*abs(zp).^2+omn.*abs(zn).^2,kappa.^2);
ombar=vmean(om,1,squared(kappa));

sig=sqrt(frac((upp.^2+(omp-vrep(ombar,size(om,1),1)).^2).*abs(zp).^2+...
    (upn.^2+(omn-vrep(ombar,size(om,1),1)).^2).*abs(zn).^2,kappa.^2));
sigbar=sqrt(vmean(sig.^2,1,squared(kappa)));

[x,y]=vectmult(sqrt(2)*tmat',zp,zn);
[kappa,lambda,theta,phi]=ellparams(x,y);
[ba,bd,bp]=ellband(dt,kappa,lambda,theta,phi);
bom=om-vrep(ombar,size(om,1),1);

sig2=sqrt(ba.^2+bd.^2+bp.^2+bom.^2);
sigbar2=sqrt(vmean(sig2.^2,1,squared(kappa)));

%plot(sig,'b'),hold on,plot(sig2,'r')  %Visually identical
err=sum(squared(sigbar-sigbar2))./sum(squared(sigbar));
reporttest('ELLBAND rotary and elliptical bandwidths match for shifted ellipse, non-unit sample rate',err<1e-5)

function[]=ellband_test4

load solomon
use solomon

%Choose central portion where polarization is elliptical
vindex(x,y,z,420:580,1);
vfilt(x,y,z,10,'zeros');
[x,y,z]=anatrans(x,y,z,'mirror');

[kappa,lambda,theta,phi,alpha,beta]=ellparams(x,y,z);
[a,ombar,upbar]=instmom([x,y,z],1,2);
[a,b,c,d,e]=ellband(kappa,lambda,theta,phi,alpha,beta);
upbar1=sqrt(a.^2+b.^2+d.^2+abs(e).^2);

err=abs(upbar-upbar1).^2./abs(upbar).^2;
err=sort(err,'descend');
err=err(20:end);
%figure, plot([upbar upbar1])
reporttest('ELLBAND trivariate case, sum of bandwith terms for Solomon Islands (removing worst outliers)',allall(err<0.05))


function [ seed, xd, td ] = triangulation_order3_sample ( node_num, node_xy, ...
  triangle_num, triangle_node, num_ran, seed )

%*****************************************************************************80
%
%% TRIANGULATION_ORDER3_SAMPLE returns random points in a triangulation.
%
%  Discussion:
%
%    It is assumed that the triangulation consists of a set of non-overlapping
%    triangles.
%
%    The point is chosen uniformly in the area covered by the triangulation.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 December 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, real NODE_XY(2,NODE_NUM), the node coordinates.
%
%    Input, integer TRIANGLE_NUM, the number of triangles.
%
%    Input, integer TRIANGLE_NODE(3,TRIANGLE_NUM), the nodes that make up the triangles.
%
%    Input, integer NUM_RAN, the number of points to sample.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, integer SEED, a seed for the random number generator.
%
%    Output, real XD(2,NUM_RAN), the sample points.
%
%    Output, integer TD(NUM_RAN), the triangle to which each sample point
%    belongs.
%
  dim_num = 2;
%
%  Compute the areas of the triangles.
%  Build a cumulative area vector.
%  Convert it to a relative cumulative area vector.
%
  area_cum(0+1) = 0.0;

  for i = 1 : triangle_num

    i1 = triangle_node(1,i);
    i2 = triangle_node(2,i);
    i3 = triangle_node(3,i);

    t(1:dim_num,1) = node_xy(1:dim_num,i1);
    t(1:dim_num,2) = node_xy(1:dim_num,i2);
    t(1:dim_num,3) = node_xy(1:dim_num,i3);

    area = triangle_area_2d ( t );

    area_cum(i+1) = area_cum(i-1+1) + area;

  end

  area_total = area_cum(triangle_num+1);

  area_cum(0+1:triangle_num+1) = area_cum(0+1:triangle_num+1) / area_total;
%
%  Pick random values.  A random value R indicates the corresponding triangle
%  whose cumulative relative area contains R.
%
%  Bracket the random value in the cumulative relative areas,
%  indicating a triangle.
%
%  Pick a random point in the triangle.
%
  for i = 1 : num_ran

    [ r, seed ] = r8_uniform_01 ( seed );

    [ left, right ] = r8vec_bracket ( triangle_num+1, area_cum, r );

    td(i) = right - 1;

    i1 = triangle_node(1,td(i));
    i2 = triangle_node(2,td(i));
    i3 = triangle_node(3,td(i));

    t(1:dim_num,1) = node_xy(1:dim_num,i1);
    t(1:dim_num,2) = node_xy(1:dim_num,i2);
    t(1:dim_num,3) = node_xy(1:dim_num,i3);

    [ xd(1:dim_num,i), seed ] = triangle_sample ( t, 1, seed );

  end

  return
end


function val=intPowSinPow(u,n,m)
%%INTPOWSINPOW Evaluate the integral of u^n*sin(u)^m du.  A definite
%           integral can be evaluated, or an indefinite integral (with a
%           particular additive constant).
%
%INPUTS: u A 2XN (for definite integral) or a 1XN (for indefinite
%          integrals) set of N points. For definite integrals, u(1,:) are
%          the real lower bounds and u(2,:) are the real upper bounds. For
%          indefinite integrals, the integral is evaluated at the points in
%          u. The values in u should be real.
%        n The positive integer exponent of u.
%        m The positive integer exponent of the sine term.
%
%OUTPUTS: val The 1XN set of values of the integral of u^n*sin(u)^m.
%
%This function implements formulas 4 and 5 of Section 2.631 of [1].
%
%REFERENCES:
%[1] I. S. Gradshteyn and I. M. Ryzhik, Tables of Integrals, Series, and
%    Products, Corrected and Enlarged Edition. New York: Academic Press,
%    1980, translated from Russian.
%
%October 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

numDim=size(u,1);

if(isempty(u))
   val=[];
   return;
end

if(numDim==1)%An indefinite integral
    val=indefIntPowSinPow(u,n,m);
else%A definite integral
    val=indefIntPowSinPow(u(2,:),n,m)-indefIntPowSinPow(u(1,:),n,m);
end

end

function val=indefIntPowSinPow(u,n,m)

if(mod(m,2)==0)%Use formula 4.
    m=m/2;

    val=0;
    for k=0:(m-1)
        a=2*(m-k);
        x=a*u;
        val=val+(-1)^k*binomial(2*m,k)*(1/a)^(n+1)*intPowCos(x,n);
    end

    val=(-1)^m/(2^(2*m-1))*val;
    val=val+binomial(2*m,m)*u.^(n+1)/(2^(2*m)*(n+1));
else%Use formula 5.
    m=(m-1)/2;

    val=0;
    for k=0:m
        a=2*(m-k)+1;
        x=a*u;
        val=val+(-1)^k*binomial(2*m+1,k)*(1/a)^(n+1)*intPowSin(x,n);
    end

    val=val*(-1)^m/(2^(2*m));
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.


% PCA with automatic selection of the method: covariance or gram matrix
% Usage: [X, eigvec, eigval, Xm] = pca (X, dout, center, verbose)
%   X       input vector set (1 vector per column)
%   dout    number of principal components to be computed
%   center  need to center data?
%
% Note: the eigenvalues are given in decreasing order of magnitude
%
% Author: Herve Jegou, 2011.
% Last revision: 08/10/2013
function [X, eigvec, eigval, Xm] = yael_pca (X, dout, center, verbose)

if nargin < 3,         center = true; end
if ~exist ('verbose', 'var'), verbose = false; end

X = double (X);
d = size (X, 1);
n = size (X, 2);

if nargin < 2
  dout = d;
end

if center
  Xm = mean (X, 2);
  X = bsxfun (@minus, X, Xm);
else
  Xm = zeros (d, 1);
end

opts.issym = true;
opts.isreal = true;
opts.tol = eps;
opts.disp = 0;

% PCA with covariance matrix
if n > d
  if verbose, fprintf ('PCA with covariance matrix: %d -> %d\n', d, dout); end
  Xcov = X * X';
  Xcov = (Xcov + Xcov') / (2 * n);

  if dout < d
    [eigvec, eigval] = eigs (Xcov, dout, 'LM', opts);
  else
    [eigvec, eigval] = eig (Xcov);
  end
else
  % PCA with gram matrix
  if verbose, fprintf ('PCA with gram matrix: %d -> %d\n', d, dout); end
  Xgram = X' * X;
  Xgram = (Xgram + Xgram') / 2;
  if dout < d
    [eigvec, eigval] = eigs (Xgram, dout, 'LM', opts);
  else
    [eigvec, eigval] = eig (Xgram);
  end
  eigvec = single (X * eigvec);
  eigvec = utls.yael_vecs_normalize (eigvec);
end

X = eigvec' * X;
X = single (X);
eigval = diag(eigval);

% We prefer a consistent order
[~, eigord] = sort (eigval, 'descend');
eigval = eigval (eigord);
eigvec = eigvec (:, eigord);
X = X(eigord, :);


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni
%
%
% problem 6 - Computation of linear convolution

% a)
x1=[1 2 3 4];
x2=[ 5 4 3 2 1];
y=conv(x1,x2)

% b)
N1=length(x1);
N2=length(x2);
M=N1+N2-1;
x1(M)=0;
x2(M)=0;
y=circonv(x1,x2)

% c)
X1=fft(x1,M);
X2=fft(x2,M);
y=ifft(X1.*X2)


function R = rand_rotation(n)
  % Uniformly sample SO(n)
  %
  % R = rand_rotation(n)
  %
  % Inputs:
  %   n  dimension
  % Output:
  %   R  n by n rotation matrix
  %

  % "How to generate a random unitary matrix" [Maris Ozols 2006]
  % http://home.lu.lv/~sd20008/papers/essays/Random%20unitary%20[paper].pdf
  [Q,~] = qr(randn(n));
  s = (2*(rand>0.5)-1);
  Q(:,1) = Q(:,1)*s;
  %b = det(Q);
  b = s*(2*mod(n,2)-1);
  Q(:,2) = b*Q(:,2);
  assert(abs(det(Q)-1)<1e-10)
  % rename as R
  R = Q;
end


function angles = tetrahedron_face_angles_3d ( tetra )

%*****************************************************************************80
%
%% TETRAHEDRON_FACE_ANGLES_3D returns the 12 face angles of a tetrahedron 3D.
%
%  Discussion:
%
%    The tetrahedron has 4 triangular faces.  This routine computes the
%    3 planar angles associated with each face.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 July 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real TETRA(3,4) the tetrahedron vertices.
%
%    Output, real ANGLES(3,4), the face angles.
%

%
%  Face 123
%
  tri(1:3,1:3) = tetra(1:3,1:3);
  angles(1:3,1) = triangle_angles_3d ( tri );
%
%  Face 124
%
  tri(1:3,1:2) = tetra(1:3,1:2);
  tri(1:3,3) = tetra(1:3,4);
  angles(1:3,2) = triangle_angles_3d ( tri );
%
%  Face 134
%
  tri(1:3,1) = tetra(1:3,1);
  tri(1:3,2:3) = tetra(1:3,3:4);
  angles(1:3,3) = triangle_angles_3d ( tri );
%
%  Face 234
%
  tri(1:3,1:3) = tetra(1:3,2:4);
  angles(1:3,4) = triangle_angles_3d ( tri );

  return
end


% Author: Jai Juneja (adapted from video lecture series by Joan Sola)
% Date: 12/02/2013
%
% Given a range-bearing measurement y, obtain position p of observed
% landmark in the robot's frame.
%
% Inputs:
%   y = [d a]       :   Laser range and bearing measurement
%
% Outputs:
%   p_r = [p_x p_y]':   Cartesian position of observation in robot's frame
%   Optional:
%   P_y             :   Jacobian of p wrt. y

function [p_r, P_y] = getInvMeasurement(y)
    d = y(1, :);           % Range measurement
    a = y(2, :);           % Bearing measurement

    p_x = d .* cos(a);   % x-position of observation in robot's frame
    p_y = d .* sin(a);   % y-position of observation in robot's frame

    p_r = [p_x; p_y];

    if nargout > 1      % Compute Jacobian (only works for single measurement)
        P_y = [...
            cos(a)  -d*sin(a)
            sin(a)  d*cos(a)];
    end
end


clear all;
clc;
clf;
% coordinates and connectivities
 x=[0 2 2 0];
 y=[0 0.5 1 1];

a = 3;
%mesh number in one side
n=2^(a-1);%(nxn)
% numberElements: number of elements
numberElements=n^2;
% numberNodes: number of nodes
numberNodes=(n+1)^2;
x1=linspace(x(1),x(2),n+1);
x2=linspace(x(4),x(3),n+1);
y1=linspace(y(1),y(4),n+1);
y2=linspace(y(2),y(3),n+1);
tempY=zeros(n+1,n+1);
tempX=zeros(n+1,n+1);
for i=1:n+1
    tempY(i,:)=linspace(y1(i),y2(i),n+1);
    tempX(:,i)=linspace(x1(i),x2(i),n+1);
end
ct=1;
nodeCoordinates=zeros(numberNodes,2);
for i=1:n+1
    for j=1:n+1
        nodeCoordinates(ct,:)=[tempX(i,j) tempY(i,j)];
        ct=ct+1;
    end
end

elementNodes=zeros(numberElements,4);
for i=1:n
    for j=1:n
        elementNodes(j+n*(i-1),:)=...
            [j+(n+1)*(i-1)...
            j+1+(n+1)*(i-1)...
            j+n+2+(n+1)*(i-1)...
            j+n+1+(n+1)*(i-1)];
    end
end
drawingMesh(nodeCoordinates,elementNodes,'Q4','k-');


%% Example 2.5: Viscous Conservation Law
% In this example, we consider the viscous conservation law
%
% $$ u_t + f(u)_x = \mu u_{xx}, \qquad u(x,0)=u_0(x).$$
%
% A common method to derive efficient methods for this equation is to split
% the evolution into two different operators
%
% $$ S(t): \quad v_t + f(v)_x = 0, \qquad v(x,0)=v_0(x)$$
%
% $$ H(t): \quad w_t = \mu w_{xx}, \qquad w(y,0)=w_0(y)$$
%
% The approximate solution is constructed from the formula
%
% $$u(x,t)\approx [H(\Delta t)\circ S(\Delta t) ]^n u_0(x)$$
%
% This way, one can utilize highly efficient solvers for each of the two
% subequations. Here, we use a spectral difference method for the heat
% equation. Moreover, because the purpose here is simply to demonstrate the
% operator splitting, we use the simple first-order Lax-Friedrichs scheme
% for the hyperbolic conservation law ($r=\Delta t/\Delta x$)
%
% $$u_i^{n+1} = \frac12\bigl(u_{i-1}^n + u_{i+1}^n\bigr)
%     - \frac12 r \bigl[ f(u_{i+1}^n) - f(u_{i-1}^n)\bigr]$$

%% Initial setup
N  = 512; h = 2*pi/N;
T  = 6;
x  = -pi+(0:N)*h; x = 0.5*(x(1:end-1)+x(2:end));
u0 = exp(-4*sin((x+2)/2).^2);

%% Burgers' equation
% A classical example of a viscous conservation law is the so-called
% Burgers' equation
%
% $$ u_t + ( 0.5 u^2)_x = \mu u_{xx}
%
% which can be considered a simplified model of the momentum equation from
% the Navier-Stokes equations. Here, we will consider the evolution of a
% smooth profile for \mu=0.01 using periodic boundary conditions.
nsplit = 50;
u = consheat('flux', u0, x, T, nsplit, 0.01);
subplot(2,1,1), plot(x,u(:,1:10:nsplit+1))
subplot(2,1,2), surf(x,linspace(0,T,nsplit+1),u')
shading interp; view(-10,50), axis tight

%%
% As we see from the figure, the nonlinear convective forces sharpen the
% smooth profile into a shock layer whose width is determined by the
% balance of the diffusion term and the self-sharpening mechanisms in the
% nonlinear flux function.

%% Compare different time steps
% Next, we increase \mu to 0.1 and consider the effect of different number
% of splitting steps.
for n=1:4,
	nsplit = 4*3.^(n-1);
	u = consheat('flux', u0, x, T, nsplit);
   subplot(2,2,n); contourf(x, linspace(0,T,nsplit+1), u');
   xlabel('x'), ylabel('t'), colorbar, title([num2str(nsplit) ' steps']);
end;
%%
% The approximations computed with four and twelve splitting steps clearly
% capture the dominant evolutionary behavior of the solution, but also
% contain nonphysical wiggles caused by splitting errors. As we increase
% the number of splitting steps, the splitting errors decrease and the
% approximate solutions are able to capture the whole evolution without
% significant artifacts created by our splitting strategy.

%% Comparing different shock widths
% As a last example, we will look at the evolution of the smooth profile
% for different balances between the convective and diffusive forces.
mu = 4; nsplit = 50;
t  = linspace(0,T,nsplit+1);
for n=1:4,
   mu = 0.25*mu;
	u  = consheat('flux',u0,x,T,nsplit,mu);
	subplot(2,2,n); contourf(x,t,u');
   xlabel('x'), ylabel('t'), colorbar, title(['mu = ', num2str(mu)]);
end;
%%
% For large values of \mu, the diffusive forces dominate and the solution
% profile evolves toward a flat surface. As \mu decreases, the convective
% forces take over and the solution develops into a viscous shock that
% travels to the right.


function idx = findClosestCentroids(X, centroids)
%FINDCLOSESTCENTROIDS computes the centroid memberships for every example
%   idx = FINDCLOSESTCENTROIDS (X, centroids) returns the closest centroids
%   in idx for a dataset X where each row is a single example. idx = m x 1
%   vector of centroid assignments (i.e. each entry in range [1..K])
%

% Set K
K = size(centroids, 1);

% You need to return the following variables correctly.
idx = zeros(size(X,1), 1);

% ====================== YOUR CODE HERE ======================
% Instructions: Go over every example, find its closest centroid, and store
%               the index inside idx at the appropriate location.
%               Concretely, idx(i) should contain the index of the centroid
%               closest to example i. Hence, it should be a value in the
%               range 1..K
%
% Note: You can use a for-loop over the examples to compute this.
%

m = size(X,1)

for i = 1:m
    distance_array = zeros(1,K);
    for j = 1:K
        distance_array(1,j) = sqrt(sum(power((X(i,:)-centroids(j,:)),2)));
    end
    [d, d_idx] = min(distance_array);
    idx(i,1) = d_idx;
end

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

end


function [R,W] = rodrigues(N,phi)
  % RODRIGUES Construct the rotation matrix for rotating vectors around each
  % given vector.
  %
  % Inputs:
  %   N  #N by 3 list of 3D vectors
  %   phi  angle to rotate by.
  % Outputs:
  %   R  3#N by 3#N "rotation" matrix
  %   W  3#N by 3#N cross product matrix
  %
  % Example:
  %   % Given a scalar function Z on a mesh (V,F)
  %   G = grad(V,F);
  %   N = normalizerow(normals(V,F));
  %   X = reshape(G*Z,[],3);
  %   Y = reshape(rodrigues(N,pi/2)*G*Z,[],3);
  %   BC = barycenter(V,F);
  %   tsurf(F,V,'CData',Z,fphong,'EdgeColor','none');
  %   hold on;
  %   quiver3(BC(:,1),BC(:,2),BC(:,3),X(:,1),X(:,2),X(:,3))
  %   quiver3(BC(:,1),BC(:,2),BC(:,3),Y(:,1),Y(:,2),Y(:,3))
  %   hold off;
  %

  m = size(N,1);
  % Either uniform angle or per-vector
  if numel(phi)>1
    assert(numel(phi) == m);
    phi = diag(sparse(phi));
  end
  % https://math.stackexchange.com/a/142831
  %
  W = sparse( ...
    [1 2 0 2 0 1]*m+(1:m)', ...
    [0 0 1 1 2 2]*m+(1:m)', ...
    [N(:,3) -N(:,2) -N(:,3) N(:,1) N(:,2) -N(:,1)], ...
    3*m,3*m);
  if isempty(phi)
    R = [];
  else
    R = speye(3*m,3*m) + sin(phi)*W + (2*sin(phi/2)^2)*W*W;
  end
end


function result = ball_unit_07_3d ( func )

%*****************************************************************************80
%
%% BALL_UNIT_07_3D approximates an integral inside the unit ball in 3D.
%
%  Integration region:
%
%    Points (X,Y,Z) such that:
%
%      X**2 + Y**2 + Z**2 <= 1.
%
%  Discussion:
%
%    A 64 point 7-th degree formula is used.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 May 2004
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Arthur H Stroud,
%    Approximate Calculation of Multiple Integrals,
%    Prentice Hall, 1971.
%
%  Parameters:
%
%    Input, external FUNC, the name of the user supplied
%    function which evaluates F(X,Y,Z), of the form
%      function value = func ( x, y, z )
%
%    Output, real RESULT, the approximate integral of the function.
%
  norder = 4;
%
%  This is the 5 point Gauss-Legendre rule,
%  but with the midpoint deleted, and with different weights.
%
  xtab1(1:4) = [...
    -0.906179845938663992797626878299E+00, ...
    -0.538469310105683091036314420700E+00, ...
     0.538469310105683091036314420700E+00, ...
     0.906179845938663992797626878299E+00 ];

  weight1(1:4) = [ ...
    0.19455533421780251826E+00, ...
    0.13877799911553081506E+00, ...
    0.13877799911553081506E+00, ...
    0.19455533421780251826E+00 ];
%
%  Set XTAB2 and WEIGHT2.
%
  for j = 1 : norder
    angle = pi * ( 2 * j - 1 ) / ( 2 * norder );
    xtab2(j) = cos ( angle );
  end

  weight2(1:norder) = 1.0E+00;
%
%  Set XTAB3 and WEIGHT3 for the interval [-1,1].
%
  [ xtab3, weight3 ] = legendre_set ( norder );

  w = 3.0E+00 / 16.0E+00;

  quad = 0.0E+00;

  for i = 1 : norder
    for j = 1 : norder
      for k = 1 : norder

        x = xtab1(i) * sqrt ( 1.0E+00 - xtab2(j)^2 ) ...
                     * sqrt ( 1.0E+00 - xtab3(k)^2 );
        y = xtab1(i) * xtab2(j) * sqrt ( 1.0E+00 - xtab3(k)^2 );
        z = xtab1(i) * xtab3(k);

        quad = quad + w * weight1(i) * weight2(j) * weight3(k) ...
          * feval ( func, x, y, z );

      end
    end
  end

  volume = ball_unit_volume_3d ( );
  result = quad * volume;

  return
end


function [theta, J_history] = gradientDescent(X, y, theta, alpha, num_iters)
    %% GRADIENTDESCENT Performs gradient descent to learn theta
    %   theta = GRADIENTDESENT(X, y, theta, alpha, num_iters) updates theta by
    %   taking num_iters gradient steps with learning rate alpha

    % Initialize some useful values
    n = length(y); % number of training examples
    J_history = zeros(num_iters, 1);

    for iter = 1 : num_iters

        % ====================== YOUR CODE HERE ======================
        % Instructions: Perform a single gradient step on the parameter vector
        %               theta.
        %
        % Hint: While debugging, it can be useful to print out the values
        %       of the cost function (computeCost) and gradient here.
        %

        % compute the gradient
        grad = zeros(size(theta));
        error = X * theta - y;
        for j = 1 : length(grad)
            grad(j) = alpha * sum(error .* X(:, j)) / n;
        end

        % simultaneously update all theta entries
        theta = theta - grad;

        % Save the cost J in every iteration
        J_history(iter) = computeCost(X, y, theta);
    end
end


function value = r4_acosh ( x )

%*****************************************************************************80
%
%% R4_ACOSH evaluates the arc-hyperbolic 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-hyperbolic cosine of X.
%
  persistent dln2;
  persistent xmax;

  if ( isempty ( xmax ) )
    dln2 = 0.69314718055994530941723212145818;
    xmax = 1.0 / sqrt ( r4_tiny ( ) );
  end

  if ( x < 1.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_ACOSH - Fatal error!\n' );
    fprintf ( 1, '  X < 1.0\n' );
    error ( 'R4_ACOSH - Fatal error!' )
  elseif ( x < xmax )
    value = log ( x + sqrt ( x * x - 1.0 ) );
  else
    value = dln2 + log ( x );
  end

  return
end


function value = stirling2_value ( n, m )

%*****************************************************************************80
%
%% STIRLING2_VALUE computes a Stirling number of the second kind.
%
%  Discussion:
%
%    S2(N,M) represents the number of distinct partitions of N elements
%    into M nonempty sets.  For a fixed N, the sum of the Stirling
%    numbers S2(N,M) is represented by B(N), called "Bell's number",
%    and represents the number of distinct partitions of N elements.
%
%    For example, with 4 objects, there are:
%
%    1 partition into 1 set:
%
%      (A,B,C,D)
%
%    7 partitions into 2 sets:
%
%      (A,B,C) (D)
%      (A,B,D) (C)
%      (A,C,D) (B)
%      (A) (B,C,D)
%      (A,B) (C,D)
%      (A,C) (B,D)
%      (A,D) (B,C)
%
%    6 partitions into 3 sets:
%
%      (A,B) (C) (D)
%      (A) (B,C) (D)
%      (A) (B) (C,D)
%      (A,C) (B) (D)
%      (A,D) (B) (C)
%      (A) (B,D) (C)
%
%    1 partition into 4 sets:
%
%      (A) (B) (C) (D)
%
%    So S2(4,1) = 1, S2(4,2) = 7, S2(4,3) = 6, S2(4,4) = 1, and B(4) = 15.
%
%
%  First terms:
%
%    N/M: 1    2    3    4    5    6    7    8
%
%    1    1    0    0    0    0    0    0    0
%    2    1    1    0    0    0    0    0    0
%    3    1    3    1    0    0    0    0    0
%    4    1    7    6    1    0    0    0    0
%    5    1   15   25   10    1    0    0    0
%    6    1   31   90   65   15    1    0    0
%    7    1   63  301  350  140   21    1    0
%    8    1  127  966 1701 1050  266   28    1
%
%  Recursion:
%
%    S2(N,1) = 1 for all N.
%    S2(I,I) = 1 for all I.
%    S2(I,J) = 0 if I < J.
%
%    S2(N,M) = M * S2(N-1,M) + S2(N-1,M-1)
%
%  Properties:
%
%    sum ( 1 <= K <= M ) S2(I,K) * S1(K,J) = Delta(I,J)
%
%    X**N = sum ( 0 <= K <= N ) S2(N,K) X_K
%    where X_K is the falling factorial function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 August 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of rows of the table.
%
%    Input, integer M, the number of columns of the table.
%
%    Output, integer VALUE, the value of S2(N,M).
%
  if ( n <= 0 )
    value = 0;
    return
  end

  if ( m <= 0 )
    value = 0;
    return
  end

  s2(1,1) = 1;
  s2(1,2:m) = 0;

  for i = 2 : n

    s2(i,1) = 1;

    for j = 2 : m
      s2(i,j) = j * s2(i-1,j) + s2(i-1,j-1);
    end

  end

  value = s2(n,m);

  return
end


function trans = createScaling3d(varargin)
%CREATESCALING3D Create the 4x4 matrix of a 3D scaling
%
%   TRANS = createScaling3d(S);
%   returns the scaling transform corresponding to a scaling factor S in
%   each direction. S can be a scalar, or a 1x3 vector containing the
%   scaling factor in each direction.
%
%   TRANS = createScaling3d(SX, SY, SZ);
%   returns the scaling transform corresponding to a different scaling
%   factor in each direction.
%
%   The returned matrix has the form :
%   [SX  0  0  0]
%   [ 0 SY  0  0]
%   [ 0  0 SZ  0]
%   [ 0  0  0  0]
%
%   See also:
%   transforms3d, transformPoint3d, transformVector3d, createTranslation3d,
%   createRotationOx, createRotationOy, createRotationOz
%
%   ---------
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 20/04/2006.
%

%   HISTORY
%   25/11/2008 rename from scale3d to scaling3d
%   30/04/2009 rename to createScaling3d

% process input parameters
if isempty(varargin)
    % assert uniform scaling in each direction
    sx = 1;
    sy = 1;
    sz = 1;
elseif length(varargin)==1
    % only one argument
    var = varargin{1};
    if length(var)==1
        % same scaling factor in each direction
        sx = var;
        sy = var;
        sz = var;
    elseif length(var)==3
        % scaling is a vector, giving different scaling in each direction
        sx = var(1);
        sy = var(2);
        sz = var(3);
    else
        error('wrong size for first parameter of "createScaling3d"');
    end
elseif length(varargin)==3
    % 3 arguments, giving scaling in each direction
    sx = varargin{1};
    sy = varargin{2};
    sz = varargin{3};
else
    error('wrong number of arguments for "createScaling3d"');
end

% create the scaling matrix
trans = [sx 0 0 0;0 sy 0 0;0 0 sz 0;0 0 0 1];


clc;
close all;
clearvars;
rand('seed', 0);
randn('seed', 0);

m = 1000; % number of examples
n = 100;  % number of features
% A random matrix
A = randn(m,n);
% Original signal
x0 = 10*randn(n,1);
% Measurements
b = A*x0;
% Introduce sparse noise in the signal
% Select a subset of indices [2% of m indices]
idx = randsample(m,ceil(m/50));
% Introduce noise in these indices
b(idx) = b(idx) + 1e2*randn(size(idx));
% Perform Least absolute deviation optimization
tstart = tic;
options.verbose  = 1;
x = spx.opt.admm.lad(A, b, options);
toc(tstart)
% measure the maximum difference
recovery_error = x - x0;
max_diff = max(abs(x - x0));
fprintf('Maximum difference: %e\n', max_diff);
fprintf('Recovery error norm: %e\n', norm(recovery_error));


function dy_dx = parabolderiv( x, y, k, graphics_flag )

% function dy_dx = parabolderiv( x, y, k, graphics_flag )
%
% default: example from Ref. [1], p. 322
%
% or try: parabolderiv( ( -2 : 0.01 : 2 )', exp( -2 : 0.01 : 2 )', 2 )
%         parabolderiv( ( -2 : 0.01 : 2 )', ( exp( -2 : 0.01 : 2 ) + 0.01 * rand( 1, 401 ) )', 25 )
%         parabolderiv( ( ( 0 : 200 * pi ) / 100 )', ( sin( 0 : 0.01 : 2 * pi ) + 0.01 * rand( 1, 629 ) )', 40 )
%           for demonstration of noisy input and edge deviations
%
% graphics_flag = 1: show result in graph (default)
%                    (includes comparison to 2 other methods)
%                 0: show no graph
%
% This program differentiates an empirical function (an array of evenly
% spaced values), for instance periodically sampled data from an
% experiment.
%
% Method used: basically, a parabola is fit from k points to the left to k
% points to the right of the point where the derivative is required, this
% is then analytically differentiated. These calculations are not actually
% performed, but the method as given by Lanczos in Ref. [1] is used: only
% 1 fit parameter of the parabola is needed, which is calculated directly
% from the data; at the edges a parabola is fit through the first (last)
% 2k points, from which the derivative is calculated directly. Additional
% information (on accuracy, &c.) in Ref. [2]. Noise is handled well. An
% example from Ref. [1] is included.
%
% The datapoints need to be equidistant. If the graphics_flag is set, the
% result of this method is compared to applying the standard Matlab diff()
% function on the raw data as well as on adjacent averaged filtered data
% (zero phase delay). The example commands given above illustrate this.
%
% References:
%
% [1] Title                    : Applied analysis / by Cornelius Lanczos
%     Author                   : Cornelius Lanczos
%     Edition                  : 3rd print.
%     Publisher                : London : Pitman, 1964
%     Pages                    : 539 p.
%     Bibliographic annotation : 1st print: 1957
%     see pp. 321 - 324
%
% [2] Title                    : Digital filters / by Richard W. Hamming
%     Author                   : Richard W. Hamming
%     Edition                  : 3rd ed.
%     Publisher                : Englewoood Cliffs : Prentice-Hall, 1989
%     Pages                    : XIV, 284 p.
%     Bibliographic annotation : 1st print: 1977
%     ISBN                     : 0-13-212812-8
%     see p. 137
%
% Last update 24-03-2004 by Robert Klein-Douwel
%
% mail: robertkdkd@yahoo.co.uk, R.J.H.Klein-Douwel@tue.nl
% web:  http://www.sci.kun.nl/mlf/robertkd/

linewidth = 1; % can be changed for plotting purposes

% fill in some default values

if nargin < 4
    graphics_flag = 1;
end

if nargin < 3 % take example from Ref. [1], p. 322
    y = [ 0
          4
         25
         50
         67.4
        124.9
        172.0
        201.4
        288.1
        321.3
        387.1 ];

    x = ( 0 : 1 : length( y ) - 1 )';

    k = 2;
end

% initialise stuff

if length( x ) ~= length( y )
    error( 'x and y vectors have different lengths (RKD)' );
end

n = length( y ); %number of points

if 2 * k + 1 > n
    error( 'k too large or too few datapoints (RKD)' );
end

dy = zeros( n, 1 );

% check equidistancy of x
dx = diff( x );
d2x = diff( dx );

rel_error_d2x = max( abs( d2x ) ) / min( dx );
if rel_error_d2x > 401 * eps
    % vectors like ( -2 : 0.01 : 2 ) are not equidistant, but have a small variation
    % in dx and d2x; if this is not more than 401 * eps, then it is neglected
    disp( [ 'relative error in d2x = ' num2str( rel_error_d2x ) ] );
    disp( [ 'relative error / eps = ' num2str( rel_error_d2x / eps ) ] );
    disp( [ 'min( d2x ) = ' num2str( min( d2x ) ) ] );
    disp( [ 'max( d2x ) = ' num2str( max( d2x ) ) ] );
    error( 'non-equidistant data points (RKD)' );
end
h = dx( 1 ); % stepsize

% start calculations
dy_denominator = 2 * h * sum( ( 1 : k ).^2 );

a = - k : + k;
for i = k + 1 : n - k
    dy( i ) = sum( a .* y( i + a )' );
end

dy = dy / dy_denominator;

% first and last k points:
% fit parabola over first and last 2k points
kk = 2 * k;

% first kk points
p1 = polyfit( x( 1 : kk ), y( 1 : kk ), 2 );
q1 = polyder( p1 ); % take derivative
dy( 1 : k ) = polyval( q1, x( 1 : k ) );

% last kk points
p2 = polyfit( x( n - kk + 1 : n ), y( n - kk + 1 : n ), 2 );
q2 = polyder( p2 ); % take derivative
dy( n - k + 1 : n ) = polyval( q2, x( n - k + 1 : n ) );

if nargout ~= 0 % output
    dy_dx = dy;
end

% end of calculations (everything below is just for making comparisons and nice output)

if graphics_flag == 1 % make some comparisons
    xdiff_range = x( 1 : n - 1 ) + h / 2; % x coordinates for results of diff() function

    % compare with diff()
    dydx_matlab = diff( y ) / h;

    if nargin >=3 % not enough data points in example to apply this filter

        % another test: try first adjacent averaging and then diff()
        % use k points to left and to right
        kkk = 2 * k + 1;
        b = ones( 1, kkk ) / kkk; % k point averaging filter
        y_filt = filtfilt( b, 1, y ); % noncausal filtering, zero phase distortion

        % calculate dy_filt/dx
        dy_filtdx = diff( y_filt ) / h;
    end

    if nargin < 3 % show results of example
        disp( '      x         y         dy' );
        disp( [ x, y, dy ] );
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% show results
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if graphics_flag == 1
    if nargout == 0
        close all;
    end

    dx_idx_range = k + 1 : n - k;

    % parabolic fits to first (last) kk points
    x1_fit = min( x( 1 : kk ) ) : h / 100 : max( x( 1 : kk ) );
    y1_fit = polyval( p1, x1_fit );

    x2_fit = min( x( n - kk + 1 : n ) ) : h / 100 : max( x( n - kk + 1 : n ) );
    y2_fit = polyval( p2, x2_fit );

    figure;
    hold on;
    plot( x, y, '-ob', 'LineWidth', linewidth );
    plot( x( dx_idx_range ), dy( dx_idx_range ), '-or', 'LineWidth', linewidth );
    plot( x( 1 : k ), dy( 1 : k ), '-sr', 'LineWidth', linewidth );
    plot( x1_fit, y1_fit, '--r', 'LineWidth', linewidth );
    plot( xdiff_range, dydx_matlab, ':sg', 'LineWidth', linewidth );

    if nargin >= 3
        plot( xdiff_range, dy_filtdx, ':dm', 'LineWidth', linewidth );
    end

    legend( 'y', [ 'dy/dx_{Lanczos: {\pm}' num2str( k ) '}' ], ...
        [ 'first/last ' num2str( k ) ' pts' ], ...
        [ 'y_{fit, parabola, ' num2str( kk ) ' pts}' ], 'diff( y )/ dx', ...
        [ 'diff( y_{adj avg ({\pm}' num2str( k ) ')} )/ dx' ], 0 );

    plot( x( n - k + 1 : n ), dy( n - k + 1 : n ), '-sr', 'LineWidth', linewidth );
    plot( x2_fit, y2_fit, '--r', 'LineWidth', linewidth );

    plot( x, dy, '-r', 'LineWidth', linewidth );

    % replot to put these on top again
    plot( x( dx_idx_range ), dy( dx_idx_range ), '-or', 'LineWidth', linewidth );
    plot( x( 1 : k ), dy( 1 : k ), '-sr', 'LineWidth', linewidth );
    hold off;

    xlabel( 'x' );
    ylabel( 'y' );

    title( 'parabolic derivative (Lanczos)' );
end


classdef VonMisesD
%%VONMISESD Functions to handle the von Mises distribution, which is a
%    common circular distribution. The distribution is given in Chapter
%    3.5.4 of [1].
%Implemented methods are: circVar, PDF, CDF,trigMoment, normProdDist,
%                         convDistApprox, params4TrigMoment, rand, entropy
%
%REFERENCES:
%[1] K. V. Mardia and P. E. Jupp, Directional Statistics. Chichester: John
%    Wiley and Sons, 2000.
%
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

methods(Static)

function val=circVar(kappa)
%%CIRCVAR Obtain the circular variance of a particular von Mises
%         distribution.
%
%INPUTS: kappa The concentration of the distribution 0<=kappa<inf.
%
%OUTPUTS: val The circular variance of the distribution.
%
%The circular variance is defined as 1-abs(r), where r is the mean
%resultant value of the second moment. The mean resultant values for all
%moment of the von Mises distribution are given in Chapter 2.2.4 of [1].
%
%EXAMPLE:
%Here, we verify that the result agrees with the value obtained by
%sampling.
% numRuns=1e5;
% kappa=100;
% mu=0.25;
% x=VonMisesD.rand([numRuns,1],mu,kappa);
% circVarEst=findCircVar(x);
% abs(VonMisesD.circVar(kappa)-circVarEst)./abs(VonMisesD.circVar(kappa))
%One will see that the relative error is on the order of 1e-3.
%
%REFERENCES:
%[1] S. R. Jammalamadaka and A. SenGupta, Topics in Circular Statistics.
%    Singapore: World Scientific, 2001.
%
%April 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.

val=1-BesseliRatio(1,kappa);

end

function val=PDF(x,mu,kappa)
%%PDF Evaluate a von Mises PDF at a given point with the specified
%     parameters.
%
%INPUTS: x The point(s) at which the von Mises distribution is to be
%          evaluated.
%       mu The mean direction of the distribution. This is normally between
%          -pi and pi.
%    kappa The concentration of the distribution 0<=kappa<inf.
%
%OUTPUTS: val The value(s) of the von Mises PDF with the specified
%             parameters evaluated at x.
%
%The von Mises distribution is used to model errors on angular quantities.
%It is described in [1] and [2].
%
%REFERENCES:
%[1] N. Fisher, Statistical Analysis of Circular Data. Cambridge, UK:
%    Cambridge University Press, 1993.
%[2] K. V. Mardia and P. E. Jupp, Directional Statistics. Chichester,
%    England: John Wiley & Sons, 2000.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.

    val=exp(kappa*cos(x-mu))/(2*pi*besseli(0,kappa));
end

function vals=CDF(x,mu,kappa,startAng)
%%CDF Evaluate  the cumulative distribution function (CDF) of the von Mises
%     distribution. Since this is a circular distribution, the CDF has to
%     be defined in terms of a specific starting angle. Increading density
%     goes in the direction of increasing angle from the starting point
%     (around the circle).
%
%INPUTS: z The point(s) at which the CDF should be evaluated.
%       mu The mean direction of the distribution. This is normally between
%          -pi and pi.
%    kappa The concentration of the distribution 0<=kappa<inf.
%  startAng The starting angle. This is where the CDF is zero. The default
%           if this parameter is omitted or an empty matrix is passed is
%           zero.
%
%OUTPUTS: vals The scalar value(s) of the von Mises CDF with the given PDF
%              evaluated at the points in z.
%
%The function Besseli0Inc gives the CDF of the von Mises distribution
%starting from -pi. Thus, by taking the difference of two evaluations of
%Besseli0Inc (and determining whether we are going the long or the short
%way around the circle) one can evaluate the CDF.
%
%August 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.

if(nargin<4||isempty(startAng))
    startAng=-pi;
end

vals=Besseli0Inc(x-mu,kappa)-Besseli0Inc(startAng-mu,kappa);

sel=vals<0;
vals(sel)=vals(sel)+1;
end

function [rho,theta,R]=trigMoment(n,mu,kappa)
%%TRIGMOMENT Compute the nth trigonometric moment of a von Mises
%            distribution having the given parameters. A direction theta on
%            the unit circle can be modeled as a complex quantity having
%            unit magnitude as exp(1j*theta). The nth raw complex
%            trigonometric moment is the expected value of exp(1j*n*theta),
%            where the integral for the expected value is taken over any
%            interval of length 2*pi.
%
%INPUTS: n The order of the moment desired. This is >=1.
%       mu The mean direction of the distribution. This is normally between
%          -pi and pi.
%    kappa The concentration of the distribution 0<=kappa<inf. For
%          kappa>500, the results might be inaccurate.
%
%OUTPUTS: rho The (complex) mean resultant value for the nth moment. Note
%             that rho=R*exp(1j*theta).
%       theta The (real) trigonometric mean angle in radians for the nth
%             moment. This is between -pi and pi.
%           R The (real) mean resultant length for the nth moment.
%
%The moments of a von Mises distribution are given in Chapter 2.2.4 of [1]
%and Chapter 3.5.4 of [2].
%
%EXAMPLE:
%Here, we verify the first trigonometric moment based on samples.
% numRuns=1e5;
% kappa=100;
% mu=0.25;
% x=VonMisesD.rand([numRuns,1],mu,kappa);
% [rhoS,thetaS,RS]=findTrigMomentFromSamp(1,x);
% [rho,theta,R]=VonMisesD.trigMoment(1,mu,kappa);
% abs(rhoS-rho)
% abs(thetaS-theta)
% abs(RS-R)
%Here, we will see that the errors tend to be on the order of 1e-4,
%dominated by the angular error.
%
%REFERENCES:
%[1] S. R. Jammalamadaka and A. SenGupta, Topics in Circular Statistics.
%    Singapore: World Scientific, 2001.
%[2] K. V. Mardia and P. E. Jupp, Directional Statistics. Chichester: John
%    Wiley and Sons, 2000.
%
%August 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.

%For n=1 or 2, there should be no need to use anything but the default
%maximum number of iterations in BesseliRatio.
if(n==1)
    rho=BesseliRatio(1,kappa)*exp(1j*n*mu);
elseif(n==2)
    rho=BesseliRatio(2,kappa)*BesseliRatio(1,kappa)*exp(1j*n*mu);
else
    rho=besseli(n,kappa,1)/besseli(0,kappa,1)*exp(1j*n*mu);
end

if(nargout>1)
    theta=angle(rho);
    R=abs(rho);
end
end

function [mu,kappa]=normProdDist(mu1,kappa1,mu2,kappa2)
%%NORMPRODDIST The product of two von Mises distributions is an
%              unnormalized von Mises distribution. This finds the
%              parameters of the product distribution.
%
%INPUTS: mu1, kappa1 The mean direction and concentration of the first
%                    distribution. Note that 0<=kappa<inf.
%        mu2, kappa2 The mean direction and concentration of the second
%                    distribution.
%
%OUTPUTS: mu,kappa The mean direction and concentration of the product
%                  distribution -pi<=mu<pi.
%
%The product of two von Mises distributions is described in [1].
%
%REFERENCES:
%[1] M. Azmani, S. Reboul, J.-B. Choquel, and M. Benkelloun, "A recursive
%    fusion filter for angular data," in Proceedings of the 2009 IEEE
%    International Conference on Robotics and Biomimetics, Guilin, China,
%    19-23 Dec. 2009, pp. 882-887.
%
%August 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.

C=kappa1*cos(mu1)+kappa2*cos(mu2);
S=kappa1*sin(mu1)+kappa2*sin(mu2);
mu=atan2(S,C);
kappa=sqrt(C^2+S^2);
end

function [mu,kappa]=convDistApprox(mu1,kappa1,mu2,kappa2)
%%CONVDISTAPPROX The convolution of two von Mises distributions is not a
%          von Mises distribution. However, it can be approximated by a von
%          Mises distribution that matches the moments, as is done in
%          Equation 7 of [1]. This function returns the paramters of the
%          approximation.
%
%INPUTS: mu1, kappa1 The mean direction and concentration of the first
%                    distribution. Note that 0<=kappa<inf.
%        mu2, kappa2 The mean direction and concentration of the second
%                    distribution.
%
%OUTPUTS: mu, kappa The mean direction and concentration of a von Mises
%                   approximation to the product distribution. -pi<=mu<pi.
%
%REFERENCES:
%[1] I. Markovic and I. Petrovic, "Bearings-only tracking with a mixture of
%    von Mises distributions," in IEEE/RSJ International Conference on
%    Intelligent Robots and Systems, Vilamoura, Algarve, Portugal, 7-12
%    Oct. 2012, pp. 707-712.
%
%April 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.

    mu=wrapRange(mu1+mu2,-pi,pi);

    AkProd=BesseliRatio(1,kappa1)*BesseliRatio(1,kappa2);
    kappa=BesseliRatioInv(1,AkProd);
end

function [mu,kappa]=params4TrigMoment(rho)
%%PARAMS4TRIGMOMENT Obtain the parameters (mu and kappa) of the von Mises
%           distribution such that the von Mises distribution matches the
%           given complex mean resultant length for the first moment.
%
%INPUTS: rho The (complex) mean resultant value for the first moment. Note
%            that abs(rho)<=1.
%
%OUTPUTS: mu, kappa The mean direction and the concentration parameter of
%                   the von Mises distribution with the specified mean
%                   resultant value.
%
%The moments of a von Mises distribution are given in Chapter 2.2.4 of [1].
%This function just implements an inverse.
%
%REFERENCES:
%[1] S. R. Jammalamadaka and A. SenGupta, Topics in Circular Statistics.
%    Singapore: World Scientific, 2001.
%
%August 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.

    mu=atan2(imag(rho),real(rho));
    kappa= BesseliRatioInv(1,abs(rho));
end

function vals=rand(N,mu,kappa)
%%RAND Generate von Mises random variables with the given parameters.
%
%INPUTS: N If N is a scalar, then rand returns an NXN matrix of random
%          variables. If N=[M,N1] is a two-element row vector, then rand
%          returns an MXN1 matrix of random variables.
%       mu The mean direction of the distribution. This is normally
%          between -pi and pi.
%    kappa The concentration of the distribution 0<=kappa<inf.
%
%OUTPUTS: vals A matrix whose dimensions are determined by N of the
%              generated von Mises random variables.
%
%The algorithm if that of Section 4 of [1], which is a rejection sampling
%method based on using a wrapped Cauchy distribution as a proposal density.
%However, the range of the random variable u1 in [1] does not appear to be
%correct. The correct formulation is based on Chapter 7.3 of [2] using the
%errata available at http://luc.devroye.org/errors.pdf so u1 is randomly
%generated between -1 and 1 as opposed to between 0 and 1.
%
%REFERENCES:
%[1] D. J. Best and N. I. Fisher, "Efficient simulation of the von Mises
%    distribution," Journal of the Royal Statistical Society. Seriec C
%    (Applied Statistics), vol. 28, no. 2, pp. 152-157, 1979.
%[2] L. Devroye, Non-Uniform random Variate Generation. New York:
%    Springer-Verlag, 1986.
%
%August 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.

    if(isscalar(N))
        dims=[N N];
    else
        dims=N;
    end

    vals=zeros(dims);%Allocate space
    numSamp=prod(dims);

    %Step 0
    tau=1+sqrt(1+4*kappa^2);
    rho=(tau-sqrt(2*tau))/(2*kappa);
    r=(1+rho^2)/(2*rho);

    for curSamp=1:numSamp
        while(1)
            %Step 1
            u1=2*rand(1)-1;%In range [-1,1]
            z=cos(pi*u1);
            f=(1+r*z)/(r+z);
            c=kappa*(r-f);

            %Step 2
            u2=rand(1);%in range [0,1]
            if(c*(2-c)-u2>0)
               break;
            end

            %Step 3
            if(log(c/u2)+1-c>=0)
                break;
            end
        end
        %Step 4
        u3=2*rand(1)-1;%In range [-1,1]
        vals(curSamp)=wrapRange(sign(u3)*acos(f)+mu,-pi,pi);
    end
end

function entropyVal=entropy(kappa)
%%ENTROPY Obtain the differential entropy of the von Mises distribution
%         given in nats. The differential entropy of a continuous
%         distribution is entropy=-int_x p(x)*log(p(x)) dx where the
%         integral is over all values of x. Units of nats mean that the
%         natural logarithm is used in the definition. Unlike the Shannon
%         entropy for discrete variables, the differential entropy of
%         continuous variables can be both positive and negative.
%
%INPUTS: kappa The concentration of the distribution 0<=kappa<inf.
%
%OUTPUTS: entropyVal The value of the differential entropy in nats.
%
%Differential entropy is defined in Chapter 8 of [1].
%
%If kappa is very large, besseli(0,kappa) will overflow and a warning will
%be issued. In such an instance, an approximation based on the first term
%of the series on page 289 of [2] is used. The approximation becomes
%increasingly bad as kappa becomes larger.
%
%REFERENCES:
%[1] T. M. Cover and J. A. Thomas, Elements of Information Theory, 2nd ed.
%    Hoboken, NJ: Wiley-Interscience, 2006.
%[2] S. R. Jammalamadaka and A. SenGupta, Topics in Circular Statistics.
%    Singapore: World Scientific, 2001.
%
%April 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.

    besseliVal=besseli(0,kappa);

    if(isfinite(besseliVal))
        entropyVal=-kappa*BesseliRatio(1,kappa)+log(2*pi*besseliVal);
    else
        %Use the approximation exp(kappa)/sqrt(2*pi*kappa) for values of
        %kappa so large that besseli overflows
        warning('besseli(0,kappa) overflowed; using an approximation.')
        entropyVal=-kappa*BesseliRatio(1,kappa)+(1/2)*(4000+log(pi/1000));
    end
end

end
end

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


function [ x, w ] = fejer1_rule_compute ( n )

%*****************************************************************************80
%
%% FEJER1_RULE_COMPUTE computes a Fejer Type 1 rule.
%
%  Modified:
%
%    06 March 2007
%
%  Author:
%
%    Joerg Waldvogel
%
%  Reference:
%
%    Joerg Waldvogel,
%    Fast Construction of the Fejer and Clenshaw-Curtis Quadrature Rules,
%    BIT Numerical Mathematics
%    Volume 43, Number 1, pages 1-18, 2003.
%
%  Parameters:
%
%    Input, integer N, the order of the rule.
%
%    Output, real X(N), W(N), the abscissas and weights of the rule.
%
  N = [ 1 : 2 : n-1 ]';
  L = length ( N );
  m = n - L;
  K = [ 0 : m-1 ]';

  s = zeros ( L+1, 1 );
  v0 = [ 2 * exp(i*pi*K/n) ./ ( 1 - 4 * K.^2 ); s ];

  v1 = v0(1:end-1) + conj ( v0(end:-1:2) );
  w = ifft ( v1 );

  x = cos ( pi * ( (n:-1:1)' - 1 / 2 ) / n );

  return
end


function [p,s]=permutes(n)
%PERMUTES All N! permutations of 1:N + signatures [P,S]=(N)
% The output P is a matrix of size (N!,N) where each row
% contains a permutation of the numbers 1:N. The rows are in
% lexically sorted order.
%
% To permute the elements of an arbitrary vector V use
% V(PERMUTES(LENGTH(V))).

% PERMUTES(N) is the same as SORTROWS(PERMS(1:N)) but much faster.

% Thanks to Peter J Acklam for several improvements.

%      Copyright (c) 1998 Mike Brookes,  mike.brookes@ic.ac.uk
%      Version: $Id: permutes.m 713 2011-10-16 14:45:43Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

p=1;
m=1;
if n>1
  for a=2:n
    q=zeros(a*m,a);
    r=2:a+1;
       ix=1:m;
    for b=1:a
       q(ix,1)=b;
      q(ix,2:a)=r(p);
      r(b)=b;
      ix=ix+m;
    end
    m=m*a;
    p=q;
  end
end
if nargout>1 s=1-2*rem(fix((1:m)'/2),2); end


function [tri, x] = delaunay_sphere(N, xi)
% DELAUNAY_SPHERE  Triangulation of a sphere or a surface topologically
% equivalent to a sphere so that it can be used as a mesh
%
% [TRI, X] = delaunay_sphere(N)
%
%   Number of samples of the sphere (in latitude and longitude).
%
%   TRI is a 3-column matrix with a triangulation of the sphere. Each
%   element in TRI is an index to a row in X. Each row represents the three
%   vertices of a triangle on the sphere.
%
%   X is a 3-column matrix with the Euclidean coordinates of the
%   triangulation vertices.
%
%   The surface can be plotted running
%
%     trisurf(tri, x(:, 1), x(:, 2), x(:, 3));
%
% ... = delaunay_sphere(N, XI)
%
%   XI is an (N+1,N+1,3)-array with the coordinates of the vertices of a
%   surface topologically equivalent to a sphere. By default, XI is the
%   concatenation of the output of sphere(N), and the surface is a unit
%   sphere.
%
%   The surface can be plotted running
%
%     surf(xi(:, :, 1), xi(:, :, 2), xi(:, :, 3))
%
% See also: scimat_tri_to_raster.

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2013 University of Oxford
% Version: 0.1.0
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% 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. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

% check arguments
narginchk(1, 2);
nargoutchk(0, 2);

if (nargin > 1 ...
        && (size(xi, 1) ~= N+1 || size(xi, 2) ~= N+1 || size(xi, 3) ~= 3))
    error('XI must be a (N+1, N+1, 3) array')
end

% sphere surface
[x, y, z] = sphere(N);

% remove repeated points
[x, idx] = unique([x(:) y(:) z(:)], 'rows');

% triangulate the sphere and extract the surface
tri = DelaunayTri(x);
tri = freeBoundary(tri);

% if an input point configuration was provided, we use it to replace the
% unit sphere points coordinates
if (nargin > 1)
    xi = reshape(xi, (N+1)*(N+1), 3);
    x = xi(idx, :);
end


function [CL, u_U, v_U, w_U, l_t, l_s] = LiftCoeff(gamma, panel, geo, Fu_bar, Fv_bar, Fw_bar)
%Determine the lift coefficient given the vortex strengths
%and the influence coefficients from the vortex lattice method

u_U.c = 1/(4*pi)*Fu_bar*[gamma.c]';  %Eqn 25 in NASA paper
v_U.c = 1/(4*pi)*Fv_bar*[gamma.c]';    %Eqn 23 in NASA paper
w_U.c = 1/(4*pi)*Fw_bar*[gamma.c]';

u_U.alpha = 1/(4*pi)*Fu_bar*[gamma.alpha]';
v_U.alpha = 1/(4*pi)*Fv_bar*[gamma.alpha]';
w_U.alpha = 1/(4*pi)*Fw_bar*[gamma.alpha]';

for i = 1:geo.ns
    for j = 1:geo.nc
        if i == geo.ns  %wingtip
            DeltaGamma(i,j).c = gamma(i,j).c;
            DeltaGamma(i,j).alpha = gamma(i,j).alpha;
        elseif j == 1 %Leading edge
            DeltaGamma(i,j).c = gamma(i,j).c-gamma(i+1,j).c;
            DeltaGamma(i,j).alpha = gamma(i,j).alpha-gamma(i+1,j).alpha;
        else
            DeltaGamma(i,j).c = DeltaGamma(i,j-1).c + gamma(i,j).c - gamma(i+1,j).c;
            DeltaGamma(i,j).alpha = DeltaGamma(i,j-1).alpha + gamma(i,j).alpha - gamma(i+1,j).alpha;
        end
    end
end

l_t.c = 2/geo.S*[DeltaGamma.c]'.*[panel.cc]'.*[v_U.c];  %Eqn 24 in NASA paper
l_t.alpha = 2/geo.S*[DeltaGamma.alpha]'.*[panel.cc]'.*[v_U.alpha];  %Eqn 24 in NASA paper

l_s.c = 2/geo.S*[gamma.c]'*2.*[panel.s]'.*((ones(size(u_U))-[u_U.c])+[v_U.c].*tan([panel.sweep]'))*cos(geo.dih);  %Eqn 28 in NASA paper
l_s.alpha = 2/geo.S*[gamma.alpha]'*2.*[panel.s]'.*((ones(size(u_U))-[u_U.alpha])+[v_U.alpha].*tan([panel.sweep]'))*cos(geo.dih);  %Eqn 28 in NASA paper

% reshape([l_t.c],geo.ns,geo.nc)
% reshape([l_t.alpha],geo.ns,geo.nc)
% reshape([l_s.c],geo.ns,geo.nc)
% reshape(((ones(size(u_U))-[u_U.alpha])+[v_U.alpha].*tan([panel.sweep]')),geo.ns,geo.nc)
% reshape([panel.s],geo.ns,geo.nc)
% reshape([l_s.alpha],geo.ns,geo.nc)
% reshape([u_U.alpha],geo.ns,geo.nc)
% reshape([v_U.alpha],geo.ns,geo.nc)
% reshape([gamma.alpha],geo.ns,geo.nc)
% sum([gamma.c])
% geo.S

CL.c = 2*(sum([l_t.c])+sum([l_s.c]));
CL.alpha = 2*(sum([l_t.alpha])+sum([l_s.alpha]));


classdef grassmannian < handle
    % Ways to generate Grassmannian frames.
    properties
    end

    methods(Static)

        function result = minimum_coherence(m, n)
            % Returns the minimum coherence of an m times n matrix
            num = n - m;
            den = m * (n - 1);
            result = sqrt(num / den);
        end

        function n = n_upper_bound(m)
            % for a given value of m, returns the upper bound on n.
            a = m * (m  + 1) / 2;
            n = m  * (m + 1) / 2 - 1;
            lower_limit = m;
            while true
                b = (n - m) * (n - m + 1) / 2;
                upper_limit = min (a, b);
                if n < upper_limit
                    break;
                end
                n = n- 1;
            end
        end
        function [ns, coherences] = min_coherence_max_n(ms)
            if ~isvector(ms)
                error('ms must be a vector');
            end
            mm = length(ms);
            ns = zeros(1, mm);
            coherences = zeros(1, mm);
            for i=1:mm
                m = ms(i);
                n = spx.dict.grassmannian.n_upper_bound(m);
                ns(i) = n;
                coherences(i) = spx.dict.grassmannian.minimum_coherence(m, n);
            end
        end

        function n = max_n_for_coherence(m, mu)
            % estimates a value of n for m x n matrix which achieves a
            % given coherence
            num = (mu^2 -1) * m;
            den = mu^2 * m - 1;
            n = floor(num / den);
            if n < 0
                error('A Grassmannian frame for this dimension will never have so high coherence.');
            end
        end

        function result = alternate_projections(dict, options)
            % Converts a given dictionary to Grassmannian frame via
            % alternate projections
            [n, d] = size(dict);
            % We assume that the dictionary contains unit norm atoms
            target_mu  = spx.dict.grassmannian.minimum_coherence(n, d);
            % Compute the Gram matrix
            gram_matrix = dict' * dict;
            iterations = 1000;
            entry_shrinkage_factor = 0.9;
            coherence_shrinkage_factor = 0.9;
            verbose = false;
            if nargin > 1 && isstruct(options)
                if isfield(options, 'iterations')
                    iterations = options.iterations;
                end
                if isfield(options, 'entry_shrinkage_factor')
                    entry_shrinkage_factor = options.entry_shrinkage_factor;
                end
                if isfield(options, 'coherence_shrinkage_factor')
                    coherence_shrinkage_factor = options.coherence_shrinkage_factor;
                end
                if isfield(options, 'verbose')
                    verbose = options.verbose;
                end
            end
            total_entries = d^2 - d; % excluding the main diagonal.
            % Identify the indices in the gram matrix which are off diagonal
            off_diagonal_indices = abs(gram_matrix(:) - 1)> 1e-6;
            coherence_array = zeros(iterations, 1);
            mean_coherence_array = zeros(iterations, 1);
            for k=1:iterations
                % Convert gram matrix to an array
                gram_array = gram_matrix(:);
                % Absolute Gram matrix
                abs_gram_array = abs(gram_array);
                % sort the inner products by their absolute values
                sorted_gram_array = sort(abs_gram_array);
                upper_threhold_index = round(entry_shrinkage_factor * total_entries);
                upper_threshold = sorted_gram_array(upper_threhold_index);
                % identify coherence values above the threshold
                above_indices = off_diagonal_indices & (abs_gram_array > upper_threshold);
                % map the boolean flags to positions.
                above_indices = find(above_indices);
                above_values = abs_gram_array(above_indices);
                current_mu = max(above_values);
                mean_mu = mean(above_values);
                coherence_array(k) = current_mu;
                mean_coherence_array(k) = mean_mu;
                if verbose
                    fprintf('%6d:, Coherence:: target: %12.8f, ', k, target_mu);
                    fprintf('current: %12.8f, threshold: %12.8f, above: %d, mean: %12.8f\n', ...
                        current_mu, upper_threshold, length(above_values), mean_mu);
                else
                    fprintf('.');
                    if mod(k, 100) == 0
                        fprintf('\n');
                    end
                end
                % Update off diagonal entries
                gram_matrix(above_indices)=gram_matrix(above_indices)*coherence_shrinkage_factor;

                % reduce the rank back to n
                [U,S,V]=svd(gram_matrix);
                % Ensure that all higher singular values are set to 0.
                S(n+1:end,n+1:end)=0;
                % Reconstruct the Gram matrix.
                gram_matrix=U*S*V';
                % Ensure that the diagonal elements of G continue to be 1.
                % Compute  d = diag(G)  d^{-1/2}.
                gram_diag_sqrt_inv = diag(1./sqrt(diag(gram_matrix)));
                % Compute G = d^{-1/2} G d^{-1/2}.
                gram_matrix = gram_diag_sqrt_inv*gram_matrix*gram_diag_sqrt_inv;
            end
            % final dictionary
            % perform SVD of the gram matrix.
            [U,S,V]=svd(gram_matrix);
            % Construct the dictionary
            dict=S(1:n,1:n).^0.5*U(:,1:n)';
            result.dictionary = dict;
            result.coherence_array = coherence_array;
            result.mean_coherence_array = mean_coherence_array;
            result.target_coherence = target_mu;
            result.iterations = iterations;
        end
    end
end


%% KEOLLOGG Problem
%
% KELLOGG solves a diffusion equation with jump coefficients with AFEM.
%
% KELLOGG solves the problem within maxN number of vertices. The
% input argument theta is a parameter used in the marking step.
%
% The KELLOGG command, if no input arguments, use maxN = 5e3 and theta = 0.5.
%
% EXAMPLE
%
%    Kellogg
%
% See also  crack, Lshape
%
% TODO: rewrite M-lint
%
% Created by Chen-Song Zhang. Modified by Long Chen.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

close all
%% Problem setting
% $$-\nabla\cdot(d\nabla u) = 0  \quad \Omega = (-1,1)\times (-1,1)$$
%
% $$u = g_D \quad \partial \Omega$$
%
% The diffusion constant is discontinous; see the figure below. We set a2  =
% 1; a1 = 161.4476387975881 and choose boundary condition g_D such that the
% exact solution is $z = r^{0.1}\mu(\theta)$ in the poloar coordinate, where
% the formula of mu can be found in exactu function.

[x,y] = meshgrid(-1:1:1,-1:1:1);
z = 0*x;
surf(x,y,z,'linewidth',2); view(2);
axis equal; axis tight;
text(0.5,0.5,'a1','FontSize',12,'FontWeight','bold');
text(-0.5,-0.5,'a1','FontSize',12,'FontWeight','bold');
text(-0.5,0.5,'a2','FontSize',12,'FontWeight','bold');
text(0.5,-0.5,'a2','FontSize',12,'FontWeight','bold');

%% Parameters
maxN = 3e3;     theta = 0.2;    maxIt = 1000;
N = zeros(maxIt,1);     uIuhErr = zeros(maxIt,1);
errH1 = zeros(maxIt,1);

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

%% Set up PDE data
pde = Kelloggdata;

%%  Adaptive Finite Element Method
% *SOLVE* -> *ESTIMATE* -> *MARK* -> *REFINE*

for k = 1:maxIt
    % Step 1: SOLVE
    [u,Du,eqn] = Poisson(node,elem,pde);
    % Plot mesh and solution
    figure(1);  showresult(node,elem,u,[27,26]);
    % Step 2: ESTIMATE
   eta = estimaterecovery(node,elem,u);            % recovery type
%     eta = estimateresidual(node,elem,u,pde);    % residual type
    % Record error and number of vertices
    uI = pde.exactu(node);
    uIuhErr(k) = sqrt((uI-u)'*eqn.A*(uI-u));
    errH1(k) = getH1error(node,elem,@pde.Du,u);
    N(k) = size(node,1);
    if (N(k)>maxN), break; end
    % Step 3: MARK
    markedElem = mark(elem,eta,theta);
    % Step 4: REFINE
    [node,elem,bdFlag] = bisect(node,elem,markedElem,bdFlag);
end

%%  Plot convergent rates in energy norm
N = N(1:k);
uIuhErr = uIuhErr(1:k);
errH1 = errH1(1:k);
figure(2)
showrate2(N,uIuhErr,10,'-*','||Du_I-Du_h||',...
          N,errH1,10,'k-.','||Du-Du_h||');


function graph_demo (n)
%GRAPH_DEMO graph partitioning demo
%   graph_demo(n) constructs an set of n-by-n 2D grids, partitions them, and
%   plots them in one-second intervals.  n is optional; it defaults to 60.
%
%   Example:
%       graph_demo
%
%   See also DELSQ, NUMGRID, GPLOT, TREEPLOT

%   Copyright 2006-2007, Timothy A. Davis
%   http://www.cise.ufl.edu/research/sparse

if (nargin < 1)
    % construct a 60-by-60 grid
    n = 60 ;
end

figure (1)
clf

for regions = {'Square', 'C' 'Disc', 'Annulus', 'Heart', 'Butterfly', 'L'}

    % construct the grid
    region = regions {1} ;
    g = numgrid (region (1), n) ;
    x = repmat (0:n-1, n, 1) ;
    y = repmat (((n-1):-1:0)', 1, n)  ;
    A = delsq (g) ;
    x = x (find (g)) ;							    %#ok
    y = y (find (g)) ;							    %#ok

    % plot the original grid
    clf
    subplot (2,2,1)
    my_gplot (A, x, y)
    title (sprintf ('%s-shaped 2D grid', region)) ;
    axis equal
    axis off

    % bisect the graph
    s = bisect (A) ;
    [i j] = find (A) ;
    subplot (2,2,2)
    my_gplot (sparse (i, j, s(i) == s(j)), x, y) ;
    title ('node bisection') ;
    axis equal
    axis off

    % nested dissection
    nsmall = floor (size (A,1) / 2) ;
    defaults = 0 ;
    while (1)
        if (defaults)
            % use defaults
            [p cp cmember] = nesdis (A) ;
        else
            [p cp cmember] = nesdis (A, 'sym', nsmall) ;
        end

        % plot the components
        subplot (2,2,3)
        my_gplot (sparse (i, j, cmember(i) == cmember (j)), x, y) ;
        if (defaults)
            title ('nested dissection (defaults)') ;
        else
            title (sprintf ('nested dissection, nsmall %d', nsmall)) ;
        end
        axis equal
        axis off

        % plot the separator tree
        subplot (2,2,4)
        treeplot (cp, 'ko')
        title ('separator tree') ;
        axis equal
        axis off

        drawnow
        pause (0.1)

        if (defaults)
            break ;
        end

        nsmall = floor (nsmall / 2) ;
        if (nsmall < 20)
            defaults = 1 ;
            pause (0.2)
        end
    end
end

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

function my_gplot (A, x, y)
% my_gplot : like gplot, just a lot faster
[i, j] = find (A) ;
[ignore, p] = sort (max(i, j)) ;
i = i (p) ;
j = j (p) ;
nans = repmat (NaN, size (i)) ;
x = [ x(i) x(j) nans ]' ;
y = [ y(i) y(j) nans ]' ;
plot (x (:), y (:)) ;


function [L] = lap2DDir(N, h);
%
% [L] = lap2DDir(N, h)
%
%  Constructs the 2D Laplacian for a square mesh with zero Dirichlet
%     boundary conditions using the standard 5-point stencil.
%
%  Returns:
%     L = discrete Laplacian
%
%  Input:
%     N = number of mesh points in each direction
%     h = mesh width
%
%
%
%  License: This code is free to use for any purposes, provided
%           any publications resulting from the use of this code
%           reference the original code/author.
%
%  Author:  Samuel Isaacson (isaacson@math.utah.edu)
%  Date:    11/2007
%
%  Please notify the author of any bugs, and contribute any
%  modifications or bug fixes back to the original author.
%
%  Disclaimer:
%   This code is provided as is. The author takes no responsibility
%   for its results or effects.

M = N * N;

% 2D Laplace Operator on Square:
e              = ones(M,1);
ed             = -4*e;
eu1            = ones(M,1);
eu1((N+1):N:M) = 0;
ed1            = ones(M,1);
ed1(N:N:M)     = 0;

L = spdiags([e ed1 ed eu1 e], [-N -1:1 N], M, M);

L = L ./ (h*h);


function  a = polygon_area(x,y)
% AREA  Area of a planar polygon.
%	AREA(X,Y) Calculates the area of a 2-dimensional
%	polygon formed by vertices with coordinate vectors
%	X and Y. The result is direction-sensitive: the
%	area is positive if the bounding contour is counter-
%	clockwise and negative if it is clockwise.
%
%	See also TRAPZ.

%  Copyright (c) 1995 by Kirill K. Pankratov,
%	kirill@plume.mit.edu.
%	04/20/94, 05/20/95

 % Make polygon closed .............
x = [x(:); x(1)];
y = [y(:); y(1)];

 % Calculate contour integral Int -y*dx  (same as Int x*dy).
lx = length(x);
a = -(x(2:lx)-x(1:lx-1))'*(y(1:lx-1)+y(2:lx))/2;
a = abs(a);


function diff = getDiffuseness_SV(i_vecs)
%GETDIFFUSENESS_SV Compute the DoA spherical variance diffuseness
%
%   This routine estimates diffuseness based on the assumption that DoA
%   estimates should have a small variance around the source DoA, if the
%   direct-to-diffuse ratio is high, and be completely uniformly
%   distributed if the sound is fully diffuse. This variation can be
%   captured on the spherical variance of the DoA estimates. The method can
%   utilize intensity vectors, or any other vectors that indicate DoAs. The
%   statistics can be computed from multiple temporal observations or
%   across different frequency bands. This estimator has been used e.g. in
%
%       Politis, A., Delikaris-Manias, S. and Pulkki, V., 2015.
%       Direction-of-arrival and diffuseness estimation above spatial aliasing for symmetrical directional microphone arrays.
%       In 2015 IEEE International Conference on Acoustics, Speech and Signal Processing (ICASSP).
%
%   Inputs:
%       i_vecs:  Kx3 matrix of DoA vectors, for K temporal or frequency
%           observations
%
%   Outputs:
%       diff:       diffuseness value
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% GETDIFFUSENESS_SV.M - 5/10/2016
% Archontis Politis, archontis.politis@aalto.fi
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% unit DoA vectors
doa_vecs = i_vecs ./ (sqrt(sum(i_vecs.^2,2))*ones(1,3));
% mean DoA vector
mean_doa_vec = mean(doa_vecs,1);
% spherical variance
diff = 1 - sqrt(sum(mean_doa_vec.^2, 2));

end


% Author : FUAT COGUN
% Date   : 16.07.2009
%
%
% Find the Covariance Matrix of given region.
%
%
% Inputs  : -I                                                  (uint8 Matrix)
% ======    -x position                                         (Integer)
%           -y position                                         (Integer)
%           -size                                               (Integer)
%
% Outputs : -Covariance Matrix                                  (7x7 Matrix)
% =======

function CR = findCovarianceMatrix(I, positionX, positionY, size)

    % First and second derivatives
    d = [-1 0 1];
    dd = [-1 2 -1];
    dI = double(I);

    % Ix, Iy, Ixx, Iyy
    Ix = conv2(d, dI);
    Iy = conv2(d,1,dI);
    Ixx = conv2(dd, dI);
    Iyy = conv2(dd,1,dI);

    for j = 1:size
        for i = 1:size

            f(i+(j-1)*size,:) = [positionX+i-1 positionY+j-1 dI(positionY+j-1, positionX+i-1) ...
                         Ix(positionY+j-1, positionX+i-1) Iy(positionY+j-1, positionX+i-1) ...
                        Ixx(positionY+j-1, positionX+i-1) Iyy(positionY+j-1,positionX+i-1)];

        end
    end

    % vector of means of features in region R
    uR = mean(f);

    T = zeros(7);
    for k = 1:size^2
        temp = (f(k,:)-uR)'*(f(k,:)-uR);
        T = T + temp;
    end

    CR = (1/size^2)*T;


% DEMO  --  Chebyshev Polynomial Integral
% UPDATED  --  October 28, 2013
% Written by Matthew Kelly, Cornell University
%
% This demo is designed to show how to compute integrals of chebyshev
% polynomials.
%
% Results - Note that there are relative large errors between the chebyshev
% approximation of the integral and the analytic integral, BUT the last
% value (which is the value of the definite integral) is very accurate.
%
% The last point is so accurate because it is the solution to the definite
% integral over the domain using Clenshaw-Curtis quadrature for an order
% "order" accurate polynomial.
%

%% General Settings
clear; clc;

%What order should the approximation be?
order = 50;

%What domain should we be looking at?
d = [0,1];

%Get chebyshev values at the nodes:
[x,w] = chebyshevPoints(order+1,d);
[~,f] = testFunction(x);
If= chebyshevIntegral(f,d);

%Set the time vector
time = linspace(d(1),d(2),10000);
[Ig, g] = testFunction(time);

%Adjust the constants of integration S.T. they all start at zero:
Ig = Ig-Ig(1);

%Interpolate the chebyshev approximations:
y = chebyshevInterpolate(f,time,d);
Iy = chebyshevInterpolate(If,time,d);

%Show results
figure(407); clf;
subplot(2,2,1); hold on;
    plot(time,g,'b-','LineWidth',2)
    plot(x,f,'ko','MarkerSize',5);
    title(['function approximation - order ' num2str(order)]);
    legend('Analytic','Cheb. nodes','Location','NorthWest')
subplot(2,2,3); hold on;
    plot(time,Ig,'b-','LineWidth',2)
    plot(x,If,'ko','MarkerSize',5);
    title(['integral approximation - order ' num2str(order)]);
        legend('Analytic','Cheb. nodes','Location','NorthWest')
subplot(2,2,2);
    semilogy(time,abs(g-y))
    title('error in function');
subplot(2,2,4); hold on;
    semilogy(time,abs(Ig-Iy))
    semilogy(time(end),abs(Ig(end)-Iy(end)),...
        'bo','MarkerSize',10,'LineWidth',2)
    title(['error in definite integral: ' num2str(Ig(end)-Iy(end))]);
    set(gca,'yscale','log')


function prob_test132 ( )

%*****************************************************************************80
%
%% TEST132 tests RAYLEIGH_MEAN, RAYLEIGH_SAMPLE, RAYLEIGH_VARIANCE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  nsample = 1000;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST132\n' );
  fprintf ( 1, '  For the Rayleigh PDF:\n' );
  fprintf ( 1, '  RAYLEIGH_MEAN computes the mean;\n' );
  fprintf ( 1, '  RAYLEIGH_SAMPLE samples;\n' );
  fprintf ( 1, '  RAYLEIGH_VARIANCE computes the variance.\n' );

  a = 2.0;

  check = rayleigh_check ( a );

  if ( ~check );
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TEST132 - Fatal error!\n' );
    fprintf ( 1, '  The parameters are not legal.\n' );
    return
  end

  mean = rayleigh_mean ( a );
  variance = rayleigh_variance ( a );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  PDF parameter A =             %14f\n', a );
  fprintf ( 1, '  PDF mean =                    %14f\n', mean );
  fprintf ( 1, '  PDF variance =                %14f\n', variance );

  for i = 1 : nsample
    [ x(i), seed ] = rayleigh_sample ( a, seed );
  end

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

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Sample size =     %6d\n', nsample );
  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 [ pn, dist ] = circle_arc_point_near_2d ( r, center, theta1, ...
  theta2, p )

%*****************************************************************************80
%
%% CIRCLE_ARC_POINT_NEAR_2D : nearest point on a circular arc.
%
%  Discussion:
%
%    A circular arc is defined by the portion of a circle (R,C)
%    between two angles (THETA1,THETA2).
%
%    Thus, a point (X,Y) on a circular arc satisfies
%
%      ( X - C(1) ) * ( X - C(1) ) + ( Y - C(2) ) * ( Y - C(2) ) = R * R
%
%    and
%
%      Theta1 <= Theta <= Theta2
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 January 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real R, the radius of the circle.
%
%    Input, real CENTER(2,1), the center of the circle.
%
%    Input, real THETA1, THETA2, the angles defining the arc,
%    in radians.  Normally, THETA1 < THETA2.
%
%    Input, real P(2,1), the point to be checked.
%
%    Output, real PN(2,1), a point on the circular arc which is
%    nearest to the point.
%
%    Output, real DIST, the distance to the nearest point.
%
  dim_num = 2;
%
%  Special case, the zero circle.
%
  if ( r == 0.0 )
    pn(1:dim_num,1) = center(1:dim_num,1);
    dist = sqrt ( sum ( ( p(1:dim_num,1) - pn(1:dim_num,1) ).^2 ) );
    return
  end
%
%  Determine the angle made by the point.
%
  theta = atan4 ( p(2,1) - center(2,1), p(1,1) - center(1,1) );
%
%  If the angle is between THETA1 and THETA2, then you can
%  simply project the point onto the arc.
%
  if ( r8_modp ( theta  - theta1,  2.0 * pi ) <= ...
       r8_modp ( theta2 - theta1,  2.0 * pi ) )

    r2 = sqrt ( sum ( ( p(1:dim_num,1) - center(1:dim_num,1) ).^2 ) );

    pn(1:dim_num,1) = center(1:dim_num,1) ...
      + ( p(1:dim_num,1) - center(1:dim_num,1) ) * r / r2;
%
%  Otherwise, if the angle is less than the negative of the
%  average of THETA1 and THETA2, it's on the side of the arc
%  where the endpoint associated with THETA2 is closest.
%
  elseif ( r8_modp ( theta - 0.5 * ( theta1 + theta2 ), 2.0 * pi ) <= pi )

    pn(1:dim_num,1) = center(1:dim_num,1) + r * [ cos ( theta2 ), sin ( theta2 ) ];
%
%  Otherwise, the endpoint associated with THETA1 is closest.
  else

    pn(1:dim_num,1) = center(1:dim_num,1) + r * [ cos ( theta1 ), sin ( theta1 ) ];

  end

  dist = sqrt ( sum ( ( p(1:dim_num,1) - pn(1:dim_num,1) ).^2 ) );

  return
end


function [X_poly] = polyFeatures(X, p)
%POLYFEATURES Maps X (1D vector) into the p-th power
%   [X_poly] = POLYFEATURES(X, p) takes a data matrix X (size m x 1) and
%   maps each example into its polynomial features where
%   X_poly(i, :) = [X(i) X(i).^2 X(i).^3 ...  X(i).^p];
%

% You need to return the following variables correctly.
X_poly = zeros(numel(X), p);

% ====================== YOUR CODE HERE ======================
% Instructions: Given a vector X, return a matrix X_poly where the p-th
%               column of X contains the values of X to the p-th power.
%
%
for i=1:p
	X_poly(:,i)=X .^ i;
end

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

end