 function [smap, x, y, z] = ir_mri_sensemap_sim(varargin)
%function [smap, x. y, z] = ir_mri_sensemap_sim(varargin)
%|
%| Simulate 2D or 3D sensitivity maps for sensitivity-encoded MRI
%| based on grivich:00:tmf doi:10.1119/1.19461
%| This code makes maps for multiple coils, but does not model coupling
%| between coils so most likely it is an approximation at best.
%|
%| option
%|	nx, ny, nz		image size (default: [64 64 1])
%|	dx, dy, dz		pixel/voxel dimensions (default: [3 3 3])
%|	ncoil			# of coils total (default: 4)
%|	nring			# of rings of coils (default: 1)
%|	rcoil			coil radius (default: 100mm)
%|	dz_coil			ring spacing in z.  (def: nz*dz/nring)
%|				(3D geometry is a cylinder)
%|	coil_distance		distance of coil center from isocenter for
%|				central ring of coils as a multiple of FOVx,
%|				where FOVx=nx*dx (default: 1.2)
%|	orbit			default: 360
%|	scale			'' (default)
%|				'ssos_center' : make SSoS of center = 1
%|
%| out
%|	smap	[nx ny nz ncoil]	simulated sensitivity maps (complex!)
%|
%| all length parameters must have same units (e.g., mm or cm)
%|
%| Copyright 2005-6-20, Jeff Fessler and Amanda Funai, University of Michigan
%| 2014-08-19 JF more testing, verifying phase is correct
%| 2014-09-09 modified for 3D by Mai Le
%| 2016-05-03 JF fixes

if nargin < 1, ir_usage, end

if nargin == 1 && streq(varargin{1}, 'test', 4)
	ir_mri_sensemap_sim_test(varargin{1})
	return
end

arg.nx = 64;
arg.ny = [];
arg.nz = 1; % 2D
arg.dx = 3; % pixel size in mm
arg.dy = [];
arg.dz = [];
arg.ncoil = 4; % # of coils
arg.nring = 1;
arg.rcoil = 100; % coil radius in mm
arg.orbit = 360;
arg.orbit_start = 0; % can be [nring] to give each ring an offset [degrees]
arg.dz_coil = [];
arg.coil_distance = 1.2; % multiplies fov/2
arg.scale = '';
arg.chat = nargout == 0;

arg = vararg_pair(arg, varargin);

if isempty(arg.dy), arg.dy = arg.dx; end
if isempty(arg.dz), arg.dz = arg.dx; end
if isempty(arg.ny), arg.ny = arg.nx; end
if isempty(arg.rcoil), arg.rcoil = arg.dx * arg.nx / 2 * 0.50; end
if isempty(arg.dz_coil), arg.dz_coil = arg.dz * arg.nz / arg.nring; end

coils_per_ring = round(arg.ncoil / arg.nring);
if arg.nring * coils_per_ring ~= arg.ncoil
	fail('nring must be divisor of ncoil')
end

[ring_smap, x, y, z] = ir_mri_sensemap_sim_do(...
	arg.nx, arg.ny, arg.nz, ...
	arg.dx, arg.dy, arg.dz, ...
	arg.ncoil, coils_per_ring, arg.rcoil, arg.dz_coil, ...
	arg.orbit, arg.orbit_start, arg.coil_distance, arg.chat);

if arg.nz == 1
	smap = reshape(ring_smap, [arg.nx arg.ny arg.ncoil]);
	scale_center = 1 / sqrt(sum(abs(smap(end/2,end/2,:).^2)));
else
	smap = reshape(ring_smap, [arg.nx arg.ny arg.nz arg.ncoil]);
	scale_center = 1 / sqrt(sum(abs(smap(end/2,end/2,end/2,:).^2)));
end

switch arg.scale
case ''
case 'ssos_center'
	smap = smap * scale_center;
otherwise
	fail('unknown scale method "%s"', arg.scale)
end

% ir_mri_sensemap_sim_do()
function [smap x y z] = ir_mri_sensemap_sim_do(nx, ny, nz, ...
		dx, dy, dz, ncoil, ncoilpr, rcoil, dz_coil, ...
		orbit, orbit_start, coil_distance, chat)

nring = ncoil / ncoilpr;
rlist = rcoil * ones(ncoilpr,nring,'single'); % coil radii

plist = zeros(ncoilpr,nring,3,'single'); % position of coil center [x y z]
nlist = zeros(ncoilpr,nring,3,'single'); % normal vector (inward) from coil center
olist = zeros(ncoilpr,nring,3,'single'); % unit vector orthogonal to normal vector in x-y
ulist = zeros(ncoilpr,nring,3,'single'); % upward vector

if numel(orbit_start) == 1
	orbit_start = repmat(orbit_start, nring);
end

% cylindrical coil configuration, like abdominal coils
alist = deg2rad(orbit) * [0:(ncoilpr-1)] / ncoilpr; % coil angles [radians]
z_ring = ([1:nring]-(nring+1)/2) * dz_coil;
for ir = 1:nring
	for ic = 1:ncoilpr
		phi = alist(ic) + deg2rad(orbit_start(ir));
		Rad = max(nx/2 * dx, ny/2 * dy) * coil_distance;
		plist(ic,ir,:) = [Rad * [cos(phi) sin(phi)] z_ring(ir)];
		nlist(ic,ir,:) = -[cos(phi) sin(phi) 0*z_ring(ir)]; % cylinder
		olist(ic,ir,:) = [-sin(phi) cos(phi) 0];
		ulist(ic,ir,:) = [0 0 1];
	end
end

% object coordinates
x = ([1:nx] - (nx+1)/2) * dx;
y = ([1:ny] - (ny+1)/2) * dy;
z = ([1:nz] - (nz+1)/2) * dz;
[xx,yy,zz] = ndgrid(x,y,z);

smap = zeros(nx, ny, nz, ncoilpr, nring, 'single');
for ir = 1:nring
	for ic=1:ncoilpr
		% rotate coordinates to correspond to coil orientation
		zr =	(xx - plist(ic,ir,1)) .* nlist(ic,ir,1) + ...
			(yy - plist(ic,ir,2)) .* nlist(ic,ir,2) + ...
			(zz - plist(ic,ir,3)) .* nlist(ic,ir,3);
		xr =	xx .* nlist(ic,ir,2) - yy .* nlist(ic,ir,1);
		yr = zz - plist(ic,ir,3); % translate along object z axis

		if 0 % see coordinates
			im plc 1 2
			im(1, x, y, xr), xlabel x, ylabel y
			im(2, x, y, zr)
			keyboard
		end

		% compute sensitivity vectors in coil coordinates
		[sx,sy,sz] = ir_mri_smap1(xr, yr, zr, rlist(ic,ir));

		% coil response depends on tranverse magnetization only?
		% todo: unsure if this should depend on sy and ulist in 3D
		bx = sz * nlist(ic,ir,1) + sx * olist(ic,ir,1);
		by = sz * nlist(ic,ir,2) + sx * olist(ic,ir,2);
	%	bz = sz * nlist(ic,ir,3) + sx * olist(ic,ir,3);
		smap(:,:,:,ic,ir) = bx + 1i * by;

		if 0 && chat && nz == 1 && im % see field components
			im plc 2 2
			im(1, x, y, sx), cbar
			im(2, x, y, sy), cbar
			im(3, x, y, sz), cbar
			im subplot 4
			tmp = sqrt(sx.^2 + sz.^2);
			quiver(x, y, (sx./tmp)', (sz./tmp)', 0), axis square
			prompt
		end

		if 0 % see final field components vs phase
			im subplot 4
			bb = sqrt(bx.^2 + by.^2);
			quiver(x, y, (bx./bb)', (by./bb)', 0), axis square
			im(2, x, y, angle(smap(:,:,ic))), cbar
			keyboard
		end
	end
end
smap = smap * rlist(1) / (2*pi); % trick: scale so maximum is near unity

if chat && im % show smap and array geometry in z=0 plane
	if nz == 1
		ir_mri_sensemap_sim_show2(smap, x, y, dx, dy, nlist, plist, rlist)
	else
		ir_mri_sensemap_sim_show3(smap, x, y, z, dx, dy, dz, ...
		nlist, plist, rlist, olist, ulist, nring, ncoilpr, rcoil)
	end
end

% ir_mri_sensemap_sim_show3()
% shows coil geometry but not the 3D smap
function ir_mri_sensemap_sim_show3(smap, x, y, z, dx, dy, dz, ...
	nlist, plist, rlist, olist, ulist, nring, ncoilpr, rcoil)

pcolor = {'c', 'g', 'r'};
pcolor = @(i) pcolor{1+rem(i,3)};
clf, ir_plot3_cube(x,y,z)
xlabel x, ylabel y, zlabel z
hold on
plot3(plist(:,:,1), plist(:,:,2), plist(:,:,3), 'bo') % coil centers
if 1 % coil normals
	tmp1 = reshape(plist, [], 3);
	tmp2 = reshape(nlist, [], 3);
	quiver3(tmp1(:,1), tmp1(:,2), tmp1(:,3), ...
		tmp2(:,1), tmp2(:,2), tmp2(:,3), 0.2)
end
if 1 % coils
	for ir = 1:nring
	for ic = 1:ncoilpr
		tmp = linspace(0, 2*pi, 50)';
		tmp = cos(tmp) * squeeze(olist(ic,ir,:))' + ...
			sin(tmp) * squeeze(ulist(ic,ir,:))';
		tmp = repmat(squeeze(plist(ic,ir,:))', ...
			[nrow(tmp) 1]) + rcoil * tmp;
	%	plot3(tmp(:,1), tmp(:,2), tmp(:,3), 'g-')
		patch(tmp(:,1), tmp(:,2), tmp(:,3), pcolor(ir), ...
		'edgecolor', 'none', 'facealpha', 0.5)
	end
	end
end
hold off
axis equal
% end

function ir_plot3_cube(x,y,z)
x1 = x(1);
x2 = x(end);
y1 = y(1);
y2 = y(end);
z1 = z(1);
z2 = z(end);
x = [x1 x2 x2 x1 x1 x1 x2 x2 x1 x1];
y = [y1 y1 y2 y2 y1 y1 y1 y2 y2 y1];
z = [z1 z1 z1 z1 z1 z2 z2 z2 z2 z2];
plot3(x,y,z)

% ir_mri_sensemap_sim_show2()
function ir_mri_sensemap_sim_show2(smap, x, y, dx, dy, nlist, plist, rlist)
switch ndims(smap)
case 3
	[nx ny ncoil] = size(smap);
case 4
	[nx ny nz ncoil] = size(smap);
otherwise
	fail('unknown ndims(smap) = %d', ndims(smap))
end
nshow = min(max(ncoil,2), 4);
im('plc', 3, nshow)
for ii=1:min(ncoil,nshow)
	clim = [0 max(abs(smap(:)))];
	tmp = smap(:,:,ii);
	im(ii, x, y, abs(tmp), clim, 'Magnitude'), cbar
%	xmax = max(max(abs(x)), max(plist(:,1)));
%	ymax = max(max(abs(y)), max(plist(:,2)));
	xmax = max([max(abs(x)) max(abs(y)) max(col(plist(:,[1 2])))]);
%	axis([-xmax xmax -ymax ymax]*1.05)
	axis(xmax * [-1 1 -1 1] * 1.1)
	xtick([-1 0 1] * nx/2 * dx)
	ytick([-1 0 1] * ny/2 * dy)

	hold on
	plot(0,0,'.', plist(:,1), plist(:,2), 'bo')
	xdir = nlist(ii,2);
	ydir = nlist(ii,1);
	r = rlist(ii);
	plot(plist(ii,1)+r*xdir*[-1 1], plist(ii,2)+r*ydir*[1 -1], 'b-')
	hold off

%	ph = ir_unwrap(angle(tmp)); % trick: unwrap phase for pretty display
	ph = angle(tmp); % show raw phase (understandable with hsv colormap)
	im(ii+nshow, 'hsv', x, y, ph, [-pi pi], 'Phase'), cbar
	axis(xmax*[-1 1 -1 1]*1.1)
	xtick([-1 0 1] * nx/2 * dx)
	ytick([-1 0 1] * ny/2 * dy)
end

ssos = sqrt(sum(abs(smap).^2, ndims(smap)));
minmax(ssos)
ssos = ssos / ssos(end/2,end/2);
im(2*nshow+1, x, y, ssos, 'SSoS (normalized)'), cbar
xtick([-1 0 1] * nx/2 * dx)
ytick([-1 0 1] * ny/2 * dy)

if ncoil == 1
	subplot(122)
	bx = real(smap);
	by = imag(smap);
	quiver(x, y, bx', by'), title 'Field pattern in x-y plane'
	axis equal, axis tight
end

% clf, im(angle(smap(:,:,2))), cbar

% ir_mri_smap_r(r, z)
% function for testing near 0
function out = ir_mri_smap_r(r, z)
M = 4 * r ./ ((1 + r).^2 + z.^2); % = k^2, see ellipke
[K E] = ellipke(M);
out = 2 * z ./ r .* ((1 + r).^2 + z.^2).^(-0.5) .* ...
	((1 + r.^2 + z.^2) ./ ((1 - r).^2 + z.^2) .* E - K);

