function [ fea, out ] = ex_planestress6( varargin )
%EX_PLANESTRESS6 Plane stress analysis of a pressure vessel.
%
%   [ FEA, OUT ] = EX_PLANESTRESS6( VARARGIN ) Model example for plain stress
%   approximation of a pressure vessel (annular cross section with symmetry).
%
%   Accepts the following property/value pairs.
%
%       Input       Value/{Default}        Description
%       -----------------------------------------------------------------------------------
%       E           scalar {207e9}         Modulus of elasticity
%       nu          scalar {0.27}          Poissons ratio
%       sfun        string {sflag1}        Shape function for displacements
%       iplot       scalar 0/{1}           Plot solution (=1)
%                                                                                .
%       Output      Value/(Size)           Description
%       -----------------------------------------------------------------------------------
%       fea         struct                 Problem definition struct
%       out         struct                 Output struct

% Copyright 2013-2022 Precise Simulation, Ltd.

cOptDef = { ...
  'E',        207e9; ...
  'nu',       0.27; ...
  'sfun',     'sflag1'; ...
  'iplot',    1; ...
  'igrid',    1; ...
  'tol',      0.1; ...
  'fid',      1 };
[got,opt] = parseopt( cOptDef, varargin{:} );
fid       = opt.fid;

% Geometry and grid.
fea.sdim = { 'x' 'y' };   % Coordinate names.
fea.grid = ringgrid( 12, 216, 100e-3, 120e-3 );
fea.grid = delcells( fea.grid, selcells( fea.grid, '(x<=eps) | (y<=eps)') );
if( opt.igrid~=1 )
  fea.grid = quad2tri( fea.grid );
end
n_bdr = max(fea.grid.b(3,:));   % Number of boundaries.

% Problem definition.
fea = addphys( fea, @planestress );
fea.phys.pss.eqn.coef{1,end} = { opt.nu };
fea.phys.pss.eqn.coef{2,end} = { opt.E  };
fea.phys.pss.sfun            = { opt.sfun opt.sfun };

% Boundary conditions.
bctype = mat2cell( zeros(2,n_bdr), [1 1], ones(1,n_bdr) );
bctype{1,4} = 1;
bctype{2,3} = 1;
fea.phys.pss.bdr.coef{1,5} = bctype;

bccoef = mat2cell( zeros(2,n_bdr), [1 1], ones(1,n_bdr) );
bccoef{1,1} = '-nx*1e4';
bccoef{2,1} = '-ny*1e4';
fea.phys.pss.bdr.coef{1,end} = bccoef;

% Parse and solve problem.
fea       = parsephys( fea );
fea       = parseprob( fea );                          % Check and parse problem struct.
fea.sol.u = solvestat( fea, 'fid', fid, 'icub', 1+str2num(strrep(opt.sfun,'sflag','')) );   % Call to stationary solver.

% Postprocessing.
s_disp = fea.phys.pss.eqn.vars{2,end};
if( opt.iplot>0 )
  figure
  postplot( fea, 'surfexpr', s_disp )
  title( 'Total displacement' )
end

% Error checking.
s_sx     = fea.phys.pss.eqn.vars{5,end};
s_sy     = fea.phys.pss.eqn.vars{6,end};
s_sxy    = fea.phys.pss.eqn.vars{7,end};
s_sp1    = fea.phys.pss.eqn.vars{8,end};
s_sp3    = fea.phys.pss.eqn.vars{10,end};
s_ez     = fea.phys.pss.eqn.vars{13,end};
s_ep1    = fea.phys.pss.eqn.vars{15,end};
s_ep2    = fea.phys.pss.eqn.vars{16,end};
s_ep3    = fea.phys.pss.eqn.vars{17,end};
v_disp   = evalexpr( s_disp, [100e-3 120e-3-2*sqrt(eps);0 0]+sqrt(eps), fea )';
v_dref   = [2.809e-8 2.635e-8];
[v_sx(1),v_sx(2)] = minmaxsubd( s_sx, fea );
v_sxref  = [-10000 55454];
[v_sy(1),v_sy(2)] = minmaxsubd( s_sy, fea );
v_syref  = [-10000 55454];
[v_sxy(1),v_sxy(2)] = minmaxsubd( s_sxy, fea );
v_sxyref = [-32730 0];
[v_sp1(1),v_sp1(2)] = minmaxsubd( s_sp1, fea );
v_sp1ref = [4.5e4 55454];
[v_sp3(1),v_sp3(2)] = minmaxsubd( s_sp3, fea );
v_sp3ref = [-1e4 0];
[v_ez(1),v_ez(2)] = minmaxsubd( s_ez, fea );
v_ezref = [-5.929e-8 -5.929e-8];
[v_ep1(1),v_ep1(2)] = minmaxsubd( s_ep1, fea );
v_ep1ref = [2.196e-7 2.809e-7];
[v_ep2(1),v_ep2(2)] = minmaxsubd( s_ep2, fea );
v_ep2ref = [-5.929e-8 -5.929e-8];
[v_ep3(1),v_ep3(2)] = minmaxsubd( s_ep3, fea );
v_ep3ref = [-1.206e-7 -5.929e-8];
out.err  = [ abs([v_dref-v_disp])./v_dref ;
             abs([v_sxref-v_sx])./v_sxref ;
             abs([v_syref-v_sy])./v_syref ;
             abs([v_sxyref(1)-v_sxy(1)])./v_sxyref(1) 0 ;
             abs([v_sp1ref(2)-v_sp1(2)])./v_sp1ref(2) 0 ;
             abs([v_sp3ref(1)-v_sp3(1)])./v_sp3ref(1) 0 ;
             abs([v_ezref(1)-v_ez(1)])./v_ezref(1) 0 ;
             abs([v_ep1ref(1)-v_ep1(1)])./v_ep1ref(1) 0 ;
             abs([v_ep2ref(1)-v_ep2(1)])./v_ep2ref(1) 0 ;
             abs([v_ep3ref(1)-v_ep3(1)])./v_ep3ref(1) 0 ];
out.pass = all( out.err(:) <= opt.tol );

if( nargout==0 )
  clear fea out
end


close all
clear all

N=5;
names={'A','B','C','D','E'};
dag=zeros(N);
dag(1,2)=1;
dag(2,[3 4])=1;
dag([3 4],5)=1;
[xx yy] = draw_graph(dag,names,ones(N,1));
title('Cheng example.');

node_sizes=2*ones(1,N);
bnet=mk_bnet(dag,node_sizes);

if 1
  disp('Generating DataSet');
  bnet.CPD{1} = tabular_CPD(bnet, 1, [0.4 0.6]);
  bnet.CPD{2} = tabular_CPD(bnet, 2, [0.2 0.3 0.8 0.7]);
  bnet.CPD{3} = tabular_CPD(bnet, 3, [0.1 0.2 0.9 0.8]);
  bnet.CPD{4} = tabular_CPD(bnet, 4, [0.6 0.8 0.4 0.2]);
  bnet.CPD{5} = tabular_CPD(bnet, 5, [0.9 0.8 0.7 0.6 0.1 0.2 0.3 0.4]);
  m=10000;
  cheng = cell(N,m);
  for i=1:m
    cheng(:,i)=sample_bnet(bnet);
  end
  cheng = cell2num(cheng);
  %save -ascii cheng cheng
else
  load -ascii cheng.mat
end

%  profile clear
%  profile on
  [Phase_3, Phase_2, Phase_1, UPhase_3] = learn_struct_bnpc(cheng, node_sizes, 0.05, 0)
%  profile off
%  profile report report_cheng

figure
draw_graph(Phase_1,names,ones(N,1),xx,yy);
title('PhaseI');
figure
draw_graph(Phase_2,names,ones(N,1),xx,yy);
title('PhaseII');
%figure
%draw_graph(UPhase_3,names,ones(N,1),xx,yy);
%title('undirected PhaseIII');
figure
draw_graph(Phase_3,names,ones(N,1),xx,yy);
title('PhaseIII');


% ir_example_ct_lir1
% example of local impulse response (LIR) in CT
% Copyright 2012-07-28, Jeff Fessler, University of Michigan

if ~isvar('A'), printm 'setup geometry, image, sinogram'
	f.down = 4;
	ig = image_geom('nx', 512, 'fov', 30, 'down', f.down);
	ig.mask = ig.circ > 0;
	sg = sino_geom('par', 'nb', ig.nx, 'na', ig.nx, ...
			'dr', ig.dx, 'strip_width', 'd');
	sg.plot(ig);

	ell = [0 0 10 10 0 0.2;
                 0  6 3 3 0  0.2;
                 6  0 3 3 0  0.1
                 0 -6 3 3 0 -0.1;
                -6  0 3 3 0 -0.2;
		];
        xtrue = ellipse_im(ig, ell, 'oversample', 2);

%	A = Gtomo2_dscmex(sg, ig);
	A = Gtomo2_wtmex(sg, ig, 'nthread', jf('ncore'));
	ytrue = A * xtrue;
	wi = exp(-ytrue); % ideal Poisson weighting

	im plc 2 3
	clim = [0 0.4];
	im(1, xtrue, 'x', clim), cbar
	im(2, ytrue, 'y: ideal sinogram'), cbar
	im(3, wi, 'w: ideal weighting'), cbar
%	ir_savefig ir_example_ct_lir1_x_y_w
prompt
end

if ~isvar('yp'), printm 'yp'
	ix = round(ell(2:end, 1) / ig.dx + (ig.nx-1)/2);
	iy = round(ell(2:end, 2) / ig.dy + (ig.ny-1)/2);
	xp = ig.zeros;
	for ii=1:numel(ix)
		xp = xp + ig.unitv(ix(ii), iy(ii));
	end
	f.amp = 0.1;
	xp = f.amp * xp ;
	im(2, xtrue + xp)

	yp = A * xp;
	climp = [0 f.amp];
	im(3, xp, climp), cbar
	im(4, yp)
end

if ~isvar('fbp'), printm 'fbp'
	tmp = fbp2(sg, ig);
	fbp = fbp2(yp, tmp, 'window', 'boxcar,0.8');
	im(2, fbp, 'FBP', climp), cbar
prompt
end

if ~isvar('fw.fbp'), printm 'fw.fbp'
	ox = [-9:9]; oy = ox;
	fw.fun = @(x, ii) fwhm2(x(ix(ii)+ox, iy(ii)+oy));
	for ii=1:numel(ix)
		fw.fbp(ii) = fw.fun(fbp, ii);
	end
	pr fw.fbp
end

if ~isvar('kappa'), printm 'kappa: try to make resolution approximately uniform'
	kappa = sqrt( div0(A' * wi, A' * ones(size(wi))) );
	im(4, kappa), cbar
prompt
end

% use local psf to help select beta
if ~isvar('R1'), printm 'R1, R2'
	f.l2b = 0;
	W = Gdiag(wi);
	R1 = Reg1(ig.mask * kappa(end/2+1,end/2+1), 'beta', 2^f.l2b); % usual
%	qpwls_psf(A, R1, 1, ig.mask, W, 'loop', 1); % choose beta
	R2 = Reg1(kappa, 'beta', 2^f.l2b); % kappa
	qpwls_psf(A, R2, 1, ig.mask, W, 'loop', 1); % choose beta
prompt
end

%	psf = qpwls_psf(A, R1, 1, ig.mask, W);
%	init = conv2(psf, xp, 'same');
	init = fbp;
	im(init, climp)

if ~isvar('xpwls1'), printm 'pwls1'
	f.niter = 300;
	xpwls1 = pwls_pcg1(init(ig.mask), A, W, yp(:), R1, 'niter', f.niter);
	xpwls1 = ig.embed(xpwls1);
	im(3, xpwls1, 'PWLS R1'), cbar
end

if ~isvar('xpwls2'), printm 'pwls2'
	xpwls2 = pwls_pcg1(init(ig.mask), A, W, yp(:), R2, 'niter', f.niter);
	xpwls2 = ig.embed(xpwls2);
	im(4, xpwls2, 'PWLS R2'), cbar
end

if 1
	for ii=1:numel(ix)
		fw.pwls1(ii) = fw.fun(xpwls1, ii);
		fw.pwls2(ii) = fw.fun(xpwls2, ii);
	end
	pr fw.fbp
	pr fw.pwls1
	pr fw.pwls2
end

if 1
	climp = [-0.004 0.05];
%	climp = []
	im plc 1 3
	ax = [18 110 20 120];
	im(1, fbp, 'FBP', climp), axis(ax)%, cbar h
	fun = @(fw, ii) text(ix(ii), iy(ii)+15, ...
		sprintf('%3.1f', fw(ii)), 'horiz', 'center', 'color', 'green');
	for ii=1:numel(ix), fun(fw.fbp, ii); end
	im(2, xpwls1, 'PWLS standard R', climp), axis(ax)%, cbar h
	for ii=1:numel(ix), fun(fw.pwls1, ii); end
	im(3, xpwls2, 'PWLS modified R', climp), axis(ax)%, cbar h
	for ii=1:numel(ix), fun(fw.pwls2, ii); end

%	ir_savefig eps_c ir_example_ct_lir1_fwhm
end


function [ a, ipvt, rcond ] = r8mat_geco ( a, n )

