% Usage: [xopt, fmin, retcode] = nlopt_minimize_constrained
%                                          (algorithm, f, f_data,
%                                           fc, fc_data, lb, ub,
%                                           xinit, stop)
%
% Minimizes a nonlinear multivariable function f(x, f_data{:}), subject
% to nonlinear constraints described by fc and fc_data (see below), where
% x is a row vector, returning the optimal x found (xopt) along with
% the minimum function value (fmin = f(xopt)) and a return code (retcode).
% A variety of local and global optimization algorithms can be used,
% as specified by the algorithm parameter described below.  lb and ub
% are row vectors giving the upper and lower bounds on x, xinit is
% a row vector giving the initial guess for x, and stop is a struct
% containing termination conditions (see below).
%
% This function is a front-end for the external routine
% nlopt_minimize_constrained in the free NLopt nonlinear-optimization
% library, which is a wrapper around a number of free/open-source
% optimization subroutines.  More details can be found on the NLopt
% web page (ab-initio.mit.edu/nlopt) and also under
% 'man nlopt_minimize_constrained' on Unix.
%
% f should be a handle (@) to a function of the form:
%
%    [val, gradient] = f(x, ...)
%
% where x is a row vector, val is the function value f(x), and gradient
% is a row vector giving the gradient of the function with respect to x.
% The gradient is only used for gradient-based optimization algorithms;
% some of the algorithms (below) are derivative-free and only require
% f to return val (its value).  f can take additional arguments (...)
% which are passed via the argument f_data: f_data is a cell array
% of the additional arguments to pass to f.  (Recall that cell arrays
% are specified by curly brackets { ... }.  For example, pass f_data={}
% for functions that require no additional arguments.)
%
% A few of the algorithms (below) support nonlinear constraints,
% in particular NLOPT_LD_MMA and NLOPT_LN_COBYLA.  These (if any)
% are specified by fc and fc_data.  fc is a cell array of
% function handles, and fc_data is a cell array of cell arrays of the
% corresponding arguments.  Both must have the same length m, the
% number of nonlinear constraints.  That is, fc{i} is a handle
% to a function of the form:
%
%   [val, gradient] = fc(x, ...)
%
% (where the gradient is only used for gradient-based algorithms),
% and the ... arguments are given by fc_data{i}{:}.
%
% If you have no nonlinear constraints, i.e. fc = fc_data = {}, then
% it is equivalent to calling the the nlopt_minimize() function,
% which omits the fc and fc_data arguments.
%
% stop describes the termination criteria, and is a struct with a
% number of optional fields:
%     stop.ftol_rel = fractional tolerance on function value
%     stop.ftol_abs = absolute tolerance on function value
%     stop.xtol_rel = fractional tolerance on x
%     stop.xtol_abs = row vector of absolute tolerances on x components
%     stop.fmin_max = stop when f < fmin_max is found
%     stop.maxeval = maximum number of function evaluations
%     stop.maxtime = maximum run time in seconds
%     stop.verbose = > 0 indicates verbose output
% Minimization stops when any one of these conditions is met; any
% condition that is omitted from stop will be ignored.  WARNING:
% not all algorithms interpret the stopping criteria in exactly the
% same way, and in any case ftol/xtol specify only a crude estimate
% for the accuracy of the minimum function value/x.
%
% The algorithm should be one of the following constants (name and
% interpretation are the same as for the C function).  Names with
% _G*_ are global optimization, and names with _L*_ are local
% optimization.  Names with _*N_ are derivative-free, while names
% with _*D_ are gradient-based algorithms.  Algorithms:
%
% NLOPT_GD_MLSL_LDS, NLOPT_GD_MLSL, NLOPT_GD_STOGO, NLOPT_GD_STOGO_RAND,
% NLOPT_GN_CRS2_LM, NLOPT_GN_DIRECT_L, NLOPT_GN_DIRECT_L_NOSCAL,
% NLOPT_GN_DIRECT_L_RAND, NLOPT_GN_DIRECT_L_RAND_NOSCAL, NLOPT_GN_DIRECT,
% NLOPT_GN_DIRECT_NOSCAL, NLOPT_GN_ISRES, NLOPT_GN_MLSL_LDS, NLOPT_GN_MLSL,
% NLOPT_GN_ORIG_DIRECT_L, NLOPT_GN_ORIG_DIRECT, NLOPT_LD_AUGLAG_EQ,
% NLOPT_LD_AUGLAG, NLOPT_LD_LBFGS, NLOPT_LD_LBFGS_NOCEDAL, NLOPT_LD_MMA,
% NLOPT_LD_TNEWTON, NLOPT_LD_TNEWTON_PRECOND,
% NLOPT_LD_TNEWTON_PRECOND_RESTART, NLOPT_LD_TNEWTON_RESTART,
% NLOPT_LD_VAR1, NLOPT_LD_VAR2, NLOPT_LN_AUGLAG_EQ, NLOPT_LN_AUGLAG,
% NLOPT_LN_BOBYQA, NLOPT_LN_COBYLA, NLOPT_LN_NELDERMEAD,
% NLOPT_LN_NEWUOA_BOUND, NLOPT_LN_NEWUOA, NLOPT_LN_PRAXIS, NLOPT_LN_SBPLX
%
% For more information on individual algorithms, see their individual
% help pages (e.g. "help NLOPT_LN_SBPLX").
function [xopt, fmin, retcode] = nlopt_minimize_constrained(algorithm, f, f_data, fc, fc_data, lb, ub, xinit, stop)

opt = stop;
if (isfield(stop, 'minf_max'))
  opt.stopval = stop.minf_max;
end
opt.algorithm = algorithm;
opt.min_objective = @(x) f(x, f_data{:});
opt.lower_bounds = lb;
opt.upper_bounds = ub;
% opt.local_optimizer = alg;
% for i = 1:length(fc)
%   opt.fc{i} = @(x) fc{i}(x, fc_data{i}{:});
% end
opt.fc = fc;
[xopt, fmin, retcode] = nlopt(opt, xinit);


 function xs = tpl_inc(x, Gb, yi, bi, ri, R, niter, pixmax, curv, ...
		subout, enhance, gi, denom, relax0, chat)