% ir_mri_smap1()
% based on grivich:00:tmf
% for a circular coil in "x-y plane" of radius "a"
% note that coil x-y plane is not same as object x-y plane!
% returns (i,j,k) components of B vector for each (x,y,z) location
function [smap_x smap_y smap_z] = ir_mri_smap1(x, y, z, a)
x = x ./ a; % normalized units
y = y ./ a;
z = z ./ a;
r = sqrt(x.^2 + y.^2);
M = 4 * r ./ ((1 + r).^2 + z.^2); % = k^2, see ellipke
[K E] = ellipke(M);
if ir_is_octave
	K = reshape(K, size(M)); % ellipke shape bug in octave
	E = reshape(E, size(M));
end

% the following is B_z in eqn (18) in grivich:00:tmf
% and same as eqn [10] in wang:00:dop to within constant scale factor
smap_z = 2 * ((1 + r).^2 + z.^2).^(-0.5) .* ...
	(K + (1 - r.^2 - z.^2) ./ ((1 - r).^2 + z.^2) .* E);
smap_z = smap_z / a;

if 0 && any(r(:) == 0) % test code to explore when r is near 0
	r0 = linspace(0,5e-7,101);
	z0 = 0.4;
	t0 = ir_mri_smap_r(r0, z0);
	slope = 3*pi * z0 / ((1+z0^2)^2.5);
	clf, plot(r0, t0, '-', r0, slope * r0, '--'); grid, prompt
end

% the following is B_r in eqn (17) in grivich:00:tmf
smap_r = 2 * z ./ r .* ((1+r).^2 + z.^2).^(-0.5) .* ...
	((1 + r.^2 + z.^2) ./ ((1-r).^2 + z.^2) .* E - K);
bad = abs(r) < 1e-6;
smap_r(bad) = 3 * pi * z(bad) ./ ((1 + z(bad).^2).^2.5) .* r(bad);
smap_r = smap_r / a;

if any(isnan(smap_r(:))) || any(isnan(smap_z(:)))
	keyboard
end

smap_x = smap_r .* div0(x, r);
smap_y = smap_r .* div0(y, r);
%smap_z = smap_r .* div0(z, r);

%phi = atan2(y, x);
%smap_x = smap_r .* cos(phi);
%smap_y = smap_r .* sin(phi);

% ir_mri_sensemap_sim_test0
% see ellipke
function ir_mri_sensemap_sim_test0
m = linspace(0,1,101);
[k e] = ellipke(m);
clf, plot(m, k, '-', m, e, '--'), legend('k', 'e')
yaxis_pi('0 p/2 p 3*p/2')

% ir_mri_sensemap_sim_test1
% test ir_mri_smap1 routine, cf Fig. 4 of grivich:00:tmf
function ir_mri_sensemap_sim_test1
a = 1;
x = linspace(-2,2,99);
y = linspace(-2,2,97);
zlist = [0.001 0.1 0.2 0.5 1.0];
zlist(1) = [];
[xx yy zz] = ndgrid(x, y, zlist);
[smap_x smap_y smap_z] = ir_mri_smap1(xx, yy, zz, a);
smap_b = sqrt(smap_x.^2 + smap_y.^2);
if im
	im('plc', 4, numel(zlist))
	ir_mri_sensemap_sim_test1_show(smap_x, x, y, 0, zlist, 'x')
	ir_mri_sensemap_sim_test1_show(smap_y, x, y, 1, zlist, 'y')
	ir_mri_sensemap_sim_test1_show(smap_z, x, y, 2, zlist, 'z')
	ir_mri_sensemap_sim_test1_show(smap_b, x, y, 3, zlist, 'b')
end

