function VO = spm_segment(VF,PG,flags)
% Segment an MR image into Gray, White & CSF.
%
% FORMAT VO = spm_segment(PF,PG,flags)
% PF    - name(s) of image(s) to segment (must have same dimensions).
% PG    - name(s) of template image(s) for realignment.
%       - or a 4x4 transformation matrix which maps from the image to
%         the set of templates.
% flags - a structure normally based on defaults.segment
% VO    - optional output volume
%
%                      The algorithm is four step:
%
% 1) Determine the affine transform which best matches the image with a
%    template image. If the name of more than one image is passed, then
%    the first image is used in this step. This step is not performed if
%    no template images are specified.
%
% 2) Perform Cluster Analysis with a modified Mixture Model and a-priori
%    information about the likelihoods of each voxel being one of a
%    number of different tissue types. If more than one image is passed,
%    then they they are all assumed to be in register, and the voxel
%    values are fitted to multi-normal distributions.
%
% 3) Perform morphometric operations on the grey and white partitions
%    in order to more accurately identify brain tissue. This is then used
%    to clean up the grey and white matter segments.
%
% 4) If no output argument is specified, then the segmented images are
%    written to disk. The names of these images have "_seg1", "_seg2"
%    & "_seg3" appended to the name of the first image passed.
%
%_______________________________________________________________________
% Refs:
%
% Ashburner J & Friston KJ (1997) Multimodal Image Coregistration and
% Partitioning - a Unified Framework. NeuroImage 6:209-217
%
%_______________________________________________________________________
%
% The template image, and a-priori likelihood images are modified
% versions of those kindly supplied by Alan Evans, MNI, Canada
% (ICBM, NIH P-20 project, Principal Investigator John Mazziotta).
%_______________________________________________________________________
% @(#)spm_segment.m	2.28a John Ashburner 04/04/01

% Create some suitable default values
%-----------------------------------------------------------------------

def_flags.estimate.priors = str2mat(...
        fullfile(spm('Dir'),'apriori','gray.mnc'),...
        fullfile(spm('Dir'),'apriori','white.mnc'),...
        fullfile(spm('Dir'),'apriori','csf.mnc'));
def_flags.estimate.reg    = 0.01;
def_flags.estimate.cutoff = 30;
def_flags.estimate.samp   = 3;
def_flags.estimate.bb     =  [[-88 88]' [-122 86]' [-60 95]'];
def_flags.estimate.affreg.smosrc = 8;
def_flags.estimate.affreg.regtype = 'mni';
def_flags.estimate.affreg.weight = '';
def_flags.write.cleanup   = 1;
def_flags.write.wrt_cor   = 1;
def_flags.graphics        = 1;

if nargin<3, flags = def_flags; end;
if ~isfield(flags,'estimate'),        flags.estimate        = def_flags.estimate;        end;
if ~isfield(flags.estimate,'priors'), flags.estimate.priors = def_flags.estimate.priors; end;
if ~isfield(flags.estimate,'reg'),    flags.estimate.reg    = def_flags.estimate.reg;    end;
if ~isfield(flags.estimate,'cutoff'), flags.estimate.cutoff = def_flags.estimate.cutoff; end;
if ~isfield(flags.estimate,'samp'),   flags.estimate.samp   = def_flags.estimate.samp;   end;
if ~isfield(flags.estimate,'bb'),     flags.estimate.bb     = def_flags.estimate.bb;     end;
if ~isfield(flags.estimate,'affreg'), flags.estimate.affreg = def_flags.estimate.affreg; end;
if ~isfield(flags.estimate.affreg,'smosrc'),
	flags.estimate.affreg.smosrc = def_flags.estimate.affreg.smosrc;
end;
if ~isfield(flags.estimate.affreg,'regtype'),
	flags.estimate.affreg.regtype = def_flags.estimate.affreg.regtype;
end;
if ~isfield(flags.estimate.affreg,'weight'),
	flags.estimate.affreg.weight = def_flags.estimate.affreg.weight;
end;
if ~isfield(flags,'write'),         flags.write         = def_flags.write;         end;
if ~isfield(flags.write,'cleanup'), flags.write.cleanup = def_flags.write.cleanup; end;
if ~isfield(flags.write,'wrt_cor'), flags.write.wrt_cor = def_flags.write.wrt_cor; end;
if ~isfield(flags,'graphics'),      flags.graphics      = def_flags.graphics;      end;

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

if ischar(VF), VF= spm_vol(VF); end;

SP         = init_sp(flags.estimate,VF,PG);
[x1,x2,x3] = get_sampling(SP.MM,VF,flags.estimate.samp,flags.estimate.bb);
BP         = init_bp(VF, flags.estimate.cutoff, flags.estimate.reg);
CP         = init_cp(VF,x3);
sums       = zeros(8,1);

for pp=1:length(x3),
	[raw,msk] = get_raw(VF,x1,x2,x3(pp));
	s         = get_sp(SP,x1,x2,x3(pp));
	CP        = update_cp_est(CP,s,raw,msk,pp);
	sums      = sums + reshape(sum(sum(s,1),2),8,1);
end;
sums = sums/sum(sums);
CP   = shake_cp(CP);

[CP,BP,SP] = run_segment(CP,BP,SP,VF,sums,x1,x2,x3);

%save segmentation_results.mat CP BP SP VF sums

[g,w,c] = get_gwc(VF,BP,SP,CP,sums,flags.write.wrt_cor);
if flags.write.cleanup, [g,w,c] = clean_gwc(g,w,c); end;

% Create the segmented images.
%-----------------------------------------------------------------------
%offs  = cumsum(repmat(prod(VF(1).dim(1:2)),1,VF(1).dim(3)))-prod(VF(1).dim(1:2));
%pinfo = [repmat([1/255 0]',1,VF(1).dim(3)) ; offs];
[pth,nm,xt] = fileparts(deblank(VF(1).fname));
for j=1:3,
	tmp   = fullfile(pth,[nm '_seg' num2str(j) xt]);
	VO(j) = struct(...
		'fname',tmp,...
		'dim',    [VF(1).dim(1:3) 2],...
		'mat',    VF(1).mat,...
		'pinfo',  [1/255 0 0]',...
		'descrip','Segmented image');
end;

if nargout==0,
	VO = spm_create_vol(VO);

	spm_progress_bar('Init',VF(1).dim(3),'Writing Segmented','planes completed');
	for pp=1:VF(1).dim(3),
		VO(1) = spm_write_plane(VO(1),double(g(:,:,pp))/255,pp);
		VO(2) = spm_write_plane(VO(2),double(w(:,:,pp))/255,pp);
		VO(3) = spm_write_plane(VO(3),double(c(:,:,pp))/255,pp);
		spm_progress_bar('Set',pp);
	end;
	VO = spm_close_vol(VO);
	spm_progress_bar('Clear');
end;

VO(1).dat = g; VO(1).pinfo = VO(1).pinfo(1:2,:);
VO(2).dat = w; VO(2).pinfo = VO(2).pinfo(1:2,:);
VO(3).dat = c; VO(3).pinfo = VO(3).pinfo(1:2,:);

if flags.graphics, display_graphics(VF,VO,CP.mn,CP.cv,CP.mg); end;

return;
%=======================================================================

%=======================================================================
function [y1,y2,y3] = affine_transform(x1,x2,x3,M)
	y1 = M(1,1)*x1 + M(1,2)*x2 + M(1,3)*x3 + M(1,4);
	y2 = M(2,1)*x1 + M(2,2)*x2 + M(2,3)*x3 + M(2,4);
	y3 = M(3,1)*x1 + M(3,2)*x2 + M(3,3)*x3 + M(3,4);
return;
%=======================================================================

%=======================================================================
function display_graphics(VF,VS,mn,cv,mg)
% Do the graphics
nb = 3;
spm_figure('Clear','Graphics');
fg = spm_figure('FindWin','Graphics');
if ~isempty(fg),
	% Show some text
	%-----------------------------------------------------------------------
	ax = axes('Position',[0.05 0.8 0.9 0.2],'Visible','off','Parent',fg);
	text(0.5,0.80, 'Segmentation','FontSize',16,'FontWeight','Bold',...
		'HorizontalAlignment','center','Parent',ax);

	text(0,0.65, ['Image:  ' spm_str_manip(VF(1).fname,'k50d')],...
		'FontSize',14,'FontWeight','Bold','Parent',ax);

	text(0,0.40, 'Means:','FontSize',12,'FontWeight','Bold','Parent',ax);
	text(0,0.30, 'Std devs:' ,'FontSize',12,'FontWeight','Bold','Parent',ax);
	text(0,0.20, 'N vox:','FontSize',12,'FontWeight','Bold','Parent',ax);
	for j=1:nb,
		text((j+0.5)/(nb+1),0.40, num2str(mn(1,j)),...
			'FontSize',12,'FontWeight','Bold',...
			'HorizontalAlignment','center','Parent',ax);
		text((j+0.5)/(nb+1),0.30, num2str(sqrt(cv(1,1,j))),...
			'FontSize',12,'FontWeight','Bold',...
			'HorizontalAlignment','center','Parent',ax);
		text((j+0.5)/(nb+1),0.20, num2str(mg(1,j)/sum(mg(1,:))),...
			'FontSize',12,'FontWeight','Bold',...
			'HorizontalAlignment','center','Parent',ax);
	end;
	if length(VF) > 1,
		text(0,0.10,...
		'Note: only means and variances for the first image are shown',...
		'Parent',ax,'FontSize',12);
	end;

	M1 = VS(1).mat;
	M2 = VF(1).mat;
	for i=1:5,
		M   = spm_matrix([0 0 i*VF(1).dim(3)/6]);
		img = spm_slice_vol(VF(1),M,VF(1).dim(1:2),1);
		img(1,1) = eps;
		ax = axes('Position',...
			[0.05 0.75*(1-i/5)+0.05 0.9/(nb+1) 0.75/5],...
			'Visible','off','Parent',fg);
		imagesc(rot90(img), 'Parent', ax);
		set(ax,'Visible','off','DataAspectRatio',[1 1 1]);

		for j=1:3,
			img = spm_slice_vol(VS(j),M2\M1*M,VF(1).dim(1:2),1);
			ax  = axes('Position',...
				[0.05+j*0.9/(nb+1) 0.75*(1-i/5)+0.05 0.9/(nb+1) 0.75/5],...
				'Visible','off','Parent',fg);
			image(rot90(img*64), 'Parent', ax);
			set(ax,'Visible','off','DataAspectRatio',[1 1 1]);
		end;
	end;

	spm_print;
	drawnow;
end;
return;
%=======================================================================

%=======================================================================
function M = get_affine_mapping(VF,VG,aflags)

if ~isempty(VG) & ischar(VG), VG = spm_vol(VG); end;

if ~isempty(VG) & isstruct(VG),
	% Affine registration so that a priori images match the image to
	% be segmented.
	%-----------------------------------------------------------------------

	VFS = spm_smoothto8bit(VF(1),aflags.smosrc);

	% Scale all images approximately equally
	% ---------------------------------------------------------------
	for i=1:length(VG),
		VG(i).pinfo(1:2,:) = VG(i).pinfo(1:2,:)/spm_global(VG(i));
	end;
	VFS(1).pinfo(1:2,:) = VFS(1).pinfo(1:2,:)/spm_global(VFS(1));

	spm_chi2_plot('Init','Affine Registration','Mean squared difference','Iteration');
	flags     = struct('sep',aflags.smosrc, 'regtype',aflags.regtype,'WG',[],'globnorm',0,'debug',0);
	M         = eye(4);
	[M,scal]  = spm_affreg(VG, VFS, flags, M);

	if ~isempty(aflags.weight), flags.WG = spm_vol(aflags.weight); end;

	flags.sep = aflags.smosrc/2;
	M         = spm_affreg(VG, VFS, flags, M,scal);
	spm_chi2_plot('Clear');

elseif all(size(VG) == [4 4])
	% Assume that second argument is a matrix that will do the job
	%-----------------------------------------------------------------------
	M = VG;
else
	% Assume that image is normalized
	%-----------------------------------------------------------------------
	M = eye(4);
end
return;
%=======================================================================

%=======================================================================
function [x1,x2,x3] = get_sampling(MM,VF,samp,bb1)
% Voxels to sample during the cluster analysis
%-----------------------------------------------------------------------

% A bounding box for the brain in Talairach space.
%bb = [ [-88 88]' [-122 86]' [-60 95]'];
%c = [bb(1,1) bb(1,2) bb(1,3) 1
%     bb(1,1) bb(1,2) bb(2,3) 1
%     bb(1,1) bb(2,2) bb(1,3) 1
%     bb(1,1) bb(2,2) bb(2,3) 1
%     bb(2,1) bb(1,2) bb(1,3) 1
%     bb(2,1) bb(1,2) bb(2,3) 1
%     bb(2,1) bb(2,2) bb(1,3) 1
%     bb(2,1) bb(2,2) bb(2,3) 1]';
%tc = MM\c;
%tc = tc(1:3,:)';
%mx = max(tc);
%mn = min(tc);
%bb = [mn ; mx];
%vx = sqrt(sum(VF(1).mat(1:3,1:3).^2));
%samp = round(max(abs([4 4 4]./vx), [1 1 1]));
%x1 = bb(1,1):samp(1):bb(2,1);
%x2 = bb(1,2):samp(2):bb(2,2);
%x3 = bb(1,3):samp(3):bb(2,3);
%return;

% A bounding box for the brain in Talairach space.
if nargin<4, bb1 = [ [-88 88]' [-122 86]' [-60 95]']; end;

% A mapping from a unit radius sphere to a hyper-ellipse
% that is just enclosed by the bounding box in Talairach
% space.
M0 = [diag(diff(bb1)/2) mean(bb1)';[0 0 0 1]];

% The mapping from voxels to Talairach space is MM,
% so the ellipse in the space of the image becomes:
M0 = MM\M0;

% So to work out the bounding box in the space of the
% image that just encloses the hyper-ellipse.
tmp = M0(1:3,1:3);
tmp = diag(tmp*tmp'/diag(sqrt(diag(tmp*tmp'))));
bb  = round([M0(1:3,4)-tmp M0(1:3,4)+tmp])';
bb  = min(max(bb,[1 1 1 ; 1 1 1]),[VF(1).dim(1:3) ; VF(1).dim(1:3)]);

% Want to sample about every 3mm
tmp  = sqrt(sum(VF(1).mat(1:3,1:3).^2))';
samp = round(max(abs(tmp.^(-1)*samp), [1 1 1]'));

x1 = bb(1,1):samp(1):bb(2,1);
x2 = bb(1,2):samp(2):bb(2,2);
x3 = bb(1,3):samp(3):bb(2,3);

return;
%=======================================================================

%=======================================================================
function [CP,BP,SP] = run_segment(CP,BP,SP,VF,sums,x1,x2,x3)
oll = -Inf;
spm_chi2_plot('Init','Segmenting','Log-likelihood','Iteration #');

for iter = 1:64,
	ll= 0;
	for pp = 1:length(x3), % Loop over planes
		bf        = get_bp(BP,x1,x2,x3(pp));
		[raw,msk] = get_raw(VF,x1,x2,x3(pp));
		s         = get_sp(SP,x1,x2,x3(pp));
		cor       = bf.*raw;
		[P,ll0]   = get_p(cor,msk,s,sums,CP,bf);
		ll        = ll + ll0;
		CP        = update_cp_est(CP,P,cor,msk,pp);
		BP        = update_bp_est(BP,P,cor,CP,msk,x1,x2,x3(pp));
	end;

	BP = update_bp(BP);
	if iter>1, spm_chi2_plot('Set',ll); end;
	%fprintf('\t%g\n', ll);

	% Stopping criterion
	%-----------------------------------------------------------------------
	if iter == 2,
		ll2 = ll;
	elseif iter > 2 & abs((ll-oll)/(ll-ll2)) < 0.0001
		break;
	end;
	oll = ll;
end;
spm_chi2_plot('Clear');
return;
%=======================================================================

%=======================================================================
function BP = init_bp(VF,co,reg)
m        = length(VF);
tmp      = sqrt(sum(VF(1).mat(1:3,1:3).^2));
BP.nbas  = max(round((VF(1).dim(1:3).*tmp)/co),[1 1 1]);
BP.B1    = spm_dctmtx(VF(1).dim(1),BP.nbas(1));
BP.B2    = spm_dctmtx(VF(1).dim(2),BP.nbas(2));
BP.B3    = spm_dctmtx(VF(1).dim(3),BP.nbas(3));

nbas     = BP.nbas;
if prod(BP.nbas)>1,
	% Set up a priori covariance matrix
	vx = sqrt(sum(VF(1).mat(1:3,1:3).^2));
	kx=(pi*((1:nbas(1))'-1)*pi/vx(1)/VF(1).dim(1)*10).^2;
	ky=(pi*((1:nbas(2))'-1)*pi/vx(2)/VF(1).dim(2)*10).^2;
	kz=(pi*((1:nbas(3))'-1)*pi/vx(3)/VF(1).dim(3)*10).^2;

	% Cost function based on sum of squares of 4th derivatives
	IC0 =  (1*kron(kz.^4,kron(ky.^0,kx.^0)) +...
	        1*kron(kz.^0,kron(ky.^4,kx.^0)) +...
	        1*kron(kz.^0,kron(ky.^0,kx.^4)) +...
	        4*kron(kz.^3,kron(ky.^1,kx.^0)) +...
	        4*kron(kz.^3,kron(ky.^0,kx.^1)) +...
	        4*kron(kz.^1,kron(ky.^3,kx.^0)) +...
	        4*kron(kz.^0,kron(ky.^3,kx.^1)) +...
	        4*kron(kz.^1,kron(ky.^0,kx.^3)) +...
	        4*kron(kz.^0,kron(ky.^1,kx.^3)) +...
	        6*kron(kz.^2,kron(ky.^2,kx.^0)) +...
	        6*kron(kz.^2,kron(ky.^0,kx.^2)) +...
	        6*kron(kz.^0,kron(ky.^2,kx.^2)) +...
	       12*kron(kz.^2,kron(ky.^1,kx.^1)) +...
	       12*kron(kz.^1,kron(ky.^2,kx.^1)) +...
	       12*kron(kz.^1,kron(ky.^1,kx.^2)) )*reg;

	%IC0(1) = max(IC0);
	BP.IC0 = diag(IC0(2:end));

	% Initial estimate for intensity modulation field
	BP.T   = zeros(nbas(1),nbas(2),nbas(3),length(VF));
	%-----------------------------------------------------------------------
else
	BP.T   = zeros([1 1 1 length(VF)]);
	BP.IC0 = [];
end;
BP.Alpha = zeros(prod(BP.nbas(1:3)),prod(BP.nbas(1:3)),m);
BP.Beta  = zeros(prod(BP.nbas(1:3)),m);
return;
%=======================================================================

%=======================================================================
function BP = update_bp_est(BP,p,cor,CP,msk,x1,x2,x3)
if prod(BP.nbas)<=1, return; end;
B1 = BP.B1(x1,:);
B2 = BP.B2(x2,:);
B3 = BP.B3(x3,:);
for j=1:size(BP.Alpha,3),
	cr  = cor(:,:,j);
	w1 = zeros(size(cr));
	w2 = zeros(size(cr));
	for i=[1 2 3 4 5 6 7 8],
		tmp = p(:,:,i)*CP.cv(j,j,i)^(-1);
		w1  = w1 + tmp.*(CP.mn(j,i) - cr);
		w2  = w2 + tmp;
	end;
	wt1       = 1 + cr.*w1;
	wt2       = cr.*(cr.*w2 - w1);
	wt1(~msk) = 0;
	wt2(~msk) = 0;

	BP.Beta(:,j)    = BP.Beta(:,j)    + kron(B3',spm_krutil(wt1,B1,B2,0));
	BP.Alpha(:,:,j) = BP.Alpha(:,:,j) + kron(B3'*B3,spm_krutil(wt2,B1,B2,1));
end;
return;
%=======================================================================

%=======================================================================
function BP = update_bp(BP)
if prod(BP.nbas)<=1, return; end;
for j=1:size(BP.Alpha,3),
	x     = BP.T(:,:,:,j);
	x     = x(:);
	x     = x(2:end);
	Alpha = BP.Alpha(2:end,2:end,j);
	Beta  = BP.Beta(2:end,j);
	x     = (Alpha + BP.IC0)\(Alpha*x + Beta);

	BP.T(:,:,:,j) = reshape([0 ; x],BP.nbas(1:3));
	BP.Alpha      = zeros(size(BP.Alpha));
	BP.Beta       = zeros(size(BP.Beta));
end;
return;
%=======================================================================

%=======================================================================
function bf = get_bp(BP,x1,x2,x3)
bf = ones(length(x1),length(x2),size(BP.Alpha,3));
if prod(BP.nbas)<=1, return; end;
B1 = BP.B1(x1,:);
B2 = BP.B2(x2,:);
B3 = BP.B3(x3,:);
for i=1:size(BP.Alpha,3),
	t = reshape(reshape(BP.T(:,:,:,i),...
		BP.nbas(1)*BP.nbas(2),BP.nbas(3))*B3', BP.nbas(1), BP.nbas(2));
	bf(:,:,i) = exp(B1*t*B2');
end;
return;
%=======================================================================

%=======================================================================
function [dat,msk] = get_raw(VF,x1,x2,x3)
[X1,X2,X3] = ndgrid(x1,x2,x3);
for i=1:length(VF),
	[Y1,Y2,Y3] = affine_transform(X1,X2,X3,VF(i).mat\VF(1).mat);
	dat(:,:,i) = spm_sample_vol(VF(i),Y1,Y2,Y3,1);
end;
msk = all(dat,3) & all(isfinite(double(dat)),3);
return;
%=======================================================================

%=======================================================================
function CP = init_cp(VF,x3)
n = 8;
m = length(VF);
p = length(x3);
CP.mom0 = zeros(1,n,p)+eps;
CP.mom1 = zeros(m,n,p);
CP.mom2 = zeros(m,m,n,p)+eps;

% Occasionally the dynamic range of the images is such that many voxels
% all have the same intensity.  Adding cv0 is an attempt to improve the
% stability of the algorithm if this occurs. The value 0.083 was obtained
% from var(rand(1000000,1)).  It prbably isn't the best way of doing
% things, but it appears to work.
CP.cv0 = zeros(m,m);
for i=1:m,
	if spm_type(VF(i).dim(4),'intt'),
		CP.cv0(i,i)=0.083*mean(VF(i).pinfo(1,:));
	end;
end;
return;
%=======================================================================

%=======================================================================
function CP = shake_cp(CP)
CP.mom0(:,5,:)   = CP.mom0(:,1,:);
CP.mom0(:,6,:)   = CP.mom0(:,2,:);
CP.mom0(:,7,:)   = CP.mom0(:,3,:);
CP.mom1(:,5,:)   = CP.mom1(:,1,:);
CP.mom1(:,6,:)   = CP.mom1(:,2,:);
CP.mom1(:,7,:)   = CP.mom1(:,3,:);
CP.mom1(:,8,:)   = 0;
CP.mom2(:,:,5,:) = CP.mom2(:,:,1,:);
CP.mom2(:,:,6,:) = CP.mom2(:,:,2,:);
CP.mom2(:,:,7,:) = CP.mom2(:,:,3,:);
return;
%=======================================================================

%=======================================================================
function CP = update_cp_est(CP,P,dat,msk,p)
m   = size(dat,3);
d   = size(P);
P   = reshape(P,[d(1)*d(2),d(3)]);
dat = reshape(dat,[d(1)*d(2),m]);
P(~msk(:),:)   = [];
dat(~msk(:),:) = [];
for i=1:size(CP.mom0,2),
	CP.mom0(1,i,p)   = sum(P(:,i));
	CP.mom1(:,i,p)   = sum((P(:,i)*ones(1,m)).*dat)';
	CP.mom2(:,:,i,p) = ((P(:,i)*ones(1,m)).*dat)'*dat;
end;

for i=1:size(CP.mom0,2),
	CP.mg(1,i)   = sum(CP.mom0(1,i,:),3);
	CP.mn(:,i)   = sum(CP.mom1(:,i,:),3)/CP.mg(1,i);

	tmp          = (CP.mg(1,i).*CP.mn(:,i))*CP.mn(:,i)';
	tmp          = tmp-eye(size(tmp))*eps*10000;
	CP.cv(:,:,i) = (sum(CP.mom2(:,:,i,:),4) - tmp)/CP.mg(1,i) + CP.cv0;
end;
CP.mg   = CP.mg/sum(CP.mg);
return;
%=======================================================================

%=======================================================================
function [p,ll] = get_p(cor,msk,s,sums,CP,bf)
d   = [size(cor) 1 1];
n   = size(CP.mg,2);
cor = reshape(cor,d(1)*d(2),d(3));
cor = cor(msk,:);
p   = zeros(d(1)*d(2),n);
if ~any(msk), p  = reshape(p,d(1),d(2),n); ll=0; return; end;

for i=1:n,
	amp       = 1/sqrt((2*pi)^d(3) * det(CP.cv(:,:,i)));
	dst       = (cor-ones(size(cor,1),1)*CP.mn(:,i)')/sqrtm(CP.cv(:,:,i));
	dst       = sum(dst.*dst,2);
	tmp       = s(:,:,i);
	p(msk,i)  = (amp*CP.mg(1,i)/sums(i))*exp(-0.5*dst).*tmp(msk) +eps;
end;
sp = sum(p,2);
ll = sum(log(sp(msk).*bf(msk)+eps));
sp(~msk) = Inf;
for i=1:n, p(:,i) = p(:,i)./sp; end;
p  = reshape(p,d(1),d(2),n);
return;
%=======================================================================

%=======================================================================
function SP = init_sp(flags,VF,PG)
SP.VB       = spm_vol(flags.priors);
MM          = get_affine_mapping(VF,PG,flags.affreg);
%VF          = spm_vol(PF);
SP.MM       = MM*VF(1).mat;
SP.w        = 0.98;
return;
%=======================================================================

%=======================================================================
function s = get_sp(SP,x1,x2,x3)
[X1,X2,X3] = ndgrid(x1,x2,x3);
[Y1,Y2,Y3] = affine_transform(X1,X2,X3,SP.VB(1).mat\SP.MM);
w1  = SP.w;
w2  = (1-w1)/2;
s   = zeros([size(Y1),4]);
for i=1:3,
	s(:,:,i) = spm_sample_vol(SP.VB(i),Y1,Y2,Y3,1)*w1+w2;
end;
s(:,:,4:8)   = repmat(abs(1-sum(s(:,:,1:3),3))/5,[1 1 5]);
return;
%=======================================================================

%=======================================================================
function [g,w,c] = get_gwc(VF,BP,SP,CP,sums,wc)

if wc,
	VC = VF;
	for j=1:length(VF),
		[pth,nm,xt]    = fileparts(deblank(VF(j).fname));
		VC(j).fname    = fullfile(pth,['m' nm xt]);
		VC(j).descrip  = 'Bias corrected image';
	end;
	VC = spm_create_vol(VC);
end;

spm_progress_bar('Init',VF(1).dim(3),'Creating Segmented','planes completed');
x1 = 1:VF(1).dim(1);
x2 = 1:VF(1).dim(2);
x3 = 1:VF(1).dim(3);

g = uint8(0); g(VF(1).dim(1),VF(1).dim(2),VF(1).dim(3)) = 0;
w = uint8(0); w(VF(1).dim(1),VF(1).dim(2),VF(1).dim(3)) = 0;
c = uint8(0); c(VF(1).dim(1),VF(1).dim(2),VF(1).dim(3)) = 0;

for pp=1:length(x3),
	bf        = get_bp(BP,x1,x2,x3(pp));
	[raw,msk] = get_raw(VF,x1,x2,x3(pp));
	cor       = raw.*bf;
	if wc,
		for j=1:length(VC),
			VC(j) = spm_write_plane(VC(j),cor(:,:,j),pp);
		end;
	end;
	s         = get_sp(SP,x1,x2,x3(pp));
	p         = get_p(cor,msk,s,sums,CP,bf);
	g(:,:,pp) = uint8(round(p(:,:,1)*255));
	w(:,:,pp) = uint8(round(p(:,:,2)*255));
	c(:,:,pp) = uint8(round(p(:,:,3)*255));

	spm_progress_bar('Set',pp);
end;
spm_progress_bar('Clear');

if wc, spm_close_vol(VC); end;
return;
%=======================================================================

%=======================================================================
function [g,w,c] = clean_gwc(g,w,c)
b    = w;
b(1) = w(1);

% Build a 3x3x3 seperable smoothing kernel
%-----------------------------------------------------------------------
kx=[0.75 1 0.75];
ky=[0.75 1 0.75];
kz=[0.75 1 0.75];
sm=sum(kron(kron(kz,ky),kx))^(1/3);
kx=kx/sm; ky=ky/sm; kz=kz/sm;

% Erosions and conditional dilations
%-----------------------------------------------------------------------
niter = 32;
spm_progress_bar('Init',niter,'Extracting Brain','Iterations completed');
for j=1:niter,
	if j>2, th=0.15; else th=0.6; end; % Dilate after two its of erosion.
	for i=1:size(b,3),
		gp = double(g(:,:,i));
		wp = double(w(:,:,i));
		bp = double(b(:,:,i))/255;
		bp = (bp>th).*(wp+gp);
		b(:,:,i) = uint8(round(bp));
	end;
	spm_conv_vol(b,b,kx,ky,kz,-[1 1 1]);
	spm_progress_bar('Set',j);
end;
th = 0.05;
for i=1:size(b,3),
	gp       = double(g(:,:,i))/255;
	wp       = double(w(:,:,i))/255;
	cp       = double(c(:,:,i))/255;
	bp       = double(b(:,:,i))/255;
	bp       = ((bp>th).*(wp+gp))>th;
	g(:,:,i) = uint8(round(255*gp.*bp./(gp+wp+cp+eps)));
	w(:,:,i) = uint8(round(255*wp.*bp./(gp+wp+cp+eps)));
	c(:,:,i) = uint8(round(255*(cp.*bp./(gp+wp+cp+eps)+cp.*(1-bp))));
end;
spm_progress_bar('Clear');
return;


function SR = superresolve_bicubic(slidingWindow, magFactor)

    SR = imresize(slidingWindow.referenceFrame, magFactor, 'bicubic');


% ==============================================================================
%
%    Software License Agreement (BSD License)
%    Copyright (c) 2019
%    (www.aimlab.wpi.edu)
%
%    All rights reserved.
%
%    Redistribution and use in source and binary forms, with or without
%    modification, are permitted provided that the following conditions
%    are met:
%
%    * Redistributions of source code must retain the above copyright
%    notice, this list of conditions and the following disclaimer.
%
%    * Redistributions in binary form must reproduce the above
%    copyright notice, this list of conditions and the following
%    disclaimer in the documentation and/or other materials provided
%    with the distribution.
%
%    * Neither the name of authors nor the names of its contributors may
%    be used to endorse or promote products derived from this software
%    without specific prior written permission.
%
%    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.
%
%    \author:    <http://www.aimlab.wpi.edu>
%    \author:    <amunawar@wpi.edu>
%    \author:    Adnan Munawar
%    \version:   0.1$
% ==============================================================================

function [its,sizePath,run_time] =  RRTstar3D(dim,segmentLength,radius,random_world,show_output,samples)
% if samples < 4000
%     disp('ERROR! SPECIFY ATLEAST 4000 SAMPLES')
%     return
% end

% dim = 2;
% radius =0;
% segmentLength = 5;
% random_world = 0;
% n_its = 1000;
% standard length of path segments
if dim ==2
    start_cord = [5,5];
    goal_cord = [95,95];

else

    start_cord = [5,5,5];
    goal_cord = [95,95,95];
end

% create random world
Size = 100;
NumObstacles = 100;

if random_world ==1
    world = createWorld(NumObstacles,ones(1,dim)*Size,zeros(1,dim),dim);
else
    [world NumObstacles] = createKnownWorld(ones(1,dim)*Size,[0;0;0],dim);
end
% randomly select start and end nodes
%start_node = generateRandomNode(world,dim)
%end_node   = generateRandomNode(world,dim)
start_node = [start_cord,0,0,0];
end_node = [goal_cord,0,0,0];
% establish tree starting with the start node
tree = start_node;

numPaths = 0;
a = clock;
% check to see if start_node connects directly to end_node
if ( (norm(start_node(1:dim)-end_node(1:dim))<segmentLength )...
        &&(collision(start_node,end_node,world,dim)==0) )

    disp('CASE 1')
    path = [start_node; end_node];
else
    disp('CASE 2')
    if samples >0
        draw = floor(samples/8);
        its = 0;
        for i = 1:samples
            flag = 0;
            [tree,flag] = extendTree(tree,end_node,segmentLength,radius,world,flag,dim);
            numPaths = numPaths + flag;
            its = its+1;

            if its == draw
                tree_1 = tree;
            elseif its == draw*2
                tree_2 = tree;
            elseif its == draw*3
                tree_3 = tree;
            elseif its == draw*4
                tree_4 = tree;
            elseif its == draw*5
                tree_5 = tree;
            elseif its == draw*6
                tree_6 = tree;
            elseif its == draw*7
                tree_7 = tree;
            elseif its == samples
                tree_8 = tree;
            end
        end

    else
        its = 0;
        numPaths = 0;
        flag = 0;
        while numPaths < 1,
            [tree,flag] = extendTree(tree,end_node,segmentLength,radius,world,flag,dim);
            numPaths = numPaths + flag;
            its = its+1;
        end
    end

end
numPaths

% find path with minimum cost to end_node
path = findMinimumPath(tree,end_node,dim);

b = clock;
run_time = 3600*(b(4)-a(4)) + 60 * (b(5)-a(5)) + (b(6) - a(6));

path_1 = findMinimumPath(tree_1,end_node,dim);

path_2 = findMinimumPath(tree_2,end_node,dim);

path_3 = findMinimumPath(tree_3,end_node,dim);

path_4 = findMinimumPath(tree_4,end_node,dim);

path_5 = findMinimumPath(tree_5,end_node,dim);

path_6 = findMinimumPath(tree_6,end_node,dim);

path_7 = findMinimumPath(tree_7,end_node,dim);

path_8 = findMinimumPath(tree_8,end_node,dim);

sizePath = size(path,1);

if show_output == 1

    if size(path_1, 1) > 0
        figure;
        plotExpandedTree(world,tree_1,dim);
        plotWorld(world,path_1,dim);
    else
        disp('COULD NOT FIND A CONNECTING TREE TILL 1/8th SAMPLES SO NOT DRAWING THAT PATH')
    end
    if size(path_2, 1) > 0
        figure;
        plotExpandedTree(world,tree_2,dim);
        plotWorld(world,path_2,dim);
    else
        disp('COULD NOT FIND A CONNECTING TREE TILL 2/8th SAMPLES SO NOT DRAWING THAT PATH')
    end
    if size(path_3, 1) > 0
    figure;
    plotExpandedTree(world,tree_3,dim);
    plotWorld(world,path_3,dim);
    else
        disp('COULD NOT FIND A CONNECTING TREE TILL 3/8th SAMPLES SO NOT DRAWING THAT PATH')
    end
    if size(path_4, 1) > 0
    figure;
    plotExpandedTree(world,tree_4,dim);
    plotWorld(world,path_4,dim);
    else
        disp('COULD NOT FIND A CONNECTING TREE TILL 4/8th SAMPLES SO NOT DRAWING THAT PATH')
    end
    if size(path_5, 1) > 0
    figure;
    plotExpandedTree(world,tree_5,dim);
    plotWorld(world,path_5,dim);
    else
        disp('COULD NOT FIND A CONNECTING TREE TILL 5/8th SAMPLES SO NOT DRAWING THAT PATH')
    end
    if size(path_6, 1) > 0
    figure;
    plotExpandedTree(world,tree_6,dim);
    plotWorld(world,path_6,dim);
    else
        disp('COULD NOT FIND A CONNECTING TREE TILL 6/8th SAMPLES SO NOT DRAWING THAT PATH')
    end
    if size(path_7, 1) > 0
    figure;
    plotExpandedTree(world,tree_7,dim);
    plotWorld(world,path_7,dim);
    else
        disp('COULD NOT FIND A CONNECTING TREE TILL 7/8th SAMPLES SO NOT DRAWING THAT PATH')
    end
    if size(path_8, 1) > 0
    figure;
    plotExpandedTree(world,tree_8,dim);
    plotWorld(world,path_8,dim);
    else
        disp('COULD NOT FIND A CONNECTING TREE TILL 8/8th SAMPLES SO NOT DRAWING THAT PATH')
    end
    if size(path, 1) > 0
    figure;
    plotExpandedTree(world,tree,dim);
    plotWorld(world,path,dim);
    else
        disp('COULD NOT FIND A CONNECTING TREE FOR THE SPECIFIED SAMPLES. PLEASE INCREASE THE NUMBER OF SAMPLES')
    end
end
end

function world = createWorld(NumObstacles, endcorner, origincorner,dim)

if dim == 2

    % check to make sure that the region is nonempty
    if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2))
        disp('Not valid corner specifications!')
        world=[];

        % create world data structure
    else
        world.NumObstacles = NumObstacles;
        world.endcorner = endcorner;
        world.origincorner = origincorner;

        % create NumObstacles
        maxRadius = min(endcorner(1)- origincorner(1), endcorner(2)-origincorner(2));
        maxRadius = 5*maxRadius/NumObstacles/2;
        for i=1:NumObstacles,
            % randomly pick radius
            world.radius(i) = maxRadius*rand;
            % randomly pick center of obstacles
            cx = origincorner(1) + world.radius(i)...
                + (endcorner(1)-origincorner(1)-2*world.radius(i))*rand;
            cy = origincorner(2) + world.radius(i)...
                + (endcorner(2)-origincorner(2)-2*world.radius(i))*rand;
            world.cx(i) = cx;
            world.cy(i) = cy;
        end
    end

elseif dim ==3;
    % check to make sure that the region is nonempty
    if (endcorner(1) <= origincorner(1)) || (endcorner(2) <= origincorner(2)) || (endcorner(3) <= origincorner(3))
        disp('Not valid corner specifications!')
        world=[];

        % create world data structure
    else
        world.NumObstacles = NumObstacles;
        world.endcorner = endcorner;
        world.origincorner = origincorner;

        % create NumObstacles
        bounds = [endcorner(1)- origincorner(1), endcorner(2)-origincorner(2), endcorner(3)-origincorner(3)];
        maxRadius = min(bounds);
        maxRadius = 5*maxRadius/NumObstacles;
        for i=1:NumObstacles,
            % randomly pick radius
            world.radius(i) = maxRadius*rand;
            % randomly pick center of obstacles
            cx = origincorner(1) + world.radius(i)...
                + (endcorner(1)-origincorner(1)-2*world.radius(i))*rand;
            cy = origincorner(2) + world.radius(i)...
                + (endcorner(2)-origincorner(2)-2*world.radius(i))*rand;
            cz = origincorner(2) + world.radius(i)...
                + (endcorner(2)-origincorner(2)-2*world.radius(i))*rand;
            world.cx(i) = cx;
            world.cy(i) = cy;
            world.cz(i) = cz;
        end
    end
end
end

function [world NumObstacles] = createKnownWorld(endcorner, origincorner,dim)
NumObstacles = 5;
if dim == 2
    % check to make sure that the region is nonempty
    if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)),
        disp('Not valid corner specifications!')
        world=[];
        % create world data structure
    else
        world.NumObstacles = NumObstacles;
        world.endcorner = endcorner;
        world.origincorner = origincorner;

        % create NumObstacles
        maxRadius = 10;

        world.radius(1) = maxRadius;
        cx = 50;
        cy = 50;
        world.cx(1) = cx;
        world.cy(1) = cy;

        world.radius(2) = maxRadius;
        cx = 75;
        cy = 25;
        world.cx(2) = cx;
        world.cy(2) = cy;

        world.radius(3) = maxRadius;
        cx = 25;
        cy = 75;
        world.cx(3) = cx;
        world.cy(3) = cy;

        world.radius(4) = maxRadius;
        cx = 25;
        cy = 25;
        world.cx(4) = cx;
        world.cy(4) = cy;

        world.radius(5) = maxRadius;
        cx = 75;
        cy = 75;
        world.cx(5) = cx;
        world.cy(5) = cy;
    end

elseif dim == 3

    NumObstacles = 9;
    % check to make sure that the region is nonempty
    if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)) | (endcorner(3) <= origincorner(3)),
        disp('Not valid corner specifications!')
        world=[];

        % create world data structure
    else
        world.NumObstacles = NumObstacles;
        world.endcorner = endcorner;
        world.origincorner = origincorner;

        % create NumObstacles
        maxRadius = 10;

        world.radius(1) = maxRadius;
        cx = 50;
        cy = 50;
        cz = 50;
        world.cx(1) = cx;
        world.cy(1) = cy;
        world.cz(1) = cz;

        world.radius(2) = maxRadius;
        cx = 25;
        cy = 25;
        cz = 25;
        world.cx(2) = cx;
        world.cy(2) = cy;
        world.cz(2) = cz;

        world.radius(3) = maxRadius;
        cx = 75;
        cy = 75;
        cz = 75;
        world.cx(3) = cx;
        world.cy(3) = cy;
        world.cz(3) = cz;

        world.radius(4) = maxRadius;
        cx = 25;
        cy = 25;
        cz = 75;
        world.cx(4) = cx;
        world.cy(4) = cy;
        world.cz(4) = cz;

        world.radius(5) = maxRadius;
        cx = 75;
        cy = 75;
        cz = 25;
        world.cx(5) = cx;
        world.cy(5) = cy;
        world.cz(5) = cz;

        world.radius(6) = maxRadius;
        cx = 25;
        cy = 75;
        cz = 25;
        world.cx(6) = cx;
        world.cy(6) = cy;
        world.cz(6) = cz;

        world.radius(7) = maxRadius;
        cx = 75;
        cy = 25;
        cz = 25;
        world.cx(7) = cx;
        world.cy(7) = cy;
        world.cz(7) = cz;

        world.radius(8) = maxRadius;
        cx = 75;
        cy = 25;
        cz = 75;
        world.cx(8) = cx;
        world.cy(8) = cy;
        world.cz(8) = cz;

        world.radius(9) = maxRadius;
        cx = 25;
        cy = 75;
        cz = 75;
        world.cx(9) = cx;
        world.cy(9) = cy;
        world.cz(9) = cz;
    end
end
end

function node=generateRandomNode(world,dim)

if dim ==2;
    % randomly pick configuration
    px       = (world.endcorner(1)-world.origincorner(1))*rand;
    py       = (world.endcorner(2)-world.origincorner(2))*rand;

    chi      = 0;
    cost     = 0;
    node     = [px, py, chi, cost, 0];

    % check collision with obstacle
    while collision(node, node, world,dim),
        px       = (world.endcorner(1)-world.origincorner(1))*rand;
        py       = (world.endcorner(2)-world.origincorner(2))*rand;

        chi      = 0;
        cost     = 0;
        node     = [px, py, chi, cost, 0];
    end

elseif dim ==3;
    % randomly pick configuration
    px       = (world.endcorner(1)-world.origincorner(1))*rand;
    py       = (world.endcorner(2)-world.origincorner(2))*rand;
    pz       = (world.endcorner(3)-world.origincorner(3))*rand;

    chi      = 0;
    cost     = 0;
    node     = [px, py, pz, chi, cost, 0];

    % check collision with obstacle
    while collision(node, node, world,dim),
        px       = (world.endcorner(1)-world.origincorner(1))*rand;
        py       = (world.endcorner(2)-world.origincorner(2))*rand;
        pz       = (world.endcorner(3)-world.origincorner(3))*rand;

        chi      = 0;
        cost     = 0;
        node     = [px, py, pz, chi, cost, 0];
    end

end

end

function collision_flag = collision(node, parent, world,dim)

collision_flag = 0;

for i=1:dim
    if (node(i)>world.endcorner(i))|(node(i)<world.origincorner(i))
        collision_flag = 1;
    end
end

if collision_flag == 0 && dim ==2
    for sigma = 0:.2:1,
        p = sigma*node(1:dim) + (1-sigma)*parent(1:dim);
        % check each obstacle
        for i=1:world.NumObstacles,
            if (norm([p(1);p(2)]-[world.cx(i); world.cy(i)])<=1*world.radius(i)),
                collision_flag = 1;
                break;
            end
        end
    end

elseif collision_flag == 0 && dim ==3
    for sigma = 0:.2:1,
        p = sigma*node(1:dim) + (1-sigma)*parent(1:dim);
        % check each obstacle
        for i=1:world.NumObstacles,
            if (norm([p(1);p(2);p(3)]-[world.cx(i); world.cy(i); world.cz(i)])<=1*world.radius(i)),
                collision_flag = 1;
                break;
            end
        end
    end
end
end

function flag = canEndConnectToTree(tree,end_node,minDist,world,dim)
flag = 0;
% check only last node added to tree since others have been checked
if ( (norm(tree(end,1:dim)-end_node(1:dim))<minDist)...
        & (collision(tree(end,1:dim), end_node(1:dim), world,dim)==0) ),
    flag = 1;
end

end

function [new_tree,flag] = extendTree(tree,end_node,segmentLength,r,world,flag_chk,dim)

flag1 = 0;
while flag1==0,
    % select a random point
    randomPoint = ones(1,dim);
    for i=1:dim
        randomPoint(1,i) = (world.endcorner(i)-world.origincorner(i))*rand;
    end

    % find leaf on node that is closest to randomPoint
    tmp = tree(:,1:dim)-ones(size(tree,1),1)*randomPoint;
    sqrd_dist = sqr_eucl_dist(tmp,dim);
    [min_dist,idx] = min(sqrd_dist);
    min_parent_idx = idx;

    new_point = (randomPoint-tree(idx,1:dim));
    new_point = tree(idx,1:dim)+(new_point/norm(new_point))*segmentLength;

    min_cost  = cost_np(tree(idx,:),new_point,dim);
    new_node  = [new_point, 0, min_cost, idx];

    if collision(new_node, tree(idx,:), world,dim)==0

        tmp_dist = tree(:,1:dim)-(ones(size(tree,1),1)*new_point);
        dist = sqr_eucl_dist(tmp_dist,dim);
        near_idx = find(dist <= r^2);

        if size(near_idx,1)>1
            size_near = size(near_idx,1);

            for i = 1:size_near
                if collision(new_node, tree(near_idx(i),:), world,dim)==0

                    cost_near = tree(near_idx(i),dim+2)+line_cost(tree(near_idx(i),:),new_point,dim);

                    if  cost_near < min_cost
                        min_cost = cost_near;
                        min_parent_idx = near_idx(i);
                    end

                end
            end
        end

        new_node = [new_point, 0 , min_cost, min_parent_idx];
        new_tree = [tree; new_node];
        new_node_idx = size(new_tree,1);

        if size(near_idx,1)>1
            reduced_idx = near_idx;
            for j = 1:size(reduced_idx,1)
                near_cost = new_tree(reduced_idx(j),dim+2);
                lcost = line_cost(new_tree(reduced_idx(j),:),new_point,dim);
                if near_cost > min_cost + lcost ...
                        && collision(new_tree(reduced_idx(j),:),new_node,world,dim)
                    before = new_tree(reduced_idx(j),dim+3)
                    new_tree(reduced_idx(j),dim+3) = new_node_idx;
                    after = new_tree(reduced_idx(j),dim+3)
                end

            end
        end
        flag1=1;
    end
end

if flag_chk == 0
    % check to see if new node connects directly to end_node
    if ( (norm(new_node(1:dim)-end_node(1:dim))<segmentLength )...
            && (collision(new_node,end_node,world,dim)==0) )
        flag = 1;
        new_tree(end,dim+1)=1;  % mark node as connecting to end.
    else
        flag = 0;
    end

else flag = 1;
end
end

function e_dist = sqr_eucl_dist(array,dim)

sqr_e_dist = zeros(size(array,1),dim);
for i=1:dim

    sqr_e_dist(:,i) = array(:,i).*array(:,i);

end
e_dist = zeros(size(array,1),1);
for i=1:dim

    e_dist = e_dist+sqr_e_dist(:,i);

end

end

%calculate the cost from a node to a point
function [cost] = cost_np(from_node,to_point,dim)

diff = from_node(:,1:dim) - to_point;
eucl_dist = norm(diff);
cost = from_node(:,dim+2) + eucl_dist;

end

%calculate the cost from a node to a node
function [cost] = cost_nn(from_node,to_node,dim)

diff = from_node(:,1:dim) - to_node(:,1:dim);
eucl_dist = norm(diff);
cost = from_node(:,dim+2) + eucl_dist;

end

function [cost] = line_cost(from_node,to_point,dim)
diff = from_node(:,1:dim) - to_point;
cost = norm(diff);
end

function path = findMinimumPath(tree,end_node,dim)

% find nodes that connect to end_node
connectingNodes = [];
for i=1:size(tree,1),
    if tree(i,dim+1)==1,
        connectingNodes = [connectingNodes ; tree(i,:)];
    end
end

if size(connectingNodes, 1) > 0

    % find minimum cost last node
    [tmp,idx] = min(connectingNodes(:,dim+2));

    % construct lowest cost path
    path = [connectingNodes(idx,:); end_node];
    parent_node = connectingNodes(idx,dim+3);
    while parent_node>1,
        parent_node = tree(parent_node,dim+3);
        path = [tree(parent_node,:); path];
    end

else
    path = [];
end

end

function plotExpandedTree(world,tree,dim)
ind = size(tree,1);
while ind>0
    branch = [];
    node = tree(ind,:);
    branch = [ branch ; node ];
    parent_node = node(dim+3);
    while parent_node > 1
        cur_parent = parent_node;
        branch = [branch; tree(parent_node,:)];
        parent_node = tree(parent_node,dim+3);
    end
    ind = ind - 1;

    if dim == 2
        X = branch(:,1);
        Y = branch(:,2);

        p = plot(X,Y);
        set(p,'Color','r','LineWidth',0.5,'Marker','.','MarkerEdgeColor','g');
        hold on;

    elseif dim == 3
        X = branch(:,1);
        Y = branch(:,2);
        Z = branch(:,3);

        p = plot3(X,Y,Z);
        set(p,'Color','r','LineWidth',0.5,'Marker','.','MarkerEdgeColor','g');
        hold on;
    end
end
end

function plotWorld(world,path,dim)
% the first element is the north coordinate
% the second element is the south coordinate
if dim ==2

    N = 10;
    th = 0:2*pi/N:2*pi;
    axis([world.origincorner(1),world.endcorner(1),...
        world.origincorner(2), world.endcorner(2)]);
    hold on

    for i=1:world.NumObstacles,
        X = world.radius(i)*sin(th) + world.cx(i);
        Y = world.radius(i)*cos(th) + world.cy(i);
        fill(X,Y,'blue');
    end

    X = path(:,1);
    Y = path(:,2);
    p = plot(X,Y);

elseif dim ==3
    axis([world.origincorner(1),world.endcorner(1),...
        world.origincorner(2), world.endcorner(2),...
        world.origincorner(3), world.endcorner(3)]);
    hold on

    for i=1:world.NumObstacles,
        [X Y Z] = sphere(10);
        X = (X*world.radius(i));
        Y = (Y*world.radius(i));
        Z = (Z*world.radius(i));
        surf(X+world.cx(i),Y+world.cy(i),Z+world.cz(i));
        colormap([0.5 0.2 0.3]);
    end

    X = path(:,1);
    Y = path(:,2);
    Z = path(:,3);
    p = plot3(X,Y,Z);
end
set(p,'Color','black','LineWidth',3)
xlabel('X axis');
ylabel('Y axis');
zlabel('Z axis');
title('RRT Star Algorithm');
end


% Anchored Neighborhood Regression for Fast Example-Based Super-Resolution
% Example code
%
% March 22, 2013. Radu Timofte, VISICS @ KU Leuven
%
% Revised version: (includes all [1] methods)
% October 3, 2013. Radu Timofte, CVL @ ETH Zurich
%
% Updated version: (adds A+ methods [2])
% September 5, 2014. Radu Timofte, CVL @ ETH Zurich
% %
% Please reference to both:
% [1] Radu Timofte, Vincent De Smet, Luc Van Gool.
% Anchored Neighborhood Regression for Fast Example-Based Super-Resolution.
% International Conference on Computer Vision (ICCV), 2013.
%
% [2] Radu Timofte, Vincent De Smet, Luc Van Gool.
% A+: Adjusted Anchored Neighborhood Regression for Fast Super-Resolution.
% Asian Conference on Computer Vision (ACCV), 2014.
%
% For any questions, email me by timofter@vision.ee.ethz.ch
%

clear;

p = pwd;
addpath(fullfile(p, '/methods'));  % the upscaling methods

addpath(fullfile(p, '/ksvdbox')) % K-SVD dictionary training algorithm

addpath(fullfile(p, '/ompbox')) % Orthogonal Matching Pursuit algorithm

imgscale = 1; % the scale reference we work with
flag = 1;       % flag = 0 - only GR, ANR, A+, and bicubic methods, the other get the bicubic result by default
                % flag = 1 - all the methods are applied

upscaling = 4; % the magnification factor x2, x3, x4...

input_dir = 'Set5'; % Directory with input images from Set5 image dataset
%input_dir = 'Set14'; % Directory with input images from Set14 image dataset

pattern = '*.bmp'; % Pattern to process

dict_sizes = [2 4 8 16 32 64 128 256 512 1024 2048 4096 8192 16384 32768 65536];
neighbors = [1:1:12, 16:4:32, 40:8:64, 80:16:128, 256, 512, 1024];
%d = 7
%for nn=1:28
%nn= 28

clusterszA = 2048; % neighborhood size for A+

disp('The experiment corresponds to the results from Table 2 in the referenced [1] and [2] papers.');

disp(['The experiment uses ' input_dir ' dataset and aims at a magnification of factor x' num2str(upscaling) '.']);
if flag==1
    disp('All methods are employed : Bicubic, Yang et al., Zeyde et al., GR, ANR, NE+LS, NE+NNLS, NE+LLE, A+ (0.5 mil), A+, A+ (16 atoms).');
else
    disp('We run only for Bicubic, GR, ANR and A+ methods, the other get the Bicubic result by default.');
end

fprintf('\n\n');

for d=10    %1024
    %d = 9; % 512
    %d = 8; %256
    %d = 7; %128
    %d = 6; % 64
    %d = 5; % 32
    %d=4;  %16
    %d=3;  %8
    %d=2; %4
    %d=1; %2

    tag = [input_dir '_x' num2str(upscaling) '_' num2str(dict_sizes(d)) 'atoms'];

    disp(['Upscaling x' num2str(upscaling) ' ' input_dir ' with Zeyde dictionary of size = ' num2str(dict_sizes(d))]);

    mat_file = ['conf_Zeyde_' num2str(dict_sizes(d)) '_finalx' num2str(upscaling)];

    if exist([mat_file '.mat'],'file')
        disp(['Load trained dictionary...' mat_file]);
        load(mat_file, 'conf');
    else
        disp(['Training dictionary of size ' num2str(dict_sizes(d)) ' using Zeyde approach...']);
        % Simulation settings
        conf.scale = upscaling; % scale-up factor
        conf.level = 1; % # of scale-ups to perform
        conf.window = [3 3]; % low-res. window size
        conf.border = [1 1]; % border of the image (to ignore)

        % High-pass filters for feature extraction (defined for upsampled low-res.)
        conf.upsample_factor = upscaling; % upsample low-res. into mid-res.
        O = zeros(1, conf.upsample_factor-1);
        G = [1 O -1]; % Gradient
        L = [1 O -2 O 1]/2; % Laplacian
        conf.filters = {G, G.', L, L.'}; % 2D versions
        conf.interpolate_kernel = 'bicubic';

        conf.overlap = [1 1]; % partial overlap (for faster training)
        if upscaling <= 2
            conf.overlap = [1 1]; % partial overlap (for faster training)
        end

        startt = tic;
        conf = learn_dict(conf, load_images(...
            glob('CVPR08-SR/Data/Training', '*.bmp') ...
            ), dict_sizes(d));
        conf.overlap = conf.window - [1 1]; % full overlap scheme (for better reconstruction)
        conf.trainingtime = toc(startt);
        toc(startt)

        save(mat_file, 'conf');

        % train call
    end

    if dict_sizes(d) < 1024
        lambda = 0.01;
    elseif dict_sizes(d) < 2048
        lambda = 0.1;
    elseif dict_sizes(d) < 8192
        lambda = 1;
    else
        lambda = 5;
    end

    %% GR
    if dict_sizes(d) < 10000
        conf.ProjM = inv(conf.dict_lores'*conf.dict_lores+lambda*eye(size(conf.dict_lores,2)))*conf.dict_lores';
        conf.PP = (1+lambda)*conf.dict_hires*conf.ProjM;
    else
        % here should be an approximation
        conf.PP = zeros(size(conf.dict_hires,1), size(conf.V_pca,2));
        conf.ProjM = [];
    end

    conf.filenames = glob(input_dir, pattern); % Cell array

    conf.desc = {'Original', 'Bicubic', 'Yang et al.', ...
        'Zeyde et al.', 'Our GR', 'Our ANR', ...
        'NE+LS','NE+NNLS','NE+LLE','Our A+ (0.5mil)','Our A+', 'Our A+ (16atoms)'};
    conf.results = {};

    %conf.points = [1:10:size(conf.dict_lores,2)];
    conf.points = [1:1:size(conf.dict_lores,2)];

    conf.pointslo = conf.dict_lores(:,conf.points);
    conf.pointsloPCA = conf.pointslo'*conf.V_pca';

    % precompute for ANR the anchored neighborhoods and the projection matrices for
    % the dictionary

    conf.PPs = [];
    if  size(conf.dict_lores,2) < 40
        clustersz = size(conf.dict_lores,2);
    else
        clustersz = 40;
    end
    D = abs(conf.pointslo'*conf.dict_lores);

    for i = 1:length(conf.points)
        [vals idx] = sort(D(i,:), 'descend');
        if (clustersz >= size(conf.dict_lores,2)/2)
            conf.PPs{i} = conf.PP;
        else
            Lo = conf.dict_lores(:, idx(1:clustersz));
            conf.PPs{i} = 1.01*conf.dict_hires(:,idx(1:clustersz))*inv(Lo'*Lo+0.01*eye(size(Lo,2)))*Lo';
        end
    end

    ANR_PPs = conf.PPs; % store the ANR regressors

    save([tag '_' mat_file '_ANR_projections_imgscale_' num2str(imgscale)],'conf');

    %% A+ computing the regressors
    Aplus_PPs = [];

    fname = ['Aplus_x' num2str(upscaling) '_' num2str(dict_sizes(d)) 'atoms' num2str(clusterszA) 'nn_5mil.mat'];

    if exist(fname,'file')
       load(fname);
    else
        %%
       disp('Compute A+ regressors');
       ttime = tic;
       tic
       [plores phires] = collectSamplesScales(conf, load_images(...
        glob('CVPR08-SR/Data/Training', '*.bmp')), 12, 0.98);

        if size(plores,2) > 5000000
            plores = plores(:,1:5000000);
            phires = phires(:,1:5000000);
        end
        number_samples = size(plores,2);

        % l2 normalize LR patches, and scale the corresponding HR patches
        l2 = sum(plores.^2).^0.5+eps;
        l2n = repmat(l2,size(plores,1),1);
        l2(l2<0.1) = 1;
        plores = plores./l2n;
        phires = phires./repmat(l2,size(phires,1),1);
        clear l2
        clear l2n

        llambda = 0.1;

        for i = 1:size(conf.dict_lores,2)
            D = pdist2(single(plores'),single(conf.dict_lores(:,i)'));
            [~, idx] = sort(D);
            Lo = plores(:, idx(1:clusterszA));
            Hi = phires(:, idx(1:clusterszA));
            Aplus_PPs{i} = Hi*inv(Lo'*Lo+llambda*eye(size(Lo,2)))*Lo';
%Aplus_PPs{i} = Hi*(inv(Lo*Lo'+llambda*eye(size(Lo,1)))*Lo)';
        end
        clear plores
        clear phires

        ttime = toc(ttime);
        save(fname,'Aplus_PPs','ttime', 'number_samples');
        toc
    end

    %% A+ (0.5mil) computing the regressors with 0.5 milion training samples
    Aplus05_PPs = [];

    fname = ['Aplus_x' num2str(upscaling) '_' num2str(dict_sizes(d)) 'atoms' num2str(clusterszA) 'nn_05mil.mat'];

    if exist(fname,'file')
       load(fname);
    else
        %%
       disp('Compute A+ (0.5 mil) regressors');
       ttime = tic;
       tic
       [plores phires] = collectSamplesScales(conf, load_images(...
        glob('CVPR08-SR/Data/Training', '*.bmp')), 1,1);

        if size(plores,2) > 500000
            plores = plores(:,1:500000);
            phires = phires(:,1:500000);
        end
        number_samples = size(plores,2);

        % l2 normalize LR patches, and scale the corresponding HR patches
        l2 = sum(plores.^2).^0.5+eps;
        l2n = repmat(l2,size(plores,1),1);
        l2(l2<0.1) = 1;
        plores = plores./l2n;
        phires = phires./repmat(l2,size(phires,1),1);
        clear l2
        clear l2n

        llambda = 0.1;

        for i = 1:size(conf.dict_lores,2)
            D = pdist2(single(plores'),single(conf.dict_lores(:,i)'));
            [~, idx] = sort(D);
            Lo = plores(:, idx(1:clusterszA));
            Hi = phires(:, idx(1:clusterszA));
            Aplus05_PPs{i} = Hi*inv(Lo'*Lo+llambda*eye(size(Lo,2)))*Lo';
        end
        clear plores
        clear phires

        ttime = toc(ttime);
        save(fname,'Aplus05_PPs','ttime', 'number_samples');
        toc
    end

    %% load the A+ (16 atoms) for comparison results
    conf16 = [];
    fname = ['Aplus_x' num2str(upscaling) '_16atoms' num2str(clusterszA) 'nn_05mil.mat'];
    fnamec = ['Set14_x' num2str(upscaling) '_16atoms_conf_Zeyde_16_finalx' num2str(upscaling) '_ANR_projections_imgscale_' num2str(imgscale) '.mat'];
    if exist(fname,'file') && exist(fnamec,'file')
       kk = load(fnamec);
       conf16 = kk.conf;
       kk = load(fname);
       conf16.PPs = kk.Aplus05_PPs;
       clear kk
    end
    %%
    conf.result_dirImages = qmkdir([input_dir '/results_' tag]);
    conf.result_dirImagesRGB = qmkdir([input_dir '/results_' tag 'RGB']);
    conf.result_dir = qmkdir(['Results-' datestr(now, 'YYYY-mm-dd_HH-MM-SS')]);
    conf.result_dirRGB = qmkdir(['ResultsRGB-' datestr(now, 'YYYY-mm-dd_HH-MM-SS')]);

    %%
    t = cputime;

    conf.countedtime = zeros(numel(conf.desc),numel(conf.filenames));

    res =[];
    for i = 1:numel(conf.filenames)
        f = conf.filenames{i};
        [p, n, x] = fileparts(f);
        [img, imgCB, imgCR] = load_images({f});
        if imgscale<1
            img = resize(img, imgscale, conf.interpolate_kernel);
            imgCB = resize(imgCB, imgscale, conf.interpolate_kernel);
            imgCR = resize(imgCR, imgscale, conf.interpolate_kernel);
        end
        sz = size(img{1});

        fprintf('%d/%d\t"%s" [%d x %d]\n', i, numel(conf.filenames), f, sz(1), sz(2));

        img = modcrop(img, conf.scale^conf.level);
        imgCB = modcrop(imgCB, conf.scale^conf.level);
        imgCR = modcrop(imgCR, conf.scale^conf.level);

            low = resize(img, 1/conf.scale^conf.level, conf.interpolate_kernel);
            if ~isempty(imgCB{1})
                lowCB = resize(imgCB, 1/conf.scale^conf.level, conf.interpolate_kernel);
                lowCR = resize(imgCR, 1/conf.scale^conf.level, conf.interpolate_kernel);
            end

        interpolated = resize(low, conf.scale^conf.level, conf.interpolate_kernel);
        if ~isempty(imgCB{1})
            interpolatedCB = resize(lowCB, conf.scale^conf.level, conf.interpolate_kernel);
            interpolatedCR = resize(lowCR, conf.scale^conf.level, conf.interpolate_kernel);
        end

        res{1} = interpolated;

        if (flag == 1) && (dict_sizes(d) == 1024) && (upscaling==3)
            startt = tic;
            res{2} = {yima(low{1}, upscaling)};
            toc(startt)
            conf.countedtime(2,i) = toc(startt);
        else
            res{2} = interpolated;
        end

        if (flag == 1)
            startt = tic;
            res{3} = scaleup_Zeyde(conf, low);
            toc(startt)
            conf.countedtime(3,i) = toc(startt);
        else
            res{3} = interpolated;
        end

        %if flag == 1
            startt = tic;
            res{4} = scaleup_GR(conf, low);
            toc(startt)
            conf.countedtime(4,i) = toc(startt);
        %else
            %res{4} = interpolated;
        %end

        startt = tic;
        conf.PPs = ANR_PPs;
        res{5} = scaleup_ANR(conf, low);
        toc(startt)
        conf.countedtime(5,i) = toc(startt);

        if flag == 1
            startt = tic;
            if 12 < dict_sizes(d)
                res{6} = scaleup_NE_LS(conf, low, 12);
            else
                res{6} = scaleup_NE_LS(conf, low, dict_sizes(d));
            end
            toc(startt)
            conf.countedtime(6,i) = toc(startt);
        else
            res{6} = interpolated;
        end

        if flag == 1
            startt = tic;
            if 24 < dict_sizes(d)
                res{7} = scaleup_NE_NNLS(conf, low, 24);
            else
                res{7} = scaleup_NE_NNLS(conf, low, dict_sizes(d));
            end
            toc(startt)
            conf.countedtime(7,i) = toc(startt);
        else
            res{7} = interpolated;
        end

        if flag == 1
            startt = tic;
            if 24 < dict_sizes(d)
                res{8} = scaleup_NE_LLE(conf, low, 24);
            else
                res{8} = scaleup_NE_LLE(conf, low, dict_sizes(d));
            end
            toc(startt)
            conf.countedtime(8,i) = toc(startt);
        else
            res{8} = interpolated;
        end

        % A+ (0.5 mil)
        if flag == 1 && ~isempty(Aplus05_PPs)
            fprintf('A+ (0.5mil)\n');
            conf.PPs = Aplus05_PPs;
            startt = tic;
            res{9} = scaleup_ANR(conf, low);
            toc(startt)
            conf.countedtime(9,i) = toc(startt);
        else
            res{9} = interpolated;
        end

        % A+
        if ~isempty(Aplus_PPs)
            fprintf('A+\n');
            conf.PPs = Aplus_PPs;
            startt = tic;
            res{10} = scaleup_ANR(conf, low);
            toc(startt)
            conf.countedtime(10,i) = toc(startt);
        else
            res{10} = interpolated;
        end
        % A+ 16atoms
        if flag == 1 && ~isempty(conf16)
            fprintf('A+ 16atoms\n');
            startt = tic;
            res{11} = scaleup_ANR(conf16, low);
            toc(startt)
            conf.countedtime(11,i) = toc(startt);
        else
            res{11} = interpolated;
        end

        result = cat(3, img{1}, interpolated{1}, res{2}{1}, res{3}{1}, ...
            res{4}{1}, res{5}{1}, res{6}{1}, res{7}{1}, res{8}{1}, ...
            res{9}{1}, res{10}{1}, res{11}{1});

        result = shave(uint8(result * 255), conf.border * conf.scale);

        if ~isempty(imgCB{1})
            resultCB = interpolatedCB{1};
            resultCR = interpolatedCR{1};
            resultCB = shave(uint8(resultCB * 255), conf.border * conf.scale);
            resultCR = shave(uint8(resultCR * 255), conf.border * conf.scale);
        end

        conf.results{i} = {};
        for j = 1:numel(conf.desc)
            conf.results{i}{j} = fullfile(conf.result_dirImages, [n sprintf('[%d-%s]', j, conf.desc{j}) x]);
            imwrite(result(:, :, j), conf.results{i}{j});

            conf.resultsRGB{i}{j} = fullfile(conf.result_dirImagesRGB, [n sprintf('[%d-%s]', j, conf.desc{j}) x]);
            if ~isempty(imgCB{1})
                rgbImg = cat(3,result(:,:,j),resultCB,resultCR);
                rgbImg = ycbcr2rgb(rgbImg);
            else
                rgbImg = cat(3,result(:,:,j),result(:,:,j),result(:,:,j));
            end

            imwrite(rgbImg, conf.resultsRGB{i}{j});
        end
        conf.filenames{i} = f;
    end
    conf.duration = cputime - t;

    % Test performance
    scores = run_comparison(conf);
    process_scores_Tex(conf, scores,length(conf.filenames));

    run_comparisonRGB(conf); % provides color images and HTML summary
    %%
    save([tag '_' mat_file '_results_imgscale_' num2str(imgscale)],'conf','scores');
end
%


function [U,G,I,J] = remove_small_components(V,F,varargin)
  % REMOVE_SMALL_COMPONENTS
  %
  % [U,G,I,J] = remove_small_components(V,F)
  %
  % Inputs:
  %   V  #V by 3 list of vertex positions
  %   F  #F by 3 list of face indices into rows of V
  %   Optional:
  %     'Tol' followed by minimum volume (or surface area) to use OR the word
  %       'max' to keep only biggest component {0.0001*total_vol}
  %     'Volumetric' followed by whether to use volume or surface area
  % Outputs:
  %   U  #U by 3 list of vertex positions
  %   G  #G by 3 list of face indices into rows of U
  %   I  #V by 1 list of indices such that: G = I(F)
  %   J  #G by 1 list of indices into F
  %

  tol = [];
  volumetric = true;
  % Map of parameter names to variable names
  params_to_variables = containers.Map( ...
    {'Tol','Volumetric'}, {'tol','volumetric'});
  v = 1;
  while v <= numel(varargin)
    param_name = varargin{v};
    if isKey(params_to_variables,param_name)
      assert(v+1<=numel(varargin));
      v = v+1;
      % Trick: use feval on anonymous function to use assignin to this workspace
      feval(@()assignin('caller',params_to_variables(param_name),varargin{v}));
    else
      error('Unsupported parameter: %s',varargin{v});
    end
    v=v+1;
  end

  if isempty(tol)
    if volumetric
      [~,total] = centroid(V,F);
    else
      total = sum(doublearea(V,F))*0.5;
    end
    tol = 0.0001*total;
  end

  [~,C] = connected_components(F);
  nc = max(C);
  val = zeros(nc,1);
  for i = 1:nc
    Fi = F(C==i,:);
    if volumetric
      [~,val(i)] = centroid(V,Fi);
    else
      val(i) = 0.5*sum(doublearea(V,Fi));
    end
  end

  if isnumeric(tol)
    J = find(ismember(C,find(val>tol)));
  else
    assert(strcmp(tol,'max'));
    [~,max_c] = max(val);
    J = find(C==max_c);
  end
  F = F(J,:);
  [U,I] = remove_unreferenced(V,F);
  G = I(F);
end


function problem = getDefaultOptions(problem)
% problem = getDefaultOptions(problem)
%
% This function fills in any blank entries in the problem.options struct.
% It is designed to be called from inside of optimTraj.m, and not by the
% user.
%

%%%% Top-level default options:
OPT.method = 'trapezoid';
OPT.verbose = 2;
OPT.defaultAccuracy = 'medium';

%%%% Basic setup

% ensure that options is not empty
if ~isfield(problem,'options')
    problem.options.method = OPT.method;
end
opt = problem.options;

% Loop over each options struct and fill in top-level options
for i=1:length(opt)
    if ~isfield(opt(i),'method')
        opt(i).method = OPT.method;
    elseif isempty(opt(i).method)
        opt(i).method = OPT.method;
    end
    if ~isfield(opt(i),'verbose')
        opt(i).verbose = OPT.verbose;
    elseif isempty(opt(i).verbose)
        opt(i).verbose = OPT.verbose;
    end
    if ~isfield(opt(i),'defaultAccuracy')
        opt(i).defaultAccuracy = OPT.defaultAccuracy;
    elseif isempty(opt(i).defaultAccuracy)
        opt(i).defaultAccuracy = OPT.defaultAccuracy;
    end
end

% Figure out basic problem size:
nState = size(problem.guess.state,1);
nControl = size(problem.guess.control,1);

% Loop over opt and fill in nlpOpt struct:
for i=1:length(opt)
    switch opt(i).verbose
        case 0
            NLP_display = 'notify';
        case 1
            NLP_display = 'final-detailed';
        case 2
            NLP_display = 'iter';
        case 3
            NLP_display = 'iter-detailed';
        otherwise
            error('Invalid value for options.verbose');
    end
    switch opt(i).defaultAccuracy
        case 'low'
            OPT.nlpOpt = optimset(...
                'Display',NLP_display,...
                'TolFun',1e-4,...
                'MaxIter',200,...
                'MaxFunEvals',1e4*(nState+nControl));
        case 'medium'
            OPT.nlpOpt = optimset(...
                'Display',NLP_display,...
                'TolFun',1e-6,...
                'MaxIter',400,...
                'MaxFunEvals',5e4*(nState+nControl));
        case 'high'
            OPT.nlpOpt = optimset(...
                'Display',NLP_display,...
                'TolFun',1e-8,...
                'MaxIter',800,...
                'MaxFunEvals',1e5*(nState+nControl));
        otherwise
            error('Invalid value for options.defaultAccuracy')
    end
    if isfield(opt(i),'nlpOpt')
        if isstruct(opt(i).nlpOpt) && ~isempty(opt(i).nlpOpt)
            names = fieldnames(opt(i).nlpOpt);
            for j=1:length(names)
                if ~isfield(OPT.nlpOpt,names{j})
                    disp(['WARNING: options.nlpOpt.' names{j} ' is not a valid option']);
                else
                    OPT.nlpOpt.(names{j}) = opt(i).nlpOpt.(names{j});
                end
            end
        end
    end
    opt(i).nlpOpt = OPT.nlpOpt;
end

% Check ChebFun dependency:
missingChebFun = false;
for i=1:length(opt)
if strcmp(opt(i).method,'chebyshev')
try
    chebpts(3);  %Test call to chebfun
catch ME %#ok<NASGU>
    missingChebFun = true;
    opt(i).method = 'trapezoid';  %Force default method
end
end
end
if missingChebFun
   warning('''chebyshev'' method requires the Chebfun toolbox');
   disp('   --> Install Chebfun toolbox:  (http://www.chebfun.org/)');
   disp('   --> Running with default method instead (''trapezoid'')');
end

% Fill in method-specific paramters:
for i=1:length(opt)
    OPT_method = opt(i).method;
    switch OPT_method
        case 'trapezoid'
            OPT.trapezoid = defaults_trapezoid(opt(i).defaultAccuracy);
        case 'hermiteSimpson'
            OPT.hermiteSimpson = defaults_hermiteSimpson(opt(i).defaultAccuracy);
        case 'chebyshev'
            OPT.chebyshev = defaults_chebyshev(opt(i).defaultAccuracy);
        case 'multiCheb'
            OPT.multiCheb = defaults_multiCheb(opt(i).defaultAccuracy);
        case 'rungeKutta'
            OPT.rungeKutta = defaults_rungeKutta(opt(i).defaultAccuracy);
        case 'gpops'
            OPT.gpops = defaults_gpops(opt(i).defaultAccuracy);
        otherwise
            error('Invalid value for options.method');
    end
    if isfield(opt(i),OPT_method)
        if isstruct(opt(i).(OPT_method)) && ~isempty(opt(i).(OPT_method))
            names = fieldnames(opt(i).(OPT_method));
            for j=1:length(names)
                if ~isfield(OPT.(OPT_method),names{j})
                    disp(['WARNING: options.' OPT_method '.' names{j} ' is not a valid option']);
                else
                    OPT.(OPT_method).(names{j}) = opt(i).(OPT_method).(names{j});
                end
            end
        end
    end
    opt(i).(OPT_method) = OPT.(OPT_method);
end

problem.options = opt;
end

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                    Method-specific parameters                           %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

function OPT_trapezoid = defaults_trapezoid(accuracy)

switch accuracy
    case 'low'
        OPT_trapezoid.nGrid = 12;
    case 'medium'
        OPT_trapezoid.nGrid = 30;
    case 'high'
        OPT_trapezoid.nGrid = 60;
    otherwise
        error('Invalid value for options.defaultAccuracy')
end

OPT_trapezoid.adaptiveDerivativeCheck = 'off';

end

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

function OPT_hermiteSimpson = defaults_hermiteSimpson(accuracy)

switch accuracy
    case 'low'
        OPT_hermiteSimpson.nSegment = 10;
    case 'medium'
        OPT_hermiteSimpson.nSegment = 20;
    case 'high'
        OPT_hermiteSimpson.nSegment = 40;
    otherwise
        error('Invalid value for options.defaultAccuracy')
end

OPT_hermiteSimpson.adaptiveDerivativeCheck = 'off';

end

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

function OPT_chebyshev = defaults_chebyshev(accuracy)

switch accuracy
    case 'low'
        OPT_chebyshev.nColPts = 9;
    case 'medium'
        OPT_chebyshev.nColPts = 13;
    case 'high'
        OPT_chebyshev.nColPts = 23;
    otherwise
        error('Invalid value for options.defaultAccuracy')
end

end

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

function OPT_multiCheb = defaults_multiCheb(accuracy)

switch accuracy
    case 'low'
        OPT_multiCheb.nColPts = 6;
        OPT_multiCheb.nSegment = 3;
    case 'medium'
        OPT_multiCheb.nColPts = 8;
        OPT_multiCheb.nSegment = 6;
    case 'high'
        OPT_multiCheb.nColPts = 8;
        OPT_multiCheb.nSegment = 12;
    otherwise
        error('Invalid value for options.defaultAccuracy')
end

end

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

function OPT_rungeKutta = defaults_rungeKutta(accuracy)

switch accuracy
    case 'low'
        OPT_rungeKutta.nSegment = 10;
        OPT_rungeKutta.nSubStep = 2;
    case 'medium'
        OPT_rungeKutta.nSegment = 20;
        OPT_rungeKutta.nSubStep = 2;
    case 'high'
        OPT_rungeKutta.nSegment = 20;
        OPT_rungeKutta.nSubStep = 4;
    otherwise
        error('Invalid value for options.defaultAccuracy')
end

OPT_rungeKutta.adaptiveDerivativeCheck = 'off';

end

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

function OPT_gpops = defaults_gpops(accuracy)

OPT_gpops.bounds.phase.integral.lower = -inf;
OPT_gpops.bounds.phase.integral.upper = inf;
OPT_gpops.guess.phase.integral = 0;

OPT_gpops.name = 'OptimTraj_GPOPS';
OPT_gpops.auxdata = [];
OPT_gpops.nlp.solver = 'ipopt'; % {'ipopt','snopt'}
OPT_gpops.derivatives.dependencies = 'full';  %�full�, �sparse� or �sparseNaN�
OPT_gpops.derivatives.supplier = 'sparseCD'; %'sparseCD';  %'adigator'
OPT_gpops.derivatives.derivativelevel = 'first'; %'second';
OPT_gpops.mesh.method = 'hp-PattersonRao';
OPT_gpops.method = 'RPM-Integration';
OPT_gpops.mesh.phase.colpoints = 10*ones(1,10);
OPT_gpops.mesh.phase.fraction = ones(1,10)/10;
OPT_gpops.scales.method = 'none'; % { 'none' , automatic-hybridUpdate' , 'automatic-bounds';

switch accuracy
    case 'low'
        OPT_gpops.mesh.tolerance = 1e-2;
        OPT_gpops.mesh.maxiterations = 0;
    case 'medium'
        OPT_gpops.mesh.tolerance = 1e-3;
        OPT_gpops.mesh.maxiterations = 1;
    case 'high'
        OPT_gpops.mesh.tolerance = 1e-4;
        OPT_gpops.mesh.maxiterations = 3;
    otherwise
        error('Invalid value for options.defaultAccuracy')
end

end


function obj = updateTernaryPlotPro(obj, ternaryIndex)

    %-AXIS INDEX-%
    axIndex = obj.getAxisIndex(obj.State.Plot(ternaryIndex).AssociatedAxis);

    %-GET DATA STRUCTURES-%
    ternaryData = get(obj.State.Plot(ternaryIndex).Handle);
    axisData = get(obj.State.Plot(ternaryIndex).AssociatedAxis);
    figureData = get(obj.State.Figure.Handle);

    %-CHECK FOR MULTIPLE AXES-%
    [xsource, ysource] = findSourceAxis(obj, axIndex);

    %=========================================================================%
    %
    %-UPDATE TRACE PLOT-%
    %
    %=========================================================================%

    %-get plot data-%
    xData = ternaryData.XData;
    yData = ternaryData.YData;

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

    %-set trace-%
    for t = 1:size(xData,2)

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

        %-get new ternaryIndex-%
        if t > 1
            obj.PlotOptions.nPlots = obj.PlotOptions.nPlots + 1;
            ternaryIndex = obj.PlotOptions.nPlots;
        end

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

        %-trace type-%
        obj.data{ternaryIndex}.type = 'scatterternary';
        obj.data{ternaryIndex}.subplot = sprintf('ternary%d', xsource+1);

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

        %-set mode and properties for trace-%
        if ~strcmpi('none', ternaryData.Marker) && ~strcmpi('none', ternaryData.LineStyle)
            obj.data{ternaryIndex}.mode = 'lines+markers';
            obj.data{ternaryIndex}.marker = extractPatchMarker(ternaryData);

        elseif ~strcmpi('none', ternaryData.Marker)
            obj.data{ternaryIndex}.mode = 'markers';
            obj.data{ternaryIndex}.marker = extractPatchMarker(ternaryData);

        elseif ~strcmpi('none', ternaryData.LineStyle)
            obj.data{ternaryIndex}.mode = 'lines';
            obj.data{ternaryIndex}.line = extractPatchLine(ternaryData);

        else
            obj.data{ternaryIndex}.mode = 'none';
        end

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

        %-convert from cartesian coordinates to trenary points-%
        yTernData = yData(:,t); yTernData(end+1) = yData(1,t);
        xTernData = xData(:,t); xTernData(end+1) = xData(1,t);

        aData = yTernData/sin(deg2rad(60));
        bData = 1 - xTernData - yTernData*cot(deg2rad(60));

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

        %-set plot data-%
        obj.data{ternaryIndex}.a = aData;
        obj.data{ternaryIndex}.b = bData;

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

        %-some trace properties-%
        obj.data{ternaryIndex}.name = ternaryData.DisplayName;
        obj.data{ternaryIndex}.showscale = false;
        obj.data{ternaryIndex}.visible = strcmp(ternaryData.Visible,'on');

        %-trace coloring-%
        faceColor = ternaryData.FaceColor;

        if isnumeric(faceColor)
            fillColor = sprintf('rgb(%f,%f,%f)', 255*faceColor);

        else
            cMap = figureData.Colormap;
            nColors = size(cMap,1);

            switch faceColor

                case 'none'
                    fillColor = 'rgba(0,0,0,0)';

                case {'flat', 'interp'}

                    switch ternaryData.CDataMapping

                        case 'scaled'
                            cMin = axisData.CLim(1);
                            cMax = axisData.CLim(2);

                            if strcmpi(faceColor, 'flat')
                                cData = ternaryData.ZData(1,t);
                            elseif strcmpi(faceColor, 'interp')
                                cData = max(ternaryData.ZData(:,t));
                            end

                            cData = max(min(cData, cMax), cMin);
                            cData = (cData - cMin)/diff(axisData.CLim);
                            cData = 1 + floor( cData*(nColors-1) );

                            fillColor = sprintf('rgb(%f,%f,%f)', 255*cMap(cData,:));

                        case 'direct'
                            fillColor = sprintf('rgb(%f,%f,%f)', 255*cMap(ternary(1,t),:));
                    end
            end
        end

        obj.data{ternaryIndex}.fillcolor = fillColor;
        obj.data{ternaryIndex}.fill = 'toself';

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

        %-trace legend-%
        obj.data{ternaryIndex}.showlegend = false;

        %-------------------------------------------------------------------------%
    end

    %=========================================================================%
    %
    %-UPDATE TERNARY AXES-%
    %
    %=========================================================================%

    %-set domain plot-%
    xo = axisData.Position(1);
    yo = axisData.Position(2);
    w = axisData.Position(3);
    h = axisData.Position(4);

    ternary.domain.x = min([xo xo + w],1);
    ternary.domain.y = min([yo yo + h],1);

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

    %-label settings-%
    l = 1; t = 1;
    labelLetter = {'b', 'a', 'c'};

    for n = 1:length(axisData.Children)
        if strcmpi(axisData.Children(n).Type, 'text')
            stringText = axisData.Children(n).String;

            if any(isletter(stringText))
                labelIndex(l) = n;
                l = l + 1;
            else
                tickIndex(t) = n;
                t = t + 1;
            end
        end
    end

    for l = 1:length(labelIndex)
        n = labelIndex(l);
        patterText = sprintf('ternary.%saxis.title', labelLetter{l});

        labelText = axisData.Children(n).String;
        labelFontColor = sprintf('rgb(%f,%f,%f)', axisData.Children(n).Color);
        labelFontSize = 1.5 * axisData.Children(n).FontSize;
        labelFontFamily = matlab2plotlyfont(axisData.Children(n).FontName);

        eval(sprintf('%s.text = labelText;', patterText));
        eval(sprintf('%s.font.color = labelFontColor;', patterText));
        eval(sprintf('%s.font.size = labelFontColor;', patterText));
        eval(sprintf('%s.font.family = labelFontFamily;', patterText));
    end

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

    %-tick settings-%
    t0 = tickIndex(1); t1 = tickIndex(2);
    tick0 = str2num(axisData.Children(t0).String);
    tick1 = str2num(axisData.Children(t1).String);
    dtick = tick1 - tick0;

    tickFontColor = sprintf('rgb(%f,%f,%f)', axisData.Children(t0).Color);
    tickFontSize = 1.0 * axisData.Children(t0).FontSize;
    tickFontFamily = matlab2plotlyfont(axisData.Children(t0).FontName);

    for l = 1:3
        patterText = sprintf('ternary.%saxis', labelLetter{l});

        eval(sprintf('%s.tick0 = tick0;', patterText));
        eval(sprintf('%s.dtick = dtick;', patterText));
        eval(sprintf('%s.tickfont.color = tickFontColor;', patterText));
        eval(sprintf('%s.tickfont.size = tickFontSize;', patterText));
        eval(sprintf('%s.tickfont.family = tickFontFamily;', patterText));
    end

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

    %-set ternary axes to layout-%
    obj.layout = setfield(obj.layout, sprintf('ternary%d', xsource+1), ternary);

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

    obj.PlotlyDefaults.isTernary = true;

    %-----------------------------------------------------------------------------%
end

function rad = deg2rad(deg)
    rad = deg / 180 * pi;
end


function DEM = spm_MDP_DEM(DEM,demi,O,o)
% auxiliary (link) function for mixed hierarchical (MDP/DEM) models
% FORMAT DEM = spm_MDP_DEM(DEM,demi,O,o)
%
% DEM      - DEM structure
% demi     - mapping from discrete outcomes to hidden causes
%  demi.C  - cell array of true causes for each combination of outcomes
%            the appropriate array is then placed in DEM.C
%  demi.U  - cell array of hidden causes for each combination of outcomes
%            the Bayesian model average is placed in DEM.U
% O{g}     - cell array of priors over discrete outcomes
% o(g x 1) - vector of true outcomes
%
% completes the following fields:
%   DEM.X{g} - posterior probability over g models and t times
%
% This routine performs a Bayesian model comparison using (DEM) Bayesian
% filtering and places the results in fields of the DEM structure; so that
% MDP schemes can pick them up as likelihood terms in the next hierarchical
% level. The outcomes of the (discrete) MDP scheme at the superordinate
% level specify the hidden causes at the current level. These enter as
% Bayesian model averages of the continuous causes. The resulting
% posterior over hidden causes then furnishes the posterior over outcomes
% using Bayesian model reduction, based on the free energy accumulated
% (integrated) over time. This free energy is supplemented with the prior
% over discrete outcomes; thereby constituting a posterior over outcomes.
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_MDP_DEM.m 7560 2019-03-29 14:39:38Z thomas $

% evaluate true values and priors over causes given discrete states
%--------------------------------------------------------------------------
po    = spm_cross(O);
ind   = num2cell(o);
DEM.C = demi.C{ind{:}};
DEM.U = 0;
for i = 1:size(po,1)
    for j = 1:size(po,2)
        for k = 1:size(po,3)
            DEM.U = DEM.U + demi.U{i,j,k}*po(i,j,k);
        end
    end
end

% integrate system to generate data
%--------------------------------------------------------------------------
DEM.db = 0;
DEM    = spm_ADEM(DEM);

% posterior probability over discrete states models
%==========================================================================
nv    = size(DEM.U,1);
nt    = size(DEM.U,2);
ic    = (1:nv) + size(DEM.qU.C{1},1) - nv;
P     = spm_zeros(po);
for i = 1:ndims(P)
    x{i} = ones(size(P,i),1);
end

% prior potential
%--------------------------------------------------------------------------
F     = log(po);

% and accumulate log evidence
%--------------------------------------------------------------------------
for t = 1:nt

    % posteriors and priors for this time point
    %----------------------------------------------------------------------
    qE    = DEM.qU.v{end}(:,t);
    qC    = DEM.qU.C{t}(ic,ic);
    pE    = DEM.U(:,t);
    pC    = DEM.pU.C{t}(ic,ic);
    gt    = t > 4;
    for i = 1:size(po,1)
        for j = 1:size(po,2)
            for k = 1:size(po,3)
                if po(i,j,k) < exp(-3)
                    F(i,j,k) = - exp(64);
                else
                    rE       = demi.U{i,j,k}(:,t);
                    F(i,j,k) = F(i,j,k) + gt*spm_log_evidence(qE,qC,pE,pC,rE,pC);
                end
            end
        end
    end

    % Bayesian model comparison
    %----------------------------------------------------------------------
    P(:)  = spm_softmax(F(:));

    % marginal posteriors
    %----------------------------------------------------------------------
    for g = 1:numel(O)
        X{g}(:,t) = spm_dot(P,x,g);
    end
end

% return  probability over models (i.e., outcomes at subordinate level)
%--------------------------------------------------------------------------
DEM.X = X;


function hrf = glm_hrf(params, tSeries, stim)
%
% hrf = glm_hrf(params, <tSeries>, <stim>);
%
% Returns the hemodynamic impulse response function to use for GLM
% analyses, given the current event analysis settings. The hrf will
% always be returned in units of MR frames.
%
% 'tSeries' and 'stim' are optional input arguments, which are only
% needed if params.glmHRF==1: compute the HRF from the data and
% selected conditions in params.snrConds.
%
% ras, 01/2007.
verbose = prefsVerboseCheck;

if ischar(params.glmHRF)
    hrfPath = fullfile(hrfDir, params.glmHRF);
    load(hrfPath, 'hrf', 'timeWindow', 'tr');
%     if verbose, fprintf('Loading HRF from file %s\n', hrfPath); end
    return
end

% for the pre-defined (Boynton, SPM, Dale&Buckner) HRFs, we'll need
% to get the time window sampled in units of MR frames, excluding pre-
% onset periods:
if ismember(params.glmHRF, [2 3 4])
    tr = params.framePeriod;
    maxT = max(params.timeWindow);          % in secs
    t = 0:tr:maxT;                          % also in secs
end

switch params.glmHRF
    case 1,
        % estimate hrf as mean time course to all stim
        if isempty(tSeries)
            errmsg = ['Need tSeries to estimate mean trial response ' ...
                '(glmHRF option 1)'];
            error(errmsg);
        end

%         if verbose, disp('Estimating mean trial response for GLM'); end
        tSeries = squeeze(mean(tSeries,2)); % mean across voxels
        anal = er_chopTSeries2(tSeries(:),stim,params);
        postStim = find(anal.timeWindow>=0);
        hrf = anal.meanTcs(postStim,anal.condNums>0);
        hrf = hrf(:,params.snrConds);
        if size(hrf,2) > 1
            hrf = nanmeanDims(hrf,2);
        end

    case 2,  % boynton gamma function
        % create sub-params struct
%         if verbose, disp('Boynton & Heeger gamma HRF'); end
        [p params] = glm_getHrfParams(params);
        n = params.glmHRF_params(1);
        tau = params.glmHRF_params(2);
        delay = params.glmHRF_params(3);
        hrf = boyntonHIRF(t, n, tau, delay);

    case 3,  % spm difference-of-gammas
%         if verbose, disp('SPM difference of gamma HRF'); end
        [p params] = glm_getHrfParams(params);
        hrf = spm_hrf(tr, p);

    case 4, % dale & buckner HRF
%         if verbose, disp('Dale and Buckner 2000 HRF: t^2*exp(-t)'); end
        [p params] = glm_getHrfParams(params);
        hrf = fmri_hemodyn(t, p(1), p(2));

    case 5, % saved HRF from file
        % choose from file
%         if verbose, disp('HRF selected from file \n'); end
        [f p] = myUiGetFile(hrfDir, '*.mat', 'Select a saved HRF File...');
        if f==0 % user canceled
            disp('User canceled -- setting HRF to Boynton Gamma')
            params.glmHRF = 2;
        else
            params.glmHRF = f(1:end-4);
        end
        hrf = glm_hrf(params);
        % TO DO: deal w/ situations in which the HRF was
        % saved using diff't TR, time window

    otherwise, error('Invalid specification for params.glmHRF.')
end

return