%function xs = tpl_inc(x, Gb, yi, bi, ri, R, niter, pixmax, curv, ...
%		subout, enhance, gi, denom, relax0, chat)
%
% TRIOT: incremental SPS algorithm for transmission Poisson problem
% (ordered subsets separable paraboloidal surrogates)
% in
%	x	[np,1]		initial estimate
%	Gb			Gblock object (see tpl_os_sps_test.m)
%	yi,bi,ri [nb,na]	see tr_fbp.m (for model too)
%	R		penalty structure (see Robject.m)
%				(or empty for ML case)
%	niter		# iterations
%	pixmax		upper constraint for pixel values or [lower, upper]
%			can be scalar (e.g. 'inf') or an array the size of x
%	curv		'oc' for erdogan's optimal curvatures
%			'mc' for maximum curvature
%			'pc' for erdogan's fast precomputed curvatures,
%				which usually provides faster convergence,
%				but can be nonmonotone.
%	subout	[1]	nonzero value: return all subiterates
%			otherwise, return only iterates
%	enhance [1]	nonzero value: Hsiao's enhancement
%			otherwise, no enhancement
%	gi	[nd,1]	sum(G')'
%	denom	[np,1]		(if precomputed denominator)
%	relax0	[1] or [2]	relax0 or (relax0, relax_rate)
% out
%	xs [np,niter]	updated image vectors each iteration
%
% Copyright Mar 2000, Jeff Fessler, The University of Michigan
% 2002-2-20 update for Gblock object
% Modified for incremental version by Sangtae Ahn, from ~sangtaea/trans.

epsilon = 1e-10;

if nargin < 3, ir_usage, end

Gb = block_ob(Gb, 'ensure'); % make it a block object (if not already)
nblock = block_ob(Gb, 'n');
starts = subset_start(nblock);

if ~isvar('bi') || isempty(bi)
	bi = ones(size(yi));
end
if ~isvar('ri') || isempty(ri)
	ri = zeros(size(yi));
end

if ~isvar('R'), R = []; end
if ~isvar('niter')	|| isempty(niter),	niter = 2;	end
if ~isvar('pixmax')	|| isempty(pixmax),	pixmax = inf;	end
if length(pixmax) == 2
	pixmin = pixmax(1);
	pixmax = pixmax(2);
else
	pixmin = 0;
end
if ~isvar('curv')	|| isempty(curv),	curv = 'oc';	end
if ~isvar('subout')	|| isempty(subout),	subout = 0;	end
if ~isvar('enhance')	|| isempty(enhance),	enhance = 0;	end
if ~isvar('chat')	|| isempty(chat),	chat = false;	end
if ~isvar('relax0')	|| isempty(relax0),	relax0 = 1;	end

if streq(curv, 'mc')
	subscale = @(iset) iset;
else
	subscale = @(iset) 1;
end

if length(relax0) == 1
	relax_rate = 0;
elseif length(relax0) == 2
	relax_rate = relax0(2);
	relax0 = relax0(1);
else
	error relax
end

if length(niter) == 2
	osspsiter = niter(1);
	niter = niter(2);
else
	osspsiter = 1;
end

trl_check(yi, bi, ri);

if ~isvar('gi') || isempty(gi)
	gi = reshape(sum(Gb'), size(yi));	% g_i = sum_j g_ij
end

starts = subset_start(nblock);
[nb na] = size(yi);

% precompute denominator if needed
if (~isvar('denom') || isempty(denom)) && (streq(curv, 'pc') || streq(curv, 'mc'))
	ni = my_trl_curvature(yi, bi, ri, [], curv); % precomputed curvatures
	%
	% separate denominators for each subset
	%
	if streq(curv, 'mc')
		denom = zeros(numel(x), nblock);
		for iset=1:nblock
			iblock = starts(iset);
			ia = iblock:nblock:na;
			denom(:,iset) = Gb{iblock}' * col(gi(:,ia) .* ni(:,ia));
		end
	else
	%
	% one denominator shared by all subsets
	%
		denom = Gb' * col(gi .* ni) / nblock;
	end
end

% "precomputed curvature"
if ~streq(curv, 'pc')
	ni = my_trl_curvature(yi, bi, ri, [], 'pc');
	denom_pc = Gb' * col(gi .* ni) / nblock;
else
	denom_pc = denom;
end

if subout
	xs = zeros(length(x), (niter-1)*nblock+1);
else
	xs = zeros(length(x), niter);
end

x = max(x,pixmin);
x = min(x,pixmax);
xs(:,1) = x;
if subout, idx = 2; end
num_store = zeros(length(x), nblock);
den_store = zeros(length(x), nblock);

%
% initialization by os-sps-pc
%
for iter = 1:osspsiter
	for iset=1:nblock
		iblock = starts(iset);
		ia = iblock:nblock:na;

		li = Gb{iblock} * x;	% l=G*x "line integrals"
		li = reshape(li, nb, length(ia));
		bel = bi(:,ia) .* exp(-li);
		yb = bel + ri(:,ia);	% predicted meas. means
		dothi = (1 - yi(:,ia) ./ yb) .* bel;

		if streq(curv, 'oc')
			ni = my_trl_curvature(yi(:,ia), bi(:,ia), ri(:,ia), ...
				li, 'oc');
			denom = Gb{iblock}' * col(gi(:,ia) .* ni);
		end

		if isempty(R)
			grad = Gb{iblock}' * dothi(:);
			den_osps = max(denom_pc, epsilon);
			den_store(:, iset) = max(denom(:,subscale(iset)), ...
						epsilon);
		else
			grad = Gb{iblock}' * dothi(:) - R.cgrad(R, x)/nblock;
			den_osps = max(denom_pc + R.denom(R, x)/nblock, ...
					epsilon);
			den_store(:, iset) = max(denom(:,subscale(iset)) + ...
					R.denom(R, x)/nblock, epsilon);
		end

		if iter == osspsiter
			num_store(:, iset) = x.*den_store(:, iset) + grad;
		end
		x = x + grad ./ den_osps;
		x = max(x,pixmin);
		x = min(x,pixmax);
		if subout
			xs(:,idx) = x;
			idx = idx + 1;
		end
	end
	if ~subout, xs(:,iter+1) = x;	end
end
num = sum(num_store, 2);
den = sum(den_store, 2);

if 1,		% optional
	x = num ./ den;
	x = max(x,pixmin); x = min(x,pixmax);
	if subout
		xs(:,idx-1) = x;
	else
		xs(:,iter+1) = x;
	end
end

alphidx = 1;
for iter = (osspsiter+2):niter
	ticker(mfilename, iter, niter)

	relax = relax0 / (1 + relax_rate * (iter-2));

	%
	% loop over subsets
	%
	for iset=1:nblock
		iblock = starts(iset);
		ia = iblock:nblock:na;

		li = Gb{iblock} * x;	% l=G*x "line integrals"
		li = reshape(li, nb, length(ia));
		bel = bi(:,ia) .* exp(-li);
		yb = bel + ri(:,ia);	% predicted meas. means
		dothi = (1 - yi(:,ia) ./ yb) .* bel;

		% optimal curvatures (for ensured monotone increase)
		if streq(curv, 'oc')
			ni = my_trl_curvature(yi(:,ia), bi(:,ia), ri(:,ia), ...
				li, 'oc');
			denom = Gb{iblock}' * col(gi(:,ia) .* ni);
		end

		num = num - num_store(:, iset);
		den = den - den_store(:, iset);

		if isempty(R)
			grad = Gb{iblock}' * dothi(:);
			den_store(:, iset) = max(denom(:,subscale(iset)), ...
						epsilon);
		else
			grad = Gb{iblock}' * dothi(:) - R.cgrad(R, x)/nblock;
			den_store(:, iset) = max(denom(:,subscale(iset)) + ...
						R.denom(R, x)/nblock, epsilon);
		end

		num_store(:, iset) = x.*den_store(:, iset) + grad;

		num = num + num_store(:, iset);
		den = den + den_store(:, iset);

		xold = x;

		x = num ./ den;
		x = max(x,pixmin); x = min(x,pixmax);

		if enhance
			xos = xold + grad ./ (den_store(:,iset) - ...
				denom(:,subscale(iset)) + denom_pc);
			xos = max(xos,pixmin);
			xos = min(xos,pixmax);

			c2 = - sum((xos - x).^2 .* den) / 2;
			c1 = num'*(xos - x) - sum(x.*(xos-x).*den);
			c0 = num'*(x - xold) - sum(x.^2.*den - xold.^2.*den)/2;

			if 1
				if c0+c1+c2 > 0
					alpha = 1;
				else
					alpha = eql_root(-c2, -c1/2, 0.95*c0);
					alpha = min(1, alpha);
					alpha = max(0, alpha);
				end
			else
				alpha = 1/0.9;
				del_obj = -999;
				while (del_obj <=0) & (alpha > 0.0096)
					alpha = alpha * 0.9;
					del_obj = c2*alpha^2 + c1*alpha + c0;
				end

				if alpha <= 0.0096, alpha = 0; end
			end

			x = (1-alpha)*x + alpha*xos;
			alph(alphidx) = alpha;
			alphidx = alphidx + 1;

		end

		if subout
			xs(:,idx) = x;
			idx = idx + 1;
		end
	end

	if chat, printf('Range %g %g', min(x), max(x)), end
	if ~subout, xs(:,iter) = x; end
end

%figure, plot(alph, 'o-')

function obj = trl_obj(alpha, xinc, xos, linc, los, yi, bi, ri, R)

x = alpha*xos + (1 - alpha)*xinc;
li = alpha*los + (1 - alpha)*linc;
yp = bi .* exp(-li) + ri;
like = sum(yi .* log(yp) - yp);
penal = sum(R.penal(R, x));
obj = like - penal;
obj = -obj;


% [template t2 valid] = PPG_SQI(wave,anntime,annot,template_ahead,windowlen,Fs)
%
% PPG_SQI.m - PPG SQI based on beat template correlation.
% (as an advice, the algorithm get 30 beats at each call and run in loop)
% by Qiao Li 30 Mar 2011
%
% PPG sampling frequency is Fs
%
% input:
%     wave:       PPG data;
%     anntime:    PPG annotation time (samples), read from ple annot file,
%                 But the ann-time is the OFFSET based on wave(1)
%     annot:      Annotation of beats, read from from ple annot file
%                 directly
%     template:   Last PPG beat template
%     windowlen:  length of window to calculate template(default: 30s)
%     Fs       :  sampling frequency (defatult: 125 to work with pervious code)
% output:
%     annot:      ppg sqi annotation
%                     annot.typeMnemonic: E - excellent beat;
%                                         A - acceptable beat;
%                                         Q - unacceptable beat
%                     annot.subtype: SQI based on Direct compare
%                     annot.chan:    SQI based on Linear resampling
%                     annot.num:     SQI based on Dynamic time warping
%                     annot.auxInfo: SQI based on Clipping detection
%     template:   Current PPG beat template
%     valid:      1 or greater for valid template,
%                 0 for invalid template
%
%   LICENSE:
%       This software is offered freely and without warranty under
%       the GNU (v3 or later) public license. See license file for
%       more information
%
% 12-01-2107 Modified by Giulia Da Poian: replace fixed samplig frequency
% (125) with a variable Fs

function [annot template valid] = PPG_SQI(wave,anntime,annot,template,windowlen,Fs)

    if nargin < 6
        Fs =125;
    end
    if nargin < 5
        windowlen=30*Fs;
    end
    if nargin < 4
        template=[];
    end
    if nargin < 3
        sprintf('Error: must provide wave, anntime and annot');
        annot=[];
        template=[];
        valid=0;
        return;
    end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 19/09/2011 ADD baseline wander filter
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    wave = PPGmedianfilter(wave, Fs,Fs);
    % get PPG template
    [t t2 v]=template_pleth(wave(1:min(windowlen,length(wave))), anntime(find(anntime<min(windowlen,length(wave)))), 0,Fs);

    if v<1 && length(template)<1 % Current template invalid && no previous template available
        for j=1:length(annot)
            annot(j).typeMnemonic='Q'; % Unacceptable
        end
        t=[];
    else
        % Using previous template as the template
        if v<1
            t=template;
        end
        % Using t2 if available
        if v>1
            t=t2;
        end

            % Calculate the PLA of template for dynamic time warping
            d1=t;
            d1=(d1-min(d1))/(max(d1)-min(d1)).*100;
            [y1 pla1]=PLA(d1,1,1);

% Main Loop
            for j=1:length(annot)-1

%             SQI1: Direct compare

%             Calculate correlation coefficients based on the template
%             length
                beatbegin=anntime(j);
                beatend=anntime(j+1);
% 07/11/2011 ADD max beat length <= 3s detection
		if beatend-beatbegin>3*Fs
			beatend=beatbegin+3*Fs;
		end
		templatelength=length(t);
		complength=min(templatelength,beatend-beatbegin-1);
                if beatbegin+complength-1 > length(wave) || beatend > length(wave) || beatbegin < 1
                    continue;
                end
                currentb=j;
                cc=corrcoef(t(1:complength),wave(beatbegin:beatbegin+complength-1));
                c1(j)=cc(1,2);
                if (c1(j)<0)
                    c1(j)=0;
                end
                annot(currentb).subtype=int8(c1(j)*100);

%             SQI2: Linear resampling

%             Calculate correlation coefficients based on the linear
%             resampling (interp1)

                y=interp1(1:beatend-beatbegin, wave(beatbegin:beatend-1),1:(beatend-beatbegin-1)/(templatelength-1):(beatend-beatbegin),'spline');
                y(isnan(y))=0;
                cc=corrcoef(t,y);
                c2(j)=cc(1,2);
                if (c2(j)<0)
                    c2(j)=0;
                end
                annot(currentb).chan=int8(c2(j)*100);

%             SQI3: Dynamic Time Warping

%             Calculate correlation coefficients based on the dynamic time
%             warping

                d2=wave(beatbegin:beatend-1);

                % if beat too long, set SQI=0;
                if (length(d2)>length(d1)*10)
                    c3(j)=0;
                else
                    d2=(d2-min(d2))/(max(d2)-min(d2)).*100;
                   [y2 pla2]=PLA(d2,1,1);

                   [w ta tb] = simmx_dtw(y1,pla1,y2,pla2);
                   [p,q,Dm] = dp_dwt2(w,ta,tb);
                   [ym1 ym2 yout1]=draw_dtw(y1,pla1,p,y2,pla2,q);
                    cc=corrcoef(y1,ym2);
                    c3(j)=cc(1,2);
                    if (c3(j)<0)
                        c3(j)=0;
                    end
                end
                annot(currentb).num=int8(c3(j)*100);

%             SQI4: Clipping detection

                d2=wave(beatbegin:beatend-1);
                y=diff(d2);
                clipthreshold=0.5;
                c4(j)=int8(length(find(abs(y)>clipthreshold))/length(y)*100);

%             SQI: Combined

                sqibuf=[annot(currentb).subtype annot(currentb).chan annot(currentb).num c4(j)];
                if min(sqibuf)>=90
                    annot(currentb).typeMnemonic='E'; % Excellent
                else
                    if length(find(sqibuf>=90))>=3 || (median(sqibuf(1:3))>=80 && sqibuf(1)>=50 && sqibuf(4)>=70) || min(sqibuf)>=70
                        annot(currentb).typeMnemonic='A'; % Acceptable
                    else
                        annot(currentb).typeMnemonic='Q'; % Unacceptable
                    end
                end
                annot(currentb).auxInfo=num2str(int8(c4(j)));

            end

    end
    template=t;
    valid=v;


function [gx,dgdx,dgdp] = g_HRF3(Xt,P,ut,in)
% T2* contrast observation function for HRF Balloon model (log space)
% function [gx,dgdx,dgdp] = g_HRF3(Xt,P,ut,in)
% This function evaluates the hemodynamic static observation equation
% function. HEMODYNAMIC STATES ARE IN LOG SPACE.

n = size(Xt,1);

% Get parameters and states indices

try; TE = in.TE; catch, TE = 0.04; end

if isfield(in,'fullDCM') && in.fullDCM
    % estimated region-specific resting oxygen extraction fractions
    % NB: if P(in.ind1) = 0, E0 = 0.34.
%     [E0,dsdp] = sigm(P(in.ind1)-0.6633,struct('beta',1,'G0',1,'INV',0));
%     E0 = E0(:);
    E0 = 1./(1+exp(-(P(in.ind1)-0.6633)));
    dsdp = E0.*(1-E0);
    % estimated region-specific ratios of intra- to extravascular
    % components of the gradient echo signal (prior mean = 1, log-normally
    % distributed scaling factor)
    epsilon   = exp(P(in.ind2));
    % hemodynamic states indices:
    n1 = in.n1;
    n2 = in.n2;
    n3 = in.n3;
    n4 = in.n4;
    try
        n5 = in.n5;
        nreg = n./5;
    catch
        n5 = [];
        nreg = n./4;
    end
    if isfield(in,'homogeneous') && in.homogeneous
        E0 = E0.*ones(nreg,1);
        dsdp = dsdp.*ones(nreg,1);
        epsilon = epsilon.*ones(nreg,1);
        in.ind1 = [in.ind1:2:2*nreg];
        in.ind2 = [in.ind2:2:2*nreg];
    end
else
    [E0,dsdp] = VBA_sigmoid(P(1)-0.6633);
    E0 = E0(:);
    try
        epsilon = exp(P(2));
        p2 = 1;
    catch
        epsilon = 1;
        p2 = 0;
    end
    % hemodynamic states indices:
    n1 = 1;
    n2 = 2;
    n3 = 3;
    n4 = 4;
    nreg = n./4;
end

%--------------------------------------------------------------------------
% coefficients in BOLD signal model,...
V0 = 4; % resting venous volume
r0 = 25; % intravascular relaxation rate
nu0 = 40.3; % frequency offset
k1 = 4.3.*nu0.*E0.*TE;
k2 = epsilon.*r0.*E0.*TE;
k3 = 1 - epsilon;

% states ...
x3 = exp(Xt(n3,:));         % blood volume v(t)
x4 = exp(Xt(n4,:));         % dHb content q(t)

% ... and derivatives
dgdx = zeros(n,nreg);
dgdp = zeros(size(P,1),nreg);

% Evaluate observation function
x4x3 = exp(log(x4)-log(x3));
gx = V0.*(k1.*(1-x4) + k2.*(1-x4x3) + k3.*(1-x3));

% Evaluate gradient wrt states and parameters
for i=1:nreg
    if isfield(in,'fullDCM') && in.fullDCM
        if isempty(n5)
            dgdx(n1(i):n4(i),i) = [...
                0; ...
                0; ...
                -k3(i).*x3(i)+k2(i).*x4x3(i); ...
                -k1(i).*x4(i)-k2(i).*x4x3(i)].*V0;
        else
            dgdx(n1(i):n5(i),i) = [...
                0; ...
                0; ...
                -k3(i).*x3(i)+k2(i).*x4x3(i); ...
                -k1(i).*x4(i)-k2(i).*x4x3(i); 0].*V0;
        end
        dgdp(in.ind1(i),i) = V0.*4.3.*nu0.*TE.*(1-x4(i)).*dsdp(i) + ...
            V0.*epsilon(i).*r0.*TE.*(1-x4x3(i)).*dsdp(i);
        dgdp(in.ind2(i),i) = V0.*k2(i).*(1-x4x3(i)) - ...
            V0.*epsilon(i).*(1-x3(i));
    else
        dgdx(n1(i):n4(i),i) = [0; 0; ...
            -k3(i).*x3(i)+k2(i).*x4x3(i); ...
            -k1(i).*x4(i)-k2(i).*x4x3(i)].*V0;
        dgdp = V0.*4.3.*nu0.*TE.*(1-x4(i)).*dsdp(i) + ...
            V0.*epsilon(i).*r0.*TE.*(1-x4x3(i)).*dsdp(i);
        if p2
            dgdp(2) = V0.*k2(i).*(1-x4x3(i)) - ...
                V0.*epsilon(i).*(1-x3(i));
            dgdp = dgdp(:);
        end
    end
end

if isfield(in,'homogeneous') && in.homogeneous
    dgdp0 = dgdp;
    dgdp = zeros(2,nreg);
    dgdp(1,:) = sum(dgdp0(in.ind1,:),1);
    dgdp(2,:) = sum(dgdp0(in.ind2,:),1);
end

if isfield(in,'confounds') && ~isempty(in.confounds.X0)
    tsample = ut(in.confounds.indt);
    X0 = kron(eye(nreg),in.confounds.X0(tsample,:));
    gx = gx + X0*P(in.confounds.indp);
    dgdp(in.confounds.indp,:) = X0';
end


function [alpha,b,eta,zeta,kappa] = solve_hkl_square_dual_fast(eta,data,Y,lambda,varargin);

% solve the milasso for a given regularization parameter
n = size(data.X,1);
p =length(data.affinity);
pX = size(data.X,2);
affs = data.affinity;
weights = data.weights;

% optional parameters
mingap = 1e-3;
display = 0;
gapeta = 1e-3;

% READ OPTIONAL PARAMETERS
args = varargin;
nargs = length(args);
for i=1:2:nargs
    switch args{i},
        case 'mingap',        mingap = args{i+1};
        case 'gapeta',        gapeta = args{i+1};
        case 'display',        display = args{i+1};
    end
end

if isempty(eta)
    eta = 1/p ./ weights.^2;
end
x = eta .* weights.^2;
x = max(x,0);
x = x / sum(x);

if display==1
display = Inf;
end
beta_ARMIJO = .25 ;
sigma_ARMIJO = .1 ;
kmax_ARMIJO = 20;
kmax = 200;
tol_dx = 1e-8;
k = 1;
alpha=1;

% % optimization parameters for the dual problem
% optparam.tol = 1e-16;
% optparam.kmax = 200;
% optparam.tol_df = 1e-20;
% optparam.display= 0;
fx0 = Inf;

% solve the regular kernel learning problem
mY = mean(Y);

%nu =  1e-6; % added ridge on kernels
while k<=kmax;
    x = max(x,0);
    x = x / sum(x);

    % compute value of function
    eta = ( x*(1-gapeta) + gapeta/p )./ weights.^2 ;
    zeta = zeros(p,1);
    for i=1:p
        zeta(affs{i}) = zeta(affs{i}) + 1./eta(i);
    end
    zeta = 1./zeta;

    % solve the regular kernel learning problem
    K = center_gram_matrix(double(devectorize_single( single(data.kernels * zeta ))));
    alphaK = (K + n * lambda * eye(n) ) \ ( Y - mean(Y) );
    fx =  lambda/2 * ( Y - mean(Y))'* alphaK;

    gradient_zeta = zeros(p,1);
    for i=1:p
        gradient_zeta(i) = - lambda/2 *  vectorize_quad_single(data.kernels(:,i),alphaK );
    end

    gradient_eta = zeros(p,1);
    for i=1:p;
        gradient_eta(i) = sum( gradient_zeta(affs{i}) .* zeta(affs{i}).^2 ) / eta(i)^2;
    end
    gradient = (1-gapeta) * gradient_eta ./ weights.^2;

    % check optimality condition
    optcond = max(x .* ( gradient - min(gradient)));
    if max(x .* ( gradient - min(gradient))) < mingap, break; end

    fx0=fx;

    % projected gradient
    xbar = x -   gradient;
    d = - gradient;
    x0=x;
    ialpha = 1;
    while ialpha<kmax_ARMIJO
        xalpha = simplex_projection(x + d * alpha);
        % fxalpha = feval(fun,xalpha,varargin{:});
        eta = ( xalpha*(1-gapeta) + gapeta/p )./ weights.^2 ;
        zeta = zeros(p,1);
        for i=1:p
            zeta(affs{i}) = zeta(affs{i}) + 1./eta(i);
        end
        zeta = 1./zeta;

    K = center_gram_matrix(double(devectorize_single( single(data.kernels * zeta ))));
    alphaK = (K + n * lambda * eye(n) ) \ ( Y - mean(Y) );
    fxalpha =  lambda/2 * ( Y - mean(Y))'* alphaK;

        if fx - fxalpha >= - sigma_ARMIJO * gradient' * ( xalpha - x );

            % start to go up
            while ( fx - fxalpha >= - sigma_ARMIJO * gradient' * ( xalpha - x ) ) & ialpha<kmax_ARMIJO;
                alpha = alpha / beta_ARMIJO;
                xalpha = simplex_projection(x + d * alpha);
                %fxalpha = feval(fun,xalpha,varargin{:});
                eta = ( xalpha*(1-gapeta) + gapeta/p )./ weights.^2 ;
                zeta = zeros(p,1);
                for i=1:p
                    zeta(affs{i}) = zeta(affs{i}) + 1./eta(i);
                end
                zeta = 1./zeta;

    K = center_gram_matrix(double(devectorize_single( single(data.kernels * zeta ))));
    alphaK = (K + n * lambda * eye(n) ) \ ( Y - mean(Y) );
    fxalpha =  lambda/2 * ( Y - mean(Y))'* alphaK;

                % fprintf('+');
                ialpha = ialpha+1;
            end
            alpha = alpha * beta_ARMIJO;
            xalpha = simplex_projection(x + d * alpha);
            x = xalpha;
            break;
        else
            alpha = alpha * beta_ARMIJO;

        end
        ialpha = ialpha+1;
    end

    if ialpha==kmax_ARMIJO, alpha=1; end

    if ~isinf(display)
        if display==1,
            fprintf('k=%d - f=%f - armijo=%d - dx=%e - gap=%f\n',k,fx,ialpha,norm(x-x0),optcond);
        end
        if display>1,
            if mod(k,display)==1
                fprintf('k=%d - f=%f - armijo=%d - dx=%e - gap=%f\n',k,fx,ialpha,norm(x-x0),optcond);

            end
        end
    end
    if norm(x-x0)<tol_dx,  break; end

    if sum(x) > 1+1e-1, keyboard; end

    k=k+1;
end

% if k==kmax+1
%     keyboard;
% end

eta = ( x*(1-gapeta) + gapeta/p )./ weights.^2 ;
zeta = zeros(p,1);
for i=1:p
    zeta(affs{i}) = zeta(affs{i}) + 1./eta(i);
end
zeta = 1./zeta;

    K = center_gram_matrix(double(devectorize_single( single(data.kernels * zeta ))));
    alpha = (K + n * lambda * eye(n) ) \ ( Y - mean(Y) );

    pred = K * alpha;
    b=mean(Y-pred );

kappa = zeros(p,p);
for i=1:p
    kappa(i,affs{i}) = zeta(affs{i})./eta(i);
end

 if isinf(display)
            fprintf('k=%d - f=%f -  gap=%f\n',k,fx,optcond);
        end


function varargout = xyz
%XYZ   Three chebfun3 obejcts of the identity on [-1, 1, -1, 1, -1, 1].
%   CHEB.XYZ is shorthand for the expressions
%   X = CHEBFUN3(@(X,Y,Z) X),
%   Y = CHEBFUN3(@(X,Y,Z) Y), and
%   Z = CHEBFUN3(@(X,Y,Z) Z).

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

x = chebfun3(@(x,y,z) x);
y = chebfun3(@(x,y,z) y);
z = chebfun3(@(x,y,z) z);

if nargout>1
    % For the syntax [x,y,z] = cheb.xyz
    varargout{1} = x;
    varargout{2} = y;
    varargout{3} = z;
else
    % For the syntax cheb.xyz we still want to put these variables into the
    % workspace:
    assignin('base', 'x', x);
    assignin('base', 'y', y);
    assignin('base', 'z', z);
end

end


function Offspring = GLMO_SMPSOOperator(Problem, Particle, Pbest, Gbest, numberOfGroups, typeOfGroups)
% -----------------------------------------------------------------------
%  Copyright (C) 2020 Heiner Zille
%
%  This work is licensed under the Creative Commons Attribution-NonCommercial-ShareAlike 4.0
%  International License. (CC BY-NC-SA 4.0). To view a copy of this license,
%  visit http://creativecommons.org/licenses/by-nc-sa/4.0/ or see the
%  pdf-file "License-CC-BY-NC-SA-4.0.pdf" that came with this code.
%
%  You are free to:
%  * Share ? copy and redistribute the material in any medium or format
%  * Adapt ? remix, transform, and build upon the material
%  Under the following terms:
%  * Attribution ? You must give appropriate credit, provide a link to the
%     license, and indicate if changes were made. You may do so in any reasonable
%     manner, but not in any way that suggests the licensor endorses you or your use.
%  * NonCommercial ? You may not use the material for commercial purposes.
%  * ShareAlike ? If you remix, transform, or build upon the material, you must
%    distribute your contributions under the same license as the original.
%  * No additional restrictions ? You may not apply legal terms or technological
%    measures that legally restrict others from doing anything the license permits.
%
%  Author of this Code:
%   Heiner Zille <heiner.zille@ovgu.de> or <heiner.zille@gmail.com>
%
%  This code is based on the following publications:
%
%  1) Heiner Zille
%     "Large-scale Multi-objective Optimisation: New Approaches and a Classification of the State-of-the-Art"
%     PhD Thesis, Otto von Guericke University Magdeburg, 2019
%     http://dx.doi.org/10.25673/32063
%
%  2) Heiner Zille, Hisao Ishibuchi, Sanaz Mostaghim and Yusuke Nojima
%     "Mutation Operators Based on Variable Grouping for Multi-objective Large-scale Optimization"
%     IEEE Symposium Series on Computational Intelligence (SSCI), IEEE, Athens, Greece, December 2016
%     https://ieeexplore.ieee.org/document/7850214
%
%  This file is intended to work with the PlatEMO framework version 2.5.
%  Date of publication of this code: 06.04.2020
%  Last Update of this code: 06.04.2020
%  A newer version of this algorithm may be available. Please contact the author
%  or see http://www.ci.ovgu.de/Research/Codes.html.
%
% The files may have been modified in Feb 2021 by the authors of the Platemo framework to work with the Platemo 3.0 release.
% -----------------------------------------------------------------------
% This file is derived from its original version containied in the PlatEMO
% framework. The original copyright disclaimer can be found below.
% -----------------------------------------------------------------------

% Particle swarm optimization in SMPSO

    %% Parameter setting
    ParticleDec = Particle.decs;
    PbestDec    = Pbest.decs;
    GbestDec    = Gbest.decs;
    [N,D]       = size(ParticleDec);
    ParticleVel = Particle.adds(zeros(N,D));

    %% Particle swarm optimization
    W  = repmat(unifrnd(0.1,0.5,N,1),1,D);
    r1 = repmat(rand(N,1),1,D);
    r2 = repmat(rand(N,1),1,D);
    C1 = repmat(unifrnd(1.5,2.5,N,1),1,D);
    C2 = repmat(unifrnd(1.5,2.5,N,1),1,D);
    OffVel = W.*ParticleVel + C1.*r1.*(PbestDec-ParticleDec) + C2.*r2.*(GbestDec-ParticleDec);
    phi    = max(4,C1+C2);
    OffVel = OffVel.*2./abs(2-phi-sqrt(phi.^2-4*phi));
    delta  = repmat((Problem.upper-Problem.lower)/2,N,1);
    OffVel = max(min(OffVel,delta),-delta);
    OffDec = ParticleDec + OffVel;

    %% Deterministic back
    Lower  = repmat(Problem.lower,N,1);
    Upper  = repmat(Problem.upper,N,1);
    repair = OffDec < Lower | OffDec > Upper;
    OffVel(repair) = 0.001*OffVel(repair);
    OffDec = max(min(OffDec,Upper),Lower);

    %% Polynomial mutation
    disM  = 20;
    Site1 = repmat(rand(N,1)<0.15,1,D);

    [outIndexList,~] = GLMO_createGroups(numberOfGroups,OffDec,D,typeOfGroups); % 3 = random groups
    chosengroups = randi(numberOfGroups,size(outIndexList,1),1);
    Site2 = outIndexList == chosengroups;
    mu    = rand(N,1);
    mu    = repmat(mu,1,D);

    temp  = Site1 & Site2 & mu<=0.5;
    OffDec(temp) = OffDec(temp)+(Upper(temp)-Lower(temp)).*((2.*mu(temp)+(1-2.*mu(temp)).*...
                   (1-(OffDec(temp)-Lower(temp))./(Upper(temp)-Lower(temp))).^(disM+1)).^(1/(disM+1))-1);
    temp  = Site1 & Site2 & mu>0.5;
    OffDec(temp) = OffDec(temp)+(Upper(temp)-Lower(temp)).*(1-(2.*(1-mu(temp))+2.*(mu(temp)-0.5).*...
                   (1-(Upper(temp)-OffDec(temp))./(Upper(temp)-Lower(temp))).^(disM+1)).^(1/(disM+1)));
    Offspring = Problem.Evaluation(OffDec,OffVel);
end


function [g1, g2] = lfmXrbfKernGradient(lfmKern, rbfKern, t1, t2, covGrad, meanVector)

% LFMXRBFKERNGRADIENT Compute gradient between the LFM and RBF kernels.
% FORMAT
% DESC computes the gradient of an objective function with respect
% to cross kernel terms between LFM and RBF kernels for
% the multiple output kernel.
% ARG lfmKern : the kernel structure associated with the LFM
% kernel.
% ARG rbfKern : the kernel structure associated with the RBF
% kernel.
% ARG t : inputs for which kernel is to be computed.
% RETURN g1 : gradient of objective function with respect to kernel
% parameters of LFM kernel.
% RETURN g2 : gradient of objective function with respect to kernel
% parameters of RBF kernel.
%
% FORMAT
% DESC computes the gradient of an objective function with respect
% to cross kernel terms between LFM and RBF kernels for
% the multiple output kernel.
% ARG lfmKern : the kernel structure associated with the LFM
% kernel.
% ARG rbfKern : the kernel structure associated with the RBF
% kernel.
% ARG t1 : row inputs for which kernel is to be computed.
% ARG t2 : column inputs for which kernel is to be computed.
% RETURN g1 : gradient of objective function with respect to kernel
% parameters of LFM kernel.
% RETURN g2 : gradient of objective function with respect to kernel
% parameters of RBF kernel.
%
% FORMAT
% DESC computes the gradient of an objective function with respect
% to cross kernel terms between LFM and RBF kernels for
% the multiple output kernel.
% ARG lfmKern : the kernel structure associated with the LFM
% kernel.
% ARG rbfKern : the kernel structure associated with the RBF
% kernel.
% ARG t1 : row inputs for which kernel is to be computed.
% ARG t2 : column inputs for which kernel is to be computed.
% ARG meanVec : precomputed factor that is used for the switching dynamical
% latent force model.
% RETURN g1 : gradient of objective function with respect to kernel
% parameters of LFM kernel.
% RETURN g2 : gradient of objective function with respect to kernel
% parameters of RBF kernel.
%
% SEEALSO : multiKernParamInit, multiKernCompute, lfmKernParamInit, rbfKernParamInit
%
% COPYRIGHT : David Luengo, 2007, 2008
%
% MODIFICATIONS : Neil D. Lawrence, 2007
%
% MODIFICATIONS : Mauricio A. Alvarez, 2008, 2010

% KERN

subComponent = false; % This is just a flag that indicates if this kernel is part of a bigger kernel (SDLFM)

if nargin == 4
    covGrad = t2;
    t2 = t1;
elseif nargin == 6
    subComponent = true;
    if numel(meanVector)>1
        if size(meanVector,1) == 1,
            if size(meanVector, 2)~=size(covGrad, 2)
                error('The dimensions of meanVector don''t correspond to the dimensions of covGrad')
            end
        else
            if size((meanVector'), 2)~=size(covGrad,2)
                error('The dimensions of meanVector don''t correspond to the dimensions of covGrad')
            end
        end
    else
        if numel(t1)==1 && numel(t2)>1
            % matGrad will be row vector and so should be covGrad
            dimcovGrad = length(covGrad);
            covGrad = reshape(covGrad, [1 dimcovGrad]);
        elseif numel(t1)>1 && numel(t2)==1
            % matGrad will be column vector and sp should be covGrad
            dimcovGrad = length(covGrad);
            covGrad = reshape(covGrad, [dimcovGrad 1]);
        end
    end
end

if size(t1, 2) > 1 || size(t2, 2) > 1
    error('Input can only have one column');
end
if lfmKern.inverseWidth ~= rbfKern.inverseWidth
    error('Kernels cannot be cross combined if they have different inverse widths.')
end

if lfmKern.isNormalised ~= rbfKern.isNormalised
  error('Kernels cannot be cross combined if they have different normalization settings.')
end

m = lfmKern.mass;
D = lfmKern.spring;
C = lfmKern.damper;
S = lfmKern.sensitivity;

alpha = C/(2*m);
omega = sqrt(D/m-alpha^2);

sigma2 = 2/lfmKern.inverseWidth;
sigma = sqrt(sigma2);

if isreal(omega)
    gamma = alpha + j*omega;
    ComputeUpsilon1 = lfmComputeUpsilonMatrix(gamma,sigma2,t1, t2);
    if lfmKern.isNormalised
        K0 = S/(2*sqrt(2)*m*omega);
    else
        K0 = sigma*sqrt(pi)*S/(2*m*omega);
    end
else
    gamma1 = alpha + j*omega;
    gamma2 = alpha - j*omega;
    ComputeUpsilon1 = lfmComputeUpsilonMatrix(gamma2,sigma2,t1, t2);
    ComputeUpsilon2 = lfmComputeUpsilonMatrix(gamma1,sigma2,t1, t2);
    if lfmKern.isNormalised
        K0 = (S/(j*4*sqrt(2)*m*omega));
    else
        K0 = sigma*sqrt(pi)*S/(j*4*m*omega);
    end
end

g1 = zeros(1,5);

% Gradient with respect to m, C and D

for ind = 1:3 % Parameter (m, D or C)
    switch ind
        case 1  % Gradient wrt m
            gradThetaM = 1;
            gradThetaAlpha = -C/(2*(m^2));
            gradThetaOmega = (C^2-2*m*D)/(2*(m^2)*sqrt(4*m*D-C^2));
        case 2  % Gradient wrt D
            gradThetaM = 0;
            gradThetaAlpha = 0;
            gradThetaOmega = 1/sqrt(4*m*D-C^2);
        case 3  % Gradient wrt C
            gradThetaM = 0;
            gradThetaAlpha = 1/(2*m);
            gradThetaOmega = -C/(2*m*sqrt(4*m*D-C^2));
    end

    % Gradient evaluation

    if isreal(omega)
        gamma = alpha + j*omega;
        gradThetaGamma = gradThetaAlpha + j*gradThetaOmega;
        matGrad = -K0*imag(lfmGradientUpsilonMatrix(gamma,sigma2,t1, t2)*gradThetaGamma ...
            - (gradThetaM/m + gradThetaOmega/omega) ...
            * ComputeUpsilon1);
    else
        gamma1 = alpha + j*omega;
        gamma2 = alpha - j*omega;
        gradThetaGamma1 = gradThetaAlpha + j*gradThetaOmega;
        gradThetaGamma2 = gradThetaAlpha - j*gradThetaOmega;
        matGrad = K0*(lfmGradientUpsilonMatrix(gamma2,sigma2, t1, t2)*gradThetaGamma2 ...
            -  lfmGradientUpsilonMatrix(gamma1,sigma2, t1, t2)*gradThetaGamma1 ...
            - (gradThetaM/lfmKern.mass + gradThetaOmega/omega) ...
            * (ComputeUpsilon1 - ComputeUpsilon2));

    end
    if subComponent
        if size(meanVector,1) ==1,
            matGrad = matGrad*meanVector;
        else
            matGrad = (meanVector*matGrad).';
        end
    end
    g1(ind) = sum(sum(matGrad.*covGrad));
end

% Gradient with respect to sigma

if isreal(omega)
    gamma = alpha + j*omega;
    if lfmKern.isNormalised
        matGrad = -K0*imag(lfmGradientSigmaUpsilonMatrix(gamma,sigma2,t1, t2));
    else
        matGrad = -(sqrt(pi)*S/(2*m*omega)) ...
            * imag(ComputeUpsilon1 ...
            + sigma*lfmGradientSigmaUpsilonMatrix(gamma,sigma2,t1, t2));
    end
else
    gamma1 = alpha + j*omega;
    gamma2 = alpha - j*omega;
    if lfmKern.isNormalised
        matGrad = K0*(lfmGradientSigmaUpsilonMatrix(gamma2,sigma2,t1,t2) ...
            - lfmGradientSigmaUpsilonMatrix(gamma1,sigma2,t1,t2));
    else
        matGrad = (sqrt(pi)*S/(j*4*m*omega)) ...
            *(ComputeUpsilon1 - ComputeUpsilon2 ...
            + sigma*(lfmGradientSigmaUpsilonMatrix(gamma2,sigma2,t1,t2) ...
            - lfmGradientSigmaUpsilonMatrix(gamma1,sigma2,t1,t2)));
    end
end;

if subComponent
    if size(meanVector,1) ==1,
        matGrad = matGrad*meanVector;
    else
        matGrad = (meanVector*matGrad).';
    end
end

g1(4) = sum(sum(matGrad.*covGrad))*(-(sigma^3)/4); % temporarly introduced by MA
g2(1) = g1(4);

% Gradient with respect to S

if isreal(omega)
    if lfmKern.isNormalised
        matGrad = -(1/(2*sqrt(2)*m*omega))* imag(ComputeUpsilon1);
    else
        matGrad = -(sqrt(pi)*sigma/(2*m*omega))* imag(ComputeUpsilon1);
    end
else
    if lfmKern.isNormalised
        matGrad = (1/(j*4*sqrt(2)*m*omega))*(ComputeUpsilon1 - ComputeUpsilon2);
    else
        matGrad = (sqrt(pi)*sigma/(j*4*m*omega))*(ComputeUpsilon1 - ComputeUpsilon2);
    end
end;

if subComponent
    if size(meanVector,1) ==1,
        matGrad = matGrad*meanVector;
    else
        matGrad = (meanVector*matGrad).';
    end
end

g1(5) = sum(sum(matGrad.*covGrad));
g1 = real(g1);
% Gradient with respect to the "variance" of the RBF
g2(1) = 0; % Otherwise is counted twice, temporarly changed by MA
g2(2) = 0;


function [dv, dvArr, flyBy1DVVect, flyBy2DVVect, flyBy1Rp, flyBy2Rp, ...
          xferOrbitP1, xferOrbitP2, xferOrbitP3, flyBy1OrbitIn, flyBy1OrbitOut, ...
          flyBy2OrbitIn, flyBy2OrbitOut] = ...
          findOptimalTwoFlyByTraj(x, departBodyInfo, flyby1BodyInfo, flyby2BodyInfo, arrivalBodyInfo, gmuXfr)
%findOptimalTwoFlyByTraj Summary of this function goes here
%   Detailed explanation goes here
    if(length(x)~=4)
        error('Length of x in findOptimalTwoFlyByTraj must be 4');
    end

    departDate = x(1);
    phase1TOF = x(2);
    phase2TOF = x(3);
    phase3TOF = x(4);

    xDates = [departDate,
              departDate + phase1TOF,
              departDate + phase1TOF + phase2TOF,
              departDate + phase1TOF + phase2TOF + phase3TOF];

    preFunc1 = @(arrivalUT, departUT) findOptimalDepartureArrivalObjFunc(arrivalUT, departUT, departBodyInfo, flyby1BodyInfo, gmuXfr, 'departDVRadioBtn');
    objFuncDepart = @(x) preFunc1(x(2), x(1));

    objFuncFlyby1DV = @(x) flybyDVObjFunc([xDates(1) xDates(2) xDates(3)], departBodyInfo, flyby1BodyInfo, flyby2BodyInfo, gmuXfr);
    objFuncFlyby2DV = @(x) flybyDVObjFunc([xDates(2) xDates(3) xDates(4)], flyby1BodyInfo, flyby2BodyInfo, arrivalBodyInfo, gmuXfr);

    preFunc2 = @(arrivalUT, departUT) findOptimalDepartureArrivalObjFunc(arrivalUT, departUT, flyby2BodyInfo, arrivalBodyInfo, gmuXfr, 'arrivalDVRadioBtn');
    objFuncArrival = @(x) preFunc2(x(4), x(3));

    dv1 = objFuncDepart(xDates);
    [dv2, flyBy1DVVect, flyBy1Rp, xferOrbitP1, xferOrbitP2, flyBy1OrbitIn, flyBy1OrbitOut] = objFuncFlyby1DV(xDates);
    [dv3, flyBy2DVVect, flyBy2Rp, xferOrbitP2, xferOrbitP3, flyBy2OrbitIn, flyBy2OrbitOut] = objFuncFlyby2DV(xDates);
    dv4 = objFuncArrival(xDates);
    dvArr = [dv1, dv2, dv3, dv4];

    dv = dv1 + dv2 + dv3 + dv4;
end


function [V,D]=gabmuleigs(K,c,p3,varargin)
%GABMULEIGS  Eigenpairs of Gabor multiplier
%   Usage:  h=gabmuleigs(K,c,g,a);
%           h=gabmuleigs(K,c,a);
%           h=gabmuleigs(K,c,ga,gs,a);
%
%   Input parameters:
%         K     : Number of eigenvectors to compute.
%         c     : symbol of Gabor multiplier
%         g     : analysis/synthesis window
%         ga    : analysis window
%         gs    : synthesis window
%         a     : Length of time shift.
%   Output parameters:
%         V     : Matrix containing eigenvectors.
%         D     : Eigenvalues.
%
%   `gabmuleigs` has been deprecated. Please use construct a frame multiplier
%   and use |framemuleigs| instead.
%
%   A call to `gabmuleigs(K,c,ga,gs,a)` can be replaced by ::
%
%     [Fa,Fs]=framepair('dgt',ga,gs,a,M);
%     [V,D]=framemuleigs(Fa,Fs,s,K);
%
%   Original help:
%   --------------
%
%   `gabmuleigs(K,c,g,a)` computes the *K* largest eigenvalues and eigen-
%   vectors of the Gabor multiplier with symbol *c* and time shift *a*.  The
%   number of channels is deduced from the size of the symbol *c*.  The
%   window *g* will be used for both analysis and synthesis.
%
%   `gabmuleigs(K,c,ga,gs,a)` does the same using the window the window *ga*
%   for analysis and *gs* for synthesis.
%
%   `gabmuleigs(K,c,a)` does the same using the a tight Gaussian window of
%   for analysis and synthesis.
%
%   If *K* is empty, then all eigenvalues/pairs will be returned.
%
%   `gabmuleigs` takes the following parameters at the end of the line of input
%   arguments:
%
%     'tol',t      Stop if relative residual error is less than the
%                  specified tolerance. Default is 1e-9
%
%     'maxit',n    Do at most n iterations.
%
%     'iter'       Call `eigs` to use an iterative algorithm.
%
%     'full'       Call `eig` to sole the full problem.
%
%     'auto'       Use the full method for small problems and the
%                  iterative method for larger problems. This is the
%                  default.
%
%     'crossover',c
%                  Set the problem size for which the 'auto' method
%                  switches. Default is 200.
%
%     'print'      Display the progress.
%
%     'quiet'      Don't print anything, this is the default.
%
%   See also: gabmul, dgt, idgt, gabdual, gabtight

warning(['LTFAT: GABMULEIGS has been deprecated, please use FRAMEMULEIGS ' ...
         'instead. See the help on FRAMEMULEIGS for more details.']);

% Change this to 1 or 2 to see the iterative method in action.
printopts=0;

if nargin<3
  error('%s: Too few input parameters.',upper(mfilename));
end;

if nargout==2
  doV=1;
else
  doV=0;
end;

M=size(c,1);
N=size(c,2);

istight=1;
if numel(p3)==1
  % Usage: h=gabmuleigs(c,K,a);
  a=p3;
  L=N*a;
  ga=gabtight(a,M,L);
  gs=ga;
  arglist=varargin;
else
  if numel(varargin{1})==1
    % Usage: h=gabmuleigs(c,K,g,a);
    ga=p3;
    gs=p3;
    a=varargin{1};
    L=N*a;
    arglist=varargin(2:end);
  else
    if numel(varargin{2})==1
      % Usage: h=gabmuleigs(c,K,ga,gs,a);
      ga=p3;
      gs=varargin{1};
      a =varargin{2};
      L=N*a;
      istight=0;
      arglist=varargin(3:end);
    end;
  end;
end;

definput.keyvals.maxit=100;
definput.keyvals.tol=1e-9;
definput.keyvals.crossover=200;
definput.flags.print={'quiet','print'};
definput.flags.method={'auto','iter','full'};

[flags,kv]=ltfatarghelper({},definput,arglist);

% Do the computation. For small problems a direct calculation is just as
% fast.

if (flags.do_iter) || (flags.do_auto && L>kv.crossover)

  if flags.do_print
    opts.disp=1;
  else
    opts.disp=0;
  end;
  opts.isreal = false;
  opts.maxit  = kv.maxit;
  opts.tol    = kv.tol;

  % Setup afun
  afun(1,c,ga,gs,a,M,L);

  if doV
    [V,D] = eigs(@afun,L,K,'LM',opts);
  else
    D     = eigs(@afun,L,K,'LM',opts);
  end;

else
  % Compute the transform matrix.
  bigM=tfmat('gabmul',c,ga,gs,a);

  if doV
    [V,D]=eig(bigM);
  else
    D=eig(bigM);
  end;

end;

% The output from eig and eigs is a diagonal matrix, so we must extract the
% diagonal.
D=diag(D);

% Sort them in descending order
[~,idx]=sort(abs(D),1,'descend');
D=D(idx(1:K));

if doV
  V=V(:,idx(1:K));
end;

% Clean the eigenvalues, if we know that they are real-valued
%if isreal(ga) && isreal(gs) && isreal(c)
%  D=real(D);
%end;

% The function has been written in this way, because Octave (at the time
% of writing) does not accept additional parameters at the end of the
% line of input arguments for eigs
function y=afun(x,c_in,ga_in,gs_in,a_in,M_in,L_in)
  persistent c;
  persistent ga;
  persistent gs;
  persistent a;
  persistent M;
  persistent L;

  if nargin>1
    c  = c_in;
    ga = ga_in;
    gs = gs_in;
    a  = a_in;
    M  = M_in;
    L  = L_in;
  else
    y=comp_idgt(c.*comp_dgt(x,ga,a,M,[0 1],0,0,0),gs,a,[0 1],0,0);
  end;


function exampleFilter
% function exampleFilter
%
% Example of how the Kalman filter performs for real data

load raw

%Apply filter
k=Kalman_Stack_Filter(raw);
k75=Kalman_Stack_Filter(raw,0.75);

%Set up image window
minMax=[min(raw(:)),max(raw(:))];
clf, colormap gray

subplot(1,3,1)
imagesc(raw(:,:,1))
title('original')
set(gca,'clim',minMax), axis off equal

subplot(1,3,2)
imagesc(k(:,:,1))
title('filtered, gain=0.5')
set(gca,'clim',minMax), axis off equal

subplot(1,3,3)
imagesc(k75(:,:,1))
title('filtered, gain=0.75')
set(gca,'clim',minMax), axis off equal

%Loop movie
disp('crtl-c to stop movie')
while 1
    for i=1:size(k,3)

        subplot(1,3,1)
        set(get(gca,'children'),'CData',raw(:,:,i))

        subplot(1,3,2)
        set(get(gca,'children'),'CData',k(:,:,i))

        subplot(1,3,3)
        set(get(gca,'children'),'CData',k75(:,:,i))

        pause(0.05)
        drawnow
    end
end


function generateOpenfieldPB(fieldSizeX, fieldSizeY, beamletSize);

xP = -(fieldSizeX-beamletSize)/2 : beamletSize : (fieldSizeX-beamletSize)/2;
yP = -(fieldSizeY-beamletSize)/2 : beamletSize : (fieldSizeY-beamletSize)/2;
[xPosV, yPosV] = meshgrid(xP, yP);
xPosV = xPosV(:);
yPosV = yPosV(:);
w_field = ones(size(xPosV));
beamlet_delta_x = beamletSize*ones(size(xPosV));
beamlet_delta_y = beamlet_delta_x;

filename = ['PB_', num2str(fieldSizeX), 'x', num2str(fieldSizeY), '_PBsize', num2str(beamletSize), 'cm.mat']
disp ('save PB info to above filename.')
save (filename, 'xPosV', 'yPosV', 'beamlet_delta_x', 'beamlet_delta_y', 'w_field');

% Display generated PB information.
figure;hAxis2 = axes;hold on;
%axis(hAxis2, 'manual');
           %     w_colors = floor((w_field ./ max(w_field))*255)+1;
                %set(gcf, 'doublebuffer', 'on');
                for i=1:length(xPosV)
                    patch([xPosV(i) - beamlet_delta_x(i)/2 xPosV(i) - beamlet_delta_x(i)/2 xPosV(i) + beamlet_delta_x(i)/2 xPosV(i) + beamlet_delta_x(i)/2 xPosV(i) - beamlet_delta_x(i)/2], [yPosV(i) - beamlet_delta_y(i)/2 yPosV(i) + beamlet_delta_y(i)/2 yPosV(i) + beamlet_delta_y(i)/2 yPosV(i) - beamlet_delta_y(i)/2 yPosV(i) - beamlet_delta_y(i)/2], w_field(i));
                end
            %    axis([hAxis1 hAxis2], 'ij');
            %    kids = get(hAxis2, 'children');
            %     set(kids, 'edgecolor', 'none');
            %    cMap = colormap('jet');
            %   set(hAxis2, 'color', cMap(1,:));

end


function [x,f,funEvals] = minConF_BC(funObj,x,LB,UB,options)
% function [x,f] = minConF_BC(funObj,x,LB,UB,options)
%
% Function for using Two-Metric Projection to solve problems of the form:
%   min funObj(x)
%   s.t. LB_i <= x_i <= UB_i
%
%   @funObj(x): function to minimize (returns gradient as second argument)
%
%   options:
%       verbose: level of verbosity (0: no output, 1: final, 2: iter (default), 3:
%       debug)
%       optTol: tolerance used to check for progress (default: 1e-7)
%       maxIter: maximum number of calls to funObj (default: 250)
%       numDiff: compute derivatives numerically (0: use user-supplied
%       derivatives (default), 1: use finite differences, 2: use complex
%       differentials)
%       method: 'sd', 'lbfgs', 'newton'

nVars = length(x);

% Set Parameters
if nargin < 5
    options = [];
end
[verbose,numDiff,optTol,maxIter,suffDec,interp,method,corrections,damped] = ...
    myProcessOptions(...
    options,'verbose',3,'numDiff',0,'optTol',1e-6,'maxIter',500,'suffDec',1e-4,...
    'interp',1,'method','lbfgs','corrections',100,'damped',0);

% Output Log
if verbose >= 3
    fprintf('%10s %10s %15s %15s %15s\n','Iteration','FunEvals','Step Length','Function Val','Opt Cond');
end

% Make objective function (if using numerical derivatives)
funEvalMultiplier = 1;
if numDiff
    if numDiff == 2
        useComplex = 1;
    else
        useComplex = 0;
    end
    funObj = @(x)autoGrad(x,useComplex,funObj);
    funEvalMultiplier = nVars+1-useComplex;
end

% Evaluate Initial Point
x = projectBounds(x,LB,UB);
if strcmp(method,'newton')
    [f,g,H] = funObj(x);
    secondOrder = 1;
else
    [f,g] = funObj(x);
    secondOrder = 0;
end
funEvals = 1;

% Compute Working Set
working = ones(nVars,1);
working((x < LB+optTol*2) & g >= 0) = 0;
working((x > UB-optTol*2) & g <= 0) = 0;
working = find(working);

% Check Optimality
if isempty(working)
    if verbose >= 1
        fprintf('All variables are at their bound and no further progress is possible at initial point\n');
    end
    return;
elseif norm(g(working)) <= optTol
    if verbose >=1
        fprintf('All working variables satisfy optimality condition at initial point\n');
    end
    return;
end

if verbose >= 3
    switch method
        case 'sd'
            fprintf('Steepest Descent\n');
        case 'lbfgs'
            fprintf('L-BFGS\n');
        case 'bfgs'
            fprintf('BFGS\n');
        case 'newton'
            fprintf('Newton\n');
    end
end

i = 1;
while funEvals <= maxIter

    % Compute Step Direction
    d = zeros(nVars,1);
    switch(method)
        case 'sd'
            d(working) = -g(working);
        case 'lbfgs'
            if i == 1
                d(working) = -g(working);
                old_dirs = zeros(nVars,0);
                old_stps = zeros(nVars,0);
                Hdiag = 1;
            else
                if damped
                    [old_dirs,old_stps,Hdiag] = dampedUpdate(g-g_old,x-x_old,corrections,verbose==3,old_dirs,old_stps,Hdiag);
                else
                    [old_dirs,old_stps,Hdiag] = lbfgsUpdate(g-g_old,x-x_old,corrections,verbose==3,old_dirs,old_stps,Hdiag);
                end
                    curvSat = sum(old_dirs(working,:).*old_stps(working,:)) > 1e-10;
               d(working) = lbfgs(-g(working),old_dirs(working,curvSat),old_stps(working,curvSat),Hdiag);
            end
            g_old = g;
            x_old = x;
        case 'bfgs'
            if i == 1
                d(working) = -g(working);
                B = eye(nVars);
            else
                y = g-g_old;
                s = x-x_old;

                ys = y'*s;

                if i == 2
                    if ys > 1e-10
                        B = ((y'*y)/(y'*s))*eye(nVars);
                    end
                end
                if ys > 1e-10
                    B = B + (y*y')/(y'*s) - (B*s*s'*B)/(s'*B*s);
                else
                    if verbose == 2
                        fprintf('Skipping Update\n');
                    end
                end
                d(working) = -B(working,working)\g(working);
            end
            g_old = g;
            x_old = x;

        case 'newton'
            [R,posDef] = chol(H(working,working));

            if posDef == 0
                d(working) = -R\(R'\g(working));
            else
                if verbose == 3
                    fprintf('Adjusting Hessian\n');
                end
                H(working,working) = H(working,working) + eye(length(working)) * max(0,1e-12 - min(real(eig(H(working,working)))));
                d(working) = -H(working,working)\g(working);
            end
        otherwise
            fprintf('Unrecognized Method: %s\n',method);
            break;
    end

    % Check that Progress can be made along the direction
    f_old = f;
    gtd = g'*d;
    if gtd > -optTol
        if verbose >= 2
            fprintf('Directional Derivative below optTol\n');
        end
        break;
    end

    % Select Initial Guess to step length
    if i == 1 && ~secondOrder
        t = min(1,1/sum(abs(g(working))));
    else
        t = 1;
    end

    % Evaluate the Objective and Projected Gradient at the Initial Step
    x_new = projectBounds(x+t*d,LB,UB);
    if secondOrder
        [f_new,g_new,H] = funObj(x_new);
    else
        [f_new,g_new] = funObj(x_new);
    end
    funEvals = funEvals+1;

    % Backtracking Line Search
    lineSearchIters = 1;
    while f_new > f + suffDec*g'*(x_new-x) || ~isLegal(f_new)
        temp = t;
        if interp == 0 || ~isLegal(f_new) || ~isLegal(g_new)
            if verbose == 3
                fprintf('Halving Step Size\n');
            end
            t = .5*t;
        else
            if verbose == 3
                fprintf('Cubic Backtracking\n');
            end
            t = polyinterp([0 f gtd; t f_new g_new'*d]);
        end

        % Adjust if change is too small
        if t < temp*1e-3
            if verbose == 3
                fprintf('Interpolated value too small, Adjusting\n');
            end
            t = temp*1e-3;
        elseif t > temp*0.6
            if verbose == 3
                fprintf('Interpolated value too large, Adjusting\n');
            end
            t = temp*0.6;
        end

        % Check whether step has become too small
        if sum(abs(t*d)) < optTol
            if verbose == 3
                fprintf('Line Search failed\n');
            end
            t = 0;
            f_new = f;
            g_new = g;
            break;
        end

        % Evaluate New Point
        x_new = projectBounds(x+t*d,LB,UB);
        [f_new,g_new] = funObj(x_new);
        funEvals = funEvals+1;
        lineSearchIters = lineSearchIters+1;

    end

    % Take Step
    x = x_new;
    f = f_new;
    g = g_new;

    % Compute Working Set
    working = ones(nVars,1);
    working((x < LB+optTol*2) & g >= 0) = 0;
    working((x > UB-optTol*2) & g <= 0) = 0;
    working = find(working);

    % Output Log
    if verbose >= 2
        fprintf('%10d %10d %15.5e %15.5e %15.5e\n',i,funEvals*funEvalMultiplier,t,f,sum(abs(g(working))));
    end

    % Check Optimality
    if isempty(working)
        if verbose >= 1
            fprintf('All variables are at their bound and no further progress is possible\n');
        end
        break;
    elseif norm(g(working)) <= optTol
        if verbose >=1
            fprintf('All working variables satisfy optimality condition\n');
        end
        break;
    end

    % Check for lack of progress
    if sum(abs(t*d)) < optTol
        if verbose >= 1
            fprintf('Step size below optTol\n');
        end
        break;
    end

    if abs(f-f_old) < optTol
        if verbose >= 1
            fprintf('Function value changing by less than optTol\n');
        end
        break;
    end

    if funEvals*funEvalMultiplier > maxIter
        if verbose >= 1
            fprintf('Function Evaluations exceeds maxIter\n');
        end
        break;
    end

    % If necessary, compute Hessian
    if secondOrder && lineSearchIters > 1
        [f_new,g_new,H] = funObj(x);
    end

    i = i + 1;
end
end

function [x] = projectBounds(x,LB,UB)
x(x < LB) = LB(x < LB);
x(x > UB) = UB(x > UB);
end


function colorspace_demo(Cmd)
% Demo for colorspace.m - 3D visualizations of various color spaces

% Pascal Getreuer 2006

if nargin == 0
   % Create a figure with a drop-down menu
   figure('Color',[1,1,1]);
   h = uicontrol('Style','popup','Position',[15,10,90,21],...
      'BackgroundColor',[1,1,1],'Value',2,...
      'string',{'sRGB','Y''PbPr','HSV','HSL','L*a*b*','L*u*v*','L*ch'});
   set(h,'Callback',sprintf('%s(get(%.20g,''Value''))',mfilename,h));
   Cmd = 2;
end

% Plot a figure
switch Cmd
case 1
   [x,y,z,Tri] = makeshape('Cube');
   CData = [x,y,z];
   myplot((x-0.5)*0.8,(y-0.5)*0.8,(z)*0.8,Tri,CData);
   coloraxis('R''',5,0.5*0.8);
   coloraxis('G''',6,0.5*0.8);
   coloraxis('B''',3);
case 2
   [x,y,z,Tri] = makeshape('Cylinder');
   CData = colorspace('rgb<-ypbpr',[z,x/2,y/2]);
   myplot(x,y,0.8*z,Tri,CData);
   coloraxis('Y''',3);
   coloraxis('P_b',5,0.8);
   coloraxis('P_r',6,0.8);
case 3
   [x,y,z,Tri] = makeshape('Hexacone');
   CData = colorspace('rgb<-hsv',[(pi+atan2(-y,-x))*180/pi,sqrt(x.^2+y.^2),z]);
   myplot(x,y,z,Tri,CData);
   coloraxis('H',1);
   coloraxis('S',2);
   coloraxis('V',3);
case 4
   [x,y,z,Tri] = makeshape('Double Hexacone');
   CData = colorspace('rgb<-hsl',[(pi+atan2(-y,-x))*180/pi,sqrt(x.^2+y.^2),z]);
   myplot(x,y,2*z,Tri,CData);
   coloraxis('H',1);
   coloraxis('S',2);
   coloraxis('L',4);
case 5
   [x,y,z,Tri] = makeshape('Blobs');
   CData = colorspace('rgb<-lab',[z*100,x*100,y*100]);
   myplot(x,y,2*z,Tri,CData);
   coloraxis('L*',4);
   coloraxis('a*',5,2);
   coloraxis('b*',6,2);
case 6
   [x,y,z,Tri] = makeshape('Blobs');
   CData = colorspace('rgb<-luv',[z*100,x*125,y*125]);
   myplot(x,y,2*z,Tri,CData);
   coloraxis('L*',4);
   coloraxis('u*',5,2);
   coloraxis('v*',6,2);
case 7
   [x,y,z,Tri] = makeshape('Blobs');
   CData = colorspace('rgb<-lab',[z*100,x*100,y*100]);
   myplot(x,y,2*z,Tri,CData);
   coloraxis('L*',4);
   coloraxis('c',2);
   coloraxis('h',1);
end

axis equal;
axis off;
pbaspect([1,1,1]);
view(70,27);
rotate3d on;

return;

function myplot(x,y,z,Tri,CData)
% Plot a triangular mesh with color data
cla;
CData = min(max(CData,0),1);
patch('Faces',Tri,'Vertices',[x,y,z],'FaceVertexCData',CData,...
   'FaceColor','interp','EdgeColor','none');
hold on;
return;

function coloraxis(Name,Type,h)
% Draw color axes as 3D objects
FontSize = 14;

switch Type
case 1
   set(text(-0.25,-1.3,1.1,Name),'FontWeight','bold',...
      'FontSize',FontSize,'Color',[0,0,0]);
   t = linspace(0,pi*3/2,60);
   x = cos(t)*1.1;
   y = sin(t)*1.1;
   set(plot3(x,y,zeros(size(x)),'k-'),'LineWidth',2.5,'Color',[1,1,1]*0.8);
   x = [x,-0.1,0,-0.1];
   y = [y,-1.05,-1.1,-1.15];
   set(plot3(x,y,ones(size(x)),'k-'),'LineWidth',2.5);
case 2
   set(text(0,-0.6,0.15,Name),'FontWeight','bold','FontSize',FontSize,'Color',[0,0,0]);
   set(plot3([-0.05,0,0.05,0,0],[-0.9,-1,-0.9,-1,0],[0,0,0,0,0],'k-'),'LineWidth',2.5);
case 3
   set(text(0,0.15,1.3,Name),'FontWeight','bold','FontSize',FontSize,'Color',[0,0,0]);
   set(plot3([0,0,0,0,0],[-0.05,0,0.05,0,0],[1.6,1.7,1.6,1.7,0],'k-'),'LineWidth',2.5);
case 4
   set(text(0,0.15,2.4,Name),'FontWeight','bold','FontSize',FontSize,'Color',[0,0,0]);
   set(plot3([0,0,0,0,0],[-0.05,0,0.05,0,0],[2.4,2.5,2.4,2.5,0],'k-'),'LineWidth',2.5);
case 5
   set(text(0.6,0,h+0.15,Name),'FontWeight','bold','FontSize',FontSize,'Color',[0,0,0]);
   set(plot3([0.9,1,0.9,1,-1,-0.9,-1,-0.9],[-0.05,0,0.05,0,0,-0.05,0,0.05],...
      zeros(1,8)+h,'k-'),'LineWidth',2.5);
case 6
   set(text(0,0.6,h+0.15,Name),'FontWeight','bold','FontSize',FontSize,'Color',[0,0,0]);
   set(plot3([-0.05,0,0.05,0,0,-0.05,0,0.05],[0.9,1,0.9,1,-1,-0.9,-1,-0.9],...
      zeros(1,8)+h,'k-'),'LineWidth',2.5);
end
return;

function [x,y,z,Tri] = makeshape(Shape)
% Make a 3D shape: Shape = 'Cube', 'Cylinder', 'Cone', 'Double Cone', 'Blobs'

% Cube
N = 12;    % Vertices per edge
% Cylinder, Cone, Double Cone
Nth = 25;  % Vertices over angles, (Nth - 1) should be a multiple of 12
Nr = 4;    % Vertices over radius
Nz = 8;    % Vertices over z-dimension

switch lower(Shape)
case 'cube'
   [u,v] = meshgrid(linspace(0,1,N),linspace(0,1,N));
   u = u(:);
   v = v(:);
   x = [u;u;u;u;zeros(N^2,1);ones(N^2,1)];
   y = [v;v;zeros(N^2,1);ones(N^2,1);v;v];
   z = [zeros(N^2,1);ones(N^2,1);v;v;u;u];
   Tri = trigrid(N,N);
   Tri = [Tri;N^2+Tri;2*N^2+Tri;3*N^2+Tri;4*N^2+Tri;5*N^2+Tri];
case 'cylinder'
   Nth = ceil(Nth*0.75);
   [u,v] = meshgrid(linspace(0,pi*3/2,Nth),linspace(0,1,Nz));
   Tri = trigrid(Nth,Nz);
   x = cos(u(:));
   y = sin(u(:));
   z = v(:);
   [u,v] = meshgrid(linspace(0,pi*3/2,Nth),linspace(0,1,Nr));
   Tri = [Tri;Nth*Nz+trigrid(Nth,Nr);Nth*Nz+Nth*Nr+trigrid(Nth,Nr)];
   x = [x;v(:).*cos(u(:));v(:).*cos(u(:))];
   y = [y;v(:).*sin(u(:));v(:).*sin(u(:))];
   z = [z;zeros(Nth*Nr,1);ones(Nth*Nr,1)];
   [u,v] = meshgrid(linspace(0,1,Nr),linspace(0,1,Nz));
   Tri = [Tri;Nth*Nz+2*Nth*Nr+trigrid(Nr,Nz);Nth*Nz+2*Nth*Nr+Nr*Nz+trigrid(Nr,Nz)];
   x = [x;u(:);zeros(Nr*Nz,1)];
   y = [y;zeros(Nr*Nz,1);-u(:)];
   z = [z;v(:);v(:)];
case 'cone'
   [u,v] = meshgrid(linspace(0,2*pi,Nth),linspace(0,1,Nz));
   Tri = trigrid(Nth,Nz);
   x = v(:).*cos(u(:));
   y = v(:).*sin(u(:));
   z = v(:);
   [u,v] = meshgrid(linspace(0,2*pi,Nth),linspace(0,1,Nr));
   Tri = [Tri;Nth*Nz+trigrid(Nth,Nr);];
   x = [x;v(:).*cos(u(:));];
   y = [y;v(:).*sin(u(:));];
   z = [z;ones(Nth*Nr,1)];
case 'double cone'
   Nz = floor(Nz/2)*2+1;
   [u,v] = meshgrid(linspace(0,2*pi,Nth),linspace(0,1,Nz));
   Tri = trigrid(Nth,Nz);
   r = 1 - abs(2*v(:) - 1);
   x = r.*cos(u(:));
   y = r.*sin(u(:));
   z = v(:);
case 'hexacone'
   [u,v] = meshgrid(linspace(0,2*pi,Nth),linspace(0,1,Nz));
   Tri = trigrid(Nth,Nz);
   r = 0.92./max(max(abs(cos(u)),abs(cos(u - pi/3))),abs(cos(u + pi/3)));
   x = v(:).*cos(u(:)).*r(:);
   y = v(:).*sin(u(:)).*r(:);
   z = v(:);
   [u,v] = meshgrid(linspace(0,2*pi,Nth),linspace(0,1,Nr));
   Tri = [Tri;Nth*Nz+trigrid(Nth,Nr);];
   v = 0.92*v./max(max(abs(cos(u)),abs(cos(u - pi/3))),abs(cos(u + pi/3)));
   x = [x;v(:).*cos(u(:));];
   y = [y;v(:).*sin(u(:));];
   z = [z;ones(Nth*Nr,1)];
case 'double hexacone'
   Nz = floor(Nz/2)*2+1;
   [u,v] = meshgrid(linspace(0,2*pi,Nth),linspace(0,1,Nz));
   Tri = trigrid(Nth,Nz);
   r = 1 - abs(2*v - 1);
   r = 0.92*r./max(max(abs(cos(u)),abs(cos(u - pi/3))),abs(cos(u + pi/3)));
   x = r(:).*cos(u(:));
   y = r(:).*sin(u(:));
   z = v(:);
case 'blobs'
   Nz = 47;
   [u,v] = meshgrid(linspace(0,2*pi,Nth),linspace(0,1,Nz));
   Tri = trigrid(Nth,Nz);
   r = sin(v(:)*pi*3).^2.*(1 - 0.6*abs(2*v(:) - 1));
   x = r.*cos(u(:));
   y = r.*sin(u(:));
   z = v(:);
end
return;

function Tri = trigrid(Nu,Nv)
% Construct the connectivity data for a grid of triangular patches
i = (1:(Nu-1)*Nv-1).';
i(Nv:Nv:end) = [];
Tri = [i,i+1,i+Nv;i+1,i+Nv+1,i+Nv];
return;


function [Ep,Cp,Eh,F,L,dFdp,dFdpp] = spm_nlsi_GN(M,U,Y)
% Bayesian inversion of nonlinear models - Gauss-Newton/Variational Laplace
% FORMAT [Ep,Cp,Eh,F] = spm_nlsi_GN(M,U,Y)
%
% [Dynamic] MIMO models
%__________________________________________________________________________
%
% M.IS - function name f(P,M,U) - generative model
%        This function specifies the nonlinear model:
%          y = Y.y = IS(P,M,U) + X0*P0 + e
%        where e ~ N(0,C). For dynamic systems this would be an integration
%        scheme (e.g. spm_int). spm_int expects the following:
%
%     M.f  - f(x,u,P,M)
%     M.g  - g(x,u,P,M)
%     M.h  - h(x,u,P,M)
%       x  - state variables
%       u  - inputs or causes
%       P  - free parameters
%       M  - fixed functional forms and parameters in M
%
% M.FS - function name f(y,M)   - feature selection
%        This [optional] function performs feature selection assuming the
%        generalized model y = FS(y,M) = FS(IS(P,M,U),M) + X0*P0 + e
%
% M.P  - starting estimates for model parameters [optional]
%
% M.pE - prior expectation      - E{P}   of model parameters
% M.pC - prior covariance       - Cov{P} of model parameters
%
% M.hE - prior expectation      - E{h}   of log-precision parameters
% M.hC - prior covariance       - Cov{h} of log-precision parameters
%
% U.u  - inputs (or just U)
% U.dt - sampling interval
%
% Y.y  - outputs (samples x observations x ...)
% Y.dt - sampling interval for outputs
% Y.X0 - confounds or null space      (over size(y,1) samples or all vec(y))
% Y.Q  - q error precision components (over size(y,1) samples or all vec(y))
%
%
% Parameter estimates
%--------------------------------------------------------------------------
% Ep  - (p x 1)         conditional expectation    E{P|y}
% Cp  - (p x p)         conditional covariance     Cov{P|y}
% Eh  - (q x 1)         conditional log-precisions E{h|y}
%
% log evidence
%--------------------------------------------------------------------------
% F   - [-ve] free energy F = log evidence = p(y|f,g,pE,pC) = p(y|m)
%
%__________________________________________________________________________
% Returns the moments of the posterior p.d.f. of the parameters of a
% nonlinear model specified by IS(P,M,U) under Gaussian assumptions.
% Usually, IS is an integrator of a dynamic MIMO input-state-output model
%
%              dx/dt = f(x,u,P)
%              y     = g(x,u,P)  + X0*P0 + e
%
% A static nonlinear observation model with fixed input or causes u
% obtains when x = []. i.e.
%
%              y     = g([],u,P) + X0*P0e + e
%
% but static nonlinear models are specified more simply using
%
%              y     = IS(P,M,U) + X0*P0 + e
%
% Priors on the free parameters P are specified in terms of expectation pE
% and covariance pC. The E-Step uses a Fisher-Scoring scheme and a Laplace
% approximation to estimate the conditional expectation and covariance of P
% If the free-energy starts to increase,  an abbreviated descent is
% invoked.  The M-Step estimates the precision components of e, in terms
% of log-precisions.  Although these two steps can be thought of in
% terms of E and N steps they are in fact variational steps of a full
% variational Laplace scheme that accommodates conditional uncertainty
% over both parameters and log precisions (c.f. hyperparameters with hyper
% priors)
%
% An optional feature selection can be specified with parameters M.FS.
%
% For generic aspects of the scheme see:
%
% Friston K, Mattout J, Trujillo-Barreto N, Ashburner J, Penny W.
% Variational free energy and the Laplace approximation.
% NeuroImage. 2007 Jan 1;34(1):220-34.
%
% This scheme handels complex data along the lines originally described in:
%
% Sehpard RJ, Lordan BP, and Grant EH.
% Least squares analysis of complex data with applications to permittivity
% measurements.
% J. Phys. D. Appl. Phys 1970 3:1759-1764.
%
%__________________________________________________________________________
% Copyright (C) 2001-2015 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_nlsi_GN.m 7279 2018-03-10 21:22:44Z karl $

% options
%--------------------------------------------------------------------------
try, M.nograph; catch, M.nograph = 0;   end
try, M.noprint; catch, M.noprint = 0;   end
try, M.Nmax;    catch, M.Nmax    = 128; end

% figure (unless disabled)
%--------------------------------------------------------------------------
if ~M.nograph
    Fsi = spm_figure('GetWin','SI');
end

% check integrator
%--------------------------------------------------------------------------
try
    M.IS;
catch
    M.IS = 'spm_int';
end

% composition of feature selection and prediction (usually an integrator)
%--------------------------------------------------------------------------
try
    y  = Y.y;
catch
    y  = Y;
end

try

    % try FS(y,M)
    %----------------------------------------------------------------------
    try
        y  = feval(M.FS,y,M);
        IS = inline([M.FS '(' M.IS '(P,M,U),M)'],'P','M','U');

        % try FS(y)
        %------------------------------------------------------------------
    catch
        y  = feval(M.FS,y);
        IS = inline([M.FS '(' M.IS '(P,M,U))'],'P','M','U');
    end

catch

    % otherwise FS(y) = y
    %----------------------------------------------------------------------
    try
        IS = inline([M.IS '(P,M,U)'],'P','M','U');
    catch
        IS = M.IS;
    end
end

% converted to function handle
%--------------------------------------------------------------------------
IS  = spm_funcheck(IS);

% paramter update eqation
%--------------------------------------------------------------------------
if isfield(M,'f'), M.f = spm_funcheck(M.f);  end
if isfield(M,'g'), M.g = spm_funcheck(M.g);  end
if isfield(M,'h'), M.h = spm_funcheck(M.h);  end

% size of data (samples x response component x response component ...)
%--------------------------------------------------------------------------
if iscell(y)
    ns = size(y{1},1);
else
    ns = size(y,1);
end
ny   = length(spm_vec(y));          % total number of response variables
nr   = ny/ns;                       % number response components
M.ns = ns;                          % number of samples M.ns

% initial states
%--------------------------------------------------------------------------
try
    M.x;
catch
    if ~isfield(M,'n'), M.n = 0;    end
    M.x = sparse(M.n,1);
end

% input
%--------------------------------------------------------------------------
try
    U;
catch
    U = [];
end

% initial parameters
%--------------------------------------------------------------------------
try
    spm_vec(M.P) - spm_vec(M.pE);
    fprintf('\nParameter initialisation successful\n')
catch
    M.P = M.pE;
end

% time-step
%--------------------------------------------------------------------------
try
    dt = Y.dt;
catch
    dt = 1;
end

% precision components Q
%--------------------------------------------------------------------------
try
    Q = Y.Q;
    if isnumeric(Q), Q = {Q}; end
catch
    Q = spm_Ce(ns*ones(1,nr));
end
nh    = length(Q);                  % number of precision components
nq    = ny/length(Q{1});            % for compact Kronecker form of M-step

% prior moments (assume uninformative priors if not specifed)
%--------------------------------------------------------------------------
pE       = M.pE;
try
    pC   = M.pC;
catch
    np   = spm_length(M.pE);
    pC   = speye(np,np)*exp(16);
end

% confounds (if specified)
%--------------------------------------------------------------------------
try
    nb   = size(Y.X0,1);            % number of bins
    nx   = ny/nb;                   % number of blocks
    dfdu = kron(speye(nx,nx),Y.X0);
catch
    dfdu = sparse(ny,0);
end
if isempty(dfdu), dfdu = sparse(ny,0); end

% hyperpriors - expectation (and initialize hyperparameters)
%--------------------------------------------------------------------------
try
    hE  = M.hE;
    if length(hE) ~= nh
        hE = hE + sparse(nh,1);
    end
catch
    hE  = sparse(nh,1) - log(var(spm_vec(y))) + 4;
end
h       = hE;

% hyperpriors - covariance
%--------------------------------------------------------------------------
try
    ihC = spm_inv(M.hC);
    if length(ihC) ~= nh
        ihC = ihC*speye(nh,nh);
    end
catch
    ihC = speye(nh,nh)*exp(4);
end

% unpack covariance
%--------------------------------------------------------------------------
if isstruct(pC);
    pC = spm_diag(spm_vec(pC));
end

% dimension reduction of parameter space
%--------------------------------------------------------------------------
V     = spm_svd(pC,0);
nu    = size(dfdu,2);                 % number of parameters (confounds)
np    = size(V,2);                    % number of parameters (effective)
ip    = (1:np)';
iu    = (1:nu)' + np;

% second-order moments (in reduced space)
%--------------------------------------------------------------------------
pC    = V'*pC*V;
uC    = speye(nu,nu)/1e-8;
ipC   = inv(spm_cat(spm_diag({pC,uC})));

% initialize conditional density
%--------------------------------------------------------------------------
Eu    = spm_pinv(dfdu)*spm_vec(y);
p     = [V'*(spm_vec(M.P) - spm_vec(M.pE)); Eu];
Ep    = spm_unvec(spm_vec(pE) + V*p(ip),pE);

% EM
%==========================================================================
criterion = [0 0 0 0];

C.F   = -Inf;                                   % free energy
v     = -4;                                     % log ascent rate
dFdh  = zeros(nh,1);
dFdhh = zeros(nh,nh);
for k = 1:M.Nmax

    % time
    %----------------------------------------------------------------------
    tStart = tic;

    % E-Step: prediction f, and gradients; dfdp
    %======================================================================
    try

        % gradients
        %------------------------------------------------------------------
        [dfdp,f] = spm_diff(IS,Ep,M,U,1,{V});
        dfdp     = reshape(spm_vec(dfdp),ny,np);

        % check for stability
        %------------------------------------------------------------------
        normdfdp = norm(dfdp,'inf');
        revert   = isnan(normdfdp) || normdfdp > exp(32);

    catch
        revert   = true;
    end

    if revert && k > 1
        for i = 1:4

            % reset expansion point and increase regularization
            %--------------------------------------------------------------
            v        = min(v - 2,-4);

            % E-Step: update
            %--------------------------------------------------------------
            p        = C.p + spm_dx(dFdpp,dFdp,{v});
            Ep       = spm_unvec(spm_vec(pE) + V*p(ip),pE);

            % try again
            %--------------------------------------------------------------
            try
                [dfdp,f] = spm_diff(IS,Ep,M,U,1,{V});
                dfdp     = reshape(spm_vec(dfdp),ny,np);

                % check for stability
                %----------------------------------------------------------
                normdfdp = norm(dfdp,'inf');
                revert   = isnan(normdfdp) || normdfdp > exp(32);

            catch
                revert   = true;
            end

            % break
            %--------------------------------------------------------------
            if ~revert, break, end

        end
    end

    % convergence failure
    %----------------------------------------------------------------------
    if revert
        error('SPM:spm_nlsi_GN','Convergence failure.');
    end

    % prediction error and full gradients
    %----------------------------------------------------------------------
    e     =  spm_vec(y) - spm_vec(f) - dfdu*p(iu);
    J     = -[dfdp dfdu];

    % M-step: Fisher scoring scheme to find h = max{F(p,h)}
    %======================================================================
    for m = 1:8

        % precision and conditional covariance
        %------------------------------------------------------------------
        iS    = sparse(0);
        for i = 1:nh
            iS = iS + Q{i}*(exp(-32) + exp(h(i)));
        end
        S     = spm_inv(iS);
        iS    = kron(speye(nq),iS);
        Pp    = real(J'*iS*J);
        Cp    = spm_inv(Pp + ipC);

        % precision operators for M-Step
        %------------------------------------------------------------------
        for i = 1:nh
            P{i}   = Q{i}*exp(h(i));
            PS{i}  = P{i}*S;
            P{i}   = kron(speye(nq),P{i});
            JPJ{i} = real(J'*P{i}*J);
        end

        % derivatives: dLdh = dL/dh,...
        %------------------------------------------------------------------
        for i = 1:nh
            dFdh(i,1)      =   trace(PS{i})*nq/2 ...
                - real(e'*P{i}*e)/2 ...
                - spm_trace(Cp,JPJ{i})/2;
            for j = i:nh
                dFdhh(i,j) = - spm_trace(PS{i},PS{j})*nq/2;
                dFdhh(j,i) =   dFdhh(i,j);
            end
        end

        % add hyperpriors
        %------------------------------------------------------------------
        d     = h     - hE;
        dFdh  = dFdh  - ihC*d;
        dFdhh = dFdhh - ihC;
        Ch    = spm_inv(real(-dFdhh));

        % update ReML estimate
        %------------------------------------------------------------------
        dh    = spm_dx(dFdhh,dFdh,{4});
        dh    = min(max(dh,-1),1);
        h     = h  + dh;

        % convergence
        %------------------------------------------------------------------
        dF    = dFdh'*dh;
        if dF < 1e-2, break, end

    end

    % E-Step with Levenberg-Marquardt regularization
    %======================================================================

    % objective function: F(p) = log evidence - divergence
    %----------------------------------------------------------------------
    L(1) = spm_logdet(iS)*nq/2  - real(e'*iS*e)/2 - ny*log(8*atan(1))/2;            ...
    L(2) = spm_logdet(ipC*Cp)/2 - p'*ipC*p/2;
    L(3) = spm_logdet(ihC*Ch)/2 - d'*ihC*d/2;
    F    = sum(L);

    % record increases and reference log-evidence for reporting
    %----------------------------------------------------------------------
    try
        F0;
        if ~M.noprint
            fprintf(' actual: %.3e (%.2f sec)\n',full(F - C.F),toc(tStart))
        end
    catch
        F0 = F;
    end

    % if F has increased, update gradients and curvatures for E-Step
    %----------------------------------------------------------------------
    if F > C.F || k < 3

        % accept current estimates
        %------------------------------------------------------------------
        C.p   = p;
        C.h   = h;
        C.F   = F;
        C.L   = L;
        C.Cp  = Cp;

        % E-Step: Conditional update of gradients and curvature
        %------------------------------------------------------------------
        dFdp  = -real(J'*iS*e) - ipC*p;
        dFdpp = -real(J'*iS*J) - ipC;

        % decrease regularization
        %------------------------------------------------------------------
        v     = min(v + 1/2,4);
        str   = 'EM:(+)';

    else

        % reset expansion point
        %------------------------------------------------------------------
        p     = C.p;
        h     = C.h;
        Cp    = C.Cp;

        % and increase regularization
        %------------------------------------------------------------------
        v     = min(v - 2,-4);
        str   = 'EM:(-)';

    end

    % E-Step: update
    %======================================================================
    dp    = spm_dx(dFdpp,dFdp,{v});
    p     = p + dp;
    Ep    = spm_unvec(spm_vec(pE) + V*p(ip),pE);

    % Graphics
    %======================================================================
    if exist('Fsi', 'var')
        spm_figure('Select', Fsi)

        % reshape prediction if necessary
        %------------------------------------------------------------------
        e  = spm_vec(e);
        f  = spm_vec(f);
        try
            e  = reshape(e,ns,nr);
            f  = reshape(f,ns,nr);
        end

        % subplot prediction
        %------------------------------------------------------------------
        x    = (1:size(e,1))*dt;
        xLab = 'time (seconds)';
        try
            if length(M.Hz) == ns
                x    = M.Hz;
                xLab = 'Frequency (Hz)';
            end
        end

        % plot real or complex predictions
        %------------------------------------------------------------------
        tstr = sprintf('%s: %i','prediction and response: E-Step',k);
        if isreal(spm_vec(y))

            subplot(2,1,1)
            plot(x,real(f)), hold on
            plot(x,real(f + e),':'), hold off
            xlabel(xLab)
            title(tstr,'FontSize',16)
            grid on

        else
            subplot(2,2,1)
            plot(x,real(f)), hold on
            plot(x,real(f + e),':'), hold off
            xlabel(xLab)
            ylabel('real')
            title(tstr,'FontSize',16)
            grid on

            subplot(2,2,2)
            plot(x,imag(f)), hold on
            plot(x,imag(f + e),':'), hold off
            xlabel(xLab)
            ylabel('imaginary')
            title(tstr,'FontSize',16)
            grid on
        end

        % subplot parameters
        %--------------------------------------------------------------
        subplot(2,1,2)
        bar(full(V*p(ip)))
        xlabel('parameter')
        tstr = 'conditional [minus prior] expectation';
        title(tstr,'FontSize',16)
        grid on
        drawnow

    end

    % convergence
    %----------------------------------------------------------------------
    dF  = dFdp'*dp;
    if ~M.noprint
        fprintf('%-6s: %i %6s %-6.3e %6s %.3e ',str,k,'F:',full(C.F - F0),'dF predicted:',full(dF))
    end
    criterion = [(dF < 1e-1) criterion(1:end - 1)];
    if all(criterion)
        if ~M.noprint
            fprintf(' convergence\n')
        end
        break
    end

end

if exist('Fsi', 'var')
    spm_figure('Focus', Fsi)
end

% outputs
%--------------------------------------------------------------------------
Ep     = spm_unvec(spm_vec(pE) + V*C.p(ip),pE);
Cp     = V*C.Cp(ip,ip)*V';
Eh     = C.h;
F      = C.F;
L      = C.L;


%{
 * Copyright (C) 2013-2020, The Regents of The University of Michigan.
 * All rights reserved.
 * This software was developed in the Biped Lab (https://www.biped.solutions/)
 * under the direction of Jessy Grizzle, grizzle@umich.edu. This software may
 * be available under alternative licensing terms; contact the address above.
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 * 1. Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * The views and conclusions contained in the software and documentation are those
 * of the authors and should not be interpreted as representing official policies,
 * either expressed or implied, of the Regents of The University of Michigan.
 *
 * AUTHOR: Bruce JK Huang (bjhuang[at]umich.edu)
 * WEBSITE: https://www.brucerobot.com/
%}

function cost = costIoU(theta_x, theta_y, theta_z, T, X, Y, intrinsic)
    H = eye(4);
    H(1:3,1:3) = rotx(theta_x) * roty(theta_y) * rotz(theta_z);
    H(1:3,4) = T';
    P = intrinsic * [eye(3) zeros(3,1)] * H;
    L_X_transformed = P * X; % transformed points in LiDAR frame
    C_X_transformed = L_X_transformed ./ L_X_transformed(3,:);
    cost = 0;
    for i=1:size(C_X_transformed,2)/4
        k = 4*(i-1) + 1;
        vertices_X = [C_X_transformed(1,k) C_X_transformed(1,k+1) C_X_transformed(1,k+3) C_X_transformed(1,k+2);
                      C_X_transformed(2,k) C_X_transformed(2,k+1) C_X_transformed(2,k+3) C_X_transformed(2,k+2)];
        vertices_Y = [Y(1,k) Y(1,k+1) Y(1,k+3) Y(1,k+2);
                      Y(2,k) Y(2,k+1) Y(2,k+3) Y(2,k+2)];
%         vertices_X = C_X_transformed(1:2,k:k+3);
%         vertices_Y = Y(1:2,k:k+3);

%         conv_X = convhull(vertices_X(1,:)', vertices_X(2,:)');
%         conv_Y = convhull(vertices_Y(1,:)', vertices_Y(2,:)');
%         [vertices_X(1,:), vertices_X(2,:)] = poly2cw(vertices_X(1,conv_X(1:4)), vertices_X(2,conv_X(1:4)));
%         [vertices_Y(1,:), vertices_Y(2,:)] = poly2cw(vertices_Y(1,conv_Y(1:4)), vertices_Y(2,conv_Y(1:4)));
%         pgon_X =  polyshape(vertices_X(1,conv_X), vertices_X(2,conv_X));
%         pgon_Y =  polyshape(vertices_Y(1,conv_Y), vertices_Y(2,conv_Y));

        [vertices_X(1,:), vertices_X(2,:)] = poly2cw(vertices_X(1, :), vertices_X(2, :));
        [vertices_Y(1,:), vertices_Y(2,:)] = poly2cw(vertices_Y(1, :), vertices_Y(2, :));

        pgon_X =  polyshape(vertices_X(1, :), vertices_X(2, :));
        pgon_Y =  polyshape(vertices_Y(1, :), vertices_Y(2, :));
        if all(issimplified([pgon_X, pgon_Y]))
            [I_polyout,~,~] = intersect(pgon_X, pgon_Y);

            [U_polyout,~,~] = union(pgon_X ,pgon_Y);
            I_area = max(area(I_polyout), 1e-5);
            U_area = area(U_polyout);
            cost = cost + I_area/U_area;
        else
            cost = -1e3;
        end
    end
    cost = -cost;
end