% ir_mri_sensemap_sim_test1_show()
function ir_mri_sensemap_sim_test1_show(map, x, y, offset, zlist, titl)
clim = [-20 20];
for iz = 1:numel(zlist)
	p = offset*numel(zlist) + iz;
	im(p, x, y, map(:,:,iz), titl, clim), cbar
	axis equal
	if iz == 1
		xtick([-2 2]), ytick([-2 2])
	else
		xtick off, ytick off
	end
	if zlist(iz) < 0.5
		blim = [7 12 19];
	else
		blim = [1 3 5];
	end
	hold on
	contour(x, y, abs(map(:,:,iz))', blim, 'b-')
	contour(x, y, abs(map(:,:,iz))', [0 0]+0.001, 'g-')
	hold off
	if streq(titl, 'b')
		xlabelf('z = %g', zlist(iz))
	end
end
drawnow

% ir_mri_sensemap_sim_test2
% 2D test case
function ir_mri_sensemap_sim_test2

[smap x y] = ir_mri_sensemap_sim('chat', 1, 'nx', 32, ...
	'rcoil', [], 'ncoil', 4, 'coil_distance', 1.2);

if 0 % check vs old version
	old = mri_sensemap_sim('chat', 1, 'nx', 32, ...
		'rcoil', [], 'ncoil', 4, 'coil_distance', 1.2);
	equivs(smap, old)
end

if 1 % check rotational symmetry in 4-coil case
	for ic=2:4
		tmp = rot90(smap(:,:,1), (ic-1));
		equivs(abs(tmp), abs(smap(:,:,ic)))
	%	im(8+ic, x, y, abs(tmp)), cbar
		p1 = angle(tmp) + (ic-1) * pi/2; % add pi/2 to rotated
		p2 = angle(smap(:,:,ic));
		tmp = exp(1i * (p2 - p1));
		equivs(tmp, ones(size(tmp))) % trick: equivs mod 2*pi
	end
end

% ir_mri_sensemap_sim_test3
% illustrate 3D sense maps with el
function ir_mri_sensemap_sim_test3
nring = 3;
ncoil = 4 * nring;
ig = image_geom('nx', 16, 'ny', 14, 'nz', 10, 'fov', 200, 'dz', 20); % 20cm fov
%ig = image_geom('nx', 72, 'ny', 48, 'nz', 12, 'fov', 22, 'zfov', 10); % michelle
%nring = 2; ncoil = 8; % michelle
ig.mask = ig.circ > 0;
smap = ir_mri_sensemap_sim('chat', 1, 'nx', ig.nx, 'ny', ig.ny, 'nz', ig.nz, ...
	'dx', ig.dx, ...
	'dz', ig.dz, ...
	'orbit_start', 1*[0 45 0], ...
...%	'orbit_start', 0*[0 0], ...
	'rcoil', 70, ...
...%	'rcoil', 3, ...
	'nring', nring, 'ncoil', ncoil, 'coil_distance', 1.2);
if im
	prompt
	tmp = smap .* repmat(ig.mask, [1 1 1 ncoil]);
	im clf, im('row', ncoil, reshape(abs(tmp), [ig.nx ig.ny ig.nz*ncoil]))
	prompt
	tmp = permute(tmp, [1 3 2 4]); % [nx nz ny ncoil] z cuts are smooth
	im('row', ncoil, reshape(abs(tmp), [ig.nx ig.nz ig.ny*ncoil]))
	prompt
	im('row', ncoil, abs(tmp(:,:,end/2,:)))
end

% ir_mri_sensemap_sim_test()
function ir_mri_sensemap_sim_test(arg)
switch(arg)
case 'test0'
	ir_mri_sensemap_sim_test0 % ellipk
case 'test1'
	ir_mri_sensemap_sim_test1 % basic test
case 'test2'
	ir_mri_sensemap_sim_test2 % 2D test
case 'test3'
	ir_mri_sensemap_sim_test3 % 3D test
case 'test'
	ir_mri_sensemap_sim_test1
	if im, prompt, end
	ir_mri_sensemap_sim_test2
	if im, prompt, end
	ir_mri_sensemap_sim_test3
otherwise
	fail('bad argument "%s"', arg)
end


function [P,A] = GFM(X)
% Generic front modeling

%------------------------------- 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".
%--------------------------------------------------------------------------

    [N,M] = size(X);
    X     = max(X,1e-12);
    P     = ones(1,M);
    A     = ones(1,M);
    lamda = 1;
	E     = sum(repmat(A,N,1).*X.^repmat(P,N,1),2) - 1;
    MSE   = mean(E.^2);
    for epoch = 1 : 1000
        % Calculate the Jacobian matrix
        J = [repmat(A,N,1).*X.^repmat(P,N,1).*log(X),X.^repmat(P,N,1)];
        % Update the value of each weight
        while true
            Delta  = -(J'*J+lamda*eye(size(J,2)))^-1*J'*E;
            newP   = P + Delta(1:M)';
            newA   = A + Delta(M+1:end)';
            newE   = sum(repmat(newA,N,1).*X.^repmat(newP,N,1),2) - 1;
            newMSE = mean(newE.^2);
            if newMSE < MSE && all(newP>1e-3) && all(newA>1e-3)
                P     = newP;
                A     = newA;
                E     = newE;
                MSE   = newMSE;
                lamda = lamda/1.1;
                break;
            elseif lamda > 1e8
                return;
            else
                lamda = lamda*1.1;
            end
        end
    end
end


% statcond()  - compare two or more data conditions statistically using
%               standard parametric or nonparametric permutation-based ANOVA
%               (1-way or 2-way) or t-test methods. Parametric testing uses
%               fcdf() from the Matlab Statistical Toolbox.
% Usage:
%          >> [stats, df, pvals, surrog] = statcond( data, 'key','val'... );
%
% Inputs:
%   data       = one-or two-dimensional cell array of data matrices.
%                   For nonparametric, permutation-based testing, the
%                last dimension of the data arrays (which may be of up to
%                4 dimensions) is permuted across conditions, either in
%                a 'paired' fashion (not changing the, e.g., subject or
%                trial order in the last dimension) or in an umpaired
%                fashion (not respecting this order). If the number of
%                elements in the last dimension is not the same across
%                conditions, the 'paired' option is turned 'off'.  Note:
%                All other dimensions MUST be constant across conditions.
%                   For example, consider a (1,3) cell array of matrices
%                of size (100,20,x) each holding a (100,20) time/frequency
%                transform from each of x subjects. Only the last dimension
%                (here x, the number of subjects) may differ across the
%                three conditions.
%                   The test used depends on the size of the data array input.
%                When the data cell array has 2 columns and the data are
%                paired, a paired t-test is performed; when the data are
%                unpaired, an unpaired t-test is performed. If 'data'
%                has only one row (paired or unpaired) and more than 2
%                columns, a one-way ANOVA is performed. If the data cell
%                array contains several rows and columns, and the data is
%                paired, a two-way repeated measure ANOVA is performed.
%                NOTE THAT IF THE DATA is unpaired, EEGLAB will use a
%                balanced 1 or 2 way ANOVA and parametric results might not
%                be meaningful (bootstrap and permstatcondutation should be fine).
%
% Optional inputs:
%   'paired'   = ['on'|'off'] pair the data array {default: 'on' unless
%                the last dimension of data array is of different lengths}.
%                For two independent variables, this input is a cell array,
%                for example { 'on' 'off' } indicating that the first
%                independent variable is paired and the second is not.
%   'method'   = ['perm'|'bootstrap'|'param'] method for computing the p-values:
%                 'param' or 'parametric' = parametric testing (standard ANOVA
%                                           or t-test);
%                 'perm' or 'permutation' = non-parametric testing using
%                                           surrogate data
%                 'bootstrap' = non-parametric bootstrap
%                  made by permuting the input data {default: 'param'}
%   'naccu'    = [integer] Number of surrogate data copies to use in 'perm'
%                 or 'bootstrap' method estimation (see above) {default: 200}.
%   'verbose'  = ['on'|'off'] print info on the command line {default: 'on'}.
%   'variance' = ['homegenous'|'inhomogenous'] this option is exclusively
%                for parametric statistics using unpaired t-test. It allows
%                to compute a more accurate value for the degree of freedom
%                using the formula for inhomogenous variance (see
%                ttest2_cell function). Default is 'inhomegenous'.
%   'surrog'   = surrogate data array (see output).
%   'stats'    = F- or T-value array (see output).
%   'tail'     = ['one'|'two'] run one-tailed (F-test) or two tailed
%                (T-test). This option is only relevant when using the
%                'surrog' input. Otherwise it is ignored.
%   'forceanova' = ['on'|'off'] force the use of ANOVA calculation even
%                for 2x1 designs. Default is 'off'.
%   'alpha'    = [float] p-value threshold value. Allow returning
%                confidence intervals and mask (requires structoutput below).
%   'structoutput' = ['on'|'off'] return an output structure instead of
%                the regular output. Allow to output mask and confidence
%                intervals.
%
% Legacy parameters:
%   'threshold' - now 'alpha'
%   'mode'      - now 'method'
%
% Outputs:
%   stats      = F- or T-value array of the same size as input data without
%                the last dimension. A T value is returned only when the data
%                includes exactly two conditions.
%   df         = degrees of freedom, a (2,1) vector, when F-values are returned
%   pvals      = array of p-values. Same size as input data without the last
%                data dimension. All returned p-values are two-tailed.
%   surrog     = surrogate data array (same size as input data with the last
%                dimension filled with a number ('naccu') of surrogate data sets.
%
% Important note: When a two-way ANOVA is performed, outputs are cell arrays
%                 with three elements: output(1) = row effects;
%                 output(2) = column effects; output(3) = interactions
%                 between rows and columns.
%
% Examples:
%      >> a = { rand(1,10) rand(1,10)+0.5 }; % pseudo 'paired' data vectors
%         [t df pvals] = statcond(a);        % perform paired t-test
%           pvals =
%              5.2807e-04 % standard t-test probability value
%         % Note: for different rand() outputs, results will differ.
%
%         [t df pvals surog] = statcond(a, 'method', 'perm', 'naccu', 2000);
%           pvals =
%              0.0065 % nonparametric t-test using 2000 permuted data sets
%
%         a = { rand(2,11) rand(2,10) rand(2,12)+0.5 }; % pseudo 'unpaired'
%         [F df pvals] = statcond(a); % perform an unpaired ANOVA
%           pvals =
%              0.00025 % p-values for difference between columns
%              0.00002 % for each data row
%
%         a = { rand(3,4,10) rand(3,4,10) rand(3,4,10); ...
%               rand(3,4,10) rand(3,4,10) rand(3,4,10)+0.5 };
%         % pseudo (2,3)-condition data array, each entry containing
%         %                                    ten (3,4) data matrices
%         [F df pvals] = statcond(a);  % perform a paired 2-way ANOVA
%         % Output:
%           pvals{1} % a (3,4) matrix of p-values; effects across rows
%           pvals{2} % a (3,4) matrix of p-values; effects across colums
%           pvals{3} % a (3,4) matrix of p-values; interaction effects
%                                      % across rows and columns
%
% Author: Arnaud Delorme, SCCN/INC/UCSD, La Jolla, 2005-
%         With thanks to Robert Oostenveld for fruitful discussions
%         and advice on this function.
%
% See also: anova1_cell(), anova2_cell(), anova2rm_cell, fcdf()

% perform a paired t-test
% -----------------------
% a = { rand(2,10) rand(2,10) };
% [t df pval] = statcond(a); pval
% [h p t stat] = ttest( a{1}(1,:), a{2}(1,:)); p
% [h p t stat] = ttest( a{1}(2,:), a{2}(2,:)); p
%
% compare significance levels
% --------------------------
% a = { rand(1,10) rand(1,10) };
% [F df pval] = statcond(a, 'method', 'perm', 'naccu', 200); pval
% [h p t stat] = ttest( a{1}(1,:), a{2}(1,:)); p

% Copyright (C) Arnaud Delorme
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

function [ ori_vals, df, pvals, surrogval ] = statcond( data, varargin );

    if nargin < 1
        help statcond;
        return;
    end;
    try, warning('off', 'MATLAB:divideByZero'); catch, end;

    if exist('finputcheck')
        g = finputcheck( varargin, { 'naccu'      'integer'   [1 Inf]             200;
                                     'method'     'string'    { 'param','parametric','perm','permutation','bootstrap' }  'param';
                                     'mode'       'string'    { }                 '';
                                     'paired'     'string'    { 'on','off' }      'on';
                                     'surrog'     { 'real','cell' }      []       [];
                                     'stats'      { 'real','cell' }      []       [];
                                     'structoutput' 'string'  { 'on','off' }      'off';
                                     'forceanova'   'string'  { 'on','off' }      'off';
                                     'arraycomp'  'string'    { 'on','off' }      'on';
                                     'alpha'      'real'      []                  NaN;
                                     'tail'       'string'    { 'one','both','upper','lower'}    'both';
                                     'variance'   'string'    { 'homogenous','inhomogenous' }    'inhomogenous';
                                     'returnresamplingarray' 'string'    { 'on','off' }      'off';
                                     'verbose'    'string'    { 'on','off' }      'on' }, 'statcond');
        if isstr(g), error(g); end;
    else
        g = struct(varargin{:});
        if ~isfield(g, 'naccu'),     g.naccu = 200; end;
        if ~isfield(g, 'method'),    g.method  = 'param'; end;
        if ~isfield(g, 'paired'),    g.paired = 'on'; end;
        if ~isfield(g, 'surrog'),    g.surrog = []; end;
        if ~isfield(g, 'orivals'),   g.orivals = []; end;
        if ~isfield(g, 'arraycomp'), g.arraycomp = 'on'; end;
        if ~isfield(g, 'verbose'),   g.verbose = 'on'; end;
        if ~isfield(g, 'tail'),      g.tail = 'both'; end;
        if ~isfield(g, 'variance'),  g.variance = 'homogenous'; end;
        if ~isfield(g, 'structoutput'), g.structoutput = 'on'; end;
        if ~isfield(g, 'returnresamplingarray'),   g.returnresamplingarray = 'off'; end;
    end;
    if ~isempty(g.mode), g.method = g.mode; end;

    if strcmpi(g.method, 'parametric'), g.method = 'param'; end;
    if strcmpi(g.method, 'permutation'), g.method = 'perm'; end;
    if strcmpi(g.verbose, 'on'), verb = 1; else verb = 0; end;
    if strcmp(g.method, 'param' ) && exist('fcdf') ~= 2
      myfprintf('on',['statcond(): parametric testing requires fcdf() \n' ...
               '            from the Matlab StatsticaL Toolbox.\n' ...
               '            Running nonparametric permutation tests\n.']);
      g.method = 'perm';
    end
    if size(data,2) == 1, data  = transpose(data); end; % cell array transpose
    g.naccu = round(g.naccu);

    % reshape matrices
    % ----------------
    nd = size(data{1});
    nd = nd(1:end-1);
    for index = 1:prod(size(data))
        data{index} = reshape(data{index}, [prod(nd) size(data{index},myndims(data{index}))]);
    end;

    if ~strcmpi(g.method, 'param') && isempty(g.surrog)
         tmpsize   = size(data{1});
         surrogval = zeros([ tmpsize(1:end-1) g.naccu ], 'single');
    else surrogval = [];
    end;

    % check for NaNs or Inf
    % ---------------------
    for iDat = 1:length(data(:))
        if any(isnan(reshape(data{iDat}, prod(size(data{iDat})),1))) || ...
                any(isinf(reshape(data{iDat}, prod(size(data{iDat})),1)))
            error('Statcond: One of the input array contains NaNs or Infinite values');
        end;
    end;

    % bootstrap flag
    % --------------
    if strcmpi(g.method, 'bootstrap'), bootflag = 1;
    else                               bootflag = 0;
    end;

    if isempty(g.surrog)
        % test if data can be paired
        % --------------------------
        if length(unique(cellfun('size', data, ndims(data{1}) ))) > 1
            g.paired = 'off';
        end;
        if strcmpi(g.paired, 'on')
             pairflag = 1;
        else pairflag = 0;
        end;

        % return resampling array
        % -----------------------
        if strcmpi(g.returnresamplingarray, 'on')
            [ datavals datalen datadims ] = concatdata( data );
            if strcmpi(g.arraycomp, 'on')
                ori_vals = surrogdistrib( data, 'method', g.method, 'pairing', g.paired, 'naccu', g.naccu);
            else
                ori_vals = surrogdistrib( data, 'method', g.method, 'pairing', g.paired);
            end;
            return;
        end;

        % text output
        % -----------
        myfprintf(verb,'%d x %d, ', size(data,1), size(data,2));
        if strcmpi(g.paired, 'on')
             myfprintf(verb,'paired data, ');
        else myfprintf(verb,'unpaired data, ');
        end;
        if size(data,1) == 1 && size(data,2) == 2
             myfprintf(verb,'computing T values\n');
        else myfprintf(verb,'computing F values\n');
        end;
        if size(data,1) > 1
            if strcmpi(g.paired, 'on')
                 myfprintf(verb,'Using 2-way repeated measure ANOVA\n');
            else myfprintf(verb,'Using balanced 2-way ANOVA (not suitable for parametric testing, only bootstrap)\n');
            end;
        elseif size(data,2) > 2
            if strcmpi(g.paired, 'on')
                 myfprintf(verb,'Using 1-way repeated measure ANOVA\n');
            else myfprintf(verb,'Using balanced 1-way ANOVA (equivalent to Matlab anova1)\n');
            end;
        else
            if strcmpi(g.paired, 'on')
                 myfprintf(verb,'Using paired t-test\n');
            else myfprintf(verb,'Using unpaired t-test\n');
            end;
        end;
        if ~strcmpi(g.method, 'param')
            if bootflag, myfprintf(verb,'Bootstraps (of %d):', g.naccu);
            else         myfprintf(verb,'Permutations (of %d):', g.naccu);
            end;
        end;
    end;

    tail = g.tail;
    if isempty(g.surrog)
        if size(data,1) == 1, % only one row

            if size(data,2) == 2 && strcmpi(g.forceanova, 'off')

                % paired t-test (very fast)
                % -------------
                [ori_vals df] = ttest_cell_select(data, g.paired, g.variance);

                if strcmpi(g.method, 'param')

                    % Check if exist tcd.m file from the Statistics Toolbox (Bug 1352 )
                    if exist('tcdf','file') == 2  & license('test', 'Statistics_Toolbox')
                        pvals = 2*tcdf(-abs(ori_vals), df);
                    else
                        pvals = 2*mytcdf(-abs(ori_vals), df);
                    end

                    pvals = reshape(pvals, size(pvals));
                else
                    if strcmpi(g.arraycomp, 'on')
                        try
                            myfprintf(verb,'...');
                            res = surrogdistrib( data, 'method', g.method, 'pairing', g.paired, 'naccu', g.naccu);
                            surrogval = ttest_cell_select( res, g.paired, g.variance);
                        catch,
                           lasterr
                           myfprintf(verb,'\nSuperfast array computation failed because of memory limitation, reverting to standard computation');
                           g.arraycomp = 'off';
                        end;
                    end;
                    if strcmpi(g.arraycomp, 'off')
                        [res precomp] = surrogdistrib( data, 'method', g.method, 'pairing', g.paired);
                        for index = 1:g.naccu
                            res = surrogdistrib( {}, 'precomp', precomp);
                            if mod(index, 10) == 0, myfprintf(verb,'%d ', index); end;
                            if mod(index, 100) == 0, myfprintf(verb,'\n'); end;
                            if myndims(res{1}) == 1
                                 surrogval(index)     = ttest_cell_select(res, g.paired, g.variance);
                            else surrogval(:,index)   = ttest_cell_select(res, g.paired, g.variance);
                            end;
                        end;
                    end;
                end;
            else
                % one-way ANOVA (paired) this is equivalent to unpaired t-test
                % -------------
                tail = 'one';
                [ori_vals df] = anova1_cell_select( data, g.paired );
                if strcmpi(g.method, 'param')
                    pvals = 1-fcdf(ori_vals, df(1), df(2));
                else
                    if strcmpi(g.arraycomp, 'on')
                        try
                            myfprintf(verb,'...');
                            res = surrogdistrib( data, 'method', g.method, 'pairing', g.paired, 'naccu', g.naccu);
                            surrogval = anova1_cell_select( res, g.paired );
                        catch,
                            myfprintf(verb,'\nSuperfast array computation failed because of memory limitation, reverting to standard computing');
                            g.arraycomp = 'off';
                        end;
                    end;
                    if strcmpi(g.arraycomp, 'off')
                        [res precomp] = surrogdistrib( data, 'method', g.method, 'pairing', g.paired);
                        for index = 1:g.naccu
                            if mod(index, 10) == 0, myfprintf(verb,'%d ', index); end;
                            if mod(index, 100) == 0, myfprintf(verb,'\n'); end;

                            res = surrogdistrib( {}, 'precomp', precomp);
                            if myndims(data{1}) == 1
                            	 surrogval(index)     = anova1_cell_select( res, g.paired );
                            else surrogval(:,index)   = anova1_cell_select( res, g.paired );
                            end;
                        end;
                    end;
                end;
            end;
        else
            % two-way ANOVA (paired or unpaired)
            % ----------------------------------
            tail = 'one';
            [ ori_vals{1} ori_vals{2} ori_vals{3} df{1} df{2} df{3} ] = anova2_cell_select( data, g.paired );
            if strcmpi(g.method, 'param')
                pvals{1} = 1-fcdf(ori_vals{1}, df{1}(1), df{1}(2));
                pvals{2} = 1-fcdf(ori_vals{2}, df{2}(1), df{2}(2));
                pvals{3} = 1-fcdf(ori_vals{3}, df{3}(1), df{3}(2));
            else
                surrogval = { surrogval surrogval surrogval };
                dataori   = data;
                if strcmpi(g.arraycomp, 'on')
                    try
                        myfprintf(verb,'...');
                        res = surrogdistrib( data, 'method', g.method, 'pairing', g.paired, 'naccu', g.naccu);
                        [ surrogval{1} surrogval{2} surrogval{3} ] = anova2_cell_select( res, g.paired );
                    catch,
                        myfprintf(verb,'\nSuperfast array computation failed because of memory limitation, reverting to standard computing');
                        g.arraycomp = 'off';
                    end;
                end;
                if strcmpi(g.arraycomp, 'off')
                    [res precomp] = surrogdistrib( data, 'method', g.method, 'pairing', g.paired);
                    for index = 1:g.naccu
                        if mod(index, 10) == 0, myfprintf(verb,'%d ', index); end;
                        if mod(index, 100) == 0, myfprintf(verb,'\n'); end;

                        res = surrogdistrib( {}, 'precomp', precomp);
                        if myndims(data{1}) == 1
                         	 [ surrogval{1}(index)     surrogval{2}(index)     surrogval{3}(index)     ] = anova2_cell_select( res, g.paired );
                        else [ surrogval{1}(:,index)   surrogval{2}(:,index)   surrogval{3}(:,index)   ] = anova2_cell_select( res, g.paired );
                        end;
                    end;
                end;
            end;
        end;
        myfprintf(verb,'\n');
    else
        surrogval = g.surrog;
        ori_vals  = g.stats;
        df        = [];
    end;

    % compute p-values
    % ----------------
    if ~strcmpi(g.method, 'param')
        if iscell( surrogval )
            pvals{1} = stat_surrogate_pvals(surrogval{1}, ori_vals{1}, tail);
            pvals{2} = stat_surrogate_pvals(surrogval{2}, ori_vals{2}, tail);
            pvals{3} = stat_surrogate_pvals(surrogval{3}, ori_vals{3}, tail);
        else
            pvals = stat_surrogate_pvals(surrogval, ori_vals, tail);
        end;
        try, warning('on', 'MATLAB:divideByZero'); catch, end;
    end;

    [ ori_vals, pvals ] = reshape_results( nd, ori_vals, pvals);
    [ surrogval ]       = reshape_results( [nd g.naccu], surrogval);

    % confidence intervals
    % --------------------
    if ~isnan(g.alpha)
        outputstruct.ci = stat_surrogate_ci(surrogval, g.alpha, tail);
        if strcmpi(g.structoutput, 'off')
            disp('Warning: returning confidence interval requires an output structure');
        end;
        if iscell(pvals)
            for ind = 1:length(pvals)
                outputstruct.mask{ind} = pvals{ind} < g.alpha;
            end;
        else
            outputstruct.mask = pvals < g.alpha;
        end;
    end;

    % create a structure for outputing values
    % ---------------------------------------
    if strcmpi(g.structoutput, 'on')
        outputstruct.method = g.method;
        outputstruct.pval   = pvals;
        outputstruct.df     = df;
        outputstruct.surrog = surrogval;
        if length(data(:)) == 2
             outputstruct.t = ori_vals;
        else outputstruct.f = ori_vals;
        end;
        outputstruct.stat   = ori_vals;
        ori_vals = outputstruct;
    end;

% compute ANOVA 2-way
% -------------------
function [f1 f2 f3 df1 df2 df3] = anova2_cell_select( res, paired);
    if strcmpi(paired,'on')
        [f1 f2 f3 df1 df2 df3] = anova2rm_cell( res );
    else
        [f1 f2 f3 df1 df2 df3] = anova2_cell( res );
    end;

% compute ANOVA 1-way
% -------------------
function [f df] = anova1_cell_select( res, paired);
    if strcmpi(paired,'on')
        [f df] = anova1rm_cell( res );
    else
        [f df] = anova1_cell( res );
    end;

% compute t-test
% -------------------
function [t df] = ttest_cell_select( res, paired, homogenous);
    if strcmpi(paired,'on')
        [t df] = ttest_cell( res{1}, res{2});
    else
        [t df] = ttest2_cell( res{1}, res{2}, homogenous);
    end;

% function to compute the number of dimensions
% --------------------------------------------
function val = myndims(a)
    if ndims(a) > 2
        val = ndims(a);
    else
        if size(a,1) == 1,
            val = 2;
        elseif size(a,2) == 1,
            val = 1;
        else
            val = 2;
        end;
    end;

% function for verbose messages
% -----------------------------
function myfprintf(verb, varargin)
    if verb
        fprintf(varargin{:});
    end;

% function to replace tcdf
% ------------------------
function p = mytcdf(x,v)

if length(v) == 1,
    v = repmat(v, size(x));
end;

x2 = x.^2;
inds1 = (v < x2);
inds2 = (v >= x2);
if any(inds1(:)), p(inds1) = betainc(v(inds1) ./ (v(inds1) + x2(inds1)), v(inds1)/2, 0.5, 'lower') / 2; end;
if any(inds2(:)), p(inds2) = betainc(x2(inds2) ./ (v(inds2) + x2(inds2)), 0.5, v(inds2)/2, 'upper') / 2; end;
inds = (x > 0);
if any(inds)
    p(inds) = 1 - p(inds);
end;

inds = (v > 1e7);
if any(inds(:)), p(inds) = normcum(x(inds)); end;

p(x == 0) = 0.5;
if isempty(p)
    p = ones(size(x));
else
    p = reshape(p, size(x));
end;
function [p] = normcum(z)
p = 0.5 * erfc(-z ./ sqrt(2));

% reshape results
% ---------------
function varargout = reshape_results(nd, varargin)
    if length(varargin) > 1
        for index = 1:length(varargin)
            varargout{index} = reshape_results(nd, varargin{index});
        end;
    elseif iscell(varargin{1})
        for index = 1:length(varargin{1})
            varargout{1}{index} = reshape_results(nd, varargin{1}{index});
        end;
    else
        if ~isempty(varargin{1})
            if length(nd) == 1, nd = [ nd 1 ]; end;
            varargout{1} = reshape(varargin{1}, nd);
        else varargout{1} = [];
        end;
    end;


function [q,Q_la,Q_lb] = lines2Epoint(La,Lb)

% LINES2EPOINT Intersection point of 2 Plucker lines. Result in Euclidean.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

na = La(1:3);
nb = Lb(1:3);
vb = Lb(4:6);

if nargout == 1

    q = cross(na,nb)/dot(na,vb);

else % jac

    % work in homogeneous for easiear Jacobians

    [pe,PE_na,PE_nb] = crossJ(na,nb);
    [ph,PH_na,PH_vb] = dotJ(na,vb);

    p = [pe;ph];
    Z33 = zeros(3);
    Z13 = zeros(1,3);
    P_la = [PE_na Z33;PH_na Z13];
    P_lb = [PE_nb Z33;Z13 PH_vb];

    [q,Q_p] = hm2eu(p);

    Q_la = Q_p*P_la;

    Q_lb = Q_p*P_lb;

end

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function [Q2,Pk2]=EkfFilter(Q1,ImuData,t,Vm,Pk1)
%EKF filter to Gyro atitude with Accelerate & Magnetic
% derivation <A Double-Stage Kalman Filter for Orientation Tracking
%            with an Integrated Processor in 9-D IMU>
% author  Zhang Xin

if isempty(Pk1)
   Pk1=[ 0.1 , 0.01 , 0.01 , 0.01;...
           0.01 , 0.1 , 0.01 , 0.01;...
           0.01 , 0.01 , 0.1 , 0.01;...
           0.01 , 0.01 , 0.01 , 0.1  ]*0.001;

end
para1=0.0001;%0.0001;
para2=0.1;
para3=0.5;
Qk=para1*eye(4);
Rk1=para2*eye(3);
Rk2=para3*eye(3);

wx=ImuData(1,5)*t;
wy=ImuData(1,6)*t;
wz=ImuData(1,7)*t;

norm_a=norm(ImuData(1,2:4));
norm_g=norm(ImuData(1,5:7));

Ak=[ 1    , -wx/2 , -wy/2 , -wz/2 ;...
     wx/2 ,   1   ,  wz/2 , -wy/2 ;...
     wy/2 , -wz/2 ,   1   ,  wx/2 ;...
     wz/2 ,  wy/2 , -wx/2 ,   1   ];

Qp=Ak*Q1';     %Q_predict

Qp=Qp/norm(Qp);

P_k=Ak*Pk1*Ak'+Qk;

if abs(norm_a-9.8)<2 && norm_g< 2
    %R = quatern2rotMat(Qp);
    R2=[2*Qp(2)*Qp(3)+2*Qp(1)*Qp(4);...
        2*Qp(1)^2+2*Qp(3)^2-1;...
        2*Qp(3)*Qp(4)-2*Qp(1)*Qp(2)];

    R3=[2*Qp(2)*Qp(4)-2*Qp(1)*Qp(3);...
        2*Qp(3)*Qp(4)+2*Qp(1)*Qp(2);...
        2*Qp(1)^2+2*Qp(4)^2-1 ];

    J2=2*[ Qp(4), Qp(3), Qp(2) , Qp(1);...
           Qp(1),-Qp(2), Qp(3) ,-Qp(4);...
          -Qp(2),-Qp(1), Qp(4) , Qp(3)] ;

    J3=2*[-Qp(3), Qp(4),-Qp(1) , Qp(2);...
           Qp(2), Qp(1), Qp(4) , Qp(3);...
           Qp(1),-Qp(2),-Qp(3) , Qp(4)] ;

    h1=R3;                               %acc in sensor fixed frame
    h2=Vm(2)*R2+Vm(3)*R3;          %mag in sensor fixed frame

    Hk1=J3;
    Hk2=Vm(2)*J2+Vm(3)*J3;

    Kk1=P_k*Hk1'*inv(Hk1*P_k*Hk1'+Rk1);

    qe1=Kk1*(ImuData(1,2:4)'/norm_a-h1);         % Acc difference value between real & prediction from attitude

    Pk_1=(eye(4)-Kk1*Hk1)*P_k;

    Kk2=P_k*Hk2'*inv(Hk2*P_k*Hk2'+Rk2);

    mag=ImuData(1,8:10);
    mag=mag/norm(mag);

    qe2=Kk2*(mag'-h2);       % mag difference value bwtween real & prediction from attitude
    Pk2=(eye(4)-Kk2*Hk2)*Pk_1;

else
    qe1=[0;0;0;0];
    Pk_1=P_k;
    qe2=[0;0;0;0];
    Pk2=Pk_1;

end

% Kk2=P_k*Hk2'*inv(Hk2*P_k*Hk2'+Rk2);
%
% mag=ImuData(1,8:10);
% mag=mag/norm(mag);
%
% qe2=Kk2*(mag'-h2);       % mag difference value bwtween real & prediction from attitude

qt=Qp+qe1+qe2;

Q2=qt'/norm(qt);

if Q2(1)<0
    Q2=-Q2;
end

%Pk2=(eye(4)-Kk2*Hk2)*Pk_1;

end


function [PSCI,xSCI]=covarianceIntersectSCI(CovHyp,covType,xHyp,u,numSamp)
%%COVARIANCEINTERSECTSCI Perform sampling covariance intersection. This is
%                   a method of fusing the first two moments of estimates
%                   when the correlation between the estimates is unknown.
%                   Given the z(inverse) covariance matrices of the
%                   estimates, this function returns a covariance matrix.
%                   Alternatively, if the estimates themselves are given,
%                   this function can also return the merged estimate.
%                   Unlike covarianceIntersect, this function has a random
%                   component and does not specifically optimize any
%                   particular function. Compare this function to
%                   ellipsoidIntersect.
%
%INPUTS: CovHyp The xDimXxDimXN covariance matrices or inverse covariance
%           matrices of the values to be merged.
%   CovType An optional input specifying whether covariance matrices or
%           inverse covariance matrices are in CovHyp. Possible values are:
%           0 (The default if omitted or an empty matrix is passed) CovHyp
%             contains covariance matrices.
%           1 CovHyp contains inverse covariance matrices.
%      xHyp The optional xDimXN set of vectors to merge. These are only
%           needed if xSCI is requested on the output.
%         u A parameter between 0 and 1 that affects the performance of the
%           algorithm. The default if omitted or an empty matrix is passed
%           is 0.5.
%   numSamp The algorithm is stochastic. This optional input is the number
%           of samples to use. The default if this parameter is omitted or
%           an empty matrix is passed is max(100*(N-1),1);
%
%OUTPUTS: PSCI The xDimXxDim fused covariance matrix.
%         xSCI The merged estimate. This requires xHyp to be given on
%              the input. The covariance matrix associated with the
%              merged estimate is PSCI.
%
%This function implements the sampling covariance intersection algorithm of
%[1]. If only a single estimate is passed, then it is just returned. This
%algorithm was developed to fuse tracks from multiple sensors in a
%networked tracking environment.
%
%EXAMPLE:
% This is similar to the example given in the paper for when standard
% covariance intersection might be bad. , except the means are
% not identical.
% x=zeros(2,2);
% P=zeros(2,2,2);
% x(:,1)=[1;-2];
% P(:,:,1)=[1,0;0,100];
% x(:,2)=[-2;-1];
% P(:,:,2)=[100,0;0,1];
%
% [PM,xM]=covarianceIntersectSCI(P,[],x);
% figure()
% clf
% hold on
% drawEllipse(x(:,1),inv(P(:,:,1)),[],'--r')
% drawEllipse(x(:,2),inv(P(:,:,2)),[],'--g')
% drawEllipse(xM,inv(PM),[],'-b')
%
%REFERENCES:
%[1] X. Tian, Y. Bar-Shalom, and G. Chen, "A no-loss covariance
%    intersection algorithm for track-to-track fusion," in Proceedings of
%    SPIE: Signal and Data Processing of Small targets, vol. 7698,
%    Orlando, FL, 5 Apr. 2010.
%
%August 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

numDim=size(CovHyp,1);
numHyp=size(CovHyp,3);

if(nargin<5||isempty(numSamp))
    numSamp=max(100*(numHyp-1),1);
end

if(nargin<4||isempty(u))
    u=0.5;
end

if(nargin<2||isempty(covType))
   covType=0;
end

if(covType==0)%If covariance matrices are passed.
    PInvHyp=zeros(numDim,numDim,numHyp);

    for curHyp=1:numHyp
        PInvHyp(:,:,curHyp)=inv(CovHyp(:,:,curHyp));
    end
else%If inverse covariance matrices are passed.
    PInvHyp=CovHyp;
end

if(numHyp>0)
    P0=zeros(numDim,numDim);
    for curHyp=1:numHyp
        P0=P0+PInvHyp(:,:,curHyp);
    end
    P0Inv=P0;
    P0=inv(P0);

    S0=chol(P0,'lower');

    x=S0*randn(numDim,numSamp);

    maxVals=zeros(numSamp,1);
    for curSamp=1:numSamp
        xCur=x(:,curSamp);

        maxVal=-1;
        for curHyp=1:numHyp
            val=xCur'*PInvHyp(:,:,curHyp)*xCur;
            maxVal=max(val,maxVal);
        end
        maxVals(curSamp)=maxVal;
    end

    rMax=-1;
    rMin=Inf;
    for curSamp=1:numSamp
        xCur=x(:,curSamp);
        curRat=xCur'*P0Inv*xCur/maxVals(curSamp);
        rMax=max(curRat,rMax);
        rMin=min(curRat,rMin);
    end

    PSCI=P0./(u*rMin+(1-u)*rMax);
else%Given 1 or 0, don't change it.
    if(numHyp==0)
        xSCI=[];
        PSCI=[];
        return;
    end
    return
end

if(nargin>2&&~isempty(xHyp))
    xSCI=zeros(numDim,1);

    for curHyp=1:numHyp
        xSCI=xSCI+PInvHyp(:,:,curHyp)*xHyp(:,curHyp);
    end
    xSCI=P0*xSCI;
else
    xSCI=[];
end

end

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


function result=edge_association(A,B,F)
% Author:  Qu Xiao-Bo    <quxiaobo [at] xmu.edu.cn>  and Hu Chang-wei Hu , June 26, 2009
%          Postal address:
% Rom 509, Scientific Research Building # 2,Haiyun Campus, Xiamen University,Xiamen,Fujian, P. R. China, 361005
% Website: http://quxiaobo.go.8866.org

%reference paper:Objective image fusion performance measure
A=double(A);
B=double(B);
F=double(F);
[row,column]=size(A);
[gA,aA]=get_g_a(A);
[gB,aB]=get_g_a(B);
[gF,aF]=get_g_a(F);
GAF=get2G(gA,gF);
GBF=get2G(gB,gF);
AAF=get2A(aA,aF);
ABF=get2A(aB,aF);
QgAF=getQg(GAF,0.9994,-15,0.5);
QgBF=getQg(GBF,0.9994,-15,0.5);
QaAF=getQa(AAF,0.9879,-22,0.8);
QaBF=getQa(ABF,0.9879,-22,0.8);
QAF=getQ(QgAF,QaAF);
QBF=getQ(QgBF,QaBF);
a=sum(sum(QAF.*gA+QBF.*gB));
b=sum(sum(gA+gB));
result=a/(b+eps);
%%
function [g,a]=get_g_a(im)

s1=[1 2 1; 0 0 0; -1 -2 -1];
s2=[-1 0 1;-2 0 2;-1 0 1];
sx=conv2(im,s1,'same');
sy=conv2(im,s2,'same');
g=sqrt(sx.^2+sy.^2);
a=atan(sy./(sx+eps));

end
%%
function Q=getQ(Qg,Qa)
Q=Qg.*Qa;
end
%%
function Qa=getQa(A,Ta,Ka,Oa)
Qa=Ta./(1+exp(Ka.*(A-Oa)));
end
%%
function Qg=getQg(G,Tg,Kg,Og)
Qg=Tg./(1+exp(Kg.*(G-Og)));
end
%%
function A=get2A(aIm1,aIm2)
A=1-abs(((aIm1-aIm2).*2)./pi);
end
%%
function G=get2G(gIm1,gIm2)
G=min(gIm1,gIm2)./(max(gIm1,gIm2)+eps);
end
%%
end


function mb_iGMM1D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 2011 by Mark Bangert markbangert@gmail.com
% inference with an infinite Gaussian mixture model
% http://www.gatsby.ucl.ac.uk/~edward/pub/inf.mix.nips.99.pdf

% this routine may be called without input data.
% a test data set is produced from a mixture of Gaussians

numOfIterations = 1000;
numOfBurnInIter =  300;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% produce data
numOfDataPoints = 1000; % number of data points

% true values of the generating parameters
true_numOfClasses = 10;
true_alpha        = 0.5;
true_lambda       = 180;
true_r            = 0.008;
true_beta         = 3;
true_w            = 100;

% generate weights of the clusters, their means and their variances
true_mus          = randn(1,true_numOfClasses)/true_r + true_lambda;
true_precs        = gamrnd(true_beta,1/true_w,1,true_numOfClasses);
tmp               = gamrnd(true_alpha,1,1,true_numOfClasses);
true_weights      = tmp/sum(tmp); clear tmp;

% generate samples
missingData       = mb_mvrnd(true_weights,numOfDataPoints);
data              = randn(1,numOfDataPoints)./(true_precs(missingData)).^.5 + true_mus(missingData);

        % visualize data and ground truth
        scrsz = get(0,'ScreenSize');

        clusterFigHandle = figure('Position',[1 scrsz(2) scrsz(3)/2 scrsz(4)],'Name','Clustering');

        colorMx = colormap(jet);
        colorMx = colorMx(randperm(64),:);

        hold on
        [counts,bins] = hist(data,floor(numOfDataPoints/10));
        counts = counts/(sum(counts)*(bins(2)-bins(1))); % normalization of histograms
        bar(bins,counts,'EdgeColor','none');
        for i = 1:true_numOfClasses
            y = (true_precs(i)/2/pi)^.5 * exp(-(bins-true_mus(i)).^2*true_precs(i)/2); % normalized gaussian
            plot(bins,y*true_weights(i),'k--','LineWidth',2); % normalize according to weights
        end
        currIgmmVisHandle = [];
        currSolution = 0*y;
        currSolutionVisHandle = [];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% start inference

tic

dataMean           = mean(data);
dataVar            = var(data);
numOfPointsInClass = numOfDataPoints;

% start the markov chain with one class
alphaProbDensity   = @(x)x^(-3/2)*exp(-1/2/x); % eq 14
alpha              = mb_sliceSampling(alphaProbDensity,rand,1);
lam                = randn*dataVar^.5 + dataMean; % eq 3
r                  = gamrnd(1/2,2/dataVar,1,1); % eq 3
betaProbDensity    = @(x)x^(-3/2)*exp(-1/2/x); % eq 7
beta               = mb_sliceSampling(betaProbDensity,rand,1);
w                  = gamrnd(1/2,2*dataVar,1,1); % eq 7
mus                = randn/r^.5 +lam; % eq 2
precs              = gamrnd(beta/2,2/w/beta,1,1); % eq 6
cs                 = ones(1,numOfDataPoints);
numOfClasses       = 1;

% perform a large number of MCMC sampling steps
iter = 0;
while iter < numOfIterations

    iter = iter + 1;

            % visualization
            figure(clusterFigHandle);
            delete(currIgmmVisHandle);
            currIgmmVisHandle = [];

            for i = 1:numOfClasses

                currIgmmVisHandle = [currIgmmVisHandle plot(bins,(precs(i)/2/pi)^.5*numOfPointsInClass(i)/numOfDataPoints* ...
                    exp(-(bins-mus(i)).^2*precs(i)/2),'Color',colorMx(mod(i-1,size(colorMx,1))+1,:),'LineWidth',2)];

            end

    % collapsed gibbs sampling
    for i = 1:numOfClasses

        numOfDataPointsInCurrClass = numOfPointsInClass(i);
        dataOfCurrClass            = data(cs==i);
        meanOfCurrClass            = mean(dataOfCurrClass);
        precOfCurrClass            = precs(i);

        % for mu - eq 4
        newLoc   = (meanOfCurrClass*numOfDataPointsInCurrClass*precOfCurrClass + lam*r) / (numOfDataPointsInCurrClass*precOfCurrClass + r);
        newScale = 1/(numOfDataPointsInCurrClass*precOfCurrClass + r);
        mus(i)   = randn*newScale^.5 + newLoc;

        % for s - eq 8
        newLoc   = (beta + numOfDataPointsInCurrClass)/2;
        newScale = 2/(w*beta + sum((dataOfCurrClass-mus(i)).^2));
        precs(i) = gamrnd(newLoc,newScale,1,1);

    end

    % for lambda - eq 5
    newLoc   = (dataMean/dataVar + r*sum(mus)) / (1/dataVar + numOfClasses*r);
    newScale = 1 / (1/dataVar + numOfClasses*r);
    lam      = randn*newScale^.5 + newLoc;

    % for r - eq 5
    newLoc   = (1 + numOfClasses)/2;
    newScale = 2/(dataVar + sum((mus-lam).^2));
    r        = gamrnd(newLoc,newScale,1,1);

    % for w - eq 9
    newLoc   = (numOfClasses*beta + 1)/2;
    newScale = 2/(1/dataVar + beta*sum(precs));
    w        = gamrnd(newLoc,newScale,1,1);

    % for beta - eq 9 - stay in log space for a long time
    betaProbDensity = @(x)exp(-numOfClasses*gammaln(x/2) - 1/2/x - .5*(numOfClasses*x-3)*log(x/2) + sum( (x/2)*log(precs*w) - x*w*precs/2 ));
    beta = mb_sliceSampling(betaProbDensity,beta,1);

    % for alpha - eq 15 - stay in logspace for a long time (gammaln(numOfDataPoints) for numerical stability)
    alphaProbDensity = @(x)exp( (numOfClasses-3/2)*log(x) - .5/x + gammaln(x) - gammaln(x+numOfDataPoints) + gammaln(numOfDataPoints));
    alpha = mb_sliceSampling(alphaProbDensity,alpha,1);

    % assign classes to data with precomputed randomnumbers (faster)
    normRandVar = randn(1,numOfDataPoints)/r^.5 + lam;
    gamRandVar  = gamrnd(beta/2,2/w/beta,1,numOfDataPoints);
    randVar     = rand(1,numOfDataPoints);

    for i = 1:numOfDataPoints

        prob = NaN*zeros(1,numOfClasses+1);

        % calculate probability for membership in existing classes
        for j = 1:numOfClasses

            if cs(i) == j; % if data point member of this class
                nij               = numOfPointsInClass(j) - 1;
            else
                nij               = numOfPointsInClass(j);
            end

            if nij > 0
                prob(j) = nij/(numOfDataPoints-1+alpha)*(precs(j))^.5 * exp(-precs(j)/2 * (data(i)-mus(j))^2);
            else % if data point the only member in this class
                prob(j) = alpha/(numOfDataPoints-1+alpha)*(precs(j))^.5 * exp(-precs(j)/2 * (data(i)-mus(j))^2);
            end

        end

        % probability for membership in new class - eq 17
        % sample paramters for this class from priors
        mu_new    = normRandVar(i);
        s_new     = gamRandVar(i);
        prob(end) = alpha/(numOfDataPoints-1+alpha)*s_new^.5*exp(-s_new/2 * (data(i)-mu_new)^2);

        cdf       = cumsum(prob);
        rndNum    = randVar(i)*cdf(end);

        % assign data point to class according to probability
        for j = 1:numOfClasses + 1
           if cdf(j) >= rndNum % bingo this data point will be assigned to class j
               numOfPointsInClass(cs(i)) = numOfPointsInClass(cs(i)) - 1;
               if numOfPointsInClass(cs(i)) < 1
                    fprintf(['Removing class # ' num2str(cs(i)) '\n']);
                    % remove parameters
                    mus(cs(i))                = [];
                    precs(cs(i))              = [];
                    numOfPointsInClass(cs(i)) = [];
                    numOfClasses              = numOfClasses - 1;
                    j(j>cs(i))                = j - 1;
                    % rename all higher classes
                    cs(cs>cs(i))              = cs(cs>cs(i)) - 1;
               end
               cs(i) = j;
               break;
           end
        end

        if j == numOfClasses + 1
            fprintf(['Adding new class # ' num2str(j) '\n']);
            % add parameters
            mus                   = [mus mu_new];
            precs                 = [precs s_new];
            numOfPointsInClass(j) = 1;
            numOfClasses          = numOfClasses + 1;
        else
            numOfPointsInClass(j) = numOfPointsInClass(j) + 1;
        end

    end

            % visualization of final solution - start after burn in
            if iter >= numOfBurnInIter
                delete(currSolutionVisHandle);
                contributionOfCurrentSample2Solution = 0*bins;
                for i = 1:numel(mus)

                    contributionOfCurrentSample2Solution = contributionOfCurrentSample2Solution + ...
                        (precs(i)/2/pi)^.5*numOfPointsInClass(i)/numOfDataPoints*exp(-(bins-mus(i)).^2*precs(i)/2);

                end

                currSolution = (iter-numOfBurnInIter)/(iter-numOfBurnInIter+1)*currSolution + ...
                    1/(iter-numOfBurnInIter+1)*contributionOfCurrentSample2Solution;

                currSolutionVisHandle = plot(bins,currSolution,'c','LineWidth',4);
            end
            drawnow;

end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% helper functions

function rndVar = mb_mvrnd(p,n)

r      = rand(1,n);
p      = cumsum(p);
if p(end)~=1
    p = p/p(end);
end

rndVar = ones(1,n);
for i = 1:n
    rndVar(i) = find(r(i)<=p,1,'first');
end

end

function randNumbers = mb_sliceSampling(customFunc,xStart,numOfRandNumbers)
% slice sampling implementation according to section 29.7 from
% http://www.inference.phy.cam.ac.uk/itprnn/book.pdf

tic

if nargin < 2
    xStart = rand;
end
if nargin < 3
    numOfRandNumbers = 1;
end

randNumbers = NaN*ones(1,numOfRandNumbers);

for i = 1:numOfRandNumbers

    yStart     = customFunc(xStart);

    randStart  = rand*yStart;

    randStep   = rand;
    stepLength = .1;

    xLeft      = xStart - randStep*stepLength;
    xRight     = xStart + (1-randStep)*stepLength;

    counter = 0;
    while customFunc(xLeft) > randStart
        xLeft = xLeft - stepLength*2^counter;
        counter = counter + 1;
    end
    counter = 0;
    while customFunc(xRight) > randStart
        xRight = xRight + stepLength*2^counter;
        counter = counter + 1;
    end

    while 1
        randNumber = rand*(xRight-xLeft) + xLeft;
        randNumberFuncVal = customFunc(randNumber);
        if randNumberFuncVal > randStart && imag(randNumberFuncVal) == 0
            break;
        else
            if randNumber > xStart
                xRight = randNumber;
            else
                xLeft = randNumber;
            end
        end

        elTime = toc;
        if elTime > 10
            randNumbers = NaN;
            return;
        end

    end

    randNumbers(i) = randNumber;
    xStart = randNumber;

end

end


function [ z, rnorm ] = cg_it ( A, x )

%*****************************************************************************80
%
%% CG_IT carries out the conjugate gradient iteration.
%
%  Modified:
%
%    08 March 2010
%
%  Author:
%
%    Gaurav Sharma, Jos Martin
%
%  Reference:
%
%    Gaurav Sharma, Jos Martin,
%    MATLAB: A Language for Parallel Computing,
%    International Journal of Parallel Programming,
%    Volume 37, Number 1, pages 3-36, February 2009.
%
%  Parameters:
%
%    Input, real A(N,N), the matrix.
%
%    Input, real X(N,1), the right hand side.
%
%    Output, real Z(N,1), the estimated solution.
%
%    Output, real RNORM, the norm of the residual ( x - A * z );
%
  z = zeros ( size ( x ) );
  r = x;
  rho = r' * r;
  p = r;

  for i = 1 : 15
    q = A * p;
    alpha = rho / ( p' * q );
    z = z + alpha * p;
    rho0 = rho;
    r = r - alpha * q;
    rho = r' * r;
    beta = rho / rho0;
    p = r + beta * p;
  end

  rnorm =  norm ( x - A * z );

  return
end


function [ top, btri, bedg, triangle_node, triangle_neighbor ] = swapec ( ...
  i, top, btri, bedg, node_num, node_xy, triangle_num, triangle_node, ...
  triangle_neighbor, work )

%*****************************************************************************80
%
%% SWAPEC swaps diagonal edges until all triangles are Delaunay.
%
%  Discussion:
%
%    The routine swaps diagonal edges in a 2D triangulation, based on
%    the empty circumcircle criterion, until all triangles are Delaunay,
%    given that I is the index of the new vertex added to the triangulation.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 February 2005
%
%  Author:
%
%    Original FORTRAN77 version by Barry Joe,
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Barry Joe,
%    GEOMPACK - a software package for the generation of meshes
%    using geometric algorithms,
%    Advances in Engineering Software,
%    Volume 13, pages 325-331, 1991.
%
%  Parameters:
%
%    Input, integer I, the index of the new vertex.
%
%    Input/output, integer TOP, the index of the top of the stack.
%    On output, TOP is zero.
%
%    Input/output, integer BTRI, BEDG; on input, if positive, are the
%    triangle and edge indices of a boundary edge whose updated indices
%    must be recorded.  On output, these may be updated because of swaps.
%
%    Input, integer NODE_NUM, the number of node.
%
%    Input, real NODE_XY(2,NODE_NUM), the coordinates of
%    the points.
%
%    Input, integer TRIANGLE_NUM, the number of triangles.
%
%    Input/output, integer TRIANGLE_NODE(3,TRIANGLE_NUM), the triangle incidence list.
%    May be updated on output because of swaps.
%
%    Input/output, integer TRIANGLE_NEIGHBOR(3,TRIANGLE_NUM), the triangle neighbor list;
%    negative values are used for links of the counter-clockwise linked
%    list of boundary edges;  May be updated on output because of swaps.
%
%      LINK = -(3*I + J-1) where I, J = triangle, edge index.
%
%    Workspace, integer WORK(MAXST); on input, entries 1 through TOP
%    contain the indices of initial triangles (involving vertex I)
%    put in stack; the edges opposite I should be in interior;  entries
%    TOP+1 through MAXST are used as a stack.
%

%
%  Determine whether triangles in stack are Delaunay, and swap
%  diagonal edge of convex quadrilateral if not.
%
  x = node_xy(1,i);
  y = node_xy(2,i);

  while ( 1 )

    if ( top <= 0 )
      break
    end

    t = work(top);
    top = top - 1;

    if ( triangle_node(1,t) == i )
      e = 2;
      b = triangle_node(3,t);
    elseif ( triangle_node(2,t) == i )
      e = 3;
      b = triangle_node(1,t);
    else
      e = 1;
      b = triangle_node(2,t);
    end

    a = triangle_node(e,t);
    u = triangle_neighbor(e,t);

    if ( triangle_neighbor(1,u) == t )
      f = 1;
      c = triangle_node(3,u);
    elseif ( triangle_neighbor(2,u) == t )
      f = 2;
      c = triangle_node(1,u);
    else
      f = 3;
      c = triangle_node(2,u);
    end

    swap = diaedg ( x, y, node_xy(1,a), node_xy(2,a), node_xy(1,c), node_xy(2,c), ...
      node_xy(1,b), node_xy(2,b) );

    if ( swap == 1 )

      em1 = i4_wrap ( e - 1, 1, 3 );
      ep1 = i4_wrap ( e + 1, 1, 3 );
      fm1 = i4_wrap ( f - 1, 1, 3 );
      fp1 = i4_wrap ( f + 1, 1, 3 );

      triangle_node(ep1,t) = c;
      triangle_node(fp1,u) = i;
      r = triangle_neighbor(ep1,t);
      s = triangle_neighbor(fp1,u);
      triangle_neighbor(ep1,t) = u;
      triangle_neighbor(fp1,u) = t;
      triangle_neighbor(e,t) = s;
      triangle_neighbor(f,u) = r;

      if ( 0 < triangle_neighbor(fm1,u) )
        top = top + 1;
        work(top) = u;
      end

      if ( 0 < s )

        if ( triangle_neighbor(1,s) == u )
          triangle_neighbor(1,s) = t;
        elseif ( triangle_neighbor(2,s) == u )
          triangle_neighbor(2,s) = t;
        else
          triangle_neighbor(3,s) = t;
        end

        top = top + 1;

        if ( node_num < top )
          fprintf ( 1, '\n' );
          fprintf ( 1, 'SWAPEC - Fatal error!\n' );
          fprintf ( 1, '  Exceeded stacksize.\n' );
          error ( 'SWAPEC - Fatal error!' );
        end

        work(top) = t;

      else

        if ( u == btri & fp1 == bedg )
          btri = t;
          bedg = e;
        end

        l = - ( 3 * t + e - 1 );
        tt = t;
        ee = em1;

        while ( 0 < triangle_neighbor(ee,tt) )

          tt = triangle_neighbor(ee,tt);

          if ( triangle_node(1,tt) == a )
            ee = 3;
          elseif ( triangle_node(2,tt) == a )
            ee = 1;
          else
            ee = 2;
          end

        end

        triangle_neighbor(ee,tt) = l;

      end

      if ( 0 < r )

        if ( triangle_neighbor(1,r) == t )
          triangle_neighbor(1,r) = u;
        elseif ( triangle_neighbor(2,r) == t )
          triangle_neighbor(2,r) = u;
        else
          triangle_neighbor(3,r) = u;
        end

      else

        if ( t == btri & ep1 == bedg )
          btri = u;
          bedg = f;
        end

        l = - ( 3 * u + f - 1 );
        tt = u;
        ee = fm1;

        while ( 0 < triangle_neighbor(ee,tt) )

          tt = triangle_neighbor(ee,tt);

          if ( triangle_node(1,tt) == b )
            ee = 3;
          elseif ( triangle_node(2,tt) == b )
            ee = 1;
          else
            ee = 2;
          end

        end

        triangle_neighbor(ee,tt) = l;

      end

    end

  end

  return
end


function [Pro,Res] = transferoperator(HB,NL,isFreeNode)
%% TRANSFEROPERATOR transfer operators between multilevel meshes
%
%  [Pro,Res] = transferoperator(HB,NL) construct matrix representation of
%  transfer operators between levels. All output are cell(level,1).
%
%  In the input, HB is the hierarchical structure of nodes. NL records the
%  starting and ending indices of nodes in each level. see HBstructure or
%  HBstructure3.
%
%  Prolongation (Pro) and restrction (Res) is constructed using standard
%  linear finite element basis and Res = Pro'.
%
%  See also mg, HBstructure, HBstructure3.
%
%  Reference: L. Chen. MultiGrid Methods.

if ~exist('isFreeNode','var'), isFreeNode = []; end
level = length(NL)-1;
Pro = cell(level,1);
Res = cell(level,1);
for j = level:-1:2
    % fine node and coarse node index
    fineNodeRange = NL(j)+1:NL(j+1);
    fineNode = HB(fineNodeRange,1);
    nFineNode = NL(j+1)-NL(j);
    coarseNode = (1:NL(j))';
    isCoarseNode = true(NL(j+1),1);
    isCoarseNode(fineNode) = false;
    coarseNodeFineIdx = find(isCoarseNode);
    ii = [coarseNodeFineIdx; fineNode; fineNode];
    jj = [coarseNode; HB(fineNodeRange,2); HB(fineNodeRange,3)];
    ss = [ones(NL(j),1); 0.5*ones(nFineNode,1); 0.5*ones(nFineNode,1)];
    % remove fix dof from index
    if ~isempty(isFreeNode)
        isFreeNodeC = isFreeNode;
        isFreeNodeC(fineNode) = [];
        Nfree = sum(isFreeNode);
        NfreeC = sum(isFreeNodeC);
        idx = isFreeNode(ii) & isFreeNodeC(jj);
        idxMap(isFreeNode) = (1:Nfree)';
        idxMapC(isFreeNodeC) = (1:NfreeC)';
        ii = idxMap(ii(idx));
        jj = idxMapC(jj(idx));
        ss = ss(idx);
        % generate prolongation matrix
        Pro{j-1} = sparse(ii,jj,ss,Nfree,NfreeC);
        isFreeNode = isFreeNodeC;
    else
        Pro{j-1} = sparse(ii,jj,ss,NL(j+1),NL(j));
    end
    Res{j} = Pro{j-1}';
end


function [Xi,ODEs] = sparsifyDynamics(Theta,dXdt,LHS_Sym,lambda,N,Sym_Struct,disp,NormalizeLib)
% Copyright 2015, All Rights Reserved
% Code by Steven L. Brunton
% For Paper, "Discovering Governing Equations from Data:
%        Sparse Identification of Nonlinear Dynamical Systems"
% by S. L. Brunton, J. L. Proctor, and J. N. Kutz

% compute Sparse regression: sequential least squares
%%
% Modified By: K
% Last Updated
%% Normalize the library data
if NormalizeLib==1
    % Change thisinto parfor to increase the speed if you have a large
    % library

    %parfor norm_k=1:size(Theta,2)

    % Else use the normal for loop
     for norm_k=1:size(Theta,2)
        normLib(norm_k) = norm(Theta(:,norm_k));
        Theta(:,norm_k) = Theta(:,norm_k)/normLib(norm_k);
    end
end

%% Peform sparse regression
Xi = Theta\dXdt;  % initial guess: Least-squares
[n,m]=size(dXdt);

% lambda is our sparsification knob.
for k=1:N
    smallinds = (abs(Xi)<lambda);   % find small coefficients
    Xi(smallinds)=0;                % and threshold
    for ind = 1:m                   % n is state dimension
        biginds = ~smallinds(:,ind);
        % Regress dynamics onto remaining terms to find sparse Xi
        Xi(biginds,ind) = Theta(:,biginds)\dXdt(:,ind);
    end
end

%% Now output the SINDy Identified ODEs

% Now retrive the parameters
if NormalizeLib==1
    for norm_k=1:length(Xi)
        Xi(norm_k,:) = Xi(norm_k,:)/normLib(norm_k);
    end
end

for i=1:m
     ODEs(i,1)=vpa(cell2sym(Sym_Struct)*Xi(:,i));
end

%% Choose whether you want to display the final discovered equation

if disp==1
     fprintf('The SINDy-PI discovered PDE is:\n')
     digits(6)
     for i=1:m
          fprintf(strcat('\t',char(LHS_Sym),'=',char(simplify(ODEs(i,1))),'\n'));
     end
end


function op = smooth_entropy()
%SMOOTH_ENTROPY The entropy function -sum( x_i log(x_i) )
op = @smooth_entropy_impl;

function [ v, g ] = smooth_entropy_impl( x )
if any( x < 0 ),
    v = -Inf;
    if nargout > 1,
        g = NaN * ones(size(x));
    end
else
    logx = log(max(x,realmin));
    v = - tfocs_dot( x, logx );
    if nargout > 1,
        g = - logx - 1;
    end
end

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


function [out] = evap_10(p1,S,Smax,Ep,dt)
%evap_10

% Copyright (C) 2019, 2021 Wouter J.M. Knoben, Luca Trotter
% This file is part of the Modular Assessment of Rainfall-Runoff Models
% Toolbox (MARRMoT).
% MARRMoT is a free software (GNU GPL v3) and distributed WITHOUT ANY
% WARRANTY. See <https://www.gnu.org/licenses/> for details.

% Flux function
% ------------------
% Description:  Evaporation from bare soil scaled by relative storage
% Constraints:  Ea <= Ep
%               Ea <= S/dt
% @(Inputs):    p1   - fraction of area that is bare soil [-]
%               S    - current storage [mm]
%               Smax - maximum storage [mm]
%               Ep   - potential evapotranspiration rate [mm/d]
%               dt   - time step size [d]

out = max(min(p1.*S./Smax.*Ep,S/dt),0);

end


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

function y = pcev(t,f,f0,sigma, beta)
%Probability Distribution from CEV dynamics
%   dS(t) = sigma S(t)^beta dW(t)
%   t = maturity
%   f0 = spot
%   f = forward
%   sigma = volatility
%   beta = CEV exponent

nu = sigma^2*t;
k = 2 ./(nu * (2-beta)^2);
x = k.*f0^(2-beta);
w = k.*f.^(2-beta);

q = 1 / ( 2 - beta);

wbess = (2-beta) .* (x .* w.^(1-2.*beta)).^(1/(4-2*beta)) ...
    .* exp(-x-w) .*k .^(1 ./(2-beta));

Z = 2 * sqrt(x.*w);

bess = besseli(q,Z);

y= wbess.* bess;

end


function [f, df, d2f] = nlp_costfcn(om, x)
%NLP_COSTFCN  Evaluates objective function, gradient and Hessian.
%   [F, DF, D2F] = NLP_COSTFCN(OM, X)
%
%   Objective function evaluation routine, suitable for use with MIPS,
%   FMINCON, etc. Computes objective function value, gradient and Hessian.
%
%   Inputs:
%     OM : Opt-Model object
%     X : optimization vector
%
%   Outputs:
%     F   : value of objective function
%     DF  : (optional) gradient of objective function (column vector)
%     D2F : (optional) Hessian of objective function (sparse matrix)
%
%   Examples:
%       f = nlp_costfcn(om, x);
%       [f, df] = nlp_costfcn(om, x);
%       [f, df, d2f] = nlp_costfcn(om, x);
%
%   See also NLP_CONSFCN, NLP_HESSFCN.

%   MP-Opt-Model
%   Copyright (c) 1996-2020, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MP-Opt-Model.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://github.com/MATPOWER/mp-opt-model for more info.

%%----- evaluate objective function -----
%% general nonlinear costs
if nargout == 3
    [f, df, d2f]    = om.eval_nln_cost(x);
    if om.qdc.NS
        [fq, dfq, d2fq] = om.eval_quad_cost(x);
        f = f + sum(fq);
        df = df + dfq;
        d2f = d2f + d2fq;
    end
elseif nargout == 2
    [f, df]   = om.eval_nln_cost(x);
    if om.qdc.NS
        [fq, dfq] = om.eval_quad_cost(x);
        f = f + sum(fq);
        df = df + dfq;
    end
else
    f  = om.eval_nln_cost(x);
    if om.qdc.NS
        fq = om.eval_quad_cost(x);
        f = f + sum(fq);
    end
end


function Lrho = construct_connection_Laplacian(measurements)
%function Lrho = construct_connection_Laplacian(measurements)
%
% This function computes and returns the connection Laplacian for the
% rotational observations (see eq. (15) of the paper).

D = length(measurements.t{1}); % D = dimension of SE(d)
N = max(max(measurements.edges));  % N = number of nodes in the pose graph
M = size(measurements.edges,1); % M = number of edges in the pose graph

% The number of nonzero elements in the connection Laplacian; there are
% 2*D^2*M nonzero elements corresponding to the off-diagonal blocks
% containing the raw measurements (one dxd rotation matrix above the main
% diagonal, and another below, for each of the M measurements, plus D*N
% nonzero elements along the main diagonal for the scaled identity
% matrices.  Note that in the case of multiple observations between two
% nodes x_i and x_j the corresponding (i,j) block in the connection
% Laplacian should contain the *sum* of the weighted observations
% kappa_{ij} R_{ij} between these nodes.  We achieve this result below by
% exploiting the fact that MATLAB's sparse() command forms the resulting
% sparse matrix by *summing* values with the same (i,j) indices.

% Copyright (C) 2016 by David M. Rosen

D2 = D^2;
off_diag_inc = 2*D2;

NNZ = off_diag_inc * M + D*N;

%Allocate storage for the row, column, and value vectors

rows = zeros(1, NNZ);
cols = zeros(1, NNZ);
vals = zeros(1, NNZ);

degs = zeros(1, N);  %Vector to store the degrees of the nodes

%Iterate over the measurements in the pose graph
for k = 1:M

    %EXTRACT MEASUREMENT DATA
    i = measurements.edges(k, 1);  %The node that this edge leaves
    j = measurements.edges(k, 2);  %The node that this edge enters

    Rij = measurements.R{k};  %The rotation matrix for this observation

    kappa = measurements.kappa{k};  %The precision for this rotational observation

    %PROCESS MEASUREMENT DATA

    %Increment the degrees of the ith and jth nodes by kappa
    degs(i) = degs(i) + kappa;
    degs(j) = degs(j) + kappa;

    %Set the (i,j)th DxD block of Lrho = -kappa*Rij
    [r, c, Rvect] = rcvize_matrix(Rij, i, j);
    rows(off_diag_inc*(k - 1) + 1 : off_diag_inc*(k-1) + D2) = r;
    cols(off_diag_inc*(k - 1) + 1 : off_diag_inc*(k-1) + D2) = c;
    vals(off_diag_inc*(k - 1) + 1 : off_diag_inc*(k-1) + D2) = -kappa*Rvect;

    %Set the (j,i)th DxD block of Lrho = -kappa*R_ij^T
    [r, c, Rvect] = rcvize_matrix(Rij', j, i);
    rows(off_diag_inc*(k - 1) + D2 + 1 : off_diag_inc*(k-1) + off_diag_inc) = r;
    cols(off_diag_inc*(k - 1) + D2 + 1 : off_diag_inc*(k-1) + off_diag_inc) = c;
    vals(off_diag_inc*(k - 1) + D2 + 1 : off_diag_inc*(k-1) + off_diag_inc) = -kappa*Rvect;
end

%Now set the diagonal elements to be D-fold copies of the degrees of each
%node

rows(off_diag_inc*M + 1 : NNZ) = [1:D*N];
cols(off_diag_inc*M + 1 : NNZ) = [1:D*N];
vals(off_diag_inc*M + 1 : NNZ) = kron(degs, ones(1,D));

Lrho = sparse(rows, cols, vals, D*N, D*N);

end


function components_test02 ( )

%*****************************************************************************80
%
%% COMPONENTS_TEST02 tests I4MAT_COMPONENTS on a simple case.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 March 2011
%
%  Author:
%
%    John Burkardt
%
  m = 9;
  n = 17;

  a = [ ...
    0, 0, 0, 0, 0, 0, 0, 0, 0; ...
    0, 0, 1, 0, 0, 1, 0, 0, 0; ...
    0, 1, 1, 0, 1, 1, 1, 0, 0; ...
    0, 1, 1, 1, 1, 1, 1, 0, 0; ...
    0, 0, 1, 1, 1, 0, 0, 0, 0; ...
    0, 0, 1, 1, 1, 0, 0, 0, 0; ...
    0, 1, 1, 1, 0, 1, 0, 1, 0; ...
    0, 1, 1, 0, 0, 1, 0, 1, 0; ...
    0, 0, 1, 0, 0, 0, 0, 1, 0; ...
    0, 0, 0, 0, 1, 0, 1, 1, 0; ...
    0, 1, 0, 1, 1, 0, 1, 0, 0; ...
    0, 1, 1, 1, 1, 1, 0, 0, 0; ...
    0, 0, 1, 1, 0, 1, 0, 1, 0; ...
    0, 0, 1, 1, 0, 1, 0, 1, 0; ...
    0, 1, 1, 0, 1, 0, 1, 1, 0; ...
    0, 1, 0, 0, 1, 0, 1, 1, 0; ...
    0, 0, 0, 0, 0, 0, 0, 0, 0 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'COMPONENTS_TEST02\n' );
  fprintf ( 1, '  I4MAT_COMPONENTS finds and labels connected\n' );
  fprintf ( 1, '  components in a 2D integer array.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  A:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : m
    fprintf ( 1, '    ' );
    for j = 1 : n
      fprintf ( 1, '%d', a(i,j) );
    end
    fprintf ( 1, '\n' );
  end

  [ component_num, c ] = i4mat_components ( m, n, a );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of components = %d\n', component_num );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  C:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : m
    fprintf ( 1, '    ' );
    for j = 1 : n
      fprintf ( 1, '%d', c(i,j) );
    end
    fprintf ( 1, '\n' );
  end

  return
end


function [newnode,newelem]=highordertet(node,elem,order)
%
% [newnode,newelem]=highordertet(node,elem)
%
% generate high-order straight-edge tetrahedral mesh from
% the 1st order tetrahedral mesh
%
% author: Qianqian Fang, <q.fang at neu.edu>
%
% input:
%    node: list of nodes
%    elem: list of elements (each row are indices of nodes of each element)
%    order: optional, the order of the generated mesh; if missing, order=2
%
% output:
%    newnode: all new edge-nodes on the output mesh
%    newelem: the indices of the edge nodes for each original tet element
%
%    currently, this function only supports order=2
%    to combine the newnode/newelem with the old mesh, one should use
%
%    elemfull=[elem(:,1:4) newelem+size(node,1)]; % 10-node element
%    nodefull=[node;newnode];
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

if(nargin<3)
    order=2;
end
if(order>=3 || order<=1)
    error('currently this function only supports order=2');
end
[edges,idx,newelem]=uniqedges(elem(:,1:4));
newnode=node(edges',1:3);
newnode=reshape(newnode',[3,2,size(edges,1)]);
newnode=squeeze(mean(permute(newnode,[3 2 1]),2));


function y = quadratic(x, sigma, type)
%QUADRATIC   Quadratic "robust" function.
%   QUADRATIC(X, SIGMA, TYPE) evaluates the Quadratic "robust" function
%   with sigma SIGMA at point(s) X.
%   TYPE selects the evaluation type:
%    - 0: function value
%    - 1: first derivative
%    - 2: second derivative
%
%   This is a private member function of the class 'robust_function'.
%
%   Author:  Stefan Roth, Department of Computer Science, TU Darmstadt
%   Contact: sroth@cs.tu-darmstadt.de
%   $Date:  $
%   $Revision: $

% Copyright 2004-2007, Brown University, Providence, RI. USA
% Copyright 2007-2010 TU Darmstadt, Darmstadt, Germany.
%
%                          All Rights Reserved
%
% All commercial use of this software, whether direct or indirect, is
% strictly prohibited including, without limitation, incorporation into in
% a commercial product, use in a commercial service, or production of other
% artifacts for commercial purposes.
%
% Permission to use, copy, modify, and distribute this software and its
% documentation for research purposes is hereby granted without fee,
% provided that the above copyright notice appears in all copies and that
% both that copyright notice and this permission notice appear in
% supporting documentation, and that the name of the author and Brown
% University not be used in advertising or publicity pertaining to
% distribution of the software without specific, written prior permission.
%
% For commercial uses contact the Technology Venture Office of Brown University
%
% THE AUTHOR AND BROWN UNIVERSITY DISCLAIM ALL WARRANTIES WITH REGARD TO
% THIS SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND
% FITNESS FOR ANY PARTICULAR PURPOSE.  IN NO EVENT SHALL THE AUTHOR OR
% BROWN UNIVERSITY BE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL
% DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR
% PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS
% ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF
% THIS SOFTWARE.

  switch (type)
   case 0
    y = x.^2 / sigma^2;
   case 1
    y = 2 * x / sigma^2;
   case 2
    y = repmat(2 / sigma^2, size(x));
  end


classdef CEC2013_F12 < PROBLEM
% <single> <real> <large>
% Shifted Rosenbrock's function

%------------------------------- Reference --------------------------------
% X. Li, K. Tang, M. N. Omidvar, Z. Yang, and K. Qin, Benchmark functions
% for the CEC'2013 special session and competition on large-scale global
% optimization, RMIT University, Australia, 2013.
%------------------------------- 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".
%--------------------------------------------------------------------------

    properties
        Xopt;	% Optimal decision vector
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            CallStack = dbstack('-completenames');
            load(fullfile(fileparts(CallStack(1).file),'CEC2013.mat'),'Data');
            obj.Xopt = Data{12};
            obj.M    = 1;
            obj.D    = 1000;
            obj.lower    = zeros(1,obj.D) - 100;
            obj.upper    = zeros(1,obj.D) + 100;
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            PopObj = Rosenbrock(PopDec-repmat(obj.Xopt,size(PopDec,1),1));
        end
    end
end

function F = Rosenbrock(X)
    F = sum(100*(X(:,1:end-1).^2-X(:,2:end)).^2+(X(:,1:end-1)-1).^2,2);
end


%% Execute this file to collect validation data for model identification

x0 = x(end,:);
tspanV =[tspan(end):dt:20];

options = odeset('RelTol',1e-10,'AbsTol',1e-10*ones(1,n));

switch InputSignalType
    case 'sine2'
        A = 2;
        forcing = @(x,t) [(A*(sin(1*t)+sin(.1*t))).^2];
        [t_valid,x_valid]=ode45(@(t,x) LorenzSys(t,x,forcing(x,t),p),tspanV,x0,options);
        u_valid = forcing(0,tspanV);

    case 'chirp'
        A = 10;
        forcing = @(x,t) A*chirp(t,[],max(tspanV),1.).^2;
        [t_valid,x_valid]=ode45(@(t,x) LorenzSys(t,x,forcing(x,t),p),tspanV,x0,options);
        u_valid = forcing(0,tspanV);

    case 'noise'
        vareps = 0.01;
        Diff = @(t,x) [0; vareps];
        SDE = sde(@(t,x) LorenzSys(t,x,forcing(x,t),p),Diff,'StartState',x0');
        rng(1,'twister')
        [x_valid, t_valid, u_valid] = simByEuler(SDE, length(tspanV), 'DeltaTime', dt);
        u_valid = u_valid';
        x_valid = x_valid(1:end-1,:); t_valid = t_valid(1:end-1);
    case 'prbs'
        A = 1;
        taulim = [0.1 5];
        states = [-1 1];
        Nswitch = 300;
        forcing = @(x,t) A*prbs(taulim, Nswitch, states, t,0);

        [t_valid,x_valid]=ode45(@(t,x) LorenzSys(t,x,forcing(x,t),p),tspanV,x0,options);

        u_valid = zeros(size(tspanV));
        for i = 1:length(tspanV)
            u_valid(i) = forcing(0,tspanV(i));
        end
        figure,plot(tspanV,u_valid)

    case 'sphs'
        Pf = 1; % Fundamental period
        K = 8;
        A = 10;
        forcing = @(x,t) A*sphs(Pf,K,t);
        [t_valid,x_valid]=ode45(@(t,x) LorenzSys(t,x,forcing(x,t),p),tspanV,x0,options);
        u_valid = forcing(0,tspanV);

    case 'mixed'
        A = [4 5 4 3];
        Pf = 8; % Fundamental period
        K = 16;
        taulim = [0.1 3];
        states = [-1 1];
        Nswitch = 40;

        tspan1 = [0:dt:100];
        forcing = @(x,t) SI_Input(t,A,Pf,K,tspan1,taulim, Nswitch, states);
        [t_valid,x_valid] = ode45(@(t,x) LorenzSys(t,x,forcing(x,t),p),tspan1,x0,options);

        u_valid = zeros(size(tspan1));
        for i = 1:length(tspan1)
            u_valid(i) = forcing(0,tspan1(i));
        end
        figure,plot(tspan1,u_valid)

        A = 2;
        tspanv = [100:dt:200];
        forcingV = @(x,t) [(A*(sin(1*t)+sin(.1*t))).^2];
        [tv,xv]=ode45(@(t,x) LorenzSys(t,x,forcing(x,t),p),tspanv,x_valid(end,:),options);
        uv = forcingV(0,tspanv);

        u_valid = [u_valid, uv(2:end)];
        x_valid = [x_valid; xv(2:end,:)];
        t_valid = tspanV';
end

T = length(tspanV);
N = length(tspanV);

%% Show data
figure;
plot(t_valid,x_valid,'LineWidth',1.5)
xlabel('Time')
ylabel('xi')
legend('x','y','z')
set(gca,'LineWidth',1, 'FontSize',14)
set(gcf,'Position',[100 100 300 200])
set(gcf,'PaperPositionMode','auto')


function [firstCrossing, pointOfCrossing] = BF_PointOfCrossing(x,threshold)
% BF_PointOfCrossing  Linearly interpolate to the point of crossing a threshold
%
%---INPUT:
% x, a vector
% threshold, a threshold x crosses.
%
%---OUTPUTS:
% firstCrossing, the first discrete value after which a crossing event has occurred
% pointOfCrossing, the (linearly) interpolated point of crossing

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function is free software: you can redistribute it and/or modify it under
% the terms of the GNU General Public License as published by the Free Software
% Foundation, either version 3 of the License, or (at your option) any later
% version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
% details.
%
% You should have received a copy of the GNU General Public License along with
% this program. If not, see <http://www.gnu.org/licenses/>.
% ------------------------------------------------------------------------------

% Find index of x at which the first crossing event occurs:
if x(1) > threshold
    firstCrossing = find((x - threshold < 0),1,'first');
else
    firstCrossing = find((x - threshold > 0),1,'first');
end

if isempty(firstCrossing)
    % Never crosses
    N = length(x);
    firstCrossing = N;
    pointOfCrossing = N;
else
    % Continuous version---the point of crossing
    valueBeforeCrossing = x(firstCrossing - 1);
    valueAfterCrossing = x(firstCrossing);
    pointOfCrossing = firstCrossing - 1 + (threshold - valueBeforeCrossing)/(valueAfterCrossing - valueBeforeCrossing);
end

end


function T = auxstructure3(elem)
%% AUXSTRUCTURE3 auxiliary structure for a 3-D triangulation.
%
%  T = AUXSTRUCTURE3(elem) constucts the indices map between elements,
%  faces, edges and nodes, and the boundary edge information. T is a struct
%  data.
%
%  T.neighor(1:NT,1:4): the indices map of neighbor information of elements,
%  where neighbor(t,i) is the global index of the element oppoiste to the
%  i-th vertex of the t-th element.
%
%  T.elem2face(1:NT,1:4): the indices map from elements to faces, where
%  elem2face(t,i) is the global index of the  edge opposite to the i-th
%  vertex of t-th elemment.
%
%  T.face(1:NF,1:3): faces, where face(k,i) is the global index of the i-th
%  vetex of the k-th face, and face(k,1)<face(k,2)<face(k,3).
%
%  T.bdFace(1:Nbd,1:3): boundary faces with positive oritentation, where
%  bdFace(k,i) is the global index of the i-th vetex of the k-th boundary
%  face. The positive oritentation means that the order of the three
%  vertices of bdFace(k,:) satisfy the right-handed system and normal
%  points the outside of the domain. Note that this requires elem is
%  positive ordered, i.e., the signed volume of each tetrahedron is
%  positive. If not, use elem = fixorder3(node,elem) to fix the order.
%
%  T.face2elem(1:NF,1:4): the indices map from faces to elements, where
%  face2elem(k,1:2) are two global indices of the elements sharing the k-th
%  face, and face2elem(k,3:4) are local indices of e to edge2elem(k,1:2).
%
%  To save space all the data type in T is uint32. When use them as a input
%  of sparse(i,j,s,m,n), please change them into double type.
%
%  See also auxstructure.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

NT = size(elem,1);
totalFace = uint32([elem(:,[2 3 4]); elem(:,[1 4 3]); ...
                    elem(:,[1 2 4]); elem(:,[1 3 2])]);
matlabversion = version;
if str2double(matlabversion(end-5:end-2)) > 2012
    [face, i2, j] = unique(sort(totalFace,2),'rows','lagacy');
else
    [face, i2, j] = unique(sort(totalFace,2),'rows');
end
i1(j(4*NT:-1:1)) = 4*NT:-1:1; i1 = i1';
k1 = ceil(i1/NT); t1 = i1 - NT*(k1-1);
k2 = ceil(i2/NT); t2 = i2 - NT*(k2-1);
idx = (i1 ~= i2);
neighbor = uint32(accumarray([[t1(idx),k1(idx)];[t2,k2]],[t2(idx);t1],[NT 4]));
elem2face = uint32(reshape(j,NT,4));
face2elem = uint32([t1,t2,k1,k2]);
bdElem = t1(t1 == t2);
bdk1 = k1(t1 == t2);
% consistent ordering of faces is used.
bdFace = [elem(bdElem(bdk1==1),[2 3 4]); elem(bdElem(bdk1==2),[1 4 3]);...
          elem(bdElem(bdk1==3),[1 2 4]); elem(bdElem(bdk1==4),[1 3 2])];
T = struct('neighbor',neighbor,'elem2face',elem2face,'face',uint32(face),...
           'face2elem',face2elem,'bdElem',bdElem,'bdFace',uint32(bdFace));


function cvt_test11 ( )

%*****************************************************************************80
%
%% CVT_TEST11 tests CVT.
%
%  Discussion:
%
%    In this test, we initialize the generators to grid points; this is
%    an unstable CVT solution.  The data would "prefer" to be in a
%    different form.  However, even if we take 2000 steps of CVT iteration,
%    the data is still only slowly progressing towards that other
%    configuration.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 November 2006
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST11\n' );
  fprintf ( 1, '  CVT computes a Centroidal Voronoi Tessellation.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  In this test, we initialize the generators to\n' );
  fprintf ( 1, '  grid points; this is an unstable CVT solution.\n' );

  dim_num = 2;
  n = 16;
  batch = 1000;
  init = 4;
  init_string = 'user initialization';
  it_max = 40;
  it_fixed = 1;
  sample = 0;
  sample_num = 1000;
  sample_string = 'uniform';
  seed = 123456789;

  seed_init = seed;
%
%  Initialize the tuple generator.
%
  rank = -1;
  ngrid = 4;
  tuple(1:dim_num) = tuple_next_fast ( ngrid, dim_num, rank );
%
%  Pick points on a grid.
%
  for rank = 0 : n-1
    tuple(1:dim_num) = tuple_next_fast ( ngrid, dim_num, rank );
    r(1:dim_num,rank+1) = ( 2 * tuple(1:dim_num)' - 1 ) / ( 2 * ngrid );
  end

  r8mat_transpose_print ( dim_num, n, r, '  Initial generators (rows):' );

  [ r, seed, it_num, it_diff, energy ] = cvt ( dim_num, n, batch, init, ...
    sample, sample_num, it_max, it_fixed, seed, r );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Dimension DIM_NUM =        %12d\n', dim_num );
  fprintf ( 1, '  Number of points N =       %12d\n', n );
  fprintf ( 1, '  Initial SEED =             %12d\n', seed_init );
  fprintf ( 1, '  Current SEED =             %12d\n', seed );
  fprintf ( 1, '  INIT =                    "%s".\n', init_string );
  fprintf ( 1, '  Max iterations IT_MAX =    %12d\n', it_max );
  fprintf ( 1, '  IT_FIXED (fixed samples) = %12d\n', it_fixed );
  fprintf ( 1, '  Iterations IT_NUM =        %12d\n', it_num );
  fprintf ( 1, '  Difference IT_DIFF =       %14f\n', it_diff );
  fprintf ( 1, '  CVT ENERGY =               %14f\n', energy );
  fprintf ( 1, '  SAMPLE =                  "%s".\n', sample_string );
  fprintf ( 1, '  Samples SAMPLE_NUM    =    %12d\n', sample_num );
  fprintf ( 1, '  Sampling BATCH size =      %12d\n', batch );
  fprintf ( 1, '  EPSILON (unit roundoff) =  %12e\n', eps );

  r8mat_transpose_print ( dim_num, n, r, '  Generators (rows):' );

  return
end