%*****************************************************************************80
%
%% R8MAT_GECO factors a real matrix and estimates its condition number.
%
%  Discussion:
%
%    For the system A * X = B, relative perturbations in A and B
%    of size EPSILON may cause relative perturbations in X of size
%    EPSILON/RCOND.
%
%    If RCOND is so small that the logical expression
%      1.0 + RCOND == 1.0
%    is true, then A may be singular to working precision.  In particular,
%    RCOND is zero if exact singularity is detected or the estimate
%    underflows.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 June 2005
%
%  Author:
%
%    Original FORTRAN77 version by Cleve Moler.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1,
%    LC: QA214.L56.
%
%  Parameters:
%
%    Input, real A(N,N), a matrix to be factored.
%
%    Input, integer N, the order of the matrix A.
%
%    Output, real A(LDA,N), the LU factorization of the matrix.
%
%    Output, integer IPVT(N), the pivot indices.
%
%    Output, real RCOND, an estimate of the reciprocal
%    condition number of A.
%

%
%  Compute the L1 norm of A.
%
  anorm = 0.0;
  for j = 1 : n
    anorm = max ( anorm, sum ( abs ( a(1:n,j) ) ) );
  end
%
%  Compute the LU factorization.
%
  [ a, ipvt, info ] = r8mat_gefa ( a, n );
%
%  RCOND = 1 / ( norm(A) * (estimate of norm(inverse(A))) )
%
%  estimate of norm(inverse(A)) = norm(Z) / norm(Y)
%
%  where
%    A * Z = Y
%  and
%    A' * Y = E
%
%  The components of E are chosen to cause maximum local growth in the
%  elements of W, where U'*W = E.  The vectors are frequently rescaled
%  to avoid overflow.
%
%  Solve U' * W = E.
%
  ek = 1.0;
  z(1:n) = 0.0;

  for k = 1 : n

    if ( z(k) ~= 0.0 )
      ek = - abs ( ek ) * r8_sign ( z(k) );
    end

    if ( abs ( a(k,k) ) < abs ( ek - z(k) ) )
      s = abs ( a(k,k) ) / abs ( ek - z(k) );
      z(1:n) = s * z(1:n);
      ek = s * ek;
    end

    wk = ek - z(k);
    wkm = -ek - z(k);
    s = abs ( wk );
    sm = abs ( wkm );

    if ( a(k,k) ~= 0.0 )
      wk = wk / a(k,k);
      wkm = wkm / a(k,k);
    else
      wk = 1.0;
      wkm = 1.0;
    end

    if ( k+1 <= n )

      for j = k+1 : n
        sm = sm + abs ( z(j) + wkm * a(k,j) );
        z(j) = z(j) + wk * a(k,j);
        s = s + abs ( z(j) );
      end

      if ( s < sm )
        t = wkm - wk;
        wk = wkm;
        z(k+1:n) = z(k+1:n) + t * a(k,k+1:n);
      end

    end

    z(k) = wk;

  end

  z(1:n) = z(1:n) / sum ( abs ( z(1:n) ) );
%
%  Solve L' * Y = W
%
  for k = n : -1 : 1

    z(k) = z(k) + z(k+1:n) * a(k+1:n,k);

    if ( 1.0 < abs ( z(k) ) )
      z(1:n) = z(1:n) / abs ( z(k) );
    end

    l = ipvt(k);

    t = z(l);
    z(l) = z(k);
    z(k) = t;

  end

  z(1:n) = z(1:n) / sum ( abs ( z(1:n) ) );

  ynorm = 1.0;
%
%  Solve L * V = Y.
%
  for k = 1 : n

    l = ipvt(k);

    t = z(l);
    z(l) = z(k);
    z(k) = t;

    z(k+1:n) = z(k+1:n) + t * a(k+1:n,k)';

    if ( 1.0 < abs ( z(k) ) )
      ynorm = ynorm / abs ( z(k) );
      z(1:n) = z(1:n) / abs ( z(k) );
    end

  end

  s = sum ( abs ( z(1:n) ) );
  z(1:n) = z(1:n) / s;
  ynorm = ynorm / s;
%
%  Solve U * Z = V.
%
  for k = n : -1 : 1

    if ( abs ( a(k,k) ) < abs ( z(k) ) )
      s = abs ( a(k,k) ) / abs ( z(k) );
      z(1:n) = s * z(1:n);
      ynorm = s * ynorm;
    end

    if ( a(k,k) ~= 0.0 )
      z(k) = z(k) / a(k,k);
    else
      z(k) = 1.0;
    end

    z(1:k-1) = z(1:k-1) - z(k) * a(1:k-1,k)';

  end
%
%  Normalize Z in the L1 norm.
%
  s = 1.0 / sum ( abs ( z(1:n) ) );
  z(1:n) = s * z(1:n);
  ynorm = s * ynorm;

  if ( anorm ~= 0.0 )
    rcond = ynorm / anorm;
  else
    rcond = 0.0;
  end

  return
end


function r = sqrt(a)
%SQRT         Taylor square root sqrt(a)
%

% written  05/21/09     S.M. Rump
% modified 08/26/12     S.M. Rump  global variables removed
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  K1 = getappdata(0,'INTLAB_TAYLOR_ORDER') + 1;

  r = a;
  r.t(1,:) = sqrt(a.t(1,:));
  rt2 = 2*r.t(1,:);                     % almost 10 % faster
  for j=2:K1
    r.t(j,:) = ( a.t(j,:) - sum(r.t(2:j-1,:).*r.t(j-1:-1:2,:),1) ) ./ (rt2);
  end
% straight version is faster
%   for j=2:K1
%     if even(j)
%       r.t(j,:) = ( a.t(j,:)/2 - sum(r.t(2:(j/2),:).*r.t(j-1:-1:(j/2)+1,:),1) ) ./ r.t(1,:);
%     else
%       r.t(j,:) = ( a.t(j,:)/2 - sum(r.t(2:((j-1)/2),:).*r.t(j-1:-1:(j+3)/2,:),1) - ...
%                               (r.t((j+1)/2,:).^2)/2 ) ./ r.t(1,:);
%     end
%   end

  if rndold
    setround(rndold)
  end


function stroud_test03 ( )

%*****************************************************************************80
%
%% TEST03 tests BALL_UNIT_07_3D, BALL_UNIT_14_3D, BALL_UNIT_15_3D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 April 2009
%
%  Author:
%
%    John Burkardt
%
  global FUNC_3D_INDEX;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST03\n' );
  fprintf ( 1, '  For integrals in the unit ball in 3D:\n' );
  fprintf ( 1, '  BALL_UNIT_07_3D uses a formula of degree 7;\n' );
  fprintf ( 1, '  BALL_UNIT_14_3D uses a formula of degree 14;\n' );
  fprintf ( 1, '  BALL_UNIT_15_3D uses a formula of degree 15.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Unit ball volume = %f\n', ball_unit_volume_nd ( 3 ) );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    Rule:      #7             #14           #15\n' );
  fprintf ( 1, '    F(X)\n' );
  fprintf ( 1, '\n' );

  num = function_3d_num ( );

  for i = 1 : num

    FUNC_3D_INDEX = i;

    result1 = ball_unit_07_3d ( 'function_3d' );
    result2 = ball_unit_14_3d ( 'function_3d' );
    result3 = ball_unit_15_3d ( 'function_3d' );

    fname = function_3d_name ( i );

    fprintf ( 1, '  %7s  %12f  %12f  %12f\n', ...
      fname, result1, result2, result3 );

  end

  return
end


% Jiao Xianjun (putaoshu@msn.com; putaoshu@gmail.com)
% convert a sequence to a mat. each column shift previos to left by 1
% for example input sequence 1 2 3 4 5 6 7, and len_body=4
% output matrix:
% 1 2 3 4
% 2 3 4 5
% 3 4 5 6
% 4 5 6 7

% A script of project: https://github.com/JiaoXianjun/multi-rtl-sdr-calibration

function r = lin2col_shift_mat(s, len_body)

if length(s) < len_body
    disp('Length of input is too short!');
    r = -1;
    return;
end

len_tail = length(s) - len_body;

r = toeplitz(s, [s(1) zeros(1, len_tail)]);
r = r((len_tail+1):end, end:-1:1);


function faces = sphDelaunay(dirs)
%SPHDELAUNAY Computes the Delaunay triangulation on the unit sphere
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% SPHDELAUNAY.M - 10/10/2013
% Archontis Politis, archontis.politis@aalto.fi
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Convert to cartesian
N_vert = size(dirs, 1);
[tempx, tempy, tempz] = sph2cart(dirs(:,1), dirs(:,2), ones(N_vert,1));
U_vert = [tempx, tempy, tempz];

% Find the convex hull of the points on the sphere - in this special case
% the result equals the Delaunay triangulation of the points
faces = convhulln(U_vert);

% Invert the triangles
faces = faces(:, 3:-1:1);

% Shift the results to begin each triangle from the smallest entry
for n = 1:size(faces,1)
    tempface = faces(n,:);
    [~, minIdx] = min(tempface);
    faces(n, :) = circshift(tempface, [0 1-minIdx]);
end

% Sort through triangles with smaller entries first
faces = sortrows(faces, 1); % sort through first entry
maxentry = max(faces(:,1)); % sort through second entry
n = 1;
while n <= maxentry
    startIdx = find(faces(:,1) == n, 1, 'first');
    if ~isempty(startIdx)
        endIdx = find(faces(:,1) == n, 1, 'last');
        faces(startIdx:endIdx, :) = sortrows(faces(startIdx:endIdx, :), 2);
        n = n + 1;
    else
        n = n + 1;
    end
end


function [centers, radii] = SphericalHashing(data, bit)
%Spherical Hashing method by Jae-Pil Heo

    [N, D] = size(data);
    centers = random_center(data, bit);
    [O1, O2, radii, avg, stddev] = compute_statistics(data, centers);

    iter = 1;
    while true
        forces = zeros(bit, D);
        for i = 1:bit - 1
            for j = i + 1:bit
                force = 0.5 * (O2(i, j) - N / 4) / (N / 4) * (centers(i, :) - centers(j, :));
                forces(i, :) = forces(i, :) + force ./ bit;
                forces(j, :) = forces(j, :) - force ./ bit;
            end
        end
        centers = centers + forces;

        [O1, O2, radii, avg, stddev] = compute_statistics(data, centers);

        if avg <= 0.1 * N / 4 && stddev <= 0.15 * N / 4
            break;
        end
        if iter >= 100
            fprintf('iter exceed 100, avg = %f, stddev = %f\n', avg, stddev);
        end

        iter = iter + 1;
    end
    %fprintf('iteration = %d\n', iter);
end

function centers = random_center(data, bit)
    [N, D] = size(data);
    centers = zeros(bit, D);
    for i = 1:bit
        R = randperm(N);
        sample = data(R(1:5), :);
        sample = sum(sample, 1) / 5;
        centers(i, :) = sample(:);
    end
end

