function vEst=RROnlyStaticVelEst(rr,xTx,xRx,zTar)
%%RRONLYSTATICVELEST Perform unweighted least-squares estimation of a
%           target velocity vector given only range rate measurements from
%           different bistatic channels (or from multiple receivers if the
%           target is the transmitter). This function can work in 2D and
%           3D space and it will produce a least-squares estimate when more
%           than the minimum number of range rate measurements needed is
%           given. This uses a non-relativistic model for the range rates
%           and ignores possible atmospheric effects.
%
%INPUTS: rr A numMeasX1 or 1XnumMeas vector of range rates. numMeas>=3 for
%           the velocity vector to be observable in 3D and numMeas>=2 in
%           2D.
%       xTx A 6XnumMeas matrix (in 3D or a 4XnumMeas matrix in 2D) of
%           stacked transmitter position and velocity vectors. If the
%           target is the transmitter, then an empty matrix should be
%           passed. xTx(:,n)=[x;y;z;xDot;yDot;zDot] is the state (3
%           position and 3 velocity components) corresponding to the nth
%           range rate measurement. If all of the transmitters are in the
%           same spot, then a single 6X1 or 4X1 vector can be passed.
%       xRx A 6XnumMeas matrix (in 3D or a 4XnumMeas matrix in 2D) of
%           stacked transmitter position and velocity vectors.
%           states1(:,n)=[x;y;z;xDot;yDot;zDot] is the state (3 position
%           and 3 velocity components) corresponding to the nth range rate
%           measurement. If all of the receivers are in the same spot, then
%           a single 6X1 or 4X1 vector can be passed.
%      zTar The 3X1 (in 3D) or 2X1 (in 2D) Cartesian position of the
%           target.
%
%OUTPUTS: vEst The 3X1 (in 3D) or 2X1 (in 2D) least squares Cartesian
%              velocity estimate of the target.
%
%This implements the least squares velocity vector estimation procedure of
%Equation 41 in Section IV E of [1]. The case of the transmitter being the
%target is handled specially (to get rid fo the singulaity).
%
%EXAMPLE 1:
%This is just a simple example showing that for three range rate values,
%one can get back the orignal velocity value in 3D.
% zTar=[0;40e3;40e3];
% vTar=[400;-200;100];
% xTx1=[100;10e3;3e3;50;50;-50];
% xTx2=[0;0;0;0;0;-20];
% xTx3=[10e3;10e3;3e3;100;-100;100];
%
% xRx1=[-10e3;0;3e3;100;100;100];
% xRx2=[0;10e3;30;-80;-200;-20];
% xRx3=xRx2;
%
% states1=[xTx1,xTx2,xTx3];
% states2=[xRx1,xRx2,xRx3];
%
% xTar=[zTar;vTar];
%
% useHalfRange=false;
% rr=zeros(3,1);
% rr(1)=getRangeRate(xTar,useHalfRange,xTx1,xRx1);
% rr(2)=getRangeRate(xTar,useHalfRange,xTx2,xRx2);
% rr(3)=getRangeRate(xTar,useHalfRange,xTx3,xRx3);
%
% vEst=RROnlyStaticVelEst(rr,states1,states2,zTar)
%One will see that vEst=vzTar.
%
%EXAMPLE 2:
%This is the same as example 1, except the demonstration is now occurring
%in 2D.
% zTar=[0;40e3];
% vTar=[400;-200];
% xTx1=[100;10e3;50;50];
% xTx2=[0;0;0;0];
% xTx3=[10e3;10e3;100;-100];
%
% xRx1=[-10e3;0;100;100];
% xRx2=[0;10e3;-80;-200];
% xRx3=xRx2;
%
% states1=[xTx1,xTx2,xTx3];
% states2=[xRx1,xRx2,xRx3];
%
% xTar=[zTar;vTar];
%
% useHalfRange=false;
% rr=zeros(3,1);
% rr(1)=getRangeRate(xTar,useHalfRange,xTx1,xRx1);
% rr(2)=getRangeRate(xTar,useHalfRange,xTx2,xRx2);
% rr(3)=getRangeRate(xTar,useHalfRange,xTx3,xRx3);
%
% vEst=RROnlyStaticVelEst(rr,states1,states2,zTar)
%One will see that vEst=vzTar.
%
%EXAMPLE 3:
%In this example, the range rate is one-way from the target (e.g. the
%target is an emitter). The relative error of the velocity estimate with
%the truth (noiseless scenario) is within finite precision limits.
% zTar=randn(3,1);
% vTar=randn(3,1);
% numMeas=5;
% xRx=randn(6,numMeas);
% xTar=[zTar;vTar];
% useHalfRange=false;
% rr=zeros(numMeas,1);
% for k=1:numMeas
%     rr(k)=getRangeRate(xTar,useHalfRange,xTar,xRx(:,k));
% end
% vEst=RROnlyStaticVelEst(rr,[],xRx,zTar);
% RelErr=max(abs((vTar-vEst)./vTar))
%
%REFERENCES:
%[1] David F. Crouse , "Basic tracking using nonlinear 3D monostatic and
%    bistatic measurements," IEEE Aerospace and Electronic Systems
%    Magazine, vol. 29, no. 8, Part II, pp. 4-53, Aug. 2014.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

numMeas=length(rr);
rr=rr(:);

if(size(xTx,2)==1)
    xTx=repmat(xTx,1,numMeas);
end

if(size(xRx,2)==1)
    xRx=repmat(xRx,1,numMeas);
end

posDim=length(zTar);

zRx=xRx(1:posDim,:);
vRx=xRx((posDim+1):(2*posDim),:);

h=bsxfun(@minus,zTar,zRx);
hNorm=sqrt(sum(h.*h,1));
h=bsxfun(@rdivide,h,hNorm);

if(~isempty(xTx))
    %The target is not the transmitter.
    zTx=xTx(1:posDim,:);
    vTx=xTx((posDim+1):(2*posDim),:);
    hi=bsxfun(@minus,zTar,zTx);
    hiNorm=sqrt(sum(hi.*hi,1));
    hi=bsxfun(@rdivide,hi,hiNorm);

    rDotB=rr+sum(h.*vRx,1)'+sum(hi.*vTx,1)';
    Hv=bsxfun(@plus,h',hi');
else
    %The target is the transmitter.
    rDotB=rr+sum(h.*vRx,1)';
    Hv=h';
end

vEst=lsqminnorm(Hv,rDotB);

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.


% feldkamp_example.m
% example of how to use feldkamp.m for cone-beam CT reconstruction
% Copyright 2004-8-28, Nicole Caparanis, Patty Laskowsky, Taka Masuda,
% and Jeff Fessler, The University of Michigan

if ~isvar('proj'), disp 'proj'
	down = 8;
	nh = 256/down;
	nv = 240/down;
	na = 224/down;
	ds = 1024/nh;
	dt = ds;
	dis_src_det = 949.075;
	dis_iso_det = 408.075;
	dis_src_iso = dis_src_det - dis_iso_det;