function [O1, O2, radii, avg, stddev] = compute_statistics(data, centers)
    [N, D] = size(data);
    bit = size(centers, 1);

    dist = EuDist2(centers,data);
    sort_dist = sort(dist, 2);
    radii = sort_dist(:, floor(N / 2));
    dist = dist <= repmat(radii, 1, N);
    dist = dist * 1.0;

    O1 = sum(dist, 2);
    avg = 0;
    avg2 = 0;
    O2 = dist * dist';
    for i = 1:bit-1
        for j = i + 1:bit
            avg = avg + abs(O2(i, j) - N / 4);
            avg2 = avg2 + O2(i, j);
        end
    end

    avg = avg / (bit * (bit - 1) / 2);
    avg2 = avg2 / (bit * (bit - 1) / 2);
    stddev = 0;
    for i = 1:bit - 1
        for j = i + 1:bit
            stddev = stddev + (O2(i, j) - avg2) ^ 2;
        end
    end
    stddev = sqrt(stddev / (bit * (bit - 1) / 2));
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% mcs.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function [xbest,fbest,xmin,fmi,ncall,ncloc,flag]=mcs(fcn,data,u,v,prt,
% smax,nf,stop,iinit,local,gamma,hess)
% MCS global optimization for function defined by fcn in the
% n-dimensional box [u,v]
%
% Input:
% fcn = 'fun' 	name of function fun(data,x), x an n-vector
% data		data vector (or other data structure)
% [u,v]       	box in which the optimization is carried out (u, v
%             	n-vectors)
% prt		print level
% 		prt = 0: no printing
% 		prt = 1: # sweep, minimal nonempty level, # f-calls,
% 		best point and function value (default)
% 		prt > 1: only meaningful for test functions with known
% 		global minimizers
% 		in addition levels and function values of boxes
% 		containing the global minimizers of a test function
% smax        	number of levels (default: 5*n+10)
% nf         	maximum number of function evaluations (default: 50*n^2)
% stop         	stop(1) in ]0,1[:  relative error with which the known
%		 global minimum of a test function should be found
%		 stop(2) = fglob known global minimum of a test function
%		 stop(3) = safeguard parameter for absolutely small
%		 fglob
%		stop(1) >= 1: the program stops if the best function
%		 value has not been improved for stop(1) sweeps
%		stop(1) = 0: the user can specify a function value that
%		 should be reached
%                stop(2) = function value that is to be achieved
%             	(default: stop = 3*n)
% iinit       	parameter defining the initialization list
%             	= 0        corners and midpoint (default for finite u,v)
%             	= 1        safeguarded version *default otherwise)
% 		= 2        5u/6 + v/6, u/6 + 5v/6 and midpoint
%		= 3        initialization list with line searches
%             	otherwise  self-defined init. list (to be stored in
%			   init0.m)
%		for a self-defined initialization list, the user should
% 		provide an m-script file init0.m containing a matrix x0
%		with n rows and at least 3 columns and two n-vectors l
%		and L
%		the ith column of x0 contains the initialization list
%		values for the ith coordinate, their number is L(i), and
%		x0(i,l(i)) is the ith coordinate of the initial point
% local		local = 0: no local search
%		otherwise: maximal number of steps in local search
%		(default: 50)
% gamma		stopping criterion for local search (default: eps)
%           	the local search is stopped if abs(g)'*max(abs(x),
%		abs(xold)) < gamma*(f0-f)
% hess		sparsity pattern of the Hessian for local search
%		(default: hessian = ones(n,n))
%
% Output:
% xbest(1:n)  	current best point
% fbest    	function value at xbest
% xmin        	matrix with n rows; the columns are the points in the
%             	'shopping basket' (i.e. good points resp. local
%		minimizers)
% fmi         	function values corresponding to the 'shopping basket';
%		fmi(i) is the function value at xmin(:,i)
% ncall       	number of function evaluations
% ncloc		number of function evaluations used for local search
% flag        	specifies which stopping criterion has been used
%             	= 0  a (known) global minimum fglob of a test function
%                    has been found with the required relative error
%		     relerr
%             	= 1  the division procedure has been completed
%             	= 2  the maximum number nf of function calls has been
%                    reached without finding a known minimum with the
%                    required relative error or completing the division
%                    procedure
%		= 3  stop(1) sweeps without progress (for stop(1) >= 1)
%
% Uses the following m-files (directly or indirectly):
% addloc.m
% basket.m
% basket1.m
% bounds.m
% chkloc.m
% chrelerr.m
% chvtr.m
% csearch.m 	called by lsearch.m
% exgain.m
% fbestloc.m
% genbox.m
% hessian.m	called by triple.m
% init.m
% initbox.m
% initlist.m
% gls.m and its subprograms   called by lsearch.m
% lsearch.m
% minq.m and its subprograms  called by lsearch.m
% neighbor.m
% polint.m	called by exgain.m and initbox.m
% polint1.m	called by triple.m
% quadmin.m
% quadpol.m
% range.m	called by lsearch.m
% splinit.m
% split.m
% split1.m
% split2.m   	called by splrnk.m
% splrnk.m
% strtsw.m
% subint.m
% triple.m	called by lsearch.m
% updtf.m    	called by vertex.m
% updtoptl.m
% updtrec.m   	called by splinit.m and split.m
% vert1.m    	called by vertex.m
% vert2.m    	called by vertex.m
% vert3.m    	called by vertex.m
% vertex.m

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [xbest,fbest,xmin,fmi,ncall,ncloc,flag]=mcs(fcn,data,u,v,prt,smax,nf,stop,iinit,local,gamma,hess)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% global variables
global foptbox nbasket nboxes ncall nglob nsweep nsweepbest optlevel  record xglob xloc
% foptbox(1:nglob)  function value(s) of the box(es) containing the (a)
%             	global minimizer of a test function
% nbasket   	counter for boxes in the 'shopping basket'
% nboxes      	counter for boxes not in the 'shopping basket'
% nglob       	number of global minimizers of a test function
% nloc		(for local ~= 0) counter of points that have been used
% 		as starting points for a local search
% nsweep      	sweep counter
% nsweepbest    number of sweep in which fbest was updated for the last
%		time
% optlevel    	level(s) of the box(es) containing the (a) global
%             	minimum of a test function
% record(1:smax-1) record(i) points to the best non-split box at level i
%             	(record list)
% xglob(1:n,1:nglob)  xglob(:,i), i=1:nglob, are the global minimizers
% of a test function in [u,v]
% xloc(1:n,:)	(for local ~= 0) columns are the points that have been
%		used as starting points for local search
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
n = length(u);

% check box bounds
if ~isempty(find(v<u))
  error('incompatible box bounds')
elseif ~isempty(find(u==v))
  error('degenerate box bound')
end

% default values for the input parameters
if nargin < 5, prt = 1; end
if nargin < 6, smax = 5*n+10; end
if nargin < 7, nf = 50*n^2; end
if nargin < 8, stop = 3*n; end
if nargin < 9,
  if isempty(find(isinf(u))) & isempty(find(isinf(v)))
    iinit = 0;
  else
    iinit = 1;
  end
end
if nargin < 10, local = 50; end
if nargin < 11, gamma = eps; end
if nargin < 12, hess = ones(n,n); end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
xmin=[];fmi=[];		% avoid warnings in Matlab5

% initial values for the numbers of function calls (total number/local
% search)
ncall = 0;
ncloc = 0;

% some parameters needed for initializing large arrays
step1 = 10000;
step = 1000;
dim = step1;

% initialization of some large arrays
isplit = zeros(1,step1);
level = zeros(1,step1);
ipar = zeros(1,step1);
ichild = zeros(1,step1);
f = zeros(2,step1);
z = zeros(2,step1);
nogain = zeros(1,step1);

% definition of the initialization list
if iinit == 0
  x0(:,1) = u;
  x0(:,2) = (u+v)/2;
  x0(:,3) = v;
  l = 2*ones(n,1);
  L = 3*ones(n,1);
elseif iinit == 1
  for i = 1:n
    if u(i) >= 0
      x0(i,1) = u(i); [x0(i,2),x0(i,3)] = subint(u(i),v(i));x0(i,2) = 0.5*(x0(i,1)+x0(i,3));
    elseif v(i) <= 0
      x0(i,3) = v(i); [x0(i,2),x0(i,1)] = subint(v(i),u(i));x0(i,2) = 0.5*(x0(i,1)+x0(i,3));
    else
      x0(i,2) = 0; [xi,x0(i,1)] = subint(0,u(i)); [xi,x0(i,3)] = subint(0,v(i));
    end
  end
  l = 2*ones(n,1);
  L = 3*ones(n,1);
elseif iinit == 2
  x0(:,1) = (5*u + v)/6;
  x0(:,2) = 0.5*(u + v);
  x0(:,3) = (u + 5*v)/6;
  l = 2*ones(n,1);
  L = 3*ones(n,1);
elseif iinit == 3
  [x0,f0,l,L,istar,ncall1] = initlist(fcn,data,u,v);
  ncall = ncall + ncall1;
else
  init0 	%self-defined initialization list
  for i=1:size(x0,2)
    if ~isempty(find(x0(:,i)<u)) | ~isempty(find(x0(:,i)>v))
      error('incorrect initialization list')
    end
  end
end

% check whether there are infinities in the initialization list
if ~isempty(find(isinf(x0))), error('infinities in ititialization list'), end

% computation of the function values f0 appertaining to the init. list
% and the pointer istar to the best point in the initialization list
if iinit ~= 3
  [f0,istar,ncall1] = init(fcn,data,x0,l,L,n);
  ncall = ncall + ncall1;
end

% definition of the base vertex of the original box
for i = 1:n
  x(i) = x0(i,l(i));
end

% definition of the opposite vertex v1 of the original box
for i = 1:n
  if abs(x(i)-u(i)) > abs(x(i)-v(i))
    v1(i) = u(i);
  else
    v1(i) = v(i);
  end
end

% initialization of the record list, the counters nboxes, nbasket, m
% and nloc, xloc and the output flag
record = zeros(smax-1,1);
nboxes = 1;
nbasket = 0;
nbasket0 = 0;
nsweep = 0;
m = n;
record(1) = 1;
nloc = 0;
xloc = [];
flag = 1;

[ipar,level,ichild,f,isplit,p,xbest,fbest] = initbox(x0,f0,l,L,istar,u,v,prt);
% generates the boxes in the initialization procedure
f0min = fbest;
if stop(1) > 0 & stop(1) < 1
  flag = chrelerr(fbest,stop);
elseif stop(1) == 0
  flag = chvtr(fbest,stop(2));
end
if ~flag,return,end
% if the (known) minimum function value fglob has been found with the
% required tolerance, flag is set to 0 and the program is terminated

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
s = strtsw(smax,level,f(1,:));
% the vector record is updated, and the minimal level s containing
% non-split boxes is computed
nsweep = nsweep + 1;	% sweep counter

while s < smax & ncall + 1 <= nf
  par = record(s);   % the best box at level s is the current box
  [n0,x,y,x1,x2,f1,f2] = vertex(par,n,u,v,v1,x0,f0,ipar,isplit,ichild,z,f,l,L);
  % compute the base vertex x, the opposite vertex y, the 'neighboring'
  % vertices and their function values needed for quadratic
  % interpolation and the vector n0 indicating that the ith coordinate
  % has been split n0(i) times in the history of the box
  if s > 2*n*(min(n0)+1)
  % s 'large'
    [isplit(par),z(2,par)] = splrnk(n,n0,p,x,y);
    % splitting index and splitting value z(2,par) for splitting by
    % rank are computed
    % z(2,par) is set to Inf if we split according to the init. list
    splt = 1;  % indicates that the box is to be split
  else
    if nogain(par) % box has already been marked as not eligible for splitting
                   % by expected gain
      splt = 0;
    else
      [e,isplit(par),z(2,par)] = exgain(n,n0,l,L,x,y,x1,x2,f(1,par),f0,f1,f2);
      % splitting by expected gain
      % compute the expected gain vector e and the potential splitting
      % index and splitting value
      fexp = f(1,par) + min(e);
      if fexp < fbest
        splt = 1;
      else
        splt = 0;  % the box is not split since we expect no improvement
        nogain(par) = 1; % the box is marked as not eligible for splitting by expected gain
      end
    end
  end
  if splt == 1  % prepare for splitting
    i = isplit(par);
    level(par) = 0;
    if z(2,par) == Inf % prepare for splitting by initialization list
      m = m + 1;
      z(2,par) = m;
      [xbest,fbest,f0(:,m),xmin,fmi,ipar,level,ichild,f,flag,ncall1] = splinit(fcn,data,i,s,smax,par,x0,n0,u,v,x,y,x1,x2,L,l,xmin,fmi,ipar,level,ichild,f,xbest,fbest,stop,prt);
      ncall = ncall + ncall1;
    else  % prepare for default splitting
      z(1,par) = x(i);
      [xbest,fbest,xmin,fmi,ipar,level,ichild,f,flag,ncall1] = split(fcn,data,i,s,smax,par,n0,u,v,x,y,x1,x2,z(:,par),xmin,fmi,ipar,level,ichild,f,xbest,fbest,stop,prt);
      ncall = ncall + ncall1;
    end
    if nboxes > dim