%	dis_foc_src = inf; % flat detector panel
	dis_foc_src = 0; % arc detector
	offset_det_h = 0.25; % quarter detector
	offset_det_v = 0.0;
	horiz = ([-(nh-1)/2:(nh-1)/2]' - offset_det_h) * ds;
	verti = ([-(nv-1)/2:(nv-1)/2]' - offset_det_v) * dt;
	printf('rmax=%g', dis_src_iso*sin(atan(max(abs(horiz)) / dis_src_det)))

	ell = [ ...
		[20 0 0  150 150 200 0 0.01]; % 30cm diam "cylinder"
		[80 0 0 50 50 30  0 0.01]; % bone-like inserts
		[0 0 75  40 40 40  0 0.01];
		[0 70 0  30 30 30  0 0.01];
	];

	proj = ellipsoid_proj(ell, horiz, verti, na, ...
		dis_src_iso, dis_iso_det, dis_foc_src);

	nx = 256/down;
	ny = 240/down;
	nz = 200/down;
	dx = 2*down; dy = dx; dz = dx; % 2mm voxels * down-sampling
	xtrue = ellipsoids(nx, ny, nz, ell, dx, dy, dz);

	% cone-beam system geometry, generalized from fan-beam geometry.
	% see ASPIRE users guide under tech. reports on web page for details.
	args = arg_pair('system', NaN, 'nx', nx, 'ny', ny, 'nz', nz, ...
		'nv', nv, 'nh', nh, 'na', na, 'support', 'all', ...
		'orbit', 360, 'orbit_start', 0, ...
		'pixel_size', dx, 'ray_spacing', ds, 'strip_width', 0, ...
		'dis_src_det', dis_src_det, ...
		'dis_iso_det', dis_iso_det, ...
		'dis_foc_src', dis_foc_src, ...
		'offset_source', 0, ...
		'offset_det_h', offset_det_h, ...
		'offset_det_v', offset_det_v);

	pl=330;
	im(pl+1, xtrue, 'x true'), cbar
	im(pl+4, proj, 'true projections'), cbar
	drawnow
prompt
end

% noisy data and estimated line integrals
if ~isvar('li_hat'), disp 'li_hat'
	% noisy data, if blank scan value has been specified.
	if isvar('bi') && isvar('ri')
		yb = bi .* exp(-proj) + ri;
		yi = poisson(yb);
		li_hat = -log((yi-ri) ./ bi);
		li_hat(yi-ri <= 0) = 0; % fix: need something better here...
	else
		li_hat = proj; % noiseless
	end
end

% FDK cone-beam reconstruction
if ~isvar('xfdk'), disp 'fdk'
	mask = true([nx ny nz]);
	xfdk = feldkamp_old(li_hat, 'ramp', mask, args);
end

if 1
	% show results (off-center slices worse than central slice)
	im(pl+2, xfdk, 'FDK recon'), cbar
	im(pl+3, xfdk - xtrue, 'FDK error'), cbar

	subplot(pl+5)
	ix = 1:nx; iy = ceil(ny/2); iz = ceil(nz/2);
	plot(ix, xtrue(ix,iy,iz), '-', ix, xfdk(ix,iy,iz), '--')
	axis([1 nx -0.005 0.025]), legend('true', 'FDK recon', 2)
	title 'middle slice', xlabel 'ix'

	subplot(pl+6)
	iz=1:nz; ix = 1+floor(nx/2); iy = 1+floor(ny/2);
	plot(iz, squeeze(xtrue(ix,iy,iz)), '-', iz, squeeze(xfdk(ix,iy,iz)), '--')
	axis([1 nz -0.005 0.025]), legend('true', 'FDK recon', 2), xlabel 'iz'
	title(sprintf('profile at (ix,iy)=(%g,%g)', ix,iy))

prompt
end


%% CUBEAFEMQUADCURL quad curl equations on the unit cube
%
%   CUBEAFEMQUADCURL computes ND0-(CR-P0)-ND0 nonconforming approximations
%   of the quad curl equations in the unit cube. The mesh is refined
%   adaptively guided by a residual-based estimator.
%
% Copyright (C)  Long Chen. See COPYRIGHT.txt for details.

close all; clear;

%% Set up
maxN = 5e5;
maxIt = 20;
theta = 0.3;
N = zeros(maxIt,1);
h = zeros(maxIt,1);
erru = zeros(maxIt,1);
errw = zeros(maxIt,1);
errphi = zeros(maxIt,1);
etaTotal = zeros(maxIt,1);
etaw = zeros(maxIt,1);
etaphi = zeros(maxIt,1);
etau = zeros(maxIt,1);

%% Generate initial mesh
H = pi;
[node,elem,HB] = cubemesh([0,H,0,H,0,H],H/2);
bdFlag = setboundary3(node,elem,'Dirichlet');

%% PDE and options
pde = quadCurlDataSmooth1;

option.solver = 'direct';
option.printlevel = 0;

%% Finite Element Method
for k = 1:maxIt

    %% SOLVE
    [soln,eqn] = quadcurl3NC(node,elem,bdFlag,pde,option);
    N(k) = 2*length(soln.u)+3*length(soln.phi);
    %% ESTIMATE
    [erru(k), errK] = getHcurlerror3ND(node,elem,pde.curlu,soln.u);
    [etaK, est] = estimatequadcurl(node,elem,soln,pde,option);
    etaTotal(k) = sqrt(sum(etaK.^2));
    etaw(k) = sqrt(sum((est.eta1).^2));
    etaphi(k) = sqrt(sum((est.eta2).^2));
    etau(k) = sqrt(sum((est.eta3).^2));
    fprintf("Iter %2d, # Dof= %6d; \n||curl(u-u_h)|| = %8.6g,eta = %8.6g \n \n",...
           k, N(k), erru(k), etaTotal(k))
    %% Visualize
    figure(1);
    set(gcf, 'Position', [100 600 1000 300])
    subplot(1,3,1)
    showboundary3(node,elem,'z<pi/2','facealpha',0.5);

    subplot(1,3,2)
    errorE2V = accumarray(elem(:),repmat(errK,[4,1]),[size(node,1), 1]);
    showsolution3(node,elem,errorE2V,'z<pi/2 ','EdgeColor','k');

    subplot(1,3,3)
    etaE2V = accumarray(elem(:),repmat(etaK,[4,1]),[size(node,1), 1]);
    showsolution3(node,elem,etaE2V,'z<pi/2 ','EdgeColor','k');

    drawnow;
    %% MARK and REFINE
    if N(k) > maxN
        break;
    end

    markedElem = mark(elem,etaK,theta);
    if k < maxIt
        [node,elem,bdFlag,HB] = bisect3(node,elem,markedElem,bdFlag,HB);
        [elem,bdFlag] = sortelem3(elem,bdFlag);
    end

end

%%
close all;
figure(1);
r1 = showrate(N,erru,10,'r-o');
r2 = showrate(N,etaTotal,10,'b-*');
r3 = showrate(N,etaw,10,'color',[0.2, 0.8,0.7],'marker','*');
r4 = showrate(N,etaphi,10,'color',[0.2, 0.8,0.7],'marker','*');
r5 = showrate(N,etau,10,'color',[0.2, 0.8,0.7],'marker','*');
set(gca,'xscale','log','yscale','log');
T = title('Convergence');
XL = xlabel('$\#$ DoFs');
YL = ylabel('Error Magnitudes');
L = legend('$\Vert  \nabla\times(u-u_h)\Vert$', ...
    ['$N^{' num2str(r1) '}$'],...
    '$\eta(w_h,\phi_h,u_h)$', ...
    ['$N^{' num2str(r2) '}$'],...
    '$\eta_1(w_h)$', ['$N^{' num2str(r3) '}$'],...
    '$\eta_2(\phi_h)$', ['$N^{' num2str(r4) '}$'],...
    '$\eta_3(u_h)$', ['$N^{' num2str(r5) '}$'],...
    'LOCATION','Best');
set([XL,YL,L],'Interpreter','latex','FontSize', 12);
set(T,'Interpreter','latex','FontSize',16);
grid on;
set(gcf,'color','w','Position', [100 200 350 500])
drawnow;

%% sanity check
if N<5e3
    figure(2);
    subplot(1,2,1);
    option.scale = 2;
    option.plot = 'quiver3';
    center = (node(elem(:,1),:) + node(elem(:,2),:) + ...
        node(elem(:,3),:) + node(elem(:,4),:))/4;
    showvector3(center,pde.curlu(center),option);
    subplot(1,2,2);
    curluh = curlu3(node,elem,soln.u);
    showvector3(center,curluh,option);
    % showvector3(center,curlwh_comp2,option);

    set(gcf,'color','w','Position', [500 600 750 300])
    drawnow;
end


function [F,sA] = spm_multinomial_log_evidence(qA,pA,rA)
% Bayesian model reduction for multinomial distibutions
% FORMAT [F,sA] = spm_multinomial_log_evidence(qA,pA,rA)
%
% qA  - parameter of posterior of full model
% pA  - parameter of prior of full model
% rA  - parameter of prior of reduced model
%
%
% F   - (negative) free energy or log evidence of reduced model
% sA  - parameter of reduced posterior
%
% This routine computes the negative log evidence of a reduced model of a
% mutinomial distribution. This also applies for Bernoulli, Binomial, and
% Categorical distributions.
%__________________________________________________________________________
% Copyright (C) 2005 Wellcome Trust Centre for Neuroimaging

% Thomas Parr
% $Id: spm_multinomial_log_evidence.m 7679 2019-10-24 15:54:07Z spm $

% reduced posteriors
%--------------------------------------------------------------------------
sA = spm_softmax(qA + rA - pA);

% change in free energy or log model evidence
%--------------------------------------------------------------------------
k = find(rA);
F  = log(rA(k(1))) - log(pA(k(1))) + log(qA(k(1))) - log(sA(k(1)));


function [type,center,theta,foci,ecc,normform] = conic_properties(A,B,C,D,E,F)

[type,x0,y0,theta,A2,B2,C2,D2,E2,F2] = conic_standard_form(A,B,C,D,E,F);

% change from translation followed by rotation to rotation followed by
% translation
% x <- (x + u)*R
% = x*R + u*R
R = [cos(theta),sin(theta);-sin(theta),cos(theta)];
center = [x0,y0]*R;
normform = [A2,B2,C2,D2,E2,F2];

if strcmpi(type,'circle')
  foci = [0,0];
  ecc = 0;
elseif strcmpi(type,'ellipse') || strcmpi(type,'hyperbola'),
  a2 = 1 / min(abs(A2),abs(C2));
  b2 = 1 / max(abs(A2),abs(C2));
  c2 = sqrt(a2 - b2);
  if A2 < C2,
    foci = [-c2,0;c2,0];
  else
    foci = [0,-c2;0,c2];
  end
  ecc = c2 / a2;
elseif strcmpi(type,'parabola'),
  if abs(A2) > abs(C2),
    foci = [0,-E2/4];
  else
    foci = [-D2/4,0];
  end
  ecc = 1;
else % other
  foci = zeros(0,2);
  ecc = 0;
end

nfoci = size(foci,2);
if nfoci > 0,
  % translate
  foci(:,1) = foci(:,1) + x0;
  foci(:,2) = foci(:,2) + y0;
  % rotate
  foci = foci*R;
end


function [P orderstruct] = mmtimes(varargin)
% P = mmtimes(M1, M2, ... Mn)
%   return a chain matrix product P = M1*M2* ... *Mn
%
% {Mi} are matrices with compatible dimension: size(Mi,2) = size(Mi+1,1)
%
% Because the matrix multiplication is associative; the chain product can
% be carried out with different order, leading to the same result (up to
% round-off error). MMTIMES uses "optimal" order of binary product to
% reduce the computational effort (probably the accuracy is also improved).
%
% The function assumes the cost of the product of (m x n) with (n x p)
% matrices is (m*n*p). This assumption is typically true for full matrix.
%
% Notes:
%   Scalar matrix are groupped together, and the rest will be
%   multiplied with optimal order.
%
%   To get the the structure that stores the best order, call with the
%   second outputs:
%   >> [P orderstruct] = mmtimes(M1, M2, ... Mn);
%   % This structure can be used later if the input matrices have the
%   % same sizes as those in the first call (but with different contents)
%   >> P = mmtimes(M1, M2, ... Mn, orderstruct);
%
% See also: mtimes
%
% Author: Bruno Luong <brunoluong@yahoo.com>
% Orginal: 19-Jun-2010
%          20-Jun-2010: quicker top-down algorithm
%          23-Jun-2010: treat the case of scalars
%          16-Aug-2010: passing optimal order as output/input argument

Matrices = varargin;

buildexpr = false;
if ~isempty(Matrices) && isstruct(Matrices{end})
    orderstruct = Matrices{end};
    Matrices(end) = [];
else
    % Detect scalars
    iscst = cellfun('length',Matrices) == 1;
    if any(iscst)
        % scalars are multiplied apart
        cst = prod([Matrices{iscst}]);
        Matrices = Matrices(~iscst);
    else
        cst = 1;
    end
    % Size of matrices
    szmats = [cellfun('size',Matrices,1) size(Matrices{end},2)];
    s = MatrixChainOrder(szmats);

    orderstruct = struct('cst', cst, ...
                         's', s, ...
                         'szmats', szmats);

    if nargout>=2
        % Prepare to build the string expression
        vnames = arrayfun(@inputname, 1:nargin, 'UniformOutput', false);
        % Default names, e.g., M1, M2, ..., for inputs that is not single variable
        noname = cellfun('isempty', vnames);
        vnames(noname) = arrayfun(@(i) sprintf('M%d', i), find(noname), 'UniformOutput', false);
        if any(iscst)
            % String '(M1*M2*...)' for constants
            cstexpr = strcat(vnames(iscst),'*');
            cstexpr = strcat(cstexpr{:});
            cstexpr = ['(' cstexpr(1:end-1) ')'];
        else
            cstexpr = '';
        end
        vnames = vnames(~iscst);
        buildexpr = true;
    end
end

if ~isempty(Matrices)
    P = ProdEngine(1,length(Matrices),orderstruct.s,Matrices);
    if orderstruct.cst~=1
        P = orderstruct.cst*P;
    end
    if buildexpr
        expr = Prodexpr(1,length(Matrices),orderstruct.s,vnames);
        if ~isempty(cstexpr)
            % Concatenate the constant expression in front
            expr = [cstexpr '*' expr];
        end
        orderstruct.expr = expr;
    end
else
    P = orderstruct.cst;
    if nargout>=2
        orderstruct.expr = cstexpr;
    end
end

end % mmtimes

%%
function [s qmin] = MatrixChainOrder(szmats)
% Find the best ordered chain-product, the best splitting index
% of M(i)*...*M(j) is stored in s(j,i) of the array s (only the lower
% part is filled)
% Top-down dynamic programming, complexity O(n^3)

n = length(szmats)-1;
s = zeros(n);

pk = szmats(2:n);
ij = (0:n-1)*(n+1)+1;
left = zeros(1,n-1);
right = zeros(1,n-1);
L = 1;
while true % off-diagonal offset
    q = zeros(size(pk));
    for j=1:n-L % this is faster and BSXFUN or product with DIAGONAL matrix
        q(:,j) = (szmats(j)*szmats(j+L+1))*pk(:,j);
    end
    q = q + left + right;
    [qmin loc] = min(q, [], 1);
    s(ij(1:end-L)+L) = (1:n-L)+loc;

    if L<n-1
        pk = [pk(:,1:end-1);
              pk(end,2:end)];
        left = [left(:,1:end-1);
                qmin(1:end-1)];
        right = [qmin(2:end);
                 right(:,2:end)];
        L = L+1;
    else
        break
    end % if
end % while-loop

end % MatrixChainOrder

%%
function P = ProdEngine(i,j,s,Matrices)
% Perform matrix product from the optimal order, recursive engine
if i==j
    P = Matrices{i};
else
    k = s(j,i);
    P = ProdEngine(i,k-1,s,Matrices)*ProdEngine(k,j,s,Matrices);
end

end

%%
function expr = Prodexpr(i,j,s,vnames)
% Return the string expression of the optimal order
if i==j
    expr = vnames{i};
else
    k = s(j,i);
    expr = ['(' Prodexpr(i,k-1,s,vnames) '*' Prodexpr(k,j,s,vnames) ')'];
end

end % Prodexpr


function alpha = neper2db(alpha, y)
%NEPER2DB   Convert nepers to decibels.
%
% DESCRIPTION:
%       neper2db converts an attenuation coefficient in units of
%       Nepers/((rad/s)^y m)to units of dB/(MHz^y cm).
%
% USAGE:
%       alpha = neper2db(alpha)
%       alpha = neper2db(alpha, y)
%
% INPUTS:
%       alpha       - attenuation in Nepers/((rad/s)^y m)
%
% OPTIONAL INPUTS:
%       y           - power law exponent (default = 1)
%
% OUTPUTS:
%       alpha       - attenuation in dB/(MHz^y cm)
%
% ABOUT:
%       author      - Bradley Treeby
%       date        - 3rd December 2009
%       last update - 5th December 2011
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox
%
% See also db2neper

% This file is part of k-Wave. k-Wave is free software: you can
% redistribute it and/or modify it under the terms of the GNU Lesser
% General Public License as published by the Free Software Foundation,
% either version 3 of the License, or (at your option) any later version.
%
% k-Wave is distributed in the hope that it will be useful, but WITHOUT ANY
% WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for
% more details.
%
% You should have received a copy of the GNU Lesser General Public License
% along with k-Wave. If not, see <http://www.gnu.org/licenses/>.

if nargin == 1
    y = 1;
end

alpha = 20*log10(exp(1))*alpha*(2*pi/1e-6)^y/100;


%The aim of this simple benchmark is to illustrate the interest of restarting
%Nelder-Mead locally, from the last solution found, until no improvement is
%reached (to a given accuracy).
%Moreover, it shows that fminsearch has great difficulties at minimizing
%the most simple, smooth quadractic, objective function used. But
%restarting it locally corrects that.
%On the other hand, Nick Higham implementation of Nelder-Mead works fine,
%and the accuracy reached is also further improved by restarting it
%locally. Note that it may still happen that fminsearch performs better on
%other problems.

%Anyhow in theory, amongst direct search methods, one should not use even
%the restarted NM but rather MADS (C. Audet, J.E. Dennis, S. Le Digabel), which has
%guaranteed convergence even on non-smooth Clarke subdifferentiable objective functions.
%The restarted NM will also lead in practice to locally optimal solutions,
%although this is not theoretically guaranteed. It may fail in very
%particular of difficult situations. The reason for its good convergence
%properties in practice is that restarting it regenerates its search
%simplex and in the end many search directions are covered, which is a crude
%alternative to the POLL step of MADS (which is the step ensuring
%convergence). So the restarted NM will perform well even on non-smooth or
%discontinuous objective functions (not illustrated with this benchmark,
%other benchmarks are available on http://arxiv.org/abs/1104.5369).
%We put forward the restarted NM since it is easily available, and simple
%to use and will already work well enough in practice. But again, in
%theory, MADS should be used instead (to get a convergence certificate).

%For related works on optimization in systems and control, see the Matlab
%files http://www.mathworks.com/matlabcentral/fileexchange/33022 and
%http://www.mathworks.com/matlabcentral/fileexchange/33219 (and hyperlinked papers).

%Emile Simon, 18th october 2011

clear all
close all
clc

%Choice of the accuracies
precf = 10^-4;%'Objective' accuracy
precx = 10^-4;%'Simplex' accuracy
precs = 10^-4;%'Restarting' accuracy
Nmax = 20;%Number of variables tried% To be changed.
ntests = 10;%Number of tests for each N
disp = 0; %Turn to 1 to display the detail of the iterations of Nelder-Mead
if(disp==1) Disp = 'iter'; else Disp = 'off'; end

Objectif = @(x)sum(x.^2);%The objective function to be minimized, smooth quadratic. Perhaps the simplest objective function possible.
%func='sq';

options = optimset('TolF',precf,'TolX',precx,'MaxFunEvals',inf,'MaxIter',inf,'Display','off');%,'GradObj','on');%,'TolCon',precx

for s=2:Nmax
    for i =1:ntests

        x0=randn(s,1)./rand(s,1);%Difficult starting point

        tic
        [Sol1,Obj1(s,i)] = fminsearch(Objectif,x0,optimset('TolF',precf,'TolX',precx,'MaxFunEvals',inf,'MaxIter',inf,'Display',Disp));
        T1(s,i) = toc;

        acc = 1; Obj2(s,i) = Obj1(s,i); Sol2=Sol1;
        while (acc>precs)
            Objp(s,i) = Obj2(s,i);
            [Sol2,Obj2(s,i)] = fminsearch(Objectif,Sol2,optimset('TolF',precf,'TolX',precx,'MaxFunEvals',inf,'MaxIter',inf,'Display',Disp));
            acc = abs(abs(Objp(s,i)/Obj2(s,i))-1);
        end
        T2(s,i) = toc;

        %Nick Higham's implementation of Nelder-Mead
        tic
        [Sol3,Obj3(s,i),N3(s,i)]=nmsmax(@(x)-sum(x.^2),x0,[precx inf inf 0 disp]);
        T3(s,i) = toc;
        Obj3(s,i) = -Obj3(s,i);

        acc = 1; Obj4(s,i) = Obj3(s,i); Sol4=Sol3;
        while (acc>precs)
            Objp = Obj4(s,i);
            [Sol4,Obj4(s,i)] = nmsmax(@(x)-sum(x.^2),Sol4,[precx inf inf 0 disp]);
            T4(s,i)=toc;
            Obj4(s,i) = -Obj4(s,i);
            acc = Objp/Obj4(s,i)-1;
        end

    end
    s
end

figure
subplot(2,1,1)
semilogy(mean(Obj1'))
hold on
errorbar(mean(Obj1'),std(Obj1'))
errorbar(mean(Obj2'),std(Obj2'),'g')
errorbar(mean(Obj3'),std(Obj3'),'r')
errorbar(mean(Obj4'),std(Obj4'),'c')
legend('fminsearch','fminsearch','restarted fminsearch','nmsmax','restared nmsmax','Location','NorthWest')
title('Performance comparison of different versions of Nelder-Mead on \bf{min \Sigma_{i=1}^nx_i^2}')
ylabel('Avg. objective value')
subplot(2,1,2)
semilogy(mean(T1'))
hold on
errorbar(mean(T1'),std(T1'))
errorbar(mean(T2'),std(T2'),'g')
errorbar(mean(T3'),std(T3'),'r')
errorbar(mean(T4'),std(T4'),'c')
ylabel('Avg. computational time required (s)')
xlabel('Number of variables (n)')


function [ a, seed ] = d3_uniform ( n, seed )

%*****************************************************************************80
%
%% D3_UNIFORM randomizes a D3 matrix.
%
%  Discussion:
%
%    The D3 storage format is used for a tridiagonal matrix.
%    The superdiagonal is stored in entries (1,2:N), the diagonal in
%    entries (2,1:N), and the subdiagonal in (3,1:N-1).  Thus, the
%    original matrix is "collapsed" vertically into the array.
%
%  Example:
%
%    Here is how a D3 matrix of order 5 would be stored:
%
%       *  A12 A23 A34 A45
%      A11 A22 A33 A44 A55
%      A21 A32 A43 A54  *
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the linear system.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real A(3,N), the D3 matrix.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  a(1,1) = 0.0;
  [ a(1,2:n), seed ] = r8vec_uniform ( n-1, 0.0, 1.0, seed );

  [ a(2,1:n), seed ] = r8vec_uniform ( n,   0.0, 1.0, seed );

  [ a(3,1:n-1), seed ] = r8vec_uniform ( n-1, 0.0, 1.0, seed );
  a(3,n) = 0.0;

  return
end


function y=dbp(x)
% dbp(x) = 10*log10(x): the dB equivalent of the power x
y = -Inf*ones(size(x));
nonzero = x~=0;
y(nonzero) = 10*log10(abs(x(nonzero)));


function [Cp1,Cp2] = Cpre_q1q1(xy,ev)
%Cpre_q1q1  generate stabilizations for least sqrs commutator for Q1-Q1
%   [Cp1,Cp2] = Cpre_q1q1(xy,ev);
%   input
%          xy         Q2 nodal coordinate vector
%          ev         element mapping matrix
%   output
%          Cp1        pressure stabilization 1 for preconditioner
%          Cp2        pressure stabilization 2 for preconditioner
%   IFISS function: HCE; 7 July 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage

nngpt=4;
x=xy(:,1); y=xy(:,2);
xp=xy(:,1); yp=xy(:,2);
nvtx=length(x); nu=2*nvtx; np=length(xp);
nel=length(ev(:,1));
%
% initialise global matrices
Cp1 = sparse(np,np);
Cp2 = sparse(np,np);
%
% Gauss point integration rules
if (nngpt==4)        % 2x2 Gauss points
   gpt=1.0e0/sqrt(3.0e0);
   s(1) = -gpt; t(1) = -gpt; wt(1)=1;
   s(2) =  gpt; t(2) = -gpt; wt(2)=1;
   s(3) =  gpt; t(3) =  gpt; wt(3)=1;
   s(4) = -gpt; t(4) =  gpt; wt(4)=1;
elseif (nngpt==1)   % 1x1 Gauss point
   s(1) =    0; t(1) =    0; wt(1)=4;
else
   error('Check Gauss point integration specification')
end
%
% inner loop over elements
for ivtx = 1:4
   xl_v(:,ivtx) = x(ev(:,ivtx));
   yl_v(:,ivtx) = y(ev(:,ivtx));
end

%
% element assembly into global matrices
cpe1 = zeros(nel,4,4);
cpe2 = zeros(nel,4,4);

% loop over Gauss points
for igpt = 1:nngpt
   sigpt=s(igpt);
   tigpt=t(igpt);
   wght=wt(igpt);
   [jac,invjac,phi,dphidx,dphidy] = deriv(sigpt,tigpt,xl_v,yl_v);
   for j = 1:4
      for i = 1:4
          cpe1(:,i,j) = cpe1(:,i,j) + .25 * wght*(phi(:,i)-0.25) .*(phi(:,j)-0.25) ;
          cpe2(:,i,j) = cpe2(:,i,j) +  (16*jac(:)) .\ (wght*(phi(:,i)-0.25) .*(phi(:,j)-0.25));
      end
   end
% end of Gauss point loop
end

%%  element assembly into global matrices
for krow=1:4
   nrow=ev(:,krow);
   for kcol=1:4
      ncol=ev(:,kcol);
      Cp1 = Cp1 + sparse(nrow,ncol,cpe1(:,krow,kcol),nvtx,nvtx);
      Cp2 = Cp2 + sparse(nrow,ncol,cpe2(:,krow,kcol),nvtx,nvtx);
   end
end


%%***********************************************************
%% lmiexamp1: generate SDP data for the following LMI problem
%%
%%  max  -eta
%%  s.t. B*P + P*B'  <= 0
%%        -P         <= -I
%%         P - eta*I <= 0
%%         P(1,1)     = 1
%%***********************************************************
%% Here is an example on how to use this function to
%% find an optimal P.
%%
%% B = [-1  0  0; 5 -2  0; 1  1 -1];
%% [blk,At,C,b] = lmiexamp1(B);
%% [obj,X,y,Z] = sqlp(blk,At,C,b);
%% P = smat(blk(1,:),y);
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% Kim-Chuan Toh, Michael J. Todd, Reha H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

   function [blk,At,C,b] = lmiexamp1(B);

   n = length(B); n2 = n*(n+1)/2;
   I  = speye(n);
   z0 = sparse(n2,1);
   blktmp{1,1} = 's'; blktmp{1,2} = n;
%%
   blk{1,1} = 's'; blk{1,2} = n;
   blk{2,1} = 's'; blk{2,2} = n;
   blk{3,1} = 's'; blk{3,2} = n;
   blk{4,1} = 'u'; blk{4,2} = 1;
%%
   At{1,1} = [lmifun(B,I),     z0];
   At{2,1} = [lmifun(-I/2,I),  z0];
   At{3,1} = [lmifun(I/2,I),   svec(blktmp,-I,1)];
   At{4,1} = sparse([1, zeros(1,n2)]);
%%
   C{1,1} = sparse(n,n);
   C{2,1} = -speye(n);
   C{3,1} = sparse(n,n);
   C{4,1} = 1;
%%
   b = [zeros(n2,1); -1];
%%**********************************************************


function F=FPolarCoordTurn2D(T,x,turnType,discPoint,tauTurn,tauLinAccel)
%%FPOLARCOORDTURN2D The state transition matrix for a two-dimensional
%              coordinated turn model where the velocity is specified in
%              terms of a heading and a speed. The turn rate can be
%              specified in terms of a turn rate in radians per second, or
%              in terms of a transversal acceleration. Additionally, a
%              linear acceleration can be given. The turn rate and linear
%              acceleration can optionally have time constants associated
%              with them, like in the Singer model, modelling a tendancy to
%              eventually want to return to non-accelerating, straight-line
%              motion.
%
%INPUTS: T The time-duration of the propagation interval in seconds.
%        x The target state for 2D motion where the velocity is given in
%          terms of heading and speed components. If there is no linear
%          acceleration (acceleration along the direction of motion), then
%          x can either be x=[x;y;h;v;omega], where h is the heading in
%          terms of radians counterclockwise from the x-axis, v is the
%          speed, and omega is the turn rate (the derivative of h with
%          respect to time) or  x=[x;y;h;v;at] where at is the transversal
%          acceleration, which is orthogonal to the velocity and is defined
%          such that positive values of at map to positive values of omega.
%          If there is a linear acceleration, then the target state is
%          either x=[x;y;h;v;omega;al] where omega is the turn rate and al
%          is the linear acceleration or the target state is
%          x=[x;y;h;v;at;al] if the turn is expressed in terms of a
%          transversal acceleration. The dimensionality of the state is
%          used to determine whether a linear acceleration component is
%          present. The linear acceleration component changes the speed.
%          That means that it is the derivative of the speed.
% turnType A string specifying whether the turn is given in terms of a
%          turn rate in radians per second or a transversal acceleration in
%          m/s^2. Possible values are
%          'TurnRate'   The turn is specified in terms of a turn rate
%                       (The default if this parameter is omitted).
%          'TransAccel' The turn is specified in terms of a transversal
%                       acceleration.
% discPoint This optional parameter specified what value of the turn rate
%          is used for the discretized state prediction. The three possible
%          values were suggested in Li's paper, [3]. Possible values are:
%          0 (The default if omitted) use omega=x(5), or the equivalent
%            value when specifying the turn rate using a transverse
%            acceleration, from the non-predicted target state for
%            building the state transition matrix. \omega_k
%          1 Use the average value of the predicted omega (or the average
%            value of the transverse acceleration) over the interval T
%            for building the state transition matrix. \bar{\omega}
%          2 Use the approximate average value of the predicted omega
%            over the interval T for building the state transition
%            matrix. \bar{\omega} This is the suggestion of using half
%            the prior and prediction, as was given in Li's paper. When
%            given a transverse acceleration instead of a turn rate, half
%            of the prior and predicted accelerations is used.
%          3 Use the forward-predicted omega (or transverse acceleration)
%            for building the state transition matrix. \omega_{k+1}
%  tauTurn The correlation time constant for the turn rate in seconds.
%          tau must be positive but does not have to be finite. If this
%          parameter is omitted, then tauTurn is set to infinity.
% tauLinAccel The correlation time constant for the linear acceleration (if
%          present) in seconds. This parameter is not needed if there is
%          no linear acceleration. If a linear acceleration is present
%          and this parameter is omitted, then tauLinAccel is set to
%          infinity.
%
%OUTPUT: F The state transition matrix under a possibly linearlly
%          accelerating coordinated turn model where the velocity is given
%          by a heading and a speed.
%
%The state transition matrix is, in part, a realization of the unnumered
%transition Equation after equation 1b in [1], which comes from the initial
%derivation in [2], where a different definition of heading direction is
%used. Note that the order of the components in the state here is different
%than the ordering of the components in Blackman's paper. Also, Blackman's
%paper does not consider linear acceleration terms.
%
%Despite the form of the transition matrix without a linear acceleration
%term, the result is mathematically equivalent to equation 75 in [3]. After
%being modified to account for a decaying turn rate. It is assumed that the
%continuous-time turn rate model is
%omegaDot=-(1/tauTurn)*omega+noise, which discretizes to
%omega[k+1]=exp(-T/tauTurn)*omega[k]+noise.
%Analogously, the optional linear acceleration discretizes to
%al[k+1]=exp(-T/tauLinAccel)*al[k]+noise
%
%More information on discrete-time turning models is given in the comments
%to the function FCoordTurn2D.
%
%This state transition matrix goes with the process noise covariance matrix
%given by QPolarCoordTurn2D.The corresponding continuous-time drift
%function are aPolarCoordTurn2DOmega and aPolarCoordTurn2Dtrans with the
%diffusion matrix DPolarCoordTurn2D. Note that this model is a direct-
%discrete-time model and is not just a discretization of the continuous-
%time model.
%
%REFERENCES:
%[1] M. Busch and S. Blackman, "Evaluation of IMM filtering for an air
%    defense system application," in Proceedings of SPIE: Signal and Data
%    Processing of Small Targets, vol. 2561, 9 Jul. 1995, pp. 435-447.
%[2] J. L. Gertz, "Multisensor surveillance for improved aircraft
%    tracking," The Lincoln Laboratory Journal, vol. 2, no. 3, pp. 381-396,
%    1989.
%[3] X. R. Li and V. P. Jilkov, "Survey of maneuvering target tracking.
%    Part I: Dynamic models," IEEE Transactions on Aerospace and Electronic
%    Systems, vol. 39, no. 4, pp. 1333-1364, Oct. 2003.
%
%July 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%The time constant for the linear acceleration.
if(nargin<6)
    tauLinAccel=Inf;
end

%The time constant for the turn.
if(nargin<5)
   tauTurn=Inf;
end

if(nargin<4)
    discPoint=0;
end

if(nargin<3)
    turnType='TurnRate';
end

beta=exp(-T/tauTurn);
switch(discPoint)
    case 0%Use the prior value of at/omega for the linearization.
        turnVal=x(5);
    case 1
        %Use the average value of at.omega over the prediction
        %interval for the linearization.
        tau=param3;
        turnVal0=x(5);
        turnVal=(tau-beta*tau+T*turnVal0)/T;

        %Assume tau=Inf or T=0 was used, in which case the
        %asymptotic average value of at/omega should be used.
        if(~isfinite(tauTurn)||~isfinite(turnVal))
            turnVal=turnVal0;
        end
    case 2%Use the simple mean of at/omega for the linearization.
        turnVal=(beta+1)*x(5)/T;
    case 3%Use the predicted value of at/omega for the linearization.
        turnVal=beta*x(5);
    otherwise
        error('Invalid value entered for discPoint');
end

v=x(4);%The speed

switch(turnType)
    case 'TransAccel'%The turn is expressed in terms of a transverse
                     %acceleration.

        omega=turnVal/v;
        if(~isfinite(omega))%Deal with zero velocity.
            omega=0;
        end
    case 'TurnRate'%The turn is expressed in terms of a turn rate.
        omega=turnVal;
    otherwise
        error('Unknown turn type specified.');
end
%We now have the omega term for the turn.

theta=x(3);%The heading (counterclockwise from the x axis).

sinHead=sin(theta);
cosHead=cos(theta);

sinVal=sin(omega*T);
cosVal=cos(omega*T);

sinRat=sinVal/omega;
if(~isfinite(sinRat))
    sinRat=T;%The limit as omega goes to zero.
end
cosRat=(1-cosVal)/omega;
if(~isfinite(cosRat))
    cosRat=0;%The limit as omega goes to zero.
end

switch(turnType)
    case 'TransAccel'%The turn is expressed in terms of a transverse
        F=[1,0,0,cosHead*sinRat-sinHead*cosRat,0;%The x-component.
            0,1,0,sinHead*sinRat+cosHead*cosRat,0;%The y-component.
            0,0,1,0,                            T/v;%The heading component.
            0,0,0,1,                            0;%The speed component
            0,0,0,0,                            beta];%The transverse accel component.
    case 'TurnRate'
        F=[1,0,0,cosHead*sinRat-sinHead*cosRat,0;%The x-component.
            0,1,0,sinHead*sinRat+cosHead*cosRat,0;%The y-component.
            0,0,1,0,                            T;%The heading component.
            0,0,0,1,                            0;%The speed component
            0,0,0,0,                            beta];%The turn rate component.
end
if length(x)==6
    betaLinAccel=exp(-T/tauLinAccel);
    F(4,6)=T;
    F(6,6)=betaLinAccel;
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 model = svmTrain(X, Y, opts)
    % trains an SVM using minFunc
    % X is NxD features
    % Y is discrete array of labels that must (for now) be 1...K

    % opts.type specifies which method to use.
    % 1= linear SVM
    % 2= polynomial Kernel SVM of order opts.polyOrder
    % 3= rbf Kernel SVM with scale parameter opts.rbfScale
    %    the scale is used in equation Z = 1/sqrt(2*pi*sigma^2);
    %    to scale the output of the exponential, where sigma is the scale
    % 4= l2svm, which unlike normal svm uses squared hinge loss

    % model is to be plugged in to yhat = svmTest(model, X)

    % TODO: Support labels that are not in 1..K
    % TODO: n-fold Cross validation support

    addOnes= false;
    C= 1;
    type= 1;
    rbfScale= 1;
    polyOrder= 2;
    if nargin < 3, opts= struct; end
    if isfield(opts, 'addOnes'), addOnes= opts.addOnes; end
    if isfield(opts, 'C'), C= opts.C; end
    if isfield(opts, 'type'), type= opts.type; end
    if isfield(opts, 'rbfScale'), rbfScale= opts.rbfScale; end
    if isfield(opts, 'polyOrder'), polyOrder= opts.polyOrder; end

    if(addOnes), X= [X, ones(size(X,1), 1)]; end

    [N, D]= size(X);
    u= unique(Y);
    K = length(u);

    minFuncOpts= struct;
    minFuncOpts.display= 0;

    if type == 1

        % Linear SVM
        funObj = @(w) SSVMMultiLoss(w, X, Y, K);
        wLinear = minFunc(@penalizedL2, zeros(D*K,1), minFuncOpts, funObj, C);
        model.w = reshape(wLinear,[D K]);

    elseif type == 2

        % Polynomial SVM
        Kpoly = kernelPoly(X, X, polyOrder);
        funObj = @(v) SSVMMultiLoss(v, Kpoly, Y, K);
        uPoly = minFunc(@penalizedKernelL2_matrix, randn(N*K,1), minFuncOpts, Kpoly, K, funObj, C);
        model.X = X; % must save all the training data... (ok only support vectors, todo)
        model.uPoly= reshape(uPoly, [N, K]);
        model.polyOrder= polyOrder;

    elseif type == 3

        % RBF SVM
        Krbf = kernelRBF(X, X, rbfScale);
        funObj = @(v) SSVMMultiLoss(v, Krbf, Y, K);
        uRBF = minFunc(@penalizedKernelL2_matrix, randn(N*K,1), minFuncOpts, Krbf, K, funObj, C);
        model.X = X; % must save all the training data... (ok only support vectors, todo)
        model.urbf= reshape(uRBF,[N, K]);
        model.rbfScale= rbfScale;

    elseif type == 4

        % L2 SVM (squares the slack variables, i.e. squared hinge loss)
        funObj = @(v) l2svmloss(v, X, Y, K, C);
        wLinear = minFunc(@penalizedL2, zeros(D*K,1), minFuncOpts, funObj, C);
        model.w = reshape(wLinear,[D K]);

    else

        fprintf('Unrecognized type %d, exitting...\n', type);
        model= struct;
        return;

    end

    model.type= type;
    model.addOnes= addOnes;
    model.C= C;
    model.u= u;

    % 1-vs-all L2-svm loss function;  similar to LibLinear.
    % Originally taken from Adam Coates' code, slightly adapted
    function [loss, g] = l2svmloss(w, X, y, K, C)

      [M,N] = size(X);
      theta = reshape(w, N,K);
      Y = bsxfun(@(y,ypos) 2*(y==ypos)-1, y, 1:K);
      margin = max(0, 1 - Y .* (X*theta));
      loss = (0.5 * sum(theta.^2)) + C*sum(margin.^2);
      loss = sum(loss);
      g = theta - 2 * C * (X' * (margin .* Y));
      g = g(:);


function linpack_d_test10 ( )

%*****************************************************************************80
%
%% TEST10 tests DGEFA and DGESL.
%
%  Discussion:
%
%    Solve A*x = b where A is a given matrix, and B a right hand side.
%
%    We will also assume that A is stored in the simplest
%    possible way.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2009
%
%  Author:
%
%    John Burkardt
%
  n = 3;
  lda = n;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST10\n' );
  fprintf ( 1, '  For a general matrix,\n' );
  fprintf ( 1, '  DGEFA computes the LU factors;\n' );
  fprintf ( 1, '  DGESL solves a factored linear system;\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The number of equations is N = %d\n', n );
%
%  Set the values of the matrix A.
%
  a = [ 1.0, 2.0, 3.0;
        4.0, 5.0, 6.0;
        7.0, 8.0, 0.0 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The matrix A:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    for j = 1 : n
      fprintf ( 1, '  %12f', a(i,j) );
    end
    fprintf ( 1, '\n' );
  end
%
%  Set the values of the right hand side vector B.
%
  b(1:3) = [ 6.0, 15.0, 15.0 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The right hand side B is\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %12f\n', b(i) );
  end
%
%  Factor the matrix.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Factor the matrix\n' );

  [ a, ipvt, info ] = dgefa ( a, lda, n );

  if ( info ~= 0 )
    fprintf ( 1, '  DGEFA returned an error flag INFO = %d\n', info );
    return
  end
%
%  If no error occurred, now use DGESL to solve the system.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Solve the linear system.\n' );

  job = 0;
  b = dgesl ( a, lda, n, ipvt, b, job );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  DGESL returns the solution:\n' );
  fprintf ( 1, '  (Should be (1,1,1))\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    fprintf ( 1, '  %12f\n', b(i) );
  end

  return
end


function [ xyz, face_pointer, face_data ] = xyzf_example ( point_num, ...
  face_num, face_data_num )

%*****************************************************************************80
%
%% XYZF_EXAMPLE sets data suitable for a pair of XYZ and XYZF files.
%
%  Discussion:
%
%    There are 8 points.
%    There are 6 faces.
%    There are 24 face items.
%
%       8------7
%      /|     /|
%     / |    / |
%    5------6  |
%    |  4---|--3
%    | /    | /
%    |/     |/
%    1------2
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 January 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer POINT_NUM, the number of points.
%
%    Input, integer FACE_NUM, the number of faces.
%
%    Input, integer  FACE_DATA_NUM, the number of face items.
%
%    Output, real XY(2,POINT_NUM), the point coordinates.
%
%    Output, integer FACE_POINTER(FACE_NUM+1), pointers to the
%    first face item for each face.
%
%    Output, integer FACE_DATA(FACE_DATA_NUM), indices
%    of points that form faces.
%
  xyz(1:3,1:point_num) = [ ...
     0.0,  0.0,  0.0; ...
     1.0,  0.0,  0.0; ...
     1.0,  1.0,  0.0; ...
     0.0,  1.0,  0.0; ...
     0.0,  0.0,  1.0; ...
     1.0,  0.0,  1.0; ...
     1.0,  1.0,  1.0; ...
     0.0,  1.0,  1.0 ]';

  face_pointer(1:face_num+1) = [ 1, 5, 9, 13, 17, 21, 25 ];

  face_data(1:face_data_num) = [ ...
     1, 4, 3, 2, ...
     2, 3, 7, 6, ...
     5, 6, 7, 8, ...
     5, 8, 4, 1, ...
     1, 2, 6, 5, ...
     3, 4, 8, 7 ];

  return
end


function btv_test03 ( )

%*****************************************************************************80
%
%% BTV_TEST03 tests BURGERS_TIME_VISCOUS with the gaussian initial condition.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 April 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BTV_TEST03\n' );
  fprintf ( 1, '  Test BURGERS_TIME_VISCOUS with the gaussian initial condition.\n' );
  fprintf ( 1, '  Use a Neumann condition on the right.\n' );

  nx = 81;
  nt = 200;
  t_max = 2.0;
  nu = 0.01;
  bc = 3;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Initial condition: gaussian\n' );
  fprintf ( 1, '  Number of space nodes = %d\n', nx );
  fprintf ( 1, '  Number of time steps = %d\n', nt );
  fprintf ( 1, '  Final time T_MAX = %g\n', t_max );
  fprintf ( 1, '  Viscosity = %g\n', nu );
  fprintf ( 1, '  Boundary condition = %d\n', bc );

  U = burgers_time_viscous ( @ic_gaussian, nx, nt, t_max, nu, bc );

  x = linspace ( -1.0, +1.0, nx );

  figure ( 3 )

  plot ( x, U(1:50:(nt+1),:), 'Linewidth', 3 )
  grid on
  xlabel ( '<-- X -->' )
  ylabel ( '<-- U(X,T) -->' )
  title ( 'Burgers equation solutions over time, initial condition gaussian' )

  filename = 'btv_test03.png';
  print ( '-dpng', filename )
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Saved plot as "%s"\n', filename );

  return
end


function f = cumsum3(f)
%CUMSUM3   Triple indefinite integral of a CHEBFUN3.
%   F = CUMSUM3(F) returns the triple indefinite integral of a CHEBFUN3.
%   That is
%                  z  y  x
%                 /  /  /
%   CUMSUM3(F) = |  |  |   F(x,y,z) dx dy dz
%               /  /  /
%              e  c  a
%
%   where [a,b] x [c,d] x [e,g] is the domain of F.
%
% See also CHEBFUN3/CUMSUM, CHEBFUN3/CUMSUM2, CHEBFUN3/SUM, CHEBFUN3/SUM2
% and CHEBFUN3/SUM3.

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

% Check for empty:
if ( isempty(f) )
    f = [];
    return
end

f.cols = cumsum(f.cols);     % CUMSUM along the cols.
f.rows = cumsum(f.rows);     % CUMSUM along the rows.
f.tubes = cumsum(f.tubes);   % CUMSUM along the tubes.

end


function c = phi1(costgradf,w,param,mu)
% compute c=cost+mu*dual error
D    = param.D;
b    = param.b;
dim  = param.dim;
h    = param.h;
m    = param.m;
n  = dim(1);
nx = dim(2);
ny = dim(3);
nz = dim(4);
nt = dim(5);
hx = h(1);
hy = h(2);
hz = h(3);
ht = h(4);

px              = w(1:n*(nx-1)*ny*nz*nt);
py              = w((n*(nx-1)*ny*nt*nz+1):(n*(nx-1)*ny*nz*nt+n*nx*(ny-1)*nz*nt));
pz              = w((n*(nx-1)*ny*nz*nt+n*nx*(ny-1)*nz*nt)+1:(n*(nx-1)*ny*nz*nt+n*nx*(ny-1)*nz*nt+n*nx*ny*(nz-1)*nt));
rho             = w((n*(nx-1)*ny*nz*nt+n*nx*(ny-1)*nz*nt+n*nx*ny*(nz-1)*nt)+1:...
                       (n*(nx-1)*ny*nz*nt+n*nx*(ny-1)*nz*nt+n*nx*ny*(nz-1)*nt+n*nx*ny*nz*(nt-1)));
u               = w((end-m*nx*ny*nz*nt+1):end);
c    = costgradf(px,py,pz,rho,u,param)/hx/hy/hz/ht;
c    = c + mu*sum(abs(D*w-b(:)));
end


function pattern = elementaryCellularAutomata(rule, n, width, randfrac)
%elementaryCellularAutomata Elementary 1D cellular automaton patterns
%
%   PATTERN = elementaryCellularAutomata(RULE, NITER), where NITER is a
%   scalar, returns an NITER x 2*NITER+1 matrix whose entries are all 0 or
%   1. The I'th row of the matrix contains the state of the elementary 1D
%   cellular automaton at iteration I-1, counting the initial state as
%   iteration 0. The integer RULE specifies the rule to use as set out at
%   http://mathworld.wolfram.com/ElementaryCellularAutomaton.html.
%
%   The initial state is all zeros except for a 1 at element NITER+1 (the
%   central element) of the CA.
%
%   PATTERN = elementaryCellularAutomata(RULE, NITER, WID), where WID is a
%   scalar, returns an NITER x WID matrix. The array of cells is taken to
%   be circular, so that PATTERN(I+1,I) depends on PATTERN{(I,WID), (I,1)
%   and (I,2)}. Similarly, PATTERN(I+1,WID) depends on PATTERN{(I,WID-1),
%   (I,WID) and (I,1)}. This only matters if WID < 2*NITER+1 and RULE is
%   such that the pattern propagates outwards, so reaching the boundaries
%   of the array.
%
%   This wraparound allows long, thin patterns to be generated if an
%   appropriate rule is chosen. All patterns with a fixed width will be
%   periodic, unless some random noise is added.
%
%   The state on the first iteration is all zeros except for a 1 at element
%   floor((WID+1)/2) of the CA.
%
%   PATTERN = elementaryCellularAutomata(RULE, NITER, START) where START is
%   a 1 x WID row vector containing only the values 0 and 1, is as above
%   except that the initial state is given by the entries in START. Thus on
%   exit, PATTERN(1,:) is equal to START.
%
%   PATTERN = elementaryCellularAutomata(RULE, NITER, WIDSTART, FNOISE) is
%   as above except that noise is added to the process. WIDSTART can be
%   either a scalar giving the width or a vector giving the start state; an
%   empty matrix is equivalent to 2*NITER+1. FNOISE is a number from 0 to 1
%   giving the probability that any given cell will be set to the wrong
%   state (the complement of the state given by the rule) on any one
%   iteration.
%
%   Example
%   -------
%       % show 50 rows of each pattern
%       for rule = 0:255
%           pattern = elementaryCellularAutomata(rule, 50);
%           imshow(pattern); pause;
%       end

%   Copyright 2010 David Young

% check arguments and supply defaults
error(nargchk(2, 4, nargin));
validateattributes(rule, {'numeric'}, {'scalar' 'integer' 'nonnegative' '<=' 255}, ...
    'elementaryCellularAutomata', 'RULE');
validateattributes(n, {'numeric'}, {'scalar' 'integer' 'positive'}, ...
    'elementaryCellularAutomata', 'N');
if nargin < 3 || isempty(width)
    width = 2*n-1;
elseif isscalar(width)
    validateattributes(width, {'numeric'}, {'integer' 'positive'}, ...
        'elementaryCellularAutomata', 'WIDTH');
else
    validateattributes(width, {'numeric' 'logical'}, {'binary' 'row'}, ...
        'elementaryCellularAutomata', 'START');
end
if nargin < 4 || isempty(randfrac)
    dorand = false;
else
    validateattributes(randfrac, {'double' 'single'}, {'scalar' 'nonnegative' '<=' 1}, ...
        'elementaryCellularAutomata', 'FNOISE');
    dorand = true;
end

% set up machine
if isscalar(width)
    patt = ones(1, width);
    patt(floor((width+1)/2)) = 2;
else
    patt = width + 1;  % change 0,1 to 1,2 so can use sub2ind
    width = length(patt);
end

% unpack rule
rulearr = (bitget(rule, 1:8) + 1);

% initialise output array
pattern = zeros(n, width);

% iterate to generate rest of pattern
for i = 1:n
    pattern(i, :) = patt;   % record current state in output array

    % core step: apply CA rules to propagate to next 1D pattern
    ind = sub2ind([2 2 2], ...
        [patt(2:end) patt(1)], patt, [patt(end) patt(1:end-1)]);
    patt = rulearr(ind);

    %optional randomisation
    if dorand
        flip = rand(1, width) < randfrac;
        patt(flip) = 3 - patt(flip);
    end
end

% change symbols from 1 and 2 to 0 and 1
pattern = pattern-1;
end


function cvx_optval = sum_smallest( x, varargin )

%SUM_SMALLEST Sum of the smallest k elements of a vector.
%   For a real vector X and an integer k between 1 and length(X) inclusive,
%   y = SUM_SMALLEST(X,k) is the sum of the k smallest elements of X; e.g.,
%       temp = sort( x )
%       y = sum( temp( 1 : k ) )
%   If k=1, then SUM_SMALLEST(X,k) is equivalent to MIN(X); if k=length(X),
%   then SUM_SMALLEST(X,k) is equivalent to SUM(X).
%
%   Both X and k must be real, and k must be a scalar. But k is not, in
%   fact, constrained to be an integer between 1 and length(X); the
%   function is extended continuously and logically to all real k. For
%   example, if k <= 0, then SUM_SMALLEST(X,k)=0. If k > length(X), then
%   SUM_SMALLEST(X,k)=SUM(X). Non-integer values of k interpolate linearly
%   between their integral neighbors.
%
%   For matrices, SUM_SMALLEST(X,k) is a row vector containing the
%   application of SUM_SMALLEST to each column. For N-D arrays, the
%   SUM_SMALLEST operation is applied to the first non-singleton dimension
%   of X.
%
%   SUM_SMALLEST(X,k,DIM) performs the operation along dimension DIM of X.
%
%   Disciplined convex programming information:
%       SUM_SMALLEST(X,...) is concave and nondecreasing in X. Thus, when
%       used in CVX expressions, X must be concave (or affine). k and DIM
%       must both be constant.

error( nargchk( 2, 3, nargin ) );
cvx_optval = -sum_largest( -x, varargin{:} );

% Copyright 2010 Michael C. Grant and Stephen P. Boyd.
% See the file COPYING.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


% ASTROTIK by Francesco Santilli
% R2BP (Restricted Two Bodies Problem)
% H2f converts hyperbolic anomaly to true anomaly for an hyperbolic orbit.
%
% Usage: f = H2f(H,e)
%
% where: H(k) = hyperbolic anomaly [rad]
%        e = eccentricity [-] (e>1)
%        f(k) = true anomaly [rad]

function f = H2f(H,e)

    if ~(nargin == 2)
        error('Wrong number of input arguments.')
    end

    check(H,1)
    check(e,0)

    if e <= 1
        error('e must be strictly major than 1.')
    end

    ee = sqrt((e+1)/(e-1));
    f = 2*atan( ee*tanh(H/2) );

    kk = H/(2*pi);
    k = round(kk) - ((kk-fix(kk)) == 0.5);
    f = f + k*(2*pi);

end


function DataSet = prtDataGenNoisySinc(varargin)
% prtDataGenNoisySinc Generates noisy sinc example data
%
%   DATASET = prtDataGenNoisySinc returns a prtDataSetRegress with 100
%   samples of sinc wave with zero-mean additive Gaussian noise. The noise
%   variance is .1.
%
%   DATASET = prtDataGenNoisySinc(param,value) enables specification of
%   various parameter/value pairs:
%       nSamples - 100 - the number of random locations to sample
%       tLims - [-10 10] - 1x2 vector specifying x sampling range
%       x - [] - nx1 vector of locations to sample at; if empty, use random
%           sampling, which uses nSamples and t to randomly pick x.
%       noiseVar - 0.1 - the variance of the noise to add
%
%
%   Example:
%
%   ds1 = prtDataGenNoisySinc;
%   ds2 = prtDataGenNoisySinc('tLims',[-5 5],'nSamples',1000);
%   ds3 = prtDataGenNoisySinc('x',linspace(-10,10,30));
%   subplot(3,1,1);
%   plot(ds1);
%   V = axis;
%   subplot(3,1,2);
%   plot(ds2);
%   axis(V);
%   subplot(3,1,3);
%   plot(ds3);
%   axis(V);
%
%   See also: prtDataSetClass, prtDataGenBiModal, prtDataGenIris,
%   prtDataGenMary, prtDataGenNoisySinc, prtDataGenOldFaithful,
%   prtDataGenSpiral, prtDataGenUnimodal, prtDataGenUnimodal, prtDataGenXor

if nargin == 1
    nSamples = varargin{1};
    noiseVar = 0.1;
    tLims = [-10 10];
    x = prtUtilRandSample(tLims,nSamples);
else
    p = inputParser;
    p.addParameter('noiseVar',.1);
    p.addParameter('nSamples',100);
    p.addParameter('tLims',[-10 10]);
    p.addParameter('x',[]);
    p.parse(varargin{:});
    inputs = p.Results;
    noiseVar = inputs.noiseVar;
    nSamples = inputs.nSamples;
    tLims = inputs.tLims;
    x = inputs.x;
    x = x(:);
    if isempty(x);
        x = prtUtilRandSample(tLims,nSamples);
    end
end

t = sinc(x/pi);
y = t + noiseVar*randn(size(x));

DataSet = prtDataSetRegress(x,y,'name','Noisy Sinc');

function y = sinc(x)

y = sin(pi*x)./(pi*x);
y(x == 0) = 1;

function x = prtUtilRandSample(tLims,nSamples)

x = rand(nSamples,1)*(tLims(2)-tLims(1)) + tLims(1);


function x = r8blt_sl ( n, ml, a, b, job )

%*****************************************************************************80
%
%% R8BLT_SL solves a R8BLT system.
%
%  Discussion:
%
%    The R8BLT storage format is appropriate for a banded lower triangular matrix.
%    The matrix is assumed to be zero below the ML-th subdiagonal.
%    The matrix is stored in an ML+1 by N array, in which the diagonal
%    appears in the first row, followed by successive subdiagonals.
%    Columns are preserved.
%
%    No factorization of the lower triangular matrix is required.
%
%  Example:
%
%    N = 5, ML = 2
%
%    A11   0   0   0   0
%    A21 A22   0   0   0
%    A31 A32 A33   0   0
%      0 A42 A43 A44   0
%      0   0 A53 A54 A55
%                --- ---
%                    ---
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer ML, the lower bandwidth.
%
%    Input, real A(ML+1,N), the R8BLT matrix.
%
%    Input, real B(N), the right hand side.
%
%    Input, integer JOB, is 0 to solve the untransposed system,
%    nonzero to solve the transposed system.
%
%    Output, real X(N), the solution vector.
%
  x(1:n) = b(1:n);

  if ( job == 0 )

    for j = 1 : n
      x(j) = x(j) / a(1,j);
      ihi = min ( j + ml, n );
      for i = j+1 : ihi
        x(i) = x(i) - a(i-j+1,j) * x(j);
      end
    end

  else

    for j = n : -1 : 1
      x(j) = x(j) / a(1,j);
      ilo = max ( j - ml, 1 );
      for i = ilo : j-1
        x(i) = x(i) - a(j-i+1,i) * x(j);
      end
    end

  end

  return
end


% Lorenz Attractor equations solved by ODE Solve
%% x' = sigma*(y-x)
%% y' = x*(rho - z) - y
%% z' = x*y - beta*z
function dx = lorenzatt(X)
    rho = 28; sigma = 10; beta = 8/3;
    dx = zeros(3,1);
    dx(1) = sigma*(X(2) - X(1));
    dx(2) = X(1)*(rho - X(3)) - X(2);
    dx(3) = X(1)*X(2) - beta*X(3);
    return
end


function sMap = rsom_lininit(Distances, msize, varargin)

%RSOM_LININIT computes linear initialization for the RSOM.
%
% sM = rsom_lininit(D, msize, [argID, value, ...])
%
%  sM = rsom_lininit(D, msize, 'lattice', 'hexa');
%
% Input and output arguments:
%   D          (matrix) dissimilarity data for training, size nData x nData
%   msize      (vector) map size
%   [argID,    (string) See below.
%    value]    (varies)
%
%   sM         (struct) RSOM map struct, the initialized map
%
% Here are the valid argument IDs and corresponding values.
%   'lattice'     (string) map lattice: 'hexa' or 'rect'
%   'shape'       (string) map shape: 'sheet', 'cyl' or 'toroid'
%   'name'        (string) name of the RSOM struct
%
% For more help, try 'type rsom_lininit' or check out online documentation.
% See also RSOM_RANDINIT, RSOM_BATCHTRAIN.

%%%%%%%%%%%%% DETAILED DESCRIPTION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% rsom_lininit
%
% PURPOSE
%
% Initializes the RSOM in the input space linearly.
%
% SYNTAX
%
%  sM = rsom_lininit(D, msize);
%  sM = rsom_lininit(...,'argID',value,...);
%
% DESCRIPTION
%
% The grid in the input space is initialized along the eigenvectors
% corresponding to the largest eigenvalues. These are calculated by
% classical MDS from the dissimilarity matrix D. If D is generated from
% euclidean data, squared distances should be provided i.e.
% (D)_ij = ||x_i - x_j||^2.
%
% REFERENCES
%
%   Barbara Hammer, Alexander Hasenfuss: Topographic Mapping of Large
%   Dissimilarity Data Sets. Neural Computation 22(9): 2229-2284 (2010)
%
% REQUIRED INPUT ARGUMENTS
%
%   D          (matrix) dissimilarity data for training, size nData x nData
%   msize      (vector) map size
%
% OPTIONAL INPUT ARGUMENTS
%
%  argID (string) Argument identifier string (see below).
%  value (varies) Value for the argument (see below).
%
%  The optional arguments can be given as 'argID',value -pairs.
%  The valid IDs and corresponding values are listed below.
%
%  Below is the list of valid arguments:
%   'lattice'     (string) map lattice: 'hexa' or 'rect'
%   'shape'       (string) map shape: 'sheet', 'cyl' or 'toroid'
%   'name'        (string) name of the RSOM struct
%
% EXAMPLES
%
%   sM = rsom_lininit(D, [10 10]);
%   sM = rsom_lininit(D, [10 10], 'lattice', 'hexa');
%
% SEE ALSO
%
%   rsom_randinit    Initialize a RSOM randomly
%   rsom_batchtrain  Train a RSOM

% Contributed to SOM Toolbox vs2, December 7th, 2012 by Alexander Schulz
% Copyright (c) Alexander Schulz
% http://www.cis.hut.fi/projects/somtoolbox/

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

%% Prase input
p = inputParser;
p.addRequired('Distances', @isnumeric);
p.addRequired('msize', @isnumeric);

p.addParamValue('lattice', 'rect', @ischar);
p.addParamValue('shape', 'sheet', @ischar);
p.addParamValue('name', '', @ischar);

p.parse(Distances, msize, varargin{:});

lattice = p.Results.lattice;
shape   = p.Results.shape;
name    = p.Results.name;
clear p;
mdim     = length(msize);
nData    = size(Distances,1);

%% Init and normalize
% create the topology struct
sTopol = som_topol_struct('msize', msize, ...
                          'lattice', lattice, ...
                          'shape', shape);

Coords = som_unit_coords(msize,'rect','sheet');
% normalize the neuron positions to mean 0
Coords = bsxfun(@minus, Coords, mean(Coords,1));

% compute mds embedding of the distance matrix
[V, e]=cmdscale(Distances.^(1/2));
% keep only the first mdim dimensions
V = V(:, 1:mdim);
e = e(1:mdim);
scale = std(V);

% scale Coords to the scale of the data
%Coords = bsxfun(@rdivide, Coords, std(Coords));
%Coords = bsxfun(@times, Coords, scale);
Coords = bsxfun(@rdivide, Coords, std(Coords)+1e-10);

% project neurons onto the data
% e is n * variance
standardDev = sqrt(e/nData);
V = bsxfun(@rdivide, V, nData*standardDev'+1e-10);
%V = bsxfun(@rdivide, V, sqrt(sum(V.^2, 2))+1e-10);

cCodebook = Coords * V';
cCodebook = cCodebook + 1/nData;

%% create the resulting struct
sTrain = som_train_struct('algorithm','lininit');

sMap = struct('type', 'rsom_map', ...
              'cCodebook', cCodebook, ...
              'topol', sTopol, ...
              'labels', cell(1), ...
              'neigh', 'gaussian', ...
              'trainhist', cell(1), ...
              'name', name);

sTrain = som_set(sTrain,'time',datestr(now,0));
sMap.trainhist = sTrain;


% MAIN  --  Polar Point Mass
%
% This script runs a simulation of the polar point mass, with a controller
% that is working to hold the mass at a fixed distance from the origin
%

P.m = 1;
P.g = 9.81;
P.l = 1;
P.freq = 3*sqrt(P.g/P.l);   %Response frequence in controller
P.damp = 1.0;   %Damping ratio in controller
P.eNom = 2.5*P.m*P.g*P.l;

% z = [r; th; dr; dth];

z0min = [0.5*P.l; -pi; -0.2; -0.2];
z0max = [1.5*P.l; pi;  0.2;  0.2];

z0 = z0min + (z0max-z0min).*rand(4,1);

userFunc = @(t,z)dynamics(t,z,controller(z,P),P);

tSpan = [0;3];

sol = ode45(userFunc,tSpan,z0);

t = linspace(tSpan(1),tSpan(2),500);
z = deval(sol,t);
u = controller(z,P);

figure(234); clf;

subplot(3,2,1);
plot(t,z(1,:))
ylabel('r')

subplot(3,2,3);
plot(t,z(3,:))
ylabel('dr')

subplot(3,2,5);
plot(t,u)
ylabel('u')

subplot(3,2,2);
plot(t,z(2,:))
ylabel('th')

subplot(3,2,4);
plot(t,z(4,:))
ylabel('dth')


function price = PROJ_ForwardStarting(N, alph, r, q, T1, T2, S_0, call, rnCHF1, rnCHF2)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% About: Pricing Function for Forward Starting Options using PROJ method (uses cubic B-splines)
% Models Supported: Levy Processes, including jump diffusions and Black-Scholes model
% Returns: price of contract
% Author: Justin Lars Kirkby
%
% ----------------------
% Contract/Model Params
% ----------------------
% S_0 = initial stock price (e.g. 100)
% W   = strike  (e.g. 100)
% r   = interest rate (e.g. 0.05)
% q   = dividend yield (e.g. 0.05)
% T1  =  time to maturity (in years), e.g. T1=1
% T2  = T - T1, how much time remains in contract after the forward start date (choose 0 < T2 < T1)
% call  = 1 for call (else put)
% rnCHF1 = risk netural characteristic function of process up to T1 (function handle with single argument)
% rnCHF2 = risk netural characteristic function of process with T2 = T - T1 remaining time to maturity after forward start date
% ----------------------
% Numerical (PROJ) Params
% ----------------------
% alph  = grid with is 2*alph
% N     = budget: resolution = 2*alph/(N-1), where support is of length 2*alph
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

T = T1 + T2;

dx = 2*alph/(N-1); a = 1/dx;
dw = 2*pi/(N*dx);

omega = dw*(1:N-1);  %We calcuate coefficient of w=0 explicitly
nbar = N/2;
xmin = (1 - N/2)*dx;

%%%% Cubic Spline
b0    = 1208/2520; b1 = 1191/2520; b2 = 120/2520; b3 = 1/2520;
grand = @(w)rnCHF2(w).*(sin(w/(2*a))./w).^4./(b0 + b1*cos(w/a) +b2*cos(2*w/a) +b3*cos(3*w/a));
beta  = real(fft([1/(32*a^4) exp(-1i*xmin*omega).*feval(grand,omega)]));

%FIND Value of E[(1 - exp(X_tau))^+]
G = zeros(1,N);

G(nbar +1) = 1*(1/24 - 1/20*exp(dx)*(exp(-7/4*dx)/54 + exp(-1.5*dx)/18 + exp(-1.25*dx)/2 + 7*exp(-dx)/27));

G(nbar )   =  1*(.5 -.05*(28/27 + exp(-7/4*dx)/54 + exp(-1.5*dx)/18 + exp(-1.25*dx)/2 + 14*exp(-dx)/27 ...
            + 121/54*exp(-.75*dx) + 23/18*exp(-.5*dx) + 235/54*exp(-.25*dx)));

G(nbar -1) = 1*( 23/24 - exp(-dx)/90*( (28 + 7*exp(-dx))/3 ...
            + ( 14*exp(dx) + exp(-7/4*dx) + 242*cosh(.75*dx) + 470*cosh(.25*dx))/12 ...
            +.25*(exp(-1.5*dx) + 9*exp(-1.25*dx) + 46*cosh(.5*dx))) );

vartheta_star = 1/90*( 14/3*(2+cosh(dx)) ...
                + .5*(cosh(1.5*dx) + 9*cosh(1.25*dx) +23*cosh(.5*dx))...
                +  1/6*(cosh(7/4*dx) + 121*cosh(.75*dx) +235*cosh(.25*dx)));

G(1: nbar -2) = 1 - 1*exp(xmin +dx*(0:nbar-3))*vartheta_star;
Cons = 32*a^4;

Vbar2 = Cons/N*G(1,1:(nbar +1))*(beta(1,1:(nbar +1))');

%%% Find Second Expansion
grand = @(w)rnCHF1(w).*(sin(w/(2*a))./w).^4./(b0 + b1*cos(w/a) +b2*cos(2*w/a) +b3*cos(3*w/a));
beta  = real(fft([1/(32*a^4) exp(-1i*xmin*omega).*feval(grand,omega)]));

G(1:N) = exp(xmin + (0:(N-1))*dx);
Vbar1 = Cons/N*G*beta';
Val_put = exp(-r*T)*Vbar2*Vbar1*S_0*vartheta_star;

%%%%  PUT CALL PARITY/PRICING FORMULA
if call ==1
    Val_Proj = S_0*(exp(-q*T)-exp(-r*T2)*exp(-q*T1)) + Val_put;  %check this formula
else
    Val_Proj = Val_put;
end

price = Val_Proj;

end


function lm = energy_awgn_normapx(en0, epsil)
% Compute normapx on log m^*(E/N_0, \epsilon)

loge = log2(exp(1));
lm = en0 * loge + loge * sqrt(2*en0) * norminv(epsil) + log2(en0)/2;
%lm = en0 * loge + loge * sqrt(2*en0) * norminv(epsil) + log2(2*en0)/2;


function [ball] = pwrbal1(pp,pw,ee)
%PWRBAL1 compute the ortho-balls associated with a 1-simplex
%triangulation embedded in R^2 or R^3.
%   [BB] = PWRBAL1(PP,PW,TT) returns the set of power balls
%   associated with the edges in [PP,PW,EE], such that BB =
%   [XC,YC,RC.^2]. PW is a vector of vertex weights.

%   Darren Engwirda : 2017 --
%   Email           : de2363@columbia.edu
%   Last updated    : 02/05/2018

%---------------------------------------------- basic checks
    if ( ~isnumeric(pp) || ...
         ~isnumeric(pw) || ...
         ~isnumeric(ee) )
        error('pwrbal1:incorrectInputClass' , ...
            'Incorrect input class.');
    end

%---------------------------------------------- basic checks
    if (ndims(pp) ~= +2 || ...
        ndims(pw) ~= +2 || ...
        ndims(ee) ~= +2 )
        error('pwrbal1:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end

    if (size(pp,2) < +2 || ...
            size(pp,1)~= size(pw,1) || ...
                size(ee,2) < +2 )
        error('pwrbal1:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end

    switch (size(pp,2))
        case  +2
    %-------------------------------------------- lin offset
        pp12 = pp(ee(:,1),:) - ...
               pp(ee(:,2),:) ;

        ww12 = pw(ee(:,1),1) - ...
               pw(ee(:,2),1) ;

        dp12 = sum(pp12.*pp12,2) ;

        tpwr = +.5 * (ww12+dp12)./dp12 ;

        ball = zeros(size(ee,1),3) ;
        ball(:,1:2) = ...
            pp(ee(:,1),:) - tpwr.*pp12 ;

        vsq1 = ...
            pp(ee(:,1),:) - ball(:,1:2);
        vsq2 = ...
            pp(ee(:,2),:) - ball(:,1:2);

    %-------------------------------------------- mean radii
        rsq1 = sum(vsq1 .^ 2,2) ;
        rsq2 = sum(vsq2 .^ 2,2) ;

        rsq1 = rsq1-pw(ee(:,1)) ;
        rsq2 = rsq2-pw(ee(:,2)) ;

        ball(:,3) = (rsq1 + rsq2) / 2. ;

        case  +3
    %-------------------------------------------- lin offset
        pp12 = pp(ee(:,1),:) - ...
               pp(ee(:,2),:) ;

        ww12 = pw(ee(:,1),1) - ...
               pw(ee(:,2),1) ;

        dp12 = sum(pp12.*pp12,2) ;

        tpwr = +.5 * (ww12+dp12)./dp12 ;

        ball = zeros(size(ee,1),4) ;
        ball(:,1:3) = ...
            pp(ee(:,1),:) - tpwr.*pp12 ;

        vsq1 = ...
            pp(ee(:,1),:) - ball(:,1:3);
        vsq2 = ...
            pp(ee(:,2),:) - ball(:,1:3);

    %-------------------------------------------- mean radii
        rsq1 = sum(vsq1 .^ 2,2) ;
        rsq2 = sum(vsq2 .^ 2,2) ;

        rsq1 = rsq1-pw(ee(:,1)) ;
        rsq2 = rsq2-pw(ee(:,2)) ;

        ball(:,4) = (rsq1 + rsq2) / 2. ;

    otherwise

    error('pwrbal2:unsupportedDimension' , ...
            'Dimension not supported.');

    end

end


function res=NODDI_erfi(x)
% %erfi(x). The Imaginary error function, as it is defined in Mathematica
% %erfi(z)==erf(iz)/i (z could be complex) using
% %the incomplete gamma function in matlab: gammainc
% %Using "@": erfi = @(x) real(-sqrt(-1).*sign(x).*gammainc(-x.^2,1/2))
% %Note: limit(x->0) erfi(x)/x -> 2/sqrt(pi)
%
% %Example 1:
% x=linspace(0.001,6,100);
% y=exp(-x.^2).*erfi(x)./2./x;
% figure(1), clf;plot(x,y*sqrt(pi))
%
% %Example 2:
% [x,y]=meshgrid(linspace(-3,3,180),linspace(-3,3,180));
% z=x+i*y;
% figure(1), clf;contourf(x,y,log(erfi(z)))
% axis equal;axis off

% MATLAB only:
% xc=5.7;%cut for asymptotic approximation (when x is real)
% res=~isreal(x).*(-(sqrt(-x.^2)./(x+isreal(x))).*gammainc(-x.^2,1/2))+...
%     isreal(x).*real(-sqrt(-1).*sign(x).*((x<xc).*gammainc(-x.^2,1/2))+...
%     (x>=xc).*exp(x.^2)./x/sqrt(pi));

try % FASTER AND COMPATIBLE WITH BOTH MATLAB AND OCTAVE:
    res = Faddeeva_erfi(x);
catch
    if ~moxunit_util_platform_is_octave
    xc=5.7;%cut for asymptotic approximation (when x is real)
    res=~isreal(x).*(-(sqrt(-x.^2)./(x+isreal(x))).*gammainc(-x.^2,1/2))+...
        isreal(x).*real(-sqrt(-1).*sign(x).*((x<xc).*gammainc(-x.^2,1/2))+...
        (x>=xc).*exp(x.^2)./x/sqrt(pi));
    else
        error('Faddeeva_erfi was not build correctly. run Faddeeva_build.m')
    end
end


function auc = aucroc(p_predicted, p_target, freq)

        % Count observations by class
        nTarget     = sum(freq .* p_target);
        nBackground = sum(freq .* (1-p_target));

        % Rank data
        R = tiedrank(p_predicted);  % 'tiedrank' from Statistics Toolbox
        [R_uniq, I, J] = unique(R);
        [R_mean,R_num] = grpstats(freq,R,{'mean','numel'});
        R_freq = R_mean .* R_num;

        % adjust for counts
        rank_start_freq = cumsum(R_freq);
        rank_start_freq = [0; rank_start_freq(1:(end-1))] + 1;
        rank_mean_freq = (2*rank_start_freq+R_freq-1)/2;

        rank_orig = rank_mean_freq(J);

        % Calculate AUC
        %Error = (sum(R(Actual == 1)) - (nTarget^2 + nTarget)/2) / (nTarget * nBackground);
        auc = (sum(rank_orig .* p_target .* freq) - (nTarget^2 + nTarget)/2) / (nTarget * nBackground);


function wavelet_test09 ( )

%*****************************************************************************80
%
%% WAVELET_TEST09 tests DAUB18_TRANSFORM and DAUB18_TRANSFORM_INVERSE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 July 2011
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'WAVELET_TEST09\n' );
  fprintf ( 1, '  DAUB18_TRANSFORM computes the DAUB18 transform of a vector.\n' );
  fprintf ( 1, '  DAUB18_TRANSFORM_INVERSE inverts it.\n' );
%
%  Random data.
%
  n = 16;
  seed = 123456789;
  [ u, seed ] = r8vec_uniform_01 ( n, seed );

  v = daub18_transform ( n, u );

  w = daub18_transform_inverse ( n, v );

  fprintf ( 1, '\n' );
  fprintf ( 1, '   i      U(i)       D18(U)(i)  D18inv(D18(U))(i)\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %2d  %10.4f  %10.4f  %10.4f\n', i, u(i), v(i), w(i) );
  end
%
%  Constant signal.
%
  n = 8;
  u(1:n) = 1.0;

  v = daub18_transform ( n, u );

  w = daub18_transform_inverse ( n, v );

  fprintf ( 1, '\n' );
  fprintf ( 1, '   i      U(i)        D18(U)(i)  D18inv(D18(U))(i)\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %2d  %10.4f  %10.4f  %10.4f\n', i, u(i), v(i), w(i) );
  end
%
%  Linear signal.
%
  n = 16;
  a_first = 1.0;
  a_last = n;
  u = linspace ( a_first, a_last, n );

  v = daub18_transform ( n, u );

  w = daub18_transform_inverse ( n, v );

  fprintf ( 1, '\n' );
  fprintf ( 1, '   i      U(i)        D18(U)(i)  D18inv(D18(U))(i)\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %2d  %10.4f  %10.4f  %10.4f\n', i, u(i), v(i), w(i) );
  end
%
%  Quadratic data.
%
  n = 8;
  u(1) = 25.0;
  u(2) = 16.0;
  u(3) = 9.0;
  u(4) = 4.0;
  u(5) = 1.0;
  u(6) = 0.0;
  u(7) = 1.0;
  u(8) = 4.0;

  v = daub18_transform ( n, u );

  w = daub18_transform_inverse ( n, v );

  fprintf ( 1, '\n' );
  fprintf ( 1, '   i      U(i)        D18(U)(i)  D18inv(D18(U))(i)\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %2d  %10.4f  %10.4f  %10.4f\n', i, u(i), v(i), w(i) );
  end

  return
end


% sgwt_randmat : Compute random  (Erdos-Renyi model) graph
%
% function A=sgwt_randmat(N,thresh)
%
% Inputs :
% N - number of vertices
% thresh - probability of connection of each edge
%
% Outputs :
% A - adjacency matrix

% This file is part of the SGWT toolbox (Spectral Graph Wavelet Transform toolbox)
% Copyright (C) 2010, David K. Hammond.
%
% The SGWT toolbox 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.
%
% The SGWT toolbox is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with the SGWT toolbox.  If not, see <http://www.gnu.org/licenses/>.

function [A]=sgwt_randmat(N,thresh)
  assert(thresh<=1 && thresh>=0);
  A=rand(N)>1-thresh;
  B=triu(A);
  A=B+B';
  for i=1:size(A,1)
    A(i,i)=0;
  end
  A=sparse(A);


function [ row, col, a, seed ] = wathen_st ( nx, ny, nz_num, seed )

%*****************************************************************************80
%
%% WATHEN_ST: Wathen matrix stored in sparse triplet format.
%
%  Discussion:
%
%    When dealing with sparse matrices in MATLAB, it can be much more efficient
%    to work first with a triple of I, J, and X vectors, and only once
%    they are complete, convert to MATLAB's sparse format.
%
%    The Wathen matrix is a finite element matrix which is sparse.
%
%    The entries of the matrix depend in part on a physical quantity
%    related to density.  That density is here assigned random values between
%    0 and 100.
%
%    The matrix order N is determined by the input quantities NX and NY,
%    which would usually be the number of elements in the X and Y directions.
%
%    The value of N is
%
%      N = 3*NX*NY + 2*NX + 2*NY + 1,
%
%    The matrix is the consistent mass matrix for a regular NX by NY grid
%    of 8 node serendipity elements.
%
%    The local element numbering is
%
%      3--2--1
%      |     |
%      4     8
%      |     |
%      5--6--7
%
%    Here is an illustration for NX = 3, NY = 2:
%
%     23-24-25-26-27-28-29
%      |     |     |     |
%     19    20    21    22
%      |     |     |     |
%     12-13-14-15-16-17-18
%      |     |     |     |
%      8     9    10    11
%      |     |     |     |
%      1--2--3--4--5--6--7
%
%    For this example, the total number of nodes is, as expected,
%
%      N = 3 * 3 * 2 + 2 * 2 + 2 * 3 + 1 = 29
%
%    The matrix is symmetric positive definite for any positive values of the
%    density RHO(X,Y).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 July 2014
%
%  Author:
%
%    Original MATLAB version by Nicholas Higham.
%    Modifications by Tim Davis.
%    Modifications by John Burkardt.
%
%  Reference:
%
%    Nicholas Higham,
%    Algorithm 694: A Collection of Test Matrices in MATLAB,
%    ACM Transactions on Mathematical Software,
%    Volume 17, Number 3, September 1991, pages 289-305.
%
%    Andrew Wathen,
%    Realistic eigenvalue bounds for the Galerkin mass matrix,
%    IMA Journal of Numerical Analysis,
%    Volume 7, Number 4, October 1987, pages 449-457.
%
%  Parameters:
%
%    Input, integer NX, NY, values which determine the size of the matrix.
%
%    Input, integer NZ_NUM, the number of values used to describe the matrix.
%
%    Input/output, integer SEED, the random number seed.
%
%    Output, integer ROW(NZ_NUM), COL(NZ_NUM), the row and column indices
%    of the nonzero entries.
%
%    Output, real A(NZ_NUM), the nonzero values.
%
  if ( nargin < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'WATHEN_ST - Fatal error!\n' );
    fprintf ( 1, '  Not enough input.\n' );
    error ( 'WATHEN_ST - Fatal error!' );
  end

  if ( nargin < 2 )
    ny = nx;
    fprintf ( 1, '\n' );
    fprintf ( 1, '  NY was not supplied.  Setting NY = NX = %d.\n', ny );
  end

  if ( nargin < 3 )
    nz_num = wathen_st_size ( nx, ny );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  NZ_NUM was not supplied.  NZ_NUM = %d\n', nz_num );
  end

  if ( nargin < 4 )
    seed = 123456789;
  end

  em = [
     6.0, -6.0,  2.0, -8.0,  3.0, -8.0,  2.0, -6.0;
    -6.0, 32.0, -6.0, 20.0, -8.0, 16.0, -8.0, 20.0;
     2.0, -6.0,  6.0, -6.0,  2.0, -8.0,  3.0, -8.0;
    -8.0, 20.0, -6.0, 32.0, -6.0, 20.0, -8.0, 16.0;
     3.0, -8.0,  2.0, -6.0,  6.0, -6.0,  2.0, -8.0;
    -8.0, 16.0, -8.0, 20.0, -6.0, 32.0, -6.0, 20.0;
     2.0, -8.0,  3.0, -8.0,  2.0, -6.0,  6.0, -6.0;
    -6.0, 20.0, -8.0, 16.0, -8.0, 20.0, -6.0, 32.0 ]';

  row = zeros(nz_num,1);
  col = zeros(nz_num,1);
  a = zeros(nz_num,1);

  node = zeros(8,1);

  k = 0;

  for j = 1 : ny
    for i = 1 : nx

      node(1) = 3 * j * nx + 2 * i + 2 * j + 1;
      node(2) = node(1) - 1;
      node(3) = node(2) - 1;
      node(4) = ( 3 * j - 1 ) * nx + 2 * j + i - 1;
      node(5) = 3 * ( j - 1 ) * nx + 2 * i + 2 * j - 3;
      node(6) = node(5) + 1;
      node(7) = node(6) + 1;
      node(8) = node(4) + 1;

      [ rho, seed ] = r8_uniform_01 ( seed );
      rho = 100.0 * rho;

      for krow = 1 : 8
        for kcol = 1 : 8
          k = k + 1;
          row(k) = node(krow);
          col(k) = node(kcol);
          a(k) = rho * em(krow,kcol);
        end
      end

    end
  end

  return
end


classdef MW11 < PROBLEM
% <multi> <real> <large/none> <constrained>
% Constrained benchmark MOP proposed by Ma and Wang

%------------------------------- Reference --------------------------------
% Z. Ma and Y. Wang, Evolutionary constrained multiobjective optimization:
% Test suite construction and performance comparisons. IEEE Transactions on
% Evolutionary Computation, 2019, 23(6): 972-986.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 2;
            if isempty(obj.D); obj.D = 15; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values and constraint violations
        function Population = Evaluation(obj,varargin)
            X = varargin{1};
            X = max(min(X,repmat(obj.upper,size(X,1),1)),repmat(obj.lower,size(X,1),1));
            g = 1 + sum(2*(X(:,obj.M:end) + (X(:,obj.M-1:end-1) - 0.5).^2 - 1).^2,2);
            PopObj(:,1) = g.*X(:,1)*sqrt(1.9999);
            PopObj(:,2) = g.*sqrt(2 - (PopObj(:,1)./g).^2);
            PopCon(:,1) = -(3 - PopObj(:,1).^2 - PopObj(:,2)).*(3 - 2*PopObj(:,1).^2 - PopObj(:,2));
            PopCon(:,2) = (3 - 0.625*PopObj(:,1).^2 - PopObj(:,2)).*(3 - 7*PopObj(:,1).^2 - PopObj(:,2));
            PopCon(:,3) = -(1.62 - 0.18*PopObj(:,1).^2 - PopObj(:,2)).*(1.125 - 0.125*PopObj(:,1).^2 - PopObj(:,2));
            PopCon(:,4) = (2.07 - 0.23*PopObj(:,1).^2 - PopObj(:,2)).*(0.63 - 0.07*PopObj(:,1).^2 - PopObj(:,2));
            Population  = SOLUTION(X,PopObj,PopCon,varargin{2:end});
            obj.FE      = obj.FE + length(Population);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R(:,1)  = (0:1/(N-1):1)';
            R(:,2)  = 1 - R(:,1);
            R       = R./repmat(sqrt(sum(R.^2,2)/2),1,2);
            c1      = (3 - R(:,1).^2 - R(:,2)).*(3 - 2*R(:,1).^2 - R(:,2));
            c2      = (3 - 0.625*R(:,1).^2 - R(:,2)).*(3 - 7*R(:,1).^2 - R(:,2));
            c3      = (1.62 - 0.18*R(:,1).^2 - R(:,2)).*(1.125 - 0.125*R(:,1).^2 - R(:,2));
            c4      = (2.07 - 0.23*R(:,1).^2 - R(:,2)).*(0.63 - 0.07*R(:,1).^2 - R(:,2));
            invalid = c1<0 | c2>0 | c3<0 | c4>0;
            while any(invalid)
                R(invalid,:) = R(invalid,:).*1.001;
                R(any(R>2.2,2),:) = [];
                c1      = (3 - R(:,1).^2 - R(:,2)).*(3 - 2*R(:,1).^2 - R(:,2));
                c2      = (3 - 0.625*R(:,1).^2 - R(:,2)).*(3 - 7*R(:,1).^2 - R(:,2));
                c3      = (1.62 - 0.18*R(:,1).^2 - R(:,2)).*(1.125 - 0.125*R(:,1).^2 - R(:,2));
                c4      = (2.07 - 0.23*R(:,1).^2 - R(:,2)).*(0.63 - 0.07*R(:,1).^2 - R(:,2));
                invalid = c1<0 | c2>0 | c3<0 | c4>0;
            end
            R = [R;1,1];
            R = R(NDSort(R,1)==1,:);
        end
        %% Generate the feasible region
        function R = GetPF(obj)
            [x,y] = meshgrid(linspace(0,2.1,400));
            z     = nan(size(x));
            fes1  = -(3-x.^2-y).*(3-2*x.^2-y) <= 0;
            fes2  = (3-0.625*x.^2-y).*(3-7*x.^2-y) <= 0;
            fes3  = -(1.62-0.18*x.^2-y).*(1.125-0.125*x.^2-y) <= 0;
            fes4  = (2.07-0.23*x.^2-y).*(0.63-0.07*x.^2-y) <= 0;
            z(fes1 & fes2 & fes3 & fes4 & x.^2+y.^2>=2) = 0;
            R = {x,y,z};
        end
    end
end


function [reg_c,rho_c,eta_c] = l_corner(rho,eta,reg_param,U,s,b,method,M)
%L_CORNER Locate the "corner" of the L-curve.
%
% [reg_c,rho_c,eta_c] =
%        l_corner(rho,eta,reg_param)
%        l_corner(rho,eta,reg_param,U,s,b,method,M)
%        l_corner(rho,eta,reg_param,U,sm,b,method,M) ,  sm = [sigma,mu]
%
% Locates the "corner" of the L-curve in log-log scale.
%
% It is assumed that corresponding values of || A x - b ||, || L x ||,
% and the regularization parameter are stored in the arrays rho, eta,
% and reg_param, respectively (such as the output from routine l_curve).
%
% If nargin = 3, then no particular method is assumed, and if
% nargin = 2 then it is issumed that reg_param = 1:length(rho).
%
% If nargin >= 6, then the following methods are allowed:
%    method = 'Tikh'  : Tikhonov regularization
%    method = 'tsvd'  : truncated SVD or GSVD
%    method = 'dsvd'  : damped SVD or GSVD
%    method = 'mtsvd' : modified TSVD,
% and if no method is specified, 'Tikh' is default.  If the Spline Toolbox
% is not available, then only 'Tikh' and 'dsvd' can be used.
%
% An eighth argument M specifies an upper bound for eta, below which
% the corner should be found.

% Per Christian Hansen, IMM, July 26, 2007.
%
%
% This file is part of regtools [1] and covered by the BSD License
%
% Copyright (c) 2008, Per Christian Hansen
% All rights reserved.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are
% met:
%
%     * Redistributions of source code must retain the above copyright
%       notice, this list of conditions and the following disclaimer.
%     * Redistributions in binary form must reproduce the above copyright
%       notice, this list of conditions and the following disclaimer in
%       the documentation and/or other materials provided with the distribution
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
% POSSIBILITY OF SUCH DAMAGE.
%
% References:
% [1] http://www.mathworks.com/matlabcentral/fileexchange/file_infos/52-regtools

% Set default regularization method.
if (nargin <= 3)
  method = 'none';
  if (nargin==2), reg_param = (1:length(rho))'; end
else
  if (nargin==6), method = 'Tikh'; end
end

% Set this logical variable to 1 (true) if the corner algorithm
% should always be used, even if the Spline Toolbox is available.
alwayscorner = 0;

% Set threshold for skipping very small singular values in the
% analysis of a discrete L-curve.
s_thr = eps;  % Neglect singular values less than s_thr.

% Set default parameters for treatment of discrete L-curve.
deg   = 2;  % Degree of local smooting polynomial.
q     = 2;  % Half-width of local smoothing interval.
order = 4;  % Order of fitting 2-D spline curve.

% Initialization.
if (length(rho) < order)
  error('Too few data points for L-curve analysis')
end
if (nargin > 3)
  [p,ps] = size(s); [m,n] = size(U);
  beta = U'*b;
  if (m>n), b0 = b - U*beta; end
  if (ps==2)
    s = s(p:-1:1,1)./s(p:-1:1,2);
    beta = beta(p:-1:1);
  end
  xi = beta./s;
end

% Restrict the analysis of the L-curve according to M (if specified).
if (nargin==8)
  index = find(eta < M);
  rho = rho(index); eta = eta(index); reg_param = reg_param(index);
end

if (strncmp(method,'Tikh',4) | strncmp(method,'tikh',4))

  % The L-curve is differentiable; computation of curvature in
  % log-log scale is easy.

  % Compute g = - curvature of L-curve.
  g = lcfun(reg_param,s,beta,xi);

  % Locate the corner.  If the curvature is negative everywhere,
  % then define the leftmost point of the L-curve as the corner.
  [gmin,gi] = min(g);
  reg_c = fminbnd('lcfun',...
    reg_param(min(gi+1,length(g))),reg_param(max(gi-1,1)),...
    optimset('Display','off'),s,beta,xi); % Minimizer.
  kappa_max = - lcfun(reg_c,s,beta,xi); % Maximum curvature.

  if (kappa_max < 0)
    lr = length(rho);
    reg_c = reg_param(lr); rho_c = rho(lr); eta_c = eta(lr);
  else
    f = (s.^2)./(s.^2 + reg_c^2);
    eta_c = norm(f.*xi);
    rho_c = norm((1-f).*beta);
    if (m>n), rho_c = sqrt(rho_c^2 + norm(b0)^2); end
  end

elseif (strncmp(method,'tsvd',4) | strncmp(method,'tgsv',4) | ...
        strncmp(method,'mtsv',4) | strncmp(method,'none',4))

  % Use the adaptive pruning algorithm to find the corner, if the
  % Spline Toolbox is not available.
  if ~exist('splines','dir') | alwayscorner
    %error('The Spline Toolbox in not available so l_corner cannot be used')
    reg_c = corner(rho,eta);
    rho_c = rho(reg_c);
    eta_c = eta(reg_c);
    return
  end

  % Othersise use local smoothing followed by fitting a 2-D spline curve
  % to the smoothed discrete L-curve. Restrict the analysis of the L-curve
  % according to s_thr.
  if (nargin > 3)
    if (nargin==8)       % In case the bound M is in action.
      s = s(index,:);
    end
    index = find(s > s_thr);
    rho = rho(index); eta = eta(index); reg_param = reg_param(index);
  end

  % Convert to logarithms.
  lr = length(rho);
  lrho = log(rho); leta = log(eta); slrho = lrho; sleta = leta;

  % For all interior points k = q+1:length(rho)-q-1 on the discrete
  % L-curve, perform local smoothing with a polynomial of degree deg
  % to the points k-q:k+q.
  v = (-q:q)'; A = zeros(2*q+1,deg+1); A(:,1) = ones(length(v),1);
  for j = 2:deg+1, A(:,j) = A(:,j-1).*v; end
  for k = q+1:lr-q-1
    cr = A\lrho(k+v); slrho(k) = cr(1);
    ce = A\leta(k+v); sleta(k) = ce(1);
  end

  % Fit a 2-D spline curve to the smoothed discrete L-curve.
  sp = spmak((1:lr+order),[slrho';sleta']);
  pp = ppbrk(sp2pp(sp),[4,lr+1]);

  % Extract abscissa and ordinate splines and differentiate them.
  % Compute as many function values as default in spleval.
  P     = spleval(pp);  dpp   = fnder(pp);
  D     = spleval(dpp); ddpp  = fnder(pp,2);
  DD    = spleval(ddpp);
  ppx   = P(1,:);       ppy   = P(2,:);
  dppx  = D(1,:);       dppy  = D(2,:);
  ddppx = DD(1,:);      ddppy = DD(2,:);

  % Compute the corner of the discretized .spline curve via max. curvature.
  % No need to refine this corner, since the final regularization
  % parameter is discrete anyway.
  % Define curvature = 0 where both dppx and dppy are zero.
  k1    = dppx.*ddppy - ddppx.*dppy;
  k2    = (dppx.^2 + dppy.^2).^(1.5);
  I_nz  = find(k2 ~= 0);
  kappa = zeros(1,length(dppx));
  kappa(I_nz) = -k1(I_nz)./k2(I_nz);
  [kmax,ikmax] = max(kappa);
  x_corner = ppx(ikmax); y_corner = ppy(ikmax);

  % Locate the point on the discrete L-curve which is closest to the
  % corner of the spline curve.  Prefer a point below and to the
  % left of the corner.  If the curvature is negative everywhere,
  % then define the leftmost point of the L-curve as the corner.
  if (kmax < 0)
    reg_c = reg_param(lr); rho_c = rho(lr); eta_c = eta(lr);
  else
    index = find(lrho < x_corner & leta < y_corner);
    if ~isempty(index)
      [dummy,rpi] = min((lrho(index)-x_corner).^2 + (leta(index)-y_corner).^2);
      rpi = index(rpi);
    else
      [dummy,rpi] = min((lrho-x_corner).^2 + (leta-y_corner).^2);
    end
    reg_c = reg_param(rpi); rho_c = rho(rpi); eta_c = eta(rpi);
  end

elseif (strncmp(method,'dsvd',4) | strncmp(method,'dgsv',4))

  % The L-curve is differentiable; computation of curvature in
  % log-log scale is easy.

  % Compute g = - curvature of L-curve.
  g = lcfun(reg_param,s,beta,xi,1);

  % Locate the corner.  If the curvature is negative everywhere,
  % then define the leftmost point of the L-curve as the corner.
  [gmin,gi] = min(g);
  reg_c = fminbnd('lcfun',...
    reg_param(min(gi+1,length(g))),reg_param(max(gi-1,1)),...
    optimset('Display','off'),s,beta,xi,1); % Minimizer.
  kappa_max = - lcfun(reg_c,s,beta,xi,1); % Maximum curvature.

  if (kappa_max < 0)
    lr = length(rho);
    reg_c = reg_param(lr); rho_c = rho(lr); eta_c = eta(lr);
  else
    f = s./(s + reg_c);
    eta_c = norm(f.*xi);
    rho_c = norm((1-f).*beta);
    if (m>n), rho_c = sqrt(rho_c^2 + norm(b0)^2); end
  end

else
  error('Illegal method')
end


function r8poly_lagrange_0_test ( )

%*****************************************************************************80
%
%% R8POLY_LAGRANGE_0_TEST tests R8POLY_LAGRANGE_0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2009
%
%  Author:
%
%    John Burkardt
%
  npol = 5;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8POLY_LAGRANGE_0_TEST\n' );
  fprintf ( 1, '  R8POLY_LAGRANGE_0 evaluates the Lagrange\n' );
  fprintf ( 1, '  factor W(X) at a point.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The number of data points is %d\n', npol );
%
%  Set the abscissas of the polynomials.
%
  xlo = 0.0;
  xhi = npol - 1;

  xpol = r8vec_even ( npol, xlo, xhi );

  r8vec_print ( npol, xpol, '  Abscissas:' );
%
%  Evaluate W(X).
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '      X          W(X)\n' );
  fprintf ( 1, '\n' );

  nx = 4 * npol - 1;

  for ival = 1 : nx

    xval = r8vec_even_select ( nx, xlo, xhi, ival );

    w = r8poly_lagrange_0 ( npol, xpol, xval );

    fprintf ( 1, '%12f  %12e\n', xval, w );

  end

  return
end


function r = randp(lambda)
% randp(lambda) returns Poisson distributed Vector with mean lambda

try
  r = poissrnd(lambda);
  return
end

lambda   = reshape(lambda,1,[]);
aktiv    = find(lambda > 1e-10);
ll       = log(lambda(aktiv));
r(aktiv) = rand(1,length(aktiv));
r(find(lambda < 1e-10)) = 0;

i = 0;
while length(aktiv) > 0
		r(aktiv) = r(aktiv) - exp(i*ll - lambda(aktiv) - gammaln(i+1));
		ind = find(r(aktiv)<=0);
		r(aktiv(ind)) = i;
		aktiv(ind) = [];
		ll(ind) = [];
		i = i + 1;
end


function phi = correct_azimuth(phi)
%CORRECT_AZIMUTH ensures azimuth angle between -pi and +pi-eps
%
%   Usage: phi = correct_azimuth(phi)
%
%   Input parameters:
%       phi     - azimuth / rad. Can be a single value or a matrix.
%
%   Output paramteres:
%       phi     - angle between -pi and +pi-eps / rad
%
%   See also: correct_elevation, get_ir

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

%% ===== Checking of input  parameters ==================================
nargmin = 1;
nargmax = 1;
narginchk(nargmin,nargmax);

%% ===== Computation ====================================================
% Ensure -2pi <= phi <= 2pi
phi = rem(phi,2*pi);
% Ensure -pi <= phi < pi
phi(phi<-pi) = phi(phi<-pi) + 2*pi;
phi(phi>=pi) = phi(phi>=pi) - 2*pi;


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni
%
%
% problem 11 - graph of r[n] and of r[n+1]

% r[n]
 n=-3:6;
 u=(n>=0);
 r=n.*u;

 stem(n,r)
 title('Unit ramp sequence r[n]')
 ylim([-.1 6.1])

% r[n+1]
 figure
 n=-3:6;
 u1=(n>=-1);
 r=(n+1).*u1;

 stem(n,r)
 title('Unit ramp sequence r[n+1]')
 ylim([-.1 7.1])