% if the pre-assigned size of the `large' arrays has already been exceeded, these arrays are made larger
      isplit(nboxes+1:nboxes+step) = zeros(1,step);
      level(nboxes+1:nboxes+step) = zeros(1,step);
      ipar(nboxes+1:nboxes+step) = zeros(1,step);
      ichild(nboxes+1:nboxes+step) = zeros(1,step);
      z(:,nboxes+1:nboxes+step) = zeros(2,step);
      nogain(nboxes+1:nboxes+step) = zeros(1,step);
      f(:,nboxes+1:nboxes+step) = zeros(2,step);
      dim = nboxes + step;
    end
    if ~flag,break,end
  else  % splt=0: no splitting, increase the level by 1
    if s + 1 < smax
      level(par) = s + 1;
      updtrec(par,s+1,f(1,:));
    else
      level(par) = 0;
      nbasket = nbasket + 1;
      xmin(:,nbasket) = x;
      fmi(nbasket) = f(1,par);
    end
    if prt > 1
      [w1,w2] = bounds(n,n0,x,y,u,v);
      % compute lower and upper bounds of the box in order to be able
      % to check whether it contains a global minimizer
      iopt = [];
      % the vector iopt contains the indices of the global minimizers
      % contained in the box
      for iglob = 1:nglob
        if w1 <= xglob(:,iglob) & xglob(:,iglob) <= w2
          iopt = [iopt, iglob];
        end
        for iglob = 1:length(iopt)
          optlevel(iopt(iglob)) = s + 1;
        end
      end
    end
  end % of prepare for splitting
  s = s + 1;
  while s < smax
    if record(s) == 0
      s = s + 1;
    else
      break
    end
  end
  if s == smax  % if smax is reached, a new sweep is started
    if local,
      [fmi(nbasket0+1:nbasket),j] = sort(fmi(nbasket0+1:nbasket));
      xmin(:,nbasket0+1:nbasket) = xmin(:,nbasket0+j);
      xmin0 = [];
      fmi0 = [];
      for j = nbasket0+1:nbasket
        x = xmin(:,j);
        f1 = fmi(j);
        chkloc;
        if loc,
          addloc;
          [xbest,fbest,xmin,fmi,x,f1,loc,flag,ncall1] = basket(fcn,data,x,f1,xmin,fmi,xbest,fbest,stop,nbasket0);
          ncall = ncall + ncall1;
          if ~flag,break,end
          if loc,
            [xmin1,fmi1,nc,flag] = lsearch(fcn,data,x,f1,f0min,u,v,nf-ncall,stop,local,gamma,hess);
            ncall = ncall + nc;
            ncloc = ncloc + nc;
            if fmi1 < fbest
              xbest = xmin1;
              fbest = fmi1;
              nsweepbest = nsweep;
              if ~flag
                nbasket0 = nbasket0 + 1;
                nbasket = nbasket0;
                xmin(:,nbasket) = xmin1;
                fmi(nbasket) = fmi1;
                break
              end
              if stop(1) > 0 & stop(1) < 1
                flag = chrelerr(fbest,stop);
              elseif stop(1) == 0
                flag = chvtr(fbest,stop(2));
              end
              if ~flag,return,end
            end
            [xbest,fbest,xmin,fmi,loc,flag,ncall1] = basket1(fcn,data,xmin1,fmi1,xmin,fmi,xbest,fbest,stop,nbasket0);
            ncall = ncall + ncall1;
            if ~flag,break,end
            if loc,
              nbasket0 = nbasket0 + 1;
              xmin(:,nbasket0) = xmin1;
              fmi(nbasket0) = fmi1;
              fbestloc;
              if ~flag,
                nbasket = nbasket0; break
              end
            end
          end
        end
      end
      nbasket = nbasket0;
      if ~flag,break,end
    end
    s = strtsw(smax,level,f(1,:));
    if prt,
      if nsweep == 1
        fprintf('nsw  minl  ');
        if prt > 1
          fprintf('optl    fopt       ')
        end
        fprintf('nf     fbest        xbest\n')
      end
      minlevel=s;
      fprintf('%3i  %3i',nsweep,minlevel);
      if prt > 1
        fprintf('  %3i',optlevel);fprintf('  %10.3e',foptbox);
      end
      fprintf('  %5i  %10.3e',ncall,fbest);
      fprintf('  %10.4f',xbest);
      fprintf(1,'\n');
    end
    if stop(1) > 1
      if nsweep - nsweepbest >= stop(1),flag = 3; return,end
    end
    nsweep = nsweep + 1;
  end
end
if ncall >= nf
  flag = 2;
end
if local,
  if length(fmi) > nbasket
    xmin(:,nbasket+1:length(fmi)) = [];
    fmi(nbasket+1:length(fmi)) = [];
  end
end


function att = attenuationWater(f, T)
%ATTENUATIONWATER Calculate ultrasound attenuation in distilled water.
%
% DESCRIPTION:
%       attenuationWater calculates the ultrasonic absorption in distilled
%       water at a given temperature and frequency using a 7th order
%       polynomial fitted to the data given by Pinkerton (1949).
%
% USAGE:
%       att = attenuationWater(f, T)
%
% INPUTS:
%       f   - array of frequency values [MHz]
%       T   - water temperature [degC]
%
% OUTPUTS:
%       att - attenuation [dB/cm]
%
% ABOUT:
%       author      - Bradley E. Treeby
%       date        - 10th November 2008
%       last udpate - 10th November 2008
%
% REFERENCES:
%   [1] Pinkerton (1949) "The Absorption of Ultrasonic Waves in Liquids and
%       its Relation to Molecular Constitution," Proceedings of the
%       Physical Society. Section B, 2, 129-141
%
% 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 speedSoundWater

% 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 T < 0 || T > 60
    disp('WARNING: Temperature outside range of experimental data');
end

% conversion factor between Nepers and dB
NEPER2DB = 8.686;

% coefficients for 7th order polynomial fit
a_0 = 56.723531840522710;
a_1 = -2.899633796917384;
a_2 = 0.099253401567561;
a_3 = -0.002067402501557;
a_4 = 2.189417428917596e-005;
a_5 = -6.210860973978427e-008;
a_6 = -6.402634551821596e-010;
a_7 = 3.869387679459408e-012;

% compute attenuation
a_on_fsqr = (a_0 + a_1*T + a_2*T.^2 + a_3*T.^3 + a_4*T.^4 + a_5*T.^5 + a_6*T.^6 + a_7*T.^7)*1e-17;
att = NEPER2DB*1e12*f.^2*a_on_fsqr;


%% Dense vs Sparse SNLE
clc
fun = @(x) [10*(x(2) - x(1)^2)
            sqrt(90)*(x(4)-x(3)^2)
            sqrt(10)*(x(2) + x(4) - 2)
            (1/sqrt(10))*(x(2) - x(4))];

x0 = [-30 -10 -30 -10]';

Opt = opti('fun',fun,'x0',x0,'options',optiset('solver','auto'))

[x,f,e,i] = solve(Opt)

%% Sparse above
fun = @(x) -1;
nleq = @(x) [10*(x(2) - x(1)^2)
            sqrt(90)*(x(4)-x(3)^2)
            sqrt(10)*(x(2) + x(4) - 2)
            (1/sqrt(10))*(x(2) - x(4))];
cl = zeros(4,1);
cu = zeros(4,1);

x0 = [-30 -10 -30 -10]';

Opt = opti('fun',fun,'nl',nleq,cl,cu,'x0',x0,'options',optiset('solver','auto'))

[x,f,e,i] = solve(Opt)

%% As above but new construct [no grad]
clc
nleq = @(x) [10*(x(2) - x(1)^2)
            sqrt(90)*(x(4)-x(3)^2)
            sqrt(10)*(x(2) + x(4) - 2)
            (1/sqrt(10))*(x(2) - x(4))];

x0 = [-30 -10 -30 -10]';

Opt = opti('nleq',nleq,'x0',x0,'options',optiset('solver','auto'))

[x,f,e,i] = solve(Opt)

%% As above but new construct [w grad DENSE]
clc
nleq = @(x) [10*(x(2) - x(1)^2)
            sqrt(90)*(x(4)-x(3)^2)
            sqrt(10)*(x(2) + x(4) - 2)
            (1/sqrt(10))*(x(2) - x(4))];
if (exist('syms.m','file'))
    [nljac,nljacstr] = symJac(nleq);

    x0 = [-30 -10 -30 -10]';

    Opt = opti('nleq',nleq,'nljac',nljac,'x0',x0,'options',optiset('solver','auto'))

    [x,f,e,i] = solve(Opt)
end

%% As above but new construct [w grad SPARSE]
clc
nleq = @(x) [10*(x(2) - x(1)^2)
            sqrt(90)*(x(4)-x(3)^2)
            sqrt(10)*(x(2) + x(4) - 2)
            (1/sqrt(10))*(x(2) - x(4))];
if (exist('syms.m','file'))
    [nljac,nljacstr] = symJac(nleq); nljac = @(x) sparse(nljac(x));

    x0 = [-30 -10 -30 -10]';

    Opt = opti('nleq',nleq,'nljac',nljac,'nljacstr',nljacstr,'x0',x0,'options',optiset('solver','auto'))

    [x,f,e,i] = solve(Opt)
end

%% As above but new construct [w grad SPARSE alt nl format]
clc
nleq = @(x) [10*(x(2) - x(1)^2)
            sqrt(90)*(x(4)-x(3)^2)
            sqrt(10)*(x(2) + x(4) - 2)
            (1/sqrt(10))*(x(2) - x(4))];
if (exist('syms.m','file'))
    [nljac,nljacstr] = symJac(nleq); nljac = @(x) sparse(nljac(x));

    x0 = [-30 -10 -30 -10]';

    Opt = opti('nleq',nleq,'nljac',nljac,'nljacstr',nljacstr,'x0',x0,'options',optiset('solver','auto'))

    [x,f,e,i] = solve(Opt)
end

%% As above but new construct [w grad SPARSE alt nl format w lin]
clc
nleq = @(x) [10*(x(2) - x(1)^2)
            sqrt(90)*(x(4)-x(3)^2)
            sqrt(10)*(x(2) + x(4) - 2)
            (1/sqrt(10))*(x(2) - x(4))];
if (exist('syms.m','file'))
    [nljac,nljacstr] = symJac(nleq); nljac = @(x) sparse(nljac(x));
    A = [0 1 0 1]; b = 2;

    x0 = [-30 -10 -30 -10]';

    Opt = opti('nleq',nleq,'ineq',A,b,'nljac',nljac,'nljacstr',nljacstr,'x0',x0,'options',optiset('solver','auto'))

    [x,f,e,i] = solve(Opt)
end

%% As above but new construct [w grad SPARSE alt nl format w lin + int]
clc
nleq = @(x) [10*(x(2) - x(1)^2)
            sqrt(90)*(x(4)-x(3)^2)
            sqrt(10)*(x(2) + x(4) - 2)
            (1/sqrt(10))*(x(2) - x(4))];
if (exist('syms.m','file'))
    [nljac,nljacstr] = symJac(nleq); nljac = @(x) sparse(nljac(x));
    A = [0 1 0 1]; b = 2;

    x0 = [-30 -10 -30 -10]';

    Opt = opti('nleq',nleq,'ivars',2,'lin',A,-Inf,b,'nljac',nljac,'nljacstr',nljacstr,'x0',x0,'options',optiset('solver','auto'))

    [x,f,e,i] = solve(Opt)
end

%% As above but new construct [w grad SPARSE alt nl format w ineq]
clc
nleq = @(x) [10*(x(2) - x(1)^2)
            sqrt(90)*(x(4)-x(3)^2)
            sqrt(10)*(x(2) + x(4) - 2)
            (1/sqrt(10))*(x(2) - x(4))
            x(3)];
if (exist('syms.m','file'))
    [nljac,nljacstr] = symJac(nleq); nljac = @(x) sparse(nljac(x));
    nlrhs = zeros(5,1);
    nle = [zeros(4,1);-1];
    A = [0 1 0 1]; b = 2;

    x0 = [-30 -10 -30 -10]';

    Opt = opti('nlmix',nleq,nlrhs,nle,'ineq',A,b,'nljac',nljac,'nljacstr',nljacstr,'x0',x0,'options',optiset('solver','auto'))

    [x,f,e,i] = solve(Opt)
end

%% Wiki Ex 1
clc
% System of Nonlinear Equations
 nleq = @(x) [ 2*x(1) - x(2) - exp(-x(1));
             -x(1) + 2*x(2) - exp(-x(2))];

% Starting Guess
 x0 = [-5;5];

% Create OPTI Object
 Opt = opti('nleq',nleq,'x0',x0)

% Solve the SNLE problem
[x,fval,exitflag,info] = solve(Opt)

%% Online Example
clc
% System of Nonlinear Equations
 nleq = @(x) [10*(x(2) - x(1)^2)
             sqrt(90)*(x(4) - x(3)^2)
             sqrt(10)*(x(2) + x(4) - 2)
             (1/sqrt(10))*(x(2) - x(4))];

% Nonlinear Equations Jacobian
 nlJac = @(x) sparse([-20*x(1),10,0,0
                      0,0,-6*10^(1/2)*x(3),3*10^(1/2)
                      0,10^(1/2),0,10^(1/2)
                      0,10^(1/2)/10,0,-10^(1/2)/10]);

% Jacobian Sparsity Pattern
 nlJacstr = @() sparse([1 1 0 0
                        0 0 1 1
                        0 1 0 1
                        0 1 0 1]);

% Starting Guess
 x0 = [-30;-10;-30;-10];

% Sparse SNLE OPTI Problem
 Opt = opti('nleq',nleq,'nlJac',nlJac,'nlJacstr',nlJacstr,'x0',x0)

  %Solve
[x,f,e,i] = solve(Opt)

%% SCNLE Example
clc

% Objective (Nonlinear Equations) Function
 fun = @(x) [ 2*x(1) - x(2) - exp(-x(1));
             -x(1) + 2*x(2) - exp(-x(2))];

% Bounds
 lb = [0.6;0];
 ub = [1;1];

% Starting Guess
 x0 = [-5;5];

% Create OPTI Object
 Opt = opti('nleq',fun,'bounds',lb,ub,'x0',x0,'options',optiset('display','iter'))

% Solve the SCNLE problem
[x,fval,exitflag,info] = solve(Opt)

%% Problem SCNLE
clc

A = randn(91);
B12 = randn(91,145);
B1 = randn(145,91); B2 = randn(145,91);
Constant = randn(91,1);
A_ieq = randn(12,91); b_ieq = randn(12,1);
lb = zeros(91,1); ub = 100*ones(91,1);
z0 = lb;

nleq = @(x) A*x + B12*((B1*x).*(B2*x)) + Constant;

Opt=opti('nleq',nleq,'ineq',A_ieq,b_ieq,'bounds',lb,ub,'x0',z0,'options',optiset('display','iter'))

solve(Opt)


function y = pow_pos( x, p )

%POW_POS   Power of positive part.
%   POW_POS(X,P) = POS(X).^P = MAX(X,0).^P.
%   Both P and X must be real, and P must be greater than or equal to 1.
%
%   Disciplined convex programming information:
%       POW_POS(X,P) is convex and nondecreasing in X; so when used in CVX
%       expressions, X must be convex. P must be constant, real, and
%       greater than or equal to 1.

narginchk(2,2);
if ~isnumeric( x ) || ~isreal( x ) || ~isnumeric( p ) || ~isreal( p ),
    error( 'Arguments must be real.' );
elseif any( p(:) <= 1 ),
    error( 'Second argument must be greater than or equal to 1.\nFor other exponents, use POW_P instead.', 1 ); %#ok
end
y = max(x,0).^p;

% Copyright 2005-2016 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function clenshaw_curtis_set_test ( )

%*****************************************************************************80
%
%% CLENSHAW_CURTIS_SET_TEST tests CLENSHAW_CURTIS_SET.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 April 2015
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CLENSHAW_CURTIS_SET_TEST\n' );
  fprintf ( 1, '  CLENSHAW_CURTIS_SET sets up a Clenshaw-Curtis rule;\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Estimate the integral of sqrt(abs(x)) over [-1,+1].\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   N           Estimate             Error\n' );
  fprintf ( 1, '\n' );

  exact = 4.0 / 3.0;

  for n = 1 : 10

    [ x, w ] = clenshaw_curtis_set ( n );

    v(1:n,1) = sqrt ( abs ( x(1:n,1) ) );

    q = w' * v;
    e = abs ( q - exact );

    fprintf ( 1, '  %2d  %24.16g  %14.6e\n', n, q, e );

  end

  return
end


classdef KalmanFilterX < FilterX
% KalmanFilterX class
%
% Summary of KalmanFilterX:
% This is a class implementation of a standard Kalman Filter.
%
% KalmanFilterX Properties: (**)
%   + StatePrior - A structure used to store the state prior
%   + StatePrediction - A structure used to store the state prediction
%   + MeasurementPrediction - A structure used to store the measurement prediction
%   + StatePosterior - A structure used to store posterior information
%   + MeasurementList - A (yDim x 1) matrix used to store the received measurement
%   + ControlInput - A (uDim x 1) matrix used to store the last received control input
%   + KalmanGain - A (xDim x yDim) matrix representing the last computed Kalman Gain
%   + Model - An object handle to StateSpaceModelX object
%       + Transition (*)  = Object handle to TransitionModelX SubClass
%       + Measurement (*)  = Object handle to MeasurementModelX SubClass
%       + Control (*)  = Object handle to ControlModelX SubClass
%
%   (*)  Signifies properties necessary to instantiate a class object
%   (**) xDim, yDim and uDim denote the dimentionality of the state, measurement
%        and control vectors respectively.
%
% KalmanFilterX Methods:
%   + KalmanFilterX  - Constructor method
%   + predict        - Performs KF prediction step
%   + update         - Performs KF update step
%
% (+) denotes puplic properties/methods
%
% See also TransitionModelX, MeasurementModelX and ControlModelX template classes

    properties
        StatePrior
        StatePrediction
        MeasurementPrediction
        StatePosterior
        KalmanGain
        ControlInput
    end

    properties (Dependent)
        MeasurementLikelihoods
    end

    properties (Access=protected)
        MeasurementLikelihoods_ = [];
    end

    methods (Access = protected)
        function initialise_(this, config)
            initialise_@FilterX(this,config);
            if (isfield(config,'StatePrior'))
                this.StatePrior = config.StatePrior;
                this.StatePosterior = this.StatePrior;
            end
        end
        % MeasurementList
        function measurementList = setMeasurementList(this, newMeasurementList)
            measurementList = newMeasurementList;
            this.MeasurementLikelihoods_ = [];
        end
        function StatePrior = setStatePrior(this,newStatePrior)
            if(isa(newStatePrior,'GaussianStateX'))
                StatePrior = newStatePrior;
            else
                StatePrior = GaussianStateX(newStatePrior);
            end
        end
        function StatePrediction = setStatePrediction(this,newStatePrediction)
            if(isa(newStatePrediction,'GaussianStateX'))
                StatePrediction = newStatePrediction;
            else
                StatePrediction = GaussianStateX(newStatePrediction.Mean, newStatePrediction.Covar);
            end
        end
        function MeasurementPrediction = setMeasurementPrediction(this,newMeasurementPrediction)
            if(isa(newMeasurementPrediction,'GaussianStateX'))
                MeasurementPrediction = newMeasurementPrediction;
            else
                MeasurementPrediction = GaussianStateX(newMeasurementPrediction.Mean, newMeasurementPrediction.Covar);
            end
        end
        function MeasurementLikelihoods = getMeasurementLikelihoods(this)
            if(isempty(this.MeasurementLikelihoods_))
                this.MeasurementLikelihoods_ =  mvnpdf(this.MeasurementList.Vectors',this.MeasurementPrediction.Mean',this.MeasurementPrediction.Covar)';
            end
            MeasurementLikelihoods = this.MeasurementLikelihoods_;
        end
        function StatePosterior = setStatePosterior(this,newStatePosterior)
            if(isa(newStatePosterior,'GaussianStateX'))
                StatePosterior = newStatePosterior;
            else
                StatePosterior = GaussianStateX(newStatePosterior.Mean, newStatePosterior.Covar);
            end
        end
    end

    methods
        function this = KalmanFilterX(varargin)
        % KalmanFilterX Constructor method
        %
        % Parameters
        % ----------
        % Model: StateSpaceModelX
        %   An object handle to StateSpaceModelX object.
        % StatePrior: struct, optional
        %   A StateX subclass object describing the state prior. If StatePrior
        %   is not a GaussianStateX instance, then it will be converted in
        %   one using the extracted mean and covariance.
        %
        % Usage
        % -----
        % * kf = KalmanFilterX(___,Name,Value) instantiates an object handle,
        %   configured with the options specified by one or more Name,Value
        %   pair arguments.
        %
        %  See also predict, update, smooth.

            % Call SuperClass method
            %this@FilterX(varargin{:});

            if(nargin==0)
                return;
            end

            % First check to see if a structure was received
            if(nargin==1)
                if(isstruct(varargin{1}))
                    config = varargin{1};
                    this.initialise_(config);
                end
                return;
            end

            % Otherwise, fall back to input parser
            parser = inputParser;
            parser.KeepUnmatched = true;
            parser.parse(varargin{:});
            config = parser.Unmatched;
            this.initialise_(config);
        end

        function initialise(this,varargin)
        % initialise Initialise the KalmanFilter with a certain set of
        %   parameters.
        %
        % Parameters
        % ----------
        % Model: StateSpaceModelX
        %   An object handle to StateSpaceModelX object.
        % StatePrior: StateX, optional
        %   A StateX subclass object describing the state prior. If StatePrior
        %   is not a GaussianStateX instance, then it will be converted in
        %   one using the extracted mean and covariance.
        %
        % Usage
        % -----
        % * initialise(kf,___,Name,Value) initialises the KalmanFilterX
        %   object kf with the options specified by one or more Name,Value
        %   pair arguments.
        %
        %  See also predict, update, smooth.

            if(nargin==0)
                error("Not enough input arguments.");
            end

            initialise@FilterX(this);

            % First check to see if a structure was received
            if(nargin==2)
                if(isstruct(varargin{1}))
                    config = varargin{1};
                    this.initialise_(config);
                end
                return;
            end

            % Otherwise, fall back to input parser
            parser = inputParser;
            parser.KeepUnmatched = true;
            parser.parse(varargin{:});
            config = parser.Unmatched;
            this.initialise_(config);
        end

        function [statePrediction, measurementPrediction] = predict(this, varargin)
        % Predict Perform Kalman Filter prediction step
        %
        % Parameters
        % ----------
        % prior: GaussianStateX, optional
        %   The prior state estimate.
        % timestamp: datetime, optional
        %   A timestamp indicating the time at which prediction is
        %   performed.
        %
        % Returns
        % -------
        % GaussianStateX
        %   The generated state prediction
        % GaussianStateX, optional
        %   The generated measurement prediction
        %
        %  See also update, smooth.

            % Predict state and measurement
            statePrediction = this.predictState(varargin{:});
            if nargin>1 && isa(varargin{1},'StateX')
               % Replace a potential prior with the generated prediction
               % before forwarding the arguments to the measurement
               % prediction. Failure to do so will result in errors!!!
               varargin{1} = statePrediction;
            end
            measurementPrediction = this.predictMeasurement(varargin{:});
        end

        function statePrediction = predictState(this,varargin)
        % predictState Perform Kalman Filter state prediction step
        %
        % Usage
        % -----
        % * predictState(this) calculates the predicted system state and covariance.
        %
        % See also update, smooth.

            timestamp = [];
            timestamp_old = [];
            for i = 1:min([2,nargin-1])
                if isa(varargin{i},'StateX')
                    this.StatePosterior = varargin{i};
                    timestamp_old = this.StatePosterior.Timestamp;
                elseif isdatetime(varargin{i})
                    timestamp = varargin{i};
                end
            end

            if isempty(timestamp)
                dt = this.Model.Transition.TimestepDuration;
                timestamp = this.StatePosterior.Timestamp;
            else
                dt = timestamp - timestamp_old;
            end

            % Extract model parameters
            F = this.Model.Transition.matrix(dt);
            Q = this.Model.Transition.covar(dt);
            if(~isempty(this.Model.Control))
                B   = this.Model.Control.feval();
                Qu  = this.Model.Control.covar();
            else
                this.ControlInput   = 0;
                B   = 0;
                Qu  = 0;
            end

            % Perform state prediction
            [statePredictionMean, statePredictionCovar] = ...
                this.predictState_(this.StatePosterior.Mean, this.StatePosterior.Covar, F, Q, this.ControlInput, B, Qu);

            statePrediction = GaussianStateX(statePredictionMean, statePredictionCovar, timestamp);
            this.StatePrediction = statePrediction;
        end

        function measurementPrediction = predictMeasurement(this, varargin)
        % PREDICTOBS Perform Kalman Filter measurement prediction step
        %
        % Usage
        % -----
        % * predict(this) calculates the predicted measurement,
        %   as well as the associated uncertainty covariances.
        %
        % More details
        % ------------
        % * KalmanFilterX uses the Model class property, which should be an
        %   instance of the TrackingX.Models.StateSpaceModel class, in order
        %   to extract information regarding the underlying state-space model.
        % * State prediction is performed using the Model.Transition property,
        %   which must be a subclass of TrackingX.Abstract.TransitionModel and
        %   provide the following interface functions:
        %   - Model.Transition.feval(): Returns the model transition matrix
        %   - Model.Transition.covariance(): Returns the process noise covariance
        % * Measurement prediction and innovation covariance calculation is
        %   performed usinf the Model.Measurement class property, which should be
        %   a subclass of TrackingX.Abstract.TransitionModel and provide the
        %   following interface functions:
        %   - Model.Measurement.heval(): Returns the model measurement matrix
        %   - Model.Measurement.covariance(): Returns the measurement noise covariance
        %
        % See also update, smooth.

            if nargin>1
                this.StatePrediction = varargin{1};
            end

            % Extract model parameters
            H = this.Model.Measurement.feval();
            R = this.Model.Measurement.covar();

            % Perform prediction
            [measurementPredictionMean, measurementPredictionCovar, this.KalmanGain] = ...
                this.predictMeasurement_(this.StatePrediction.Mean, this.StatePrediction.Covar, H, R);

            measurementPrediction = GaussianStateX(measurementPredictionMean,...
                                                   measurementPredictionCovar,...
                                                   this.StatePrediction.Timestamp);
            this.MeasurementPrediction = measurementPrediction;
        end

        function posterior = update(this, varargin)
        % UPDATE Perform Kalman Filter update step
        %
        % Usage
        % -----
        % * update(this) calculates the corrected sytem state and the
        %   associated uncertainty covariance.
        %
        % See also KalmanFilterX, predict, iterate, smooth.

            if nargin>1
                if isa(varargin{1},'MeasurementX')
                    this.MeasurementList = MeasurementListX(varargin{1});
                elseif isa(varargin{1}, 'StateX')
                    this.StatePrediction = varargin{1};
                    this.MeasurementList =  MeasurementListX(varargin{2});
                end
            end
            if(this.MeasurementList.NumMeasurements)
                timestamp = this.MeasurementList.Timestamp;
            else
                timestamp = this.StatePrediction.Timestamp;
            end

            if(isempty(this.MeasurementPrediction.Mean) || isempty(this.MeasurementPrediction.Covar))
                [measurementPredictionMean, measurementPredictionCovar, this.KalmanGain] = ...
                    this.predictMeasurement_(this.StatePrediction.Mean, this.StatePrediction.Covar, H, R);
                measurementPrediction = GaussianStateX(measurementPredictionMean,...
                                                       measurementPredictionCovar,...
                                                       this.StatePrediction.Timestamp);
                this.MeasurementPrediction = measurementPrediction;
            end

            % Perform single measurement update
            [posteriorMean, posteriorCovar] = this.update_(this.StatePrediction.Mean,this.StatePrediction.Covar,...
                                                           this.MeasurementList.Vectors,this.MeasurementPrediction.Mean,...
                                                           this.MeasurementPrediction.Covar,this.KalmanGain);
            posteriorCovar = (posteriorCovar+posteriorCovar')/2;

            posterior = GaussianStateX(posteriorMean, posteriorCovar, timestamp);
            this.StatePosterior = posterior;
        end

        function posterior = updatePDA(this, assocWeights, varargin)
        % UPDATEPDA Performs KF update step, for multiple measurements
        %           Update is performed according to the generic (J)PDAF equations [1]
        %
        % Usage
        % -----
        %  * updatePDA(assocWeights) Performs KF-PDA update step for multiple
        %    measurements based on the provided (1-by-Nm+1) association weights
        %    matrix assocWeights.
        %
        %   [1] Y. Bar-Shalom, F. Daum and J. Huang, "The probabilistic data association filter," in IEEE Control Models, vol. 29, no. 6, pp. 82-100, Dec. 2009.
        %
        %   See also KalmanFilterX, Predict, Iterate, Smooth, resample.

            if(this.MeasurementList.NumMeasurements)
                timestamp = this.MeasurementList.Timestamp;
            else
                timestamp = this.StatePrediction.Timestamp;
            end

            [posteriorMean, posteriorCovar] = ...
                this.updatePDA_(this.StatePrediction.Mean,this.StatePrediction.Covar,this.MeasurementList.Vectors,...
                                assocWeights,this.MeasurementPrediction.Mean,this.MeasurementPrediction.Covar,this.KalmanGain);

            posterior = GaussianStateX(posteriorMean,posteriorCovar,timestamp);
            this.StatePosterior = posterior;
        end

        function measurementLikelihoods = get.MeasurementLikelihoods(this)
            measurementLikelihoods = getMeasurementLikelihoods(this);
        end

        function statePrior = get.StatePrior(this)
            statePrior = this.StatePrior;
        end

        function set.StatePrior(this, newStatePrior)
            this.StatePrior = setStatePrior(this, newStatePrior);
        end

        function statePrediction = get.StatePrediction(this)
            statePrediction = this.StatePrediction;
        end

        function set.StatePrediction(this, newStatePrediction)
            this.StatePrediction = setStatePrediction(this, newStatePrediction);
        end

        function measurementPrediction = get.MeasurementPrediction(this)
            measurementPrediction = this.MeasurementPrediction;
        end

        function set.MeasurementPrediction(this, newMeasurementPrediction)
            this.MeasurementPrediction = setMeasurementPrediction(this, newMeasurementPrediction);
        end

        function statePosterior = get.StatePosterior(this)
            statePosterior = this.StatePosterior;
        end

        function set.StatePosterior(this, newStatePosterior)
            this.StatePosterior = setStatePosterior(this, newStatePosterior);
        end

    end

    methods (Static)

        function [xPred, PPred, yPred, S, K] = predict_(x,P,F,Q,H,R,u,B,O)
        % PREDICT_ Perform the discrete-time KF state and measurement
        % prediction steps, under the assumption of additive process noise.
        %
        % Parameters
        % ----------
        % x: column vector
        %   The (xDim x 1) state estimate at the previous time-step.
        % P: matrix
        %   The (xDim x xDim) state covariance matrix at the previous
        %   time-step.
        % F: matrix
        %   An (xDim x xDim) state transition matrix.
        % Q: matrix
        %   The (xDim x xDim) process noise covariance matrix.
        % H: matrix
        %   A (xDim x yDim) measurement matrix.
        % R: matrix
        %   The (yDim x yDim) measurement noise covariance matrix.
        % u: column vector, optional
        %   A optional (xDim x 1) control input.
        %   If omitted, no control input is used.
        % B: matrix, optional
        %   An optional (xDim x xDim) control gain matrix.
        %   If omitted, B is assumed to be 1.
        % O: matrix, optional
        %   An optional (xDim x xDim) control noise covariance
        %   matrix. If omitted, Q is assumed to be 0.
        %
        % Returns
        % -------
        % xPred: column vector
        %   The (xDim x 1) predicted state estimate.
        % PPred: matrix
        %   The (xDim x xDim) predicted state covariance matrix.
        % yPred: column vector
        %   The (yDim x 1) predicted measurement estimate.
        % Pxy: matrix
        %   The (xDim x yDim) cross-covariance matrix.
        % S: matrix
        %   The (yDim x yDim) innovation covariance matrix.
        %
        %October 2017 Lyudmil Vladimirov, University of Liverpool.

            switch(nargin)
                case(6)
                    u = 0;
                    B = 0;
                    O = 0;
                case(7)
                    B = 1;
                    O = 0;
                case(8)
                    O = 0;
            end

           [xPred, PPred] = KalmanFilterX.predictState_(x,P,F,Q,u,B,O);
           [yPred, S, K] = KalmanFilterX.predictMeasurement_(xPred,PPred,H,R);
        end

        function [xPred, PPred] = predictState_(x,P,F,Q,u,B,Qu)
        % PREDICTSTATE_ Perform the discrete-time KF state prediction
        % step, under the assumption of additive process noise.
        %
        % Parameters
        % ----------
        % x: column vector
        %   The (xDim x 1) state estimate at the previous time-step.
        % P: matrix
        %   The (xDim x xDim) state covariance matrix at the previous
        %   time-step.
        % F: matrix
        %   An (xDim x xDim) state transition matrix.
        % Q: matrix
        %   The (xDim x xDim) process noise covariance matrix.
        % u: column vector, optional
        %   An optional (xDim x 1) control input.
        %   If omitted, no control input is used.
        % B: matrix, optional
        %   An optional (xDim x xDim) control gain matrix.
        %   If omitted, B is assumed to be 1.
        % O: matrix, optional
        %   An optional (xDim x xDim) control noise covariance
        %   matrix. If omitted, Q is assumed to be 0.
        %
        % Returns
        % -------
        % xPred: column vector
        %   The (xDim x 1) predicted state estimate.
        % PPred: matrix
        %   The (xDim x xDim) predicted state covariance matrix.
        %
        %October 2017 Lyudmil Vladimirov, University of Liverpool.

            switch(nargin)
                case(4)
                    u  = 0;
                    B  = 0;
                    Qu = 0;
                case(5)
                    B  = 1;
                    Qu = 0;
                case(6)
                    Qu = 0;
            end

            % Compute predicted state mean and covariance
            xPred = F*x + B*u;
            PPred =F*P*F' + Q + B*Qu*B';
        end

        function [yPred, S, K] = predictMeasurement_(xPred,PPred,H,R)
        % PREDICTOBS_ Perform the discrete-time KF observation prediction
        % step, under the assumption of additive process noise.
        %
        % Parameters
        % ----------
        % xPred: column vector
        %   The (xDim x 1) predicted state estimate at the current
        %   time-step.
        % PPred: matrix
        %   The (xDim x xDim) predicted state covariance matrix at
        %   the current time-step.
        % H: matrix
        %   An (xDim x yDim) measurement matrix.
        % R: matrix
        %   The (yDim x yDim) measurement noise covariance matrix.
        %
        % Returns
        % -------
        % yPred: column vector
        %   The (yDim x 1) predicted measurement estimate.
        % Pxy: matrix
        %   The (xDim x yDim) cross-covariance matrix.
        % S: matrix
        %   The (yDim x yDim) innovation covariance matrix.
        %
        %October 2017 Lyudmil Vladimirov, University of Liverpool.

            % Compute predicted measurement mean and covariance
            yPred   = H*xPred;
            Pxy     = PPred*H';
            S       = H*PPred*H' + R;
            K       = Pxy/(S);
        end

        function [x,P] = update_(xPred,PPred,y,yPred,S,K)
        % UPDATE_ Perform the discrete-time KF update step, under the
        % assumption of additive process noisem for a single measurement.
        %
        % Parameters
        % ----------
        % xPred: column vector
        %   The (xDim x 1) predicted state estimate.
        % PPred: matrix
        %   The (xDim x xDim) predicted state covariance matrix.
        % y: column vector
        %   The (yDim x 1) measurement vector.
        % yPred: column vector
        %   The (yDim x 1) predicted measurement estimate.
        % S: matrix
        %   The (yDim x yDim) innovation covariance matrix.
        % K: matrix
        %   The (xDim x yDim) Kalman gain matrix at the current
        %   time-step.
        %
        % Returns
        % -------
        % x: column vector
        %   The (xDim x 1) state estimate at the current time-step.
        % P: matrix
        %   The (xDim x xDim) state covariance matrix at the current
        %   time-step.
        %
        %October 2017 Lyudmil Vladimirov, University of Liverpool.

            % Compute the filtered estimates
            x = xPred + K * (y - yPred);
            P = PPred - K*S*K';
        end

        function [x,P] = updatePDA_(xPred,PPred,Y,W,yPred,S,K)
        % UPDATEPDA_ Perform the discrete-time Probabilistic Data
        % Association (PDA) KF update step, under the assumption of additive process
        % noise, for multiple measurements (as a Gaussian Mixture)
        %
        % Parameters
        % ----------
        % xPred: column vector
        %   The (xDim x 1) predicted state estimate.
        % PPred: matrix
        %   The (xDim x xDim) predicted state covariance matrix.
        % Y: matrix
        %   The (yDim x nY) measurement vector.
        % W: row vector
        %   The (1 x nY+1) measurement association/mixture weights
        %   vector. (dummy measurement assumed at index 1)
        % yPred: column vector
        %   The (yDim x 1) predicted measurement estimate.
        % S: matrix
        %   The (yDim x yDim) innovation covariance matrix.
        % K: matrix
        %   The (xDim x yDim) Kalman gain matrix at the current
        %   time-step.
        %
        % Returns
        % -------
        % x: column vector
        %   The (xDim x 1) state estimate at the current time-step.
        % P: matrix
        %   The (xDim x xDim) state covariance matrix at the current
        %   time-step.
        %
        %October 2017 Lyudmil Vladimirov, University of Liverpool.

            % Get size of observation vector
            nY = size(Y,2);
            if(nY==0)
                x = xPred;
                P = PPred;
                return;
            end

            innov_err = Y - yPred;
            xupd = [xPred, xPred + K*innov_err];
            Pplus = PPred - K*S*K';

            try
                x = xupd*W';
            catch
                asd=2;
            end
            v_x = x - xupd;
            P = W(1)*(PPred + v_x(:,1)*v_x(:,1)');
            for j = 2:nY+1
                P = P + W(j)*(Pplus + v_x(:,j)*v_x(:,j)');
            end

%             % Compute innovation mean and (cross) covariance
%             innov_err       = Y - yPred(:,ones(1,nY));
%             tot_innov_err   = innov_err*W(2:end)';
%             Pc              = PPred - K*S*K';
%             Pgag            = K*((innov_err.*W(ones(yDim,1),2:end))*innov_err' - tot_innov_err*tot_innov_err')*K';
%
%             % Compute filtered estimates
%             x    = xPred + K*tot_innov_err;
%             P    = W(1)*PPred + (1-W(1))*Pc + Pgag;
             P    = (P+P')/2;
        end

        function config = getInitConfig()

        end
    end
end


function [lf] = eeg_infinite_monopole(monpos, elc, vol)

% EEG_INFINITE_MONOPOLE calculate the infinite medium potential for a monopole
%
% Use as
%   [lf] = eeg_infinite_monopole(monpos, elc, vol)
%
% Implemented from Malmivuo J, Plonsey R, Bioelectromagnetism (1993)
% http://www.bem.fi/book/08/08.htm
%
% See also EEG_INFINITE_DIPOLE, EEG_HALFSPACE_DIPOLE, EEG_HALFSPACE_MONOPOLE

% Copyright (C) 2011, Cristiano Micheli
% Copyright (C) 2019, Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

if ~isstruct(vol)
  % it only represents the conductivity, make a structure out of it
  vol = struct('cond', vol);
end

siz = size(monpos);
if any(siz==1)
  % positions are specified as a single vector
  Npoles = prod(siz)/3;
  monpos = monpos(:)'; % ensure that it is a row vector
elseif siz(2)==3
  % positions are specified as a Nx3 matrix -> reformat to a single vector
  Npoles = siz(1);
  monpos = monpos';
  monpos = monpos(:)'; % ensure that it is a row vector
else
  ft_error('incorrect specification of monopole locations');
end

cond     = vol.cond;
Nelc     = size(elc,1);
lf       = zeros(Nelc,Npoles);

mu0   = 4*pi*1e-7;         % Permeability of free space
c     = 2.99792458 * 1e8;  % Speed of light
e0    = 1 / (mu0*c^2);     % Permittivity of Free Space

for i=1:Npoles
  % this is the position of monopole "i"
  monopole = monpos((1:3) + 3*(i-1));

  % distances from electrodes to monopole
  r = elc - ones(Nelc,1) * monopole;
  r = sqrt(sum(r.^2,2));

  lf(:,i) = 1 ./ (4*pi*cond*r);
end


% Van Leer scheme for one-dimensional Euler equations

% Copyright 2001 P. Wesseling
% This program and its subprograms may be freely used, modified and distributed
% under the GNU General Public License: http://www.gnu.org/copyleft/gpl.html

% Theory in Section 10.5 of:

% 	P. Wesseling: Principles of Computational Fluid Dynamics
% 	Springer, Heidelberg, 2000 ISBN 3-5453-0. XII, 642 pp.
% See http://ta.twi.tudelft.nl/nw/users/wesseling/cfdbook.html

% This program makes Figs. 10.15, 10.16 in the book

% Functions called: f, problem_specification, Riemann

global  PRL  CRL MACHLEFT  gamma  pleft  pright  rholeft  rhoright  uleft...
	uright  tend  lambda		% lambda = dt/dx

		% .....................Input............................
gamma = 1.4; 	% Ratio of specific heats
J = 48;		% Number of grid cells
bouncon = 0;	% bouncon chooses outflow boundary conditions
		% = 0: Nothing happens: infinite domain
		% = 1: Solid wall at x = 1 with direct prescription of uwall
		% = 2: Solid wall at x = 1 with reflection b.c.
		% ....................End of input........................

gammab = 1/(gamma - 1); gam1 = gamma-1; gamgam = gamma^gamma;
problem_specification

h = 1/J;  				% Cell size
dt = lambda*h;				% Time step
n = floor(tend/dt);			% Number of time-steps

% 		Definition of grid numbering
%       x=0    					 x=1
% grid   |---o---|---o---|---o---  ...  --|---o---|
%            1   1   2   2   3           J-1  J

xcenter = h*[1:J] - h/2;		% Location of cell centers

press = zeros(size(xcenter));		% Preallocation of pressure,
rhoold = press; uold = press;		%       density and velocity
rhonew = press; mnew = press;		%       momentum
totenew = press;			%	total energy

for j = 1:length(xcenter)		% Initial conditions
  if xcenter(j) < 0.5, press(j) = pleft; rhoold(j) = rholeft;  uold(j) = uleft;
  else,  	     press(j) = pright;  rhoold(j) = rhoright; uold(j) = uright;
  end
end

	% Initialization of cell center variables
totenold = rhoold.*(0.5*uold.*uold + gammab*press./rhoold); % Total energy rho*E
totenleft = totenold(1); totenright = totenold(J);
mold = rhoold.*uold;					    % Momentum m
c = sqrt(gamma*press./rhoold);				    % Sound speed
mach = uold./c;						    % Mach number

% Preallocation of split fluxes
plus1 = c; minus1 = c; plus2 = c; minus2 = c; plus3 = c; minus3 = c;
% Preallocation of van Leer fluxes
flux1 = zeros(J-1,1); flux2 = flux1; flux3 = flux1;

t = 0;
for i = 1:n,  t = t + dt;
  Eflux1 = rhoold.*uold;			% Eflux1,2,3 is Euler flux
  Eflux2 = Eflux1.*uold + (1/gamma)*rhoold.*c.^2;
  Eflux3 = 0.5*Eflux1.*uold.^2 + gammab*Eflux1.*c.^2;

    for j = 1:J
    if mach(j) > 1
      plus1(j) = Eflux1(j);  plus2(j) = Eflux2(j);  plus3(j) = Eflux3(j);
    elseif mach(j) < -1
      plus1(j) = 0;         plus2(j) = 0;         plus3(j) = 0;
    else
      plus1(j) = 0.25*rhoold(j)*c(j)*(1 + mach(j))^2;
      plus2(j) = plus1(j)*c(j)*(2 + gam1*mach(j))/gamma;
      plus3(j) = (plus2(j)^2/plus1(j))*gamma^2*0.5*gammab/(gamma+1);
    end
  end
  minus1 = Eflux1 - plus1; minus2 = Eflux2 - plus2; minus3 = Eflux3 - plus3;

  for j = 1:J-1					% van Leer fluxes
    flux1(j) = plus1(j) + minus1(j+1);
    flux2(j) = plus2(j) + minus2(j+1);
    flux3(j) = plus3(j) + minus3(j+1);
  end

	% Update of state variables
  rhonew(1) = rholeft;		  rhonew(J)  = rhoright;
  mnew(1) = rholeft*uleft; 	  mnew(J)    = rhoright*uright;
  totenew(1) = totenleft; 	  totenew(J) = totenright;
  for j = 2:J-1
    rhonew(j)  = rhoold(j)   - lambda*(flux1(j) - flux1(j-1));
    mnew(j)    = mold(j)     - lambda*(flux2(j) - flux2(j-1));
    totenew(j) = totenold(j) - lambda*(flux3(j) - flux3(j-1));
  end

  if bouncon ~= 0
    if bouncon == 1
      rhowall = rhoold(J); uwall = 0;
      pwall = (gamma-1)*(totenold(J) - 0.5*mold(J)^2/rhoold(J)); % pwall = p(J)
      totenwall = pwall/(gamma-1) + 0.5*rhowall*uwall^2;
    else
      rhowall = rhoold(J); uwall = - mold(J)/rhoold(J);
      pwall = (gamma-1)*(totenold(J) - 0.5*mold(J)^2/rhoold(J)); % pwall = p(J)
      totenwall = pwall/(gamma-1) + 0.5*rhowall*uwall^2;
    end
    wallflux1 = rhowall*uwall;
    pwall = gam1*(totenwall - 0.5*wallflux1*uwall);
    wallflux2 = wallflux1.*uwall + pwall;
    wallflux3 = 0.5*wallflux1*uwall^2 + gammab*gamma*uwall*pwall;
    cwall = sqrt(gamma*pwall/rhowall); machwall = uwall/cwall;
    if machwall > 1
      plus1wall = wallflux1; plus2wall = wallflux2; plus3wall = wallflux3 ;
    elseif machwall < -1
      plus1wall = 0;      plus2wall = 0;      plus3wall = 0;
    else
      plus1wall = 0.25*rhowall*cwall*(1 + machwall)^2;
      plus2wall = plus1wall*cwall*(2 + gam1*machwall)/gamma;
      plus3wall = (plus2wall^2/plus1wall)*gamma^2*0.5*gammab/(gamma+1);
    end
    minus1wall = wallflux1 - plus1wall;
    minus2wall = wallflux2 - plus2wall; minus3wall = wallflux3 - plus3wall;

    flux1wall = plus1(J) + minus1wall;			% van Leer fluxes
    flux2wall = plus2(J) + minus2wall;
    flux3wall = plus3(J) + minus3wall;

    rhonew(J)  = rhoold(J)   - lambda*(flux1wall - flux1(J-1));
    mnew(J)    = mold(J)     - lambda*(flux2wall - flux2(J-1));
    totenew(J) = totenold(J) - lambda*(flux3wall - flux3(J-1));
  end
 	% Update of state variables
  uold = mnew./rhonew; press = gam1*(totenew - 0.5*mnew.*uold);
  rhoold = rhonew; totenold = totenew; mold = mnew;

  c = sqrt(gamma*press./rhoold); mach = uold./c;
end

entropy = log(press./rhoold.^gamma);

figure(1), clf
subplot(2,3,1),hold on,title('DENSITY','fontsize',14),plot(xcenter,rhonew,'o')
subplot(2,3,2),hold on,title('VELOCITY','fontsize',14),plot(xcenter,uold,'o')
subplot(2,3,3),hold on,title('PRESSURE','fontsize',14),plot(xcenter,press,'o')
subplot(2,3,4),hold on,title('MACHNUMBER','fontsize',14),plot(xcenter,mach,'o')
subplot(2,3,5),hold on,title('ENTROPY','fontsize',14),plot(xcenter,entropy,'o')
subplot(2,3,6),axis('off'),hold on,title('Van Leer scheme','fontsize',14)

Riemann		% Plot exact solution


function [X, spectrum] = slcmds(D, d, w, ty)
%SLMDS Performs Classical Multidimensional scaling
%
% $ Syntax $
%   - X = slcmds(D, d)
%   - X = slcmds(D, d, w)
%   - X = slcmds(D, d, w, 'sqr')
%   - [X, spectrum] = slcmds(...)
%
% $ Arguments $
%   - D:        The pairwise distance matrix (n x n)
%   - d:        The dimension of the embedding space
%   - w:        The weights of samples (1 x n or [])
%   - X:        The embedded samples (d x n)
%
% $ Description $
%   - X = slcmds(D, d) performs classic multidimensional scaling to
%     pursue an embedding space of d-dimension and the vector
%     representation in that space of the objects, such that the
%     distances are optimally preserved.
%
%   - X = slcmds(D, d, w) If w is not empty, it performs classic
%     multidimensional scaling on weighted samples.
%
%   - X = slcmds(D, d, w, 'sqr') indicates that D contains the square
%     of distances.
%
%   - [X, spectrum] = slcmds(...) additionally outputs the spectrum of
%     the embedded space
%
% $ History $
%   - Created by Dahua Lin, on Sep 8th, 2006
%

%% parse and verify input arguments

if nargin < 2
    raise_lackinput('slcmds', 2);
end

if ndims(D) ~= 2 || size(D, 1) ~= size(D, 2)
    error('sltoolbox:invalidarg', ...
        'The D should be a square matrix');
end
n = size(D, 1);

if d >= n
    error('sltoolbox:exceedbound', ...
        'The dimension d should be less than the number of samples n');
end

if nargin < 3
    w = [];
else
    if ~isempty(w)
        if ~isequal(size(w), [1, n])
            error('sltoolbox:sizmismatch', ...
                'If w is specified, it should be an 1 x n row vector');
        end
    end
end

if nargin >= 4 && strcmpi(ty, 'sqr')
    is_sqr = true;
else
    is_sqr = false;
end

%% compute

if ~is_sqr
    K = sldists2kernels(D);
else
    K = sldists2kernels(D, 'sqr');
end

[X, spectrum] = slkernelembed(K, d, w);


function [x,lambda_opt,svd_state,Y_hat] = ridge_gcv(varargin)
%[x_hat,lambda_opt] = ridgeGCV(Y,A,P)
%
% Estimates a ridge regression model, also know as Tikhonov regularization,
% or minimum norm with L2 prior.
%
% x_hat = argmin(x) ||Y-A*x||^2 + lambda*||P*x||^2
% with lambda > 0
%
%
% [..., svd_state] = ridge_gcv(...)  returns the svd decomposition. This can
% be reused if the target vector does not change.
%
% [..., Y_hat] = ridge_gcv(...)  returns the predicted target vector.
% Residuals can be computed via E = Y-Y_hat;
%
% This code is based on a previous implementation used in Valdes-Hernandez
% et al. (2009), written by Alejandro Ojeda and Pedro Valdez-Hernandez at
% the Cuban Neuroscience Center in 2009.
%
% Author: Alejandro Ojeda, SCCN/INC/UCSD, Jul-2012
%         Tim Mullen, SCCN/INC/UCSD, Jan-2013, Apr-2013
%
% References:
%   Pedro A. Valdes-Hernandez, Alejandro Ojeda, Eduardo Martinez-Montes, Agustin
%       Lage-Castellanos, Trinidad Virues-Alba, Lourdes Valdes-Urrutia, Pedro A.
%       Valdes-Sosa, 2009. White matter architecture rather than
%       cortical surface area correlates with the EEG alpha rhythm. NeuroImage 49
%       (2010) 2328–2339

arg_define([0 Inf],varargin, ...
    arg_norep({'Y','TargetVector','y'},mandatory,[],'The target vector'), ...
    arg_norep({'A','DesignMatrix'},mandatory,[],'The design matrix. This is the data matrix (ie X).'), ...
    arg({'P','PriorInvCov'},[],[],'Prior inverse covariance (precision) matrix for params. Can be an [nc x nc] matrix, where nc is the number of columns of A. Can also be a scalar, P, specifying the prior inverse variance (precision) of each parameter (diagonal covariance matrix). If empty, identity covariance matrix assumed. A sparse matrix is advised if precision matrix is not dense.'), ...
    arg_nogui({'blksz','DesignMatrixBlockSize','designMatrixBlockSize'},[],[],'Design matrix structure. Can be a tuple [numrows numcols], in which case A consists of identical blocks of this size, along the main diagonal'), ...
    arg_subswitch({'lambdaMode','LambdaSelectionMode'},'grid_gcv', { ...
        'manual' { ...
            arg({'lambda','RegularizationParam'},1,[0 Inf],'Regularization parameter (lambda)','type','denserealdouble') ...
            }, ...
         'grid_gcv' { ...
            arg({'gridSize','GridSize'},100,[0 Inf],'Grid size for regularization param search. This is used to automatically select the regularization parameter which minimizes the Generalized Cross-Validation (GCV) criteria.') ...
            arg({'plotGCV','PlotGCV'},false,[],'Plot GCV curve'), ...
            } ...
     },'Selection mode for lambda. Automatic (GCV grid search) or Manual (must provide lambda)'), ...
    arg_nogui('svd_state',[],[],'SVD decomposition structure'), ...
    arg({'verb','Verbosity'},false,[],'Verbose output') ...
    );

[nr,nc] = size(A);

% if A is not sufficiently sparse, convert to full
if issparse(A) && nnz(A)/numel(A) > 0.9
    A = full(A);
end

if isempty(svd_state)
    if verb, fprintf('Computing SVD of design matrix.\nr'); end

    % init prior covmat
    if isscalar(P)
        P=P*speye(nc);
    end
    if ~isempty(P)
        Pinv = inverse(P);
    end
    % compute SVD
    if ~isempty(P)
        APinv = A*Pinv;
    else
        APinv = A;
    end
    if isempty(blksz)
        [U,S,V] = svd_wrapper(APinv);
    else
        %warning('block optimization not yet implemented');
        [U,S,V] = svd_wrapper(APinv);
%         [U,S,V] = svd_wrapper(APinv(1:blksz(1),1:blksz(2)));
    end
    if isempty(P)
        iPV = V;
    else
        iPV = Pinv*V;
    end
    s   = diag(S);
    s2  = s.^2;
    Ut  = U';
else
    iPV = svd_state.iPV;
    s   = svd_state.s;
    s2  = svd_state.s2;
    Ut  = svd_state.Ut;
end

UtY = Ut*Y;

switch lambdaMode.arg_selection
    case 'grid_gcv'
        % search over a grid of lambda values for the value that minimizes
        % the Generalized Cross Validation (GCV) criteria
        % lambdaOpt = argmin(lambda) { GCV(lambda) }

        % automatically determine lambda range based on singular values
        tol     = max([nr nc])*eps(max(s));
        lgrid   = logspace(log10(tol),log10(max(s)),lambdaMode.gridSize);
        gcv     = zeros(lambdaMode.gridSize,1);
        for it=1:lambdaMode.gridSize
            % compute GCV criteria
            d       = lgrid(it)./(s2+lgrid(it));
            f       = diag(d)*UtY;
            gcv(it) = dot(f,f,1)/sum(d)^2;
        end
        loc = getMinima(gcv);
        if isempty(loc),
            % no minimum found, search for elbow instead
            if verb
                fprintf('no GCV minimum, finding elbow...\nr');
            end
            [val loc] = hlp_findElbow(gcv);  % min(gcv)
        end
        loc         = loc(end);
        lambda_opt  = lgrid(loc);
        if verb
            fprintf('lambda: %0.5g, GCV: %0.5g\nr',lambda_opt,gcv(loc));
        end
        if lambdaMode.plotGCV
            plotLambdaGCV(lgrid,gcv,lambda_opt,loc);
        end
    case 'manual'
        lambda_opt = lambdaMode.lambda;
    otherwise
        error('SIFT:ridge_gcv:BadLambdaRule', ...
              'Unknown lambda learning rule %s',lambdaMode.arg_selection);
end

% solve system for parameters x
x = iPV*bsxfun(@times,(s./(s2+lambda_opt^2)),UtY);

if nargout > 2
    Y_hat = A*x;
end
if nargout > 3
    svd_state.iPV = iPV;
    svd_state.s   = s;
    svd_state.s2  = s2;
    svd_state.UtY = UtY;
end

% Helper functions
% -------------------------------------------------------------------------
function [U,S,V] = svd_wrapper(A)
% compute SVD
if issparse(A)
    [U,S,V] = svds(A,min(size(A)));
else
    [U,S,V] = svd(A,'econ');
end

function plotLambdaGCV(lgrid,gcv,lambda_opt,loc)
% plot lambda versus GCV
figure;
semilogx(lgrid,gcv)
xlabel('log-lambda');
ylabel('GCV');
hold on;
plot(lambda_opt,gcv(loc),'rx','linewidth',2);
hold off; grid on;

function indmin = getMinima(x)
% get minimum of a function
fminor = diff(x)>=0;
fminor = ~fminor(1:end-1, :) & fminor(2:end, :);
fminor = [0; fminor; 0];
indmin = find(fminor);


classdef IMMOEA_F9 < PROBLEM
% <multi> <real> <large/none>
% Benchmark MOP for testing IM-MOEA

%------------------------------- Reference --------------------------------
% R. Cheng, Y. Jin, K. Narukawa, and B. Sendhoff, A multiobjective
% evolutionary algorithm using Gaussian process-based inverse modeling,
% IEEE Transactions on Evolutionary Computation, 2015, 19(6): 838-856.
%------------------------------- 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 = 30; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = [1,zeros(1,obj.D-1)+10];
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,X)
            t = X(:,2:obj.D).^(1./(1+3*repmat(2:obj.D,size(X,1),1)/obj.D)) - repmat(X(:,1),1,obj.D-1);
            g = sum(t.^2/4000,2) - prod(cos(t./repmat(sqrt(1:obj.D-1),size(X,1),1)),2) + 2;
            PopObj(:,1) = X(:,1);
            PopObj(:,2) = g.*(1-sqrt(PopObj(:,1)./g));
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R(:,1) = linspace(0,1,N)';
            R(:,2) = 1 - sqrt(R(:,1));
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            R = obj.GetOptimum(100);
        end
    end
end


function [x,y,z,s,w,flag] = msquadsolve(Q,c,A,b,C),
% MSQUADSOLVE
%
% USAGE:   [x,y,z,s,w,flag] = msquadsolve(Q,c,A,b,C)
%
% PARAMETERS:  Q -> (n,n) symetric matrix (definite positive)
%              c -> (n,1) vector
%              A -> (m,n) matrix
%              b -> (m,1) vector
%              C -> (m,1) vector
%
%            x -> primal variables
%            y -> lagrangian coeff of equality constraints
%            z -> dual variables of x
%            s -> primal auxiliary variable (only if C < Inf)
%            w -> dual variable of s
%            flag -> set to 0 => no problem, set to 1 => problem
%
% DESCRIPTION: Primal-dual method for quadratic programming
%
%            minimize c'*x + 0.5*x'*Q*x
%
%            subject to  A*x=b
%                        0<= x <= C
%            The method used here is a primal dual method with a predictor-corrector
%            approach and a logarithmic barrier. I used the heuristic from two
%            existing methods LOQO and HOPDM. The method is an iterative method. The
%            maximal number of iteration is stored in the variable 'max_iter'.
%
% ERRORS AND BUGS: 1. There is no test about the conditionning of the matrix Q. If the iteration
%                  50 has been reached, then the optimization may not be finished and the output
%                  may be wrong.
%                  2. If C contains infinite values then, the algorithm will consider that all its
%                  components are actually infinite.
%
% NOTES: 50 iterations have always been sufficient to solve all problems.
%        This code should be read with the tech. report:
%                "Regularized Symmetric Indefinite Systems in Interior Point
%                 Methods for Linear and Quadratic Optimization",
%                 A. Altman and J. Gondzio, Logilab Tech. Report 1998.6
%
% Andre Elisseeff, May. 2001

  %% init
  verbose = 0;
  n = size(Q,1);
  m = size(A,1);
  H = zeros(n+m,n+m);
  flag=1;
  maxC = max(C);

  %% Values of the original HOPDM of Gondzio and Altmann
  dinf = 10^(-14);
  smallz = 10^(-14);
  smallt = 2.3*10^(-16);
  opttol = 10^(-6);
  itref = 1;
  mu = 1;
  maxiter = 50;

  %% init values of the primal and dual variables
  x=ones(n,1);
  z=ones(n,1);
  y=ones(m,1);
  if maxC < Inf,
    s=ones(n,1);
    w=ones(n,1);
  else
    s=[];w=[];
  end;

  %% Description of variables:
  %%
  %%    x,s     -> primal variables
  %%    z,w     -> dual variables
  %%    n       -> number of variables in the initial pb (size of x)
  %%    m       -> number of constraints in A
  %%
  %%    dinf     -> smallest value for all variables
  %%    smallz   -> smallest value of z
  %%    smallt   -> smallest value for t in the computation of the matrix theta
  %%    opttol   -> acceptable tolerance for optimality conditions
  %%    itref    -> iteration counter
  %%    maxiter  -> maximum number of iteration

  %% Analyze the constraints...
  disp(sprintf('Analyzing the equality constraints...\n'));
  [QQ,RR]=qr(A',0);
  [mm,nb] = size(QQ); %% number of eq constraints
  ind = 1:1:nb;
  for i=1:nb,
    if abs(RR(i,i)) < 100*eps
      disp(sprintf('Constraints %d removed because of dependence\n',i));
      ind(i)=0;
    end;
  end;
  indice = find(ind >0);

  if (isempty(indice))
    disp(sprintf('No equality constraints... \n'));
    A=[];
    m=0;
  else
    A = A(indice,:); %% new independent eq constraints
    b = b(indice);
    y = y(indice);
    m=length(indice);
  end;
  clear QQ;clear RR;
  u = C;
  %% init values before looping
  cont = 1;
  objQ = 0.5*x'*Q*x;

  %% init values of primal and dual objective functions
  pobjo = abs(c'*x+objQ) + 1;
  if maxC < inf,
    dobjo = abs(b'*y - u'*w - objQ);
  else
    dobjo = abs(b'*y - objQ);
  end;
%%%%%%%%%%%%%%
%% MAIN LOOP
%%%%%%%%%%%%%%
  while (cont)&(itref<=maxiter)
    %% Compute the primal objective function
    objQ = 0.5*x'*Q*x;
    pobj = c'*x+objQ;
    if (maxC<Inf)
      dobj = b'*y - u'*w - objQ;
    else
      dobj = b'*y - objQ;
    end;
    dlgap = pobj - dobj;
    dp = abs(pobj)/(abs(pobjo)+1);
    dd = abs(dobj)/(abs(dobjo)+1);
    dobjo = dobj;
    pobjo = pobj;
    if verbose,
        disp(sprintf('%d - pobj : %f - dobj : %f\n',itref,pobj,dobj));
    end;
    %% Check if the solution are bounded
    if (dp > 10^6)
      disp(sprintf('Solution not bounded in the primal. Exit.\n'));
      return;
    end;
    if (dd > 10^6)
      disp(sprintf('Solution not bounded in the dual. Exit.\n'));
      return;
    end;

    %% test if optimality
    oldgap = dlgap;
    dp = abs(dobj) + 1;
    if ((abs(dlgap)/dp) <= opttol)
      disp(sprintf('Optimal solution found. Exit.\n'));
      cont = 0;
      break;
    end;

    dp = dp + abs(pobj);
    T = abs(dlgap)/dp;

    %% put the variables away from zero (from HOPDM)
    if (itref <= 3)
      ax = 2*10^(-3);
      az = 10^(-3);
    elseif (T >= 0.8)
      ax = 2*10^(-4);
      az = 10^(-4);
    elseif (T >= 0.1)
      ax = 2*10^(-5);
      az = 10^(-5);
    elseif (T >=0.01)
      ax = 2*10^(-6);
      az = 10^(-6);
    elseif (T>=0.001)
      ax = 2*10^(-7);
      az = 10^(-7);
    elseif (T>=0.0001)
      ax = 2*10^(-8);
      az = 10^(-7);
    elseif (T>=0.00001)
      ax = 2*10^(-9);
      az = 10^(-9);
    else
      ax = T*10^(-5);
      az = ax;
    end;

    %% consider only variables that can be changed
    x = x + ax;
    z = z + az;
    if maxC < Inf
      s = s + ax;
      w = w + az;
    end;
    %% Compute the values of xi_b, xi_c and xi_u
    xi_b = -A*x + b;
    xi_c = c - A'*y - z + Q*x;
    xi_z =  - x.*z;
    if maxC < Inf,
      xi_c=xi_c + w;
      xi_u = u - x - s;
      xi_w = - s.*w;
    end;
    %% Compute theta = (z/x + w/s)

    %% for bounded variables
    if maxC<Inf,
      dp = x;
      if (max(abs(dp))<= smallz)
        disp(sprintf('Conditioning problem to invert theta. Abort.\n'));
        return;
      end;
      dpp= s;
      if (max(abs(dpp))<= smallz)
        disp(sprintf('Conditioning problem to invert theta. Abort.\n'));
        return;
      end;
      theta=z./dp + w./dpp;
    end;
    %% for unbounded variables
    if (maxC==Inf),
      dp = x;
      if (max(abs(dp))<= smallz)
        disp(sprintf('Conditioning problem to invert theta. Abort.\n'));
        return;
      end;
      theta = z./dp;
    end;

    %% neglect small elements of theta array
    neglect = find(theta < smallt);
    if ~isempty(neglect)
      theta(neglect)=zeros(size(neglect));
    end;

    %% and control large elements of theta
        neglect = find(theta >= 10^8);
    if ~isempty(neglect)
      theta(neglect)=(10^4)*sqrt(theta(neglect));
    end;
    %% factorize H = [-Q-theta^(-1)   A^T]
    %%               [ A               0 ]

    H = zeros(n+m,n+m);
    H(1:n,1:n) = -Q-diag(theta);
    H(n+1:n+m,1:n) = A;
    H(1:n,n+1:n+m) = A';

    %% Compute the predictor step
    if maxC < Inf,
      f = xi_c-xi_z./x+(xi_w - xi_u.*w)./s ;
      h = xi_b;
    else
      f = xi_c - xi_z./x;
      h = xi_b;
    end;
    delta=H\[f;h];
    dx = delta(1:n);
    dy = delta(n+1:n+m);
    dz = (xi_z-z.*dx)./x;
    if maxC<Inf,
      ds = xi_u - dx;
      dw = (xi_w-w.*ds)./s;
    end;

   %% determine the maximum step size alpha_p (primal) and
   %% alpha_d (dual) to stay in feasible region
   %% (x,s,z,w must be positive and greater than dinf)
      indz = find(dz<0);
      indx = find(dx<0);
      inds=[];mins=1;
      indw=[];minw=1;
      if maxC < Inf,
        inds = find(ds<0);
        indw = find(dw<0);
        if ~isempty(inds)
          mins = min(-(s(inds)-dinf)./ds(inds));
        else
      mins = 1;
        end;
        if ~isempty(indw)
          minw = min(-(w(indw)-dinf)./dw(indw));
        else
      minw = 1;
        end;
      end;
      if ~isempty(indx)
        minx = min(-(x(indx)-dinf)./dx(indx));
      else
        minx = 1;
      end;
      apk = min([minx,mins,1]);
      if ~isempty(indz),
        minz = min(-(z(indz)-dinf)./dz(indz));
      else
        minz = 1;
      end;
      adk = min([minw,minz,1]);

      ax = sum(x.*z);
      as = sum((x+apk*dx).*(z+adk*dz));
      az = sum(dx.^2+dz.^2);
      if maxC < Inf,
        ax = ax + sum(s.*w);
        as = as + sum((s+apk*ds).*(w+adk*dw));
        az = az + sum(ds.^2+dw.^2);
      end;
      %% check if complementary gap is less than opttol
      if (as <= opttol)
        disp(sprintf('Complementary gap is less than %f\n',opttol));
        cont = 0;
        x = x + apk*dx;
        y = y + adk*dy;
        z = z + adk*dz;
        if maxC < Inf,
         s = s + apk*ds;
         w = w + adk*dw;
        end;
        break;
      end;

      %% Set the barrier parameter : LOQO's heuristic
      ap = min(apk,adk);
      mu = (ax/(2*n))*(0.95*(1/ap) -1)^2/(0.95*(1/ap)+10)^2;

      %% Compute the new direction (algo. of Mehrotra) of order 1 (corrector step)
      xi_z =  - x.*z + mu*ones(size(x)) - dx.*dz;
      f = xi_c - xi_z./x;
      if maxC < Inf
        xi_w = - s.*w + mu*ones(size(s)) - ds.*dw;
        f=f+ xi_w./s - (w.*xi_u)./s;
      end;
      h = xi_b;

      delta=H\[f;h];
      dx = delta(1:n);
      dy = delta(n+1:n+m);
      dz = (xi_z-z.*dx)./x;
      if maxC<Inf,
        ds = xi_u - dx;
        dw = (xi_w-w.*ds)./s;
      end;

      %% determine the maximum step size alpha_p (primal) and
      %% alpha_d (dual) to stay in feasible region
      %% (x,s,z,w must be positiv)
      indz = find(dz<0);
      indx = find(dx<0);
      if maxC < Inf,
        inds = find(ds<0);
        indw = find(dw<0);
        if ~isempty(inds)
          mins = min(-s(inds)./ds(inds));
        else
        mins=1;
        end;
        if ~isempty(indw)
          minw = min(-w(indw)./dw(indw));
        else
        minw = 1;
        end;
      end;
      if ~isempty(indx)
        minx = min(-x(indx)./dx(indx));
      else
        minx = 1;
      end;
      alpha_p = min([minx,mins,1]);
      if ~isempty(indz)
        minz = min(-z(indz)./dz(indz));
      else
        minz = 1;
      end;
      alpha_d = min([minw,minz,1]);

      %% Compute step factors
      fp = 0.9*min(alpha_p,alpha_d);
      fd = 0.9*min(alpha_d,alpha_p);
      x = x +fp*dx;
      y = y +fd*dy;
      z = z + fd*dz;
      if maxC < Inf,
        w = w + fd*dw;
        s = s +fp*ds;
      end;
      itref=itref+1;
  end;
%%%%%%%%%%%%%%%%%%%%
%% End of main loop
%%%%%%%%%%%%%%%%%%%%

    if (cont==0)
      disp(sprintf('Optimal Solution found after %d iteration.\n',itref));
      disp(sprintf('Value of the objective : %f.\n',pobj));
      flag=0;
    end;
end; %% function