%% --- tutorial script for spectral analysis ---

% This tutorial demonstrates the use of BCILAB to learn basic predictive models using
% spectral properties of the data, here operating on motor cortex idle oscillations.
%
% The used data set contains a sequence of trials in which a subject was instructed to imagine
% moving either the left hand or the right hand. Markers in the data set indicate the timing and
% type of these instructions (types are: 'StimulusCode_2' and 'StimulusCode_3').
%
% Notes: StimulusCode_1 indicates onset of a "resting" / "relaxing" condition.
%
% Data courtesy of Romain Grandchamp, CERCO, Toulouse, France. Reference:
%   Romain Grandchamp, Arnaud Delorme, "NeuroTRIP: A Framework for Bridging between Open Source Software. Application to Training a Brain Machine Interface,"
%   Fifth International Conference on Signal Image Technology and Internet Based Systems, pp.451-457, 2009
%
%#ok<*ASGLU,*NASGU,*SNASGU> % turn off a few editor warnings...

%% --- using the Common Spatial Pattern method ---

% load the data set (BCI2000 format)
traindata = io_loadset('bcilab:/userdata/tutorial/imag_movements1/calib/DanielS001R01.dat','channels',1:29);

% define the approach (here: Common Spatial Patterns without any customization)
myapproach = 'CSP';

% learn a predictive model
[trainloss,lastmodel,laststats] = bci_train('Data',traindata,'Approach',myapproach,'TargetMarkers',{'StimulusCode_2','StimulusCode_3'});
disp(['training mis-classification rate: ' num2str(trainloss*100,3) '%']);

% visualize results
bci_visualize(lastmodel)

%% --- using the Common Spatial Pattern method with some custom options ---

% load the data set (BCI2000 format)
traindata = io_loadset('bcilab:/userdata/tutorial/imag_movements1/calib/DanielS001R01.dat','channels',1:29);

% define the approach
% Note: The settings found in the GUI "Review/Edit Approach" Panel can be translated literally
%       into cell array representations as below. Each paradigm has a few top-level parameter groups
%       (for CSP: SignalProcessing, FeatureExtraction, etc), which in turn have sub-parameters
%       (e.g., SignalProcessing has EpochExtraction, SpectralSelection, Resampling, etc.), and so
%       on. Some parameters are numbers, strings, cell arrays, etc. You only need to specify those
%	    parameters where you actually want to deviate from the paradigm's defaults.
%
% For illustratory purposes, we use a different window relative to the target markers (0.5s to 3s after),
% and a somewhat customized FIR frequency filter with a pass-band between ~7.5Hz and ~27Hz.
myapproach = {'CSP' 'SignalProcessing',{'EpochExtraction',[0.5 3],'FIRFilter',[7 8 26 28]}};

% learn a predictive model
[trainloss,lastmodel,laststats] = bci_train('Data',traindata,'Approach',myapproach,'TargetMarkers',{'StimulusCode_2','StimulusCode_3'});
disp(['training mis-classification rate: ' num2str(trainloss*100,3) '%']);

% visualize results
bci_visualize(lastmodel)

%% --- applying the CSP model to some data (here: same data) ---

% apply the previously learned model to a data set (querying it for each target marker in the data)
[prediction,loss,teststats,targets] = bci_predict(lastmodel,traindata);

% display the results
disp(['test mis-classification rate: ' num2str(loss*100,3) '%']);
disp(['  predicted classes: ',num2str(round(prediction{2}*prediction{3})')]);  % class probabilities * class values
disp(['  true classes     : ',num2str(round(targets)')]);

%% --- the same using pseudo-online processing ---

% This function is quite flexible, but here we only ask it to query the BCI seconds after each of the given markers
[predictions,latencies] = onl_simulate(traindata,lastmodel,'markers',{'StimulusCode_2','StimulusCode_3'},'offset',3);

% now check the prediction accuracy by hand (we knew the correct target label at each of the markers)
accuracy = mean(argmax(predictions') == targets');
disp(['pseudo-online mis-classification rate: ' num2str((1-accuracy)*100,3) '%']);

%% --- using the Spectrally weighted Common Spatial Pattern (Spec-CSP) method ---
% this method automatically learns the spectral filters (within a pre-defined range), but it
% may run into a local optimum or over-fit spuriously correlated bands

% load the data set
traindata = io_loadset('bcilab:/userdata/tutorial/imag_movements1/calib/DanielS001R01.dat','channels',1:29);

% define the approach
myapproach = {'SpecCSP' 'SignalProcessing',{'EpochExtraction',[0.5 3]}};

% learn a predictive model
[trainloss,lastmodel,laststats] = bci_train('Data',traindata,'Approach',myapproach,'TargetMarkers',{'StimulusCode_2','StimulusCode_3'}); %#ok<>

% visualize results
bci_visualize(lastmodel)

%% --- test the learned model in simulated real-time processing ---
% ( click into the figure to stop the update (and make sure that your click was registered) )

% load feedback session
testdata = io_loadset('bcilab:/userdata/tutorial/imag_movements1/feedback/DanielS001R01.dat','channels',1:29);

% play it back in real time
run_readdataset('Dataset',testdata);

% process data in real time using lastmodel, and visualize outputs
run_writevisualization('Model',lastmodel, 'VisFunction','bar(y);ylim([0 1])');

% make sure that the online processing gets terminated...
disp('Click into the figure to stop online processing.');
waitforbuttonpress; onl_clear; close(gcf);

%% --- train an alternative model with parameter search ---
% (over possible values for the number of pattern pairs, using CSP; note: this takes quite some time!)
% (the number of pattern pairs found optimal should be 3 in this case)

% load the data set (BCI2000 format)
traindata = io_loadset('bcilab:/userdata/tutorial/imag_movements1/calib/DanielS001R01.dat','channels',1:29);

% define approach
myapproach = {'CSP' 'Prediction',{'FeatureExtraction',{'PatternPairs',search(1,2,3)}}};

% learn the model
[trainloss,lastmodel,laststats] = bci_train('Data',traindata,'Approach',myapproach,'TargetMarkers',{'StimulusCode_2','StimulusCode_3'});
disp(['training mis-classification rate: ' num2str(trainloss*100,3) '%']);

% visualize results
bci_visualize(lastmodel);


  function st = ct_geom_par(type, varargin)
%|function st = ct_geom_par(type, varargin)
%|
%| Same as ct_geom, but adds a rebinned variable
%| ct_geom should be changed to include a new type instead
%|
%| Create the "CT geometry" structure that describes the sampling
%| characteristics of a cone-beam CT system (axial or helical).
%| (Use sino_geom() for 2D fan-beam or parallel-beam systems.)
%|
%| in
%|	type	'fan'		multi-slice fan-beam - recommended
%|		'par'		(parallel-beam only weakly supported)
%|
%| options for all geometries
%|	'orbit_start'		default: 0
%|	'orbit'			[degrees] default: 180 for parallel / mojette
%|					or 360 for fan (negative for CW)
%|					can be 'short' for fan-beam short scan
%|	'down'			down-sampling factor, for testing
%|					can be [down_s down_t down_a]
%|	'units'			string to print distance units (default: 'mm')
%|
%| options for fan-beam
%|	'ns'			# of horizontal samples
%|	'nt'			# of vertical samples
%|	'na' | 'nbeta'		# of angular samples
%|	'ds'			horizontal sample spacing (default: 1)
%|	'dt'			vertical sample spacing (default: -ds)
%|				or {'dz', dz} to use dz * dsd / dso (usual CT)
%|	'offset_s'		unitless fraction of a channel (default: 0)
%|				(relative to line between two center channels).
%|				use 0.25 or 1.25 for "quarter-detector offset"
%|	'offset_t'		unitless (default: 0)
%|
%| options for partial scans (added by jang-hwan cho)
%|	'nframe'		# of frames to divide orbit into (default: 1)
%|	'frame'			which frame? (default: 1)
%|
%| options for helical (or step-and-shoot)
%|	'pitch'			bed_travel_per_rotation / axial_fov. default: 0.
%|				(unitless. can be negative. usually near 1.0)
%|	'source_z0'		z-location of source for first view. Default 0.
%|					It must have same units as dt and ds.
%|				Use 'center' to center the helix around z=0.
%|
%|	'user_source_zs' [na]	user-specified source z-locations for each view.
%|				usually this is empty (default) in which case
%|				source_zs is computed internally from "pitch"
%|				It must have same units as dt and ds.
%|
%|	fan beam distances:
%|	'dsd' | 'dis_src_det'	default: inf (parallel beam)
%|	'dso' | 'dis_src_iso'	default: inf (parallel beam)
%|	'dod' | 'dis_iso_det'	default: 0
%|	'dfs' | 'dis_foc_src'	default: 0 (3rd generation CT arc),
%|					use 'inf' for flat detector
%|
%| out
%|	st	(struct)	initialized structure
%|
%| methods
%|	st.shape(sino)		reshape sinograms that are columns into 3d array
%|	st.s			s sample locations
%|	st.t			t sample locations
%|	st.gamma		[nb] gamma sample values [radians]
%|	st.gamma_max		half of fan angle [radians]
%|	st.ws			(ns-1)/2 + st.offset_s
%|	st.wt			(nt-1)/2 + st.offset_t
%|	st.ad			[na] source angles in degrees
%|	st.ar			[na] source angles in radians
%|	st.dim			dimensions: [st.ns st.nt st.na]
%|	st.downsample(down)	reduce sampling by integer factor
%|	st.ones			ones(ns,nt,na, 'single')
%|	st.zeros		zeros(ns,nt,na, 'single')
%|	st.rmax			max radius within FOV
%|	st.footprint_size(ig)	max footprint width in 's'
%|	st.zfov			axial FOV
%|	st.source_zs		[na] z-locations of source for each view
%|	st.shape(sino(:))	reshape to [ns,nt,na,?]
%|	st.unitv(is,it,ia)	unit 'vector' with one nonzero element
%|	st.plot([ig])		show geometry
%|
%|	trick: you can make orbit=0 and orbit_start = column vector (length na)
%|	if you need nonuniformly spaced projection view angles.
%|
%| Copyright 2006-1-18, Jeff Fessler and Jang-Hwan Cho, University of Michigan
%|
%| 2009-12-04 modified source_zs definition to use source_z0, eliminate na/2
%| 2012-12-09 modified by Greg Handy to include the rebinned variable

if nargin == 1 && streq(type, 'test'), ct_geom_test, return, end
if nargin < 1, help(mfilename), error(mfilename), end

if streq(type, 'ge1') % special case: GE fan-beam
	st = ct_geom_ge1(type, varargin{:});
return
end

if streq(type, 'ge2') % special case: GE axial or helical
	st = ct_geom_ge2(type, varargin{:});
return
end

if streq(type, 'hd1') % special case: GE HD (UM only)
	st = ct_geom_hd1(type, varargin{:});
return
end

% defaults
st.type = type;
st.rebinned = false;
st.ns = [];
st.nt = [];
st.na = [];
st.down = 1;
st.nframe = 1; % entire scan as 1 "frame"
st.frame = 1;
st.orbit_start = 0;
st.pitch = 0; % default for axial
st.source_z0 = 0; % default for axial
st.units = 'mm';
st.user_source_zs = [];

if streq(type, 'fan')
	st = ct_geom_fan(st, varargin{:});
%elseif streq(type, 'par')
%	st = ct_geom_par(st, varargin{:});
%elseif streq(type, 'moj')
%	st = ct_geom_moj(st, varargin{:});
else
	fail('unknown sinotype %s', type)
end

if isempty(st.na), st.na = 2 * floor(st.ns * pi/2 / 2); end

ct_geom_checks(st)

meth = { ...
	's', @ct_geom_s, '()'; ...
	't', @ct_geom_t, '()'; ...
	'ws', @ct_geom_ws, '()'; ...
	'wt', @ct_geom_wt, '()'; ...
	'ad', @ct_geom_ad, '()'; ...
	'ar', @ct_geom_ar, '()'; ...
	'gamma', @ct_geom_gamma, '()'; ...
	'gamma_max', @ct_geom_gamma_max, '()'; ...
	'zfov', @ct_geom_zfov, '()'; ...
	'source_dz_per_view', @ct_geom_source_dz_per_view, '()'; ...
	'source_zs', @ct_geom_source_zs, '() -> [na]'; ...
	'orbit_short', @ct_geom_orbit_short, '()'; ...
	'xds', @ct_geom_xds, '()'; ...
	'yds', @ct_geom_yds, '()'; ...
	'downsample', @ct_geom_downsample, '()'; ...
	'dim', @ct_geom_dim, '()'; ...
	'ones', @ct_geom_ones, '()'; ...
	'rmax', @ct_geom_rmax, '()'; ...
	'footprint_size', @ct_geom_footprint_size, '(ig)'; ...
	'unitv', @ct_geom_unitv, '() | (is,it,ia)'; ...
	'zeros', @ct_geom_zeros, '()'; ...
	'shape', @ct_geom_shape, '()'; ...
	'plot', @ct_geom_plot, '() | (ig)';
	'plot3', @ct_geom_plot3, '() | (ig)';
	};

st = strum(st, meth);

if streq(st.source_z0, 'center')
	st.source_z0 = -st.na/2 * st.source_dz_per_view;
end

if any(st.down ~= 1)
	down = st.down; st.down = 1; % trick
	st = st.downsample(down);
end

if streq(type, 'fan') && streq(st.orbit, 'short')
	st.orbit = st.orbit_short / st.nframe; % jc
	st.orbit_start = st.orbit_start + (st.frame - 1) * st.orbit; % jc/jf
	if st.frame ~= 1, warn 'untested and possibly incorrect', end % jf
end

% ct_geom_orbit_short()
function os = ct_geom_orbit_short(st)
os = 180 + 2 * rad2deg(st.gamma_max);

% ct_geom_dim()
function dim = ct_geom_dim(st)
dim = [st.ns st.nt st.na];
if isempty(st.ns) || isempty(st.nt) || isempty(st.na)
	error 'dim requested without ns,nt,na'
end

% ct_geom_ones()
% sinogram of all ones
function out = ct_geom_ones(st)
out = ones(st.dim, 'single');

% ct_geom_zeros()
% sinogram of all zeros
function out = ct_geom_zeros(st)
out = zeros(st.dim, 'single');

% ct_geom_unitv()
% sinogram with just one ray
function out = ct_geom_unitv(st, is, it, ia)
out = st.zeros;
if ~isvar('is') || isempty(is)
	is = floor(st.ns/2 + 1);
	it = floor(st.nt/2 + 1);
	ia = 1;
end
out(is,it,ia) = 1;

% ct_geom_rmax()
% max radius within fov
function rmax = ct_geom_rmax(st)
smax = max(abs(st.s));
if streq(st.type, 'fan')
	if isinf(st.dso) % parallel
		rmax = smax;
	elseif st.dfs == 0 % arc
		rmax = st.dso * sin(smax / st.dsd);
	elseif isinf(st.dfs) % flat
		rmax = st.dso * sin(atan(smax / st.dsd));
	else
		error 'unknown case'
	end
end

% ct_geom_ws()
% 'middle' sample position
function ws = ct_geom_ws(st)
ws = (st.ns-1)/2 + st.offset_s;

% ct_geom_wt()
% 'middle' sample position
function wt = ct_geom_wt(st)
wt = (st.nt-1)/2 + st.offset_t;

% ct_geom_s()
% sample locations ('radial')
function s = ct_geom_s(st, varargin)
s = st.ds * ([0:st.ns-1]' - st.ws);
if length(varargin)
	s = s(varargin{:});
end

% ct_geom_t()
% sample locations ('radial')
function t = ct_geom_t(st, varargin)
t = st.dt * ([0:st.nt-1]' - st.wt);
if length(varargin)
	t = t(varargin{:});
end

% ct_geom_ad()
% angular sample locations (degrees)
function ang = ct_geom_ad(st, varargin)
ang = [0:st.na-1]'/st.na * st.orbit + st.orbit_start;
ang = ang(varargin{:});

% ct_geom_ar()
% angular sample locations (radians)
function ang = ct_geom_ar(st, varargin)
ang = deg2rad(ct_geom_ad(st));
ang = ang(varargin{:});

% ct_geom_shape()
% reshape into sinogram array
function sino = ct_geom_shape(st, sino)
sino = reshapee(sino, st.ns, st.nt, st.na, []);

% ct_geom_downsample()
% down-sample (for testing)
function st = ct_geom_downsample(si, down)
st = si;
st.down = st.down .* down;

switch numel(down)
case 1
	down_s = down;
	down_t = down;
	down_a = down;
case 3
	down_s = down(1);
	down_t = down(2);
	down_a = down(3);
otherwise
	fail('bad down %g', down)
end
clear down

st.ns = 4 * ceil(st.ns / down_s / 4); % multiple of 4 for simd
st.nt = 2 * ceil(st.nt / down_t / 2); % keep it even

user_specified = false; % did user specify zs or orbit_start vector?

if ~isempty(st.user_source_zs)
	st.user_source_zs = st.user_source_zs(1:down_a:st.na);
	user_specified = true;
end

if numel(st.orbit_start) > 1
	st.orbit_start = st.orbit_start(1:down_a:si.na);
	user_specified = true;
end

if streq(st.type, 'fan')
	st.ds = st.ds * down_s;
	st.dt = st.dt * down_t;
else
	fail('unknown sinotype "%s"', type)
end

if user_specified % if user-specified then just decimate
	st.na = length([1:down_a:st.na]);

elseif all(diff(si.source_zs) == 0) % axial
	st.na = max(1, round(si.na / down_a)); % at least one view

else % helical described by pitch and source_z0
	nturn = si.orbit / 360;
	na1 = si.na / nturn; % # of views in 1 turn, originally

	% if it is helical with equal view spacing, then adjust orbit slightly,
	% to preserve integer number of views per turn (if applicable)
	tol = 1e-4;
	if abs(round(na1) - na1) < tol % integer views per turn
		na2 = round(na1 / down_a); % new # of views in 1 turn (integer)
		tmp = nturn * na2; % new na value, roughly na/down
		if abs(round(tmp) - tmp) < tol
			st.na = round(tmp);
		else % adjust orbit
			st.na = floor(tmp);
			st.orbit = 360 / na2 * st.na;
		end
	else
		st.na = round(si.na / down_a);
	end
end

% ct_geom_zfov()
% axial FOV, considering magnification factor, as collimated at iso
function zfov = ct_geom_zfov(st)
if isinf(st.dso) || isinf(st.dsd) % parallel beam
	zfov = st.nt * st.dt;
else % cone
	zfov = st.dso / st.dsd * st.nt * st.dt;
end

% ct_geom_source_dz_per_view()
% source axial travel for each view
function out = ct_geom_source_dz_per_view(st)
if ~isempty(st.user_source_zs), fail 'undefined', end
if st.na == 1 % for tomosynthesis
	out = 0;
	return
end
if ~st.pitch
	out = 0;
	return
end
if length(st.orbit) ~= 1 || st.orbit == 0
	fail 'dz_per_view undefined for user angles'
end
na_per_360 = st.na * (360 / st.orbit); % # views per turn
out = st.pitch * st.zfov / na_per_360;

% ct_geom_checks
% check vectors user_source_zs and orbit_start
function ct_geom_checks(st)
if ~isempty(st.user_source_zs) && (st.pitch ~= 0)
	fail('only one of "pitch" (recommended) or user_source_zs can be given')
end
if ~isempty(st.user_source_zs) && length(st.user_source_zs) ~= st.na
	fail('user_source_zs size mismatch')
end
if numel(st.orbit_start) > 1 && numel(st.orbit_start) ~= st.na
	fail('orbit_start vector length')
end

% ct_geom_source_zs()
% source z locations
function source_zs = ct_geom_source_zs(st, varargin)

if ~isempty(st.user_source_zs) % user-provided
	source_zs = st.user_source_zs;
	source_zs = source_zs(varargin{:});
else
	source_dz = st.source_dz_per_view;
	source_zs = st.source_z0 + [0:st.na-1]' * source_dz;
	source_zs = source_zs(varargin{:});
end

% ct_geom_gamma()
% gamma sample values
function gamma = ct_geom_gamma(st, varargin)
switch st.dfs
case 0
	gamma = st.s / st.dsd; % 3rd gen: equiangular arc
case inf
	gamma = atan(st.s / st.dsd); % flat
otherwise
	error 'dfs not done'
end
gamma = gamma(varargin{:});

% ct_geom_gamma_max()
function gamma_max = ct_geom_gamma_max(st)
gamma_max = max(st.gamma);

% ct_geom_xds()
% center positions of detectors
function xds = ct_geom_xds(st, varargin)
switch st.type
case 'par'
	xds = st.s;
case 'fan'
	switch st.dfs
	case 0 % arc
		xds = st.dsd * sin(st.gamma);
	case inf % flat
		xds = st.s;
	otherwise
		error 'not done'
	end
otherwise
	error 'bug'
end

% ct_geom_yds()
% center positions of detectors
function yds = ct_geom_yds(st, varargin)
switch st.type
case 'par'
	yds = zeros(size(st.s));

case 'fan'
	switch st.dfs
	case 0 % arc
		yds = st.dso - st.dsd * cos(st.gamma);
	case inf % flat
		yds = -st.dod * ones(size(st.s));
	otherwise
		error 'not done'
	end
otherwise
	error 'bug'
end

% ct_geom_footprint_size()
% biggest possible footprint over image
function footprint_size = ct_geom_footprint_size(st, ig, varargin)
di = sqrt(ig.dx^2 + ig.dy^2);
smax = max(abs(st.s));
rfov = max(ig.nx * ig.dx, ig.ny * ig.dy) / 2;
dso = st.dso;
dsd = st.dsd;
if rfov > 0.99 * dso, error 'bad dso', end

if isinf(st.dso) % parallel
	footprint_size = di;
elseif st.dfs == 0 % arc3
	footprint_size = di * dsd / (dso - rfov);
elseif isinf(st.dfs) % flat
	footprint_size = di * sqrt(dsd^2 + smax^2) / (dso - rfov);
else
	error 'unknown case'
end
footprint_size = footprint_size / st.ds; % unitless

%
% ct_geom_fan()
%
function st = ct_geom_fan(st, varargin);

% defaults
st.orbit = 360; % [degrees]
st.ds		= 1;
st.dt		= [];
st.offset_s	= 0;
st.offset_t	= 0;

st.dsd = [];	% dis_src_det
st.dso = [];	% dis_src_iso
st.dod = [];	% dis_iso_det
st.dfs = 0;	% dis_foc_src (3rd gen CT)

subs = { ...
	'src_det_dis', 'dsd';
	'dis_src_det', 'dsd';
	'dis_src_iso', 'dso';
	'dis_iso_det', 'dod';
	'dis_foc_src', 'dfs';
	'nbeta', 'na';
	'source_zs', 'user_source_zs';
	};
st = vararg_pair(st, varargin, 'subs', subs);

% work out distances
if (~isempty(st.dsd) && isinf(st.dsd)) ...
|| (~isempty(st.dso) && isinf(st.dso)) % handle parallel-beam case gracefully
	st.dsd = inf; st.dso = inf; st.dod = 1;
end
if isempty(st.dsd) + isempty(st.dso) + isempty(st.dod) > 1
	error 'must provide at least two of dsd, dso, dod'
end
if isempty(st.dsd), st.dsd = st.dso + st.dod; end
if isempty(st.dso), st.dso = st.dsd - st.dod; end
if isempty(st.dod), st.dod = st.dsd - st.dso; end
if st.dso + st.dod ~= st.dsd
	error 'bad fan-beam distances'
end

if isempty(st.dt), st.dt = -st.ds; end
if iscell(st.dt)
	if length(st.dt) == 2 && streq(st.dt{1}, 'dz')
		dz = st.dt{2};
		st.dt = dz * st.dsd / st.dso;
		printm('dt = dz * dsd / dso = %g * %g / %g = %g', ...
			dz, st.dsd, st.dso, st.dt)
	else
		fail 'bad dt cell'
	end
end

% ct_geom_plot2()
% picture of 2D source position / detector geometry
function ct_geom_plot2(st, ig, varargin)
arg.tomosyn = false; % set to 1 for offset_x plotting trick
arg = vararg_pair(arg, varargin);
if arg.tomosyn, fail 'not done', end
if ~streq(st.type, 'fan'), error 'only fan done', end
x0 = 0;
y0 = st.dso;
t = linspace(0,2*pi,1001);

if isinf(st.dsd) % parallel beam
	rfov = max(abs(st.s));
	plot(	0, 0, '.', ...
		rfov * cos(t), rfov * sin(t), 'm:', ...
		st.s, -rfov, 'yo')

else % fan beam

	rot = deg2rad(st.orbit_start);
	rot = [cos(rot) sin(rot); -sin(rot) cos(rot)];
	p0 = rot * [x0; y0];
	pd = rot * [st.xds'; st.yds'];
	rfov = st.dso * sin(max(abs(st.gamma)));

	plot(	p0(1), p0(2), 'ys', ... % source
		st.dso * cos(t), st.dso * sin(t), 'c--', ... % source circle
		pd(1,:), pd(2,:), 'yo') % detector elements

	hold on
	if isvar('ig') && ~isempty(ig)
		xmin = min(ig.x); xmax = max(ig.x);
		ymin = min(ig.y); ymax = max(ig.y);
		im(ig.x, ig.y, ig.mask(:,:,1))

		plot([xmax xmin xmin xmax xmax], [ymax ymax ymin ymin ymax], 'g-')
	end

	plot(	0, 0, '.', ...
		[pd(1,1) p0(1) pd(1,end)], [pd(2,1) p0(2) pd(2,end)], 'r-', ...
		rfov * cos(t), rfov * sin(t), 'm:')

	axis equal, axis tight
	hold off

end

if isvar('ig') && ~isempty(ig)
	hold on
	xmin = min(ig.x); xmax = max(ig.x);
	ymin = min(ig.y); ymax = max(ig.y);
	plot([xmax xmin xmin xmax xmax], [ymax ymax ymin ymin ymax], 'g-')
	hold off
end
titlef('fov = %g', rfov)
axis square, zoom on

% ct_geom_plot3()
% picture of 3D helical CT geometry
function dummy = ct_geom_plot3(st, ig)
dummy = [];
if ~streq(st.type, 'fan'), error 'only fan done', end

if isinf(st.dso) || isinf(st.dsd)
	warn 'parallel-beam plot not done'
return
end

t1 = -st.dso * sin(st.ar); % source x pos
t2 = st.dso * cos(st.ar); % source y pos
t3 = st.source_zs; % source z pos
plot3(t1, t2, t3, 'y.') % source trajectory
view(-110, 22)
hold on
for ia = [1 1+floor(st.na/2) st.na]
	text(t1(ia), t2(ia), t3(ia), sprintf('%d', ia))
end

% axes
r100 = @(x) 100 * ceil(max(abs(x)) / 100);
r10 = @(x) 10 * ceil(max(abs(x)) / 10);
plot3([-1 1] * r100(t1), [0 0], [0 0])
text(r100(t1), 0, 'x')
plot3([0 0], [-1 1] * r100(t2), [0 0])
text(0, r100(t2), 'y')
plot3([0 0], [0 0], [-1 1] * r10(t3))
text(0, 0, r10(t3), 'z')

grid on
xlabelf('x (%s)', st.units)
ylabelf('y (%s)', st.units)
zlabelf('z (%s)', st.units)

if isvar('ig') && ~isempty(ig) && isfield(ig.meth,'z') % image box
	xmin = min(ig.x); xmax = max(ig.x);
	ymin = min(ig.y); ymax = max(ig.y);
	zmin = min(ig.z); zmax = max(ig.z);
	plot3(	[xmin xmax xmax xmin xmin ], ...
		[ymin ymin ymax ymax ymin ], ...
		[zmin zmin zmin zmin zmin ], ...
		'g-')
	plot3(	[xmin xmax xmax xmin xmin], ...
		[ymin ymin ymax ymax ymin], ...
		[zmax zmax zmax zmax zmax], ...
		'g-')
	plot3(xmin*[1 1], ymin*[1 1], [zmin zmax], 'g-')
	plot3(xmax*[1 1], ymin*[1 1], [zmin zmax], 'g-')
	plot3(xmin*[1 1], ymax*[1 1], [zmin zmax], 'g-')
	plot3(xmax*[1 1], ymax*[1 1], [zmin zmax], 'g-')
	plot3(xmin*ones(ig.nz,1), ymin*ones(ig.nz,1), ig.z, 'g.')
end

% detector array for source at each position
detcolor = {'r.', 'c.', 'm.'};
ia_list = [1 1+floor(st.na/2) st.na];
for ia = ia_list
	src = [t1(ia) t2(ia) t3(ia)];
	unit_vec = [-src(1) -src(2) 0] / sqrt(src(1)^2+src(2)^2);
	% diametrically opposite point of the source on the detector
	det_cen_loc = src + st.dsd * unit_vec;
	plot3(det_cen_loc(1), det_cen_loc(2), det_cen_loc(3), 'rs')

	rot = st.ar(ia);
	rot = [cos(rot) -sin(rot); sin(rot) cos(rot)];
	pd = rot * [st.xds'; st.yds'];

	for it=1:st.nt
		plot3(pd(1,:), pd(2,:), src(3) + st.t(it)*ones(1,st.ns), ...
			detcolor{find(ia == ia_list)})
	end

	if 1 && ia == 1 % line from source to middle of first detector row
		plot3([src(1) det_cen_loc(1)], [src(2) det_cen_loc(2)], ...
			[src(3) st.t(1)], 'm-')
	end
end
hold off

% ct_geom_plot()
function out = ct_geom_plot(st, varargin)
im clf
if all(st.source_zs == 0) % 2D or axial case
	ct_geom_plot2(st, varargin{:});
else
	ct_geom_plot3(st, varargin{:});
end
if nargout, out = []; end

% ct_geom_ge1()
% 'lightspeed';
% these numbers are published in IEEE T-MI Oct. 2006, p.1272-1283 wang:06:pwl
function st = ct_geom_ge1(type, varargin)

tmp.orbit = 360;
tmp = vararg_pair(tmp, varargin, 'allow_new', true);
if streq(tmp.orbit, 'short')
	na = 642; % trick: reduce na for short scans
	for ii=1:length(varargin)
		if (streq(varargin{ii}, 'orbit'))
			varargin{ii+1} = na/984*360;
			break
		end
	end
else
	na = 984;
end

st = ct_geom('fan', ...
	'ns', 888, ... % detector channels
	'nt', 1, ... % detector rows
	'na', na, ... % angular samples
	'orbit', 360, ...
	'offset_s', 1.25, ... % quarter-detector offset
	'dsd', 949.075, ...
	'dso', 541, ...
	'dfs', 0, ... % arc
	'ds', 1.0239, ... % detector pitch
	'dt', 1.0964, ... % detector row spacing for 0.625mm slices, 2009-12-06
	varargin{:});
%	'dod', 408.075, ...
% 'strip_width', [], ...

% ct_geom_ge2()
% helical CT
function st = ct_geom_ge2(type, varargin)
st = ct_geom_ge1(type, ...
	'nt', 64, ... % 64-slice
	'dt', 949.0750 / 541 * 0.625, ... % about 1.0964
	varargin{:});

% ct_geom_test()
function ct_geom_test
% axial cone-beam test
cg = ct_geom('fan', 'ns', 888, 'nt', 64, 'na', 984, ...
	'offset_s', 1.25, ...
	'dsd', 949, 'dod', 408);
cg.ad(2);
cg.downsample(2);
cg.rmax;
cg.ws;
cg.nframe; cg.frame; cg.gamma; cg.gamma_max;
cg.s(cg.ns/2+1);
if im
	cg.plot;
prompt
end

% test user_source_zs
cg = ct_geom('fan', 'dsd', 949, 'dod', 408, ...
	'ns', 888, 'ds', 1.0239, 'offset_s', 1.25, ...
	'nt', 8, 'dt', 1.0964, ...
	'na', 2*984, 'orbit', 2*360, ...
	'source_zs', []);
cg.source_zs(1:2:7);

% helical cone-beam test
cg = ct_geom('fan', 'dsd', 949, 'dod', 408, ...
	'ns', 888, 'ds', 1.0239, 'offset_s', 1.25, ...
	'nt', 8, 'dt', 1.0964, ...
	'na', 2*984, 'orbit', 2*360, ...
	'pitch', 1.0);

cg.source_zs(1:2:7);
cg.downsample(2);
cg.s(cg.ns/2+1);
if im
	cg.plot;
prompt
end

if 1 % footprint
	cg = ct_geom('ge2');
	ig = image_geom('nx', 512, 'fov', 500);
	cg.footprint_size(ig);
end


function [S,f,R,Serr]=mtspectrumpt(data,params,fscorr,t)
% Multi-taper spectrum - point process times
%
% Usage:
%
% [S,f,R,Serr]=mtspectrumpt(data,params,fscorr,t)
% Input:
%       data        (structure array of spike times with dimension channels/trials;
%                   also accepts 1d array of spike times) -- required
%       params: structure with fields tapers, pad, Fs, fpass, err, trialave
%       - optional
%           tapers : precalculated tapers from dpss or in the one of the following
%                    forms:
%                   (1) A numeric vector [TW K] where TW is the
%                       time-bandwidth product and K is the number of
%                       tapers to be used (less than or equal to
%                       2TW-1).
%                   (2) A numeric vector [W T p] where W is the
%                       bandwidth, T is the duration of the data and p
%                       is an integer such that 2TW-p tapers are used. In
%                       this form there is no default i.e. to specify
%                       the bandwidth, you have to specify T and p as
%                       well. Note that the units of W and T have to be
%                       consistent: if W is in Hz, T must be in seconds
%                       and vice versa. Note that these units must also
%                       be consistent with the units of params.Fs: W can
%                       be in Hz if and only if params.Fs is in Hz.
%                       The default is to use form 1 with TW=3 and K=5
%
%	        pad		    (padding factor for the FFT) - optional (can take values -1,0,1,2...).
%                    -1 corresponds to no padding, 0 corresponds to padding
%                    to the next highest power of 2 etc.
%			      	 e.g. For N = 500, if PAD = -1, we do not pad; if PAD = 0, we pad the FFT
%			      	 to 512 points, if pad=1, we pad to 1024 points etc.
%			      	 Defaults to 0.
%           Fs   (sampling frequency) - optional. Default 1.
%           fpass    (frequency band to be used in the calculation in the form
%                                   [fmin fmax])- optional.
%                                   Default all frequencies between 0 and Fs/2
%           err  (error calculation [1 p] - Theoretical error bars; [2 p] - Jackknife error bars
%                                   [0 p] or 0 - no error bars) - optional. Default 0.
%           trialave (average over channels/trials when 1, don't average when 0) - optional. Default 0
%       fscorr   (finite size corrections, 0 (don't use finite size corrections) or
%                1 (use finite size corrections) - optional
%                (available only for spikes). Defaults 0.
%       t        (time grid over which the tapers are to be calculated:
%                      this argument is useful when calling the spectrum
%                      calculation routine from a moving window spectrogram
%                      calculation routine). If left empty, the spike times
%                      are used to define the grid.
% Output:
%       S       (spectrum with dimensions frequency x channels/trials if trialave=0;
%               dimension frequency if trialave=1)
%       f       (frequencies)
%       R       (rate)
%       Serr    (error bars) - only if err(1)>=1
if nargin < 1; error('Need data'); end;
if nargin < 2; params=[]; end;
[tapers,pad,Fs,fpass,err,trialave,params]=getparams(params);
clear params
data=change_row_to_column(data);
if nargout > 3 && err(1)==0; error('cannot compute error bars with err(1)=0; change params and run again'); end;
if nargin < 3 || isempty(fscorr); fscorr=0;end;
if nargin < 4 || isempty(t);
   [mintime,maxtime]=minmaxsptimes(data);
   dt=1/Fs; % sampling time
   t=mintime-dt:dt:maxtime+dt; % time grid for prolates
end;
N=length(t); % number of points in grid for dpss
nfft=max(2^(nextpow2(N)+pad),N); % number of points in fft of prolates
[f,findx]=getfgrid(Fs,nfft,fpass); % get frequency grid for evaluation
tapers=dpsschk(tapers,N,Fs); % check tapers
[J,Msp,Nsp]=mtfftpt(data,tapers,nfft,t,f,findx); % mt fft for point process times
S=squeeze(mean(conj(J).*J,2));
if trialave; S=squeeze(mean(S,2));Msp=mean(Msp);end;
R=Msp*Fs;
if nargout==4;
   if fscorr==1;
      Serr=specerr(S,J,err,trialave,Nsp);
   else
      Serr=specerr(S,J,err,trialave);
   end;
end;


function [state,options] = psoplotscorediversity(options,state,flag)
% Plots a histogram containing the best and mean scores of particle swarm.

if strcmp(flag,'init')
    set(gca,'NextPlot','replacechildren',...
        'XLabel',xlabel('Scores'),...
        'YLabel',ylabel('Number of inidividuals'))
end

[n,bins] = hist(state.Score) ;
bar(bins,n,'Tag','scorehistogram','FaceColor',[0.1 0.1 0.5])


function export_VPSC(ori,filename,varargin)
% export individual orientations to the VPSC format
%
% Syntax
%   export_VPSC(ori,'file.txt')
%   export_VPSC(ori,'file.txt','weights',weights)
%
% Input
%  ori      - individual @orientation to be exported
%  filename - name of the ascii file
%  weights  - list weights with the same size as the orientations
%
% See also
% quaternion/export SO3Fun/export_VPSC

% allocate memory
d = zeros(length(ori),4);

% add Euler angles
d(:,1:3) = ori.Euler(varargin{:});
if ~check_option(varargin,{'radians','radiant','radiand'})
  d = d ./ degree;
end

w = get_option(varargin,'weights',ones(size(ori)));

% add weight
d(:,4) = w./ sum(w);

fid = efopen(filename,'w');

% header
% fourth line has to include the angle convention (B for Bunge)
% and the total number of grains in the phase
fprintf(fid,'\n\n\nB %d\n',length(ori));

fprintf(fid,'%7.2f %7.2f %7.2f %11.7f\n',d');

fclose(fid);


function [T, roffset, V, P, W] = rendertransform(ax)
% Render Transform
%   [T, offset] = rendertransform(AX) returns the 4-by-4 render transform of axes AX
%   and the render offset.
%   The render transform maps data coordinates to pixels locations in the
%   axes parent coordinate system (with flipped y direction).
%   The render offset is the [x y z] values to subtract from the data inputs
%   before transforming by the render transform.
%
%   [..., V, P, W] = rendertransform(AX) returns additionally the view,
%   projection and viewport transforms.

%   Copyright 2014 The MathWorks, Inc.

newgraphics = ishandle(ax) && isprop(handle(ax), 'XTickLabelRotation');
cpos = get(ax,'CameraPosition');
ctar = get(ax,'CameraTarget');
cup = get(ax,'CameraUpVector');
cva = get(ax,'CameraViewAngle');
cvaAuto = strcmp(get(ax,'CameraViewAngleMode'), 'auto');
oldunits = get(ax,'Units');
set(ax,'Units','pixels')
axpos = get(ax,'Position');
set(ax,'Units',oldunits)
if newgraphics
    bottomleft = floor(axpos(1:2)) - 1;
    topright = ceil(axpos(1:2) + axpos(3:4));
else
    bottomleft = round(axpos(1:2)) - 1;
    topright = round(axpos(1:2) - 1 + axpos(3:4));
end
% round position to integer pixels
viewport = [bottomleft (topright - bottomleft)];
pxscale = max(viewport(3), 1);
pyscale = max(viewport(4), 1);

pbar = get(ax,'PlotBoxAspectRatio');
warp = strcmp(get(ax,'PlotBoxAspectRatioMode'),'auto') && ...
    strcmp(get(ax,'DataAspectRatioMode'),'auto') && ...
    strcmp(get(ax,'CameraViewAngleMode'),'auto');
if warp && newgraphics
    pbar = [1 1 1];
end
perspective = strcmp(get(ax,'Projection'), 'perspective');
xreverse = strcmp(get(ax,'XDir'),'reverse');
yreverse = strcmp(get(ax,'YDir'),'reverse');
zreverse = strcmp(get(ax,'ZDir'),'reverse');
dsx = xlim;
dsy = ylim;
dsz = zlim;
dsrange = [diff(dsx) diff(dsy) diff(dsz)];
roffset = [dsx(1) dsy(1) dsz(1)];
ascale = dsrange;
dsoffset = roffset;
if any(isinf([dsx dsy dsz]))
    error('infinite limits not supported')
end
if any(isinf(dsrange))
    error('limits too large')
end
if strcmp(get(ax,'XScale'),'log') || ...
        strcmp(get(ax,'YScale'),'log') || ...
        strcmp(get(ax,'ZScale'),'log')
    error('log scale not supported')
end

% normalize the camera properties
if xreverse
    roffset(1) = dsx(2);
    cpos(1) = 2*mean(dsx) - cpos(1);
    ctar(1) = 2*mean(dsx) - ctar(1);
    ascale(1) = -ascale(1);
    cup(1) = -cup(1);
end
if yreverse
    roffset(2) = dsy(2);
    cpos(2) = 2*mean(dsy) - cpos(2);
    ctar(2) = 2*mean(dsy) - ctar(2);
    ascale(2) = -ascale(2);
    cup(2) = -cup(2);
end
if zreverse
    roffset(3) = dsz(2);
    cpos(3) = 2*mean(dsz) - cpos(3);
    ctar(3) = 2*mean(dsz) - ctar(3);
    ascale(3) = -ascale(3);
    cup(3) = -cup(3);
end
ncpos = (cpos - dsoffset).*pbar./dsrange;
nctar = (ctar - dsoffset).*pbar./dsrange;
ncup  = cup.*pbar./dsrange;
ncdir = nctar - ncpos;

% make View
dot1 = ncdir*ncdir.';
dot2 = ncdir*ncup.';
u = dot1*ncup - dot2*ncdir;
lat = cross(ncdir, u);
ncdir = ncdir/norm(ncdir);
u = u/norm(u);
lat = lat/norm(lat);
p = ncpos.';
offset = [-lat*p ; -u*p ; -ncdir*p ; 1];
V = [lat.*pbar ; u.*pbar ; ncdir.*pbar ; 0 0 0];
V = [V offset];

% make Projection
P = eye(4);
if perspective
    P(4,4) = 0;
    P(4,3) = 1;
end

% compute size
T = P*V;
cube = [ 0 0 0 0 1 1 1 1   % x
         0 0 1 1 0 0 1 1   % y
         0 1 0 1 0 1 0 1   % z
         1 1 1 1 1 1 1 1]; % w
cube2 = T*cube;
cube3 = cube2;
cube3(1,:) = cube3(1,:) ./ cube2(4,:);
cube3(2,:) = cube3(2,:) ./ cube2(4,:);
width = 2*max(abs(max(cube3(1,:))), abs(min(cube3(1,:))));
height = 2*max(abs(max(cube3(2,:))), abs(min(cube3(2,:))));

% view angle
xConstraint = false;
if cvaAuto
    if abs(pxscale*width) < abs(pyscale*height)
        xConstraint = true;
    end
end

% scale projection
fov = cva*pi/360;
if perspective
    scale = 1/(2*tan(fov));
else
    v = ncpos - nctar;
    len = norm(v);
    scale = 1/(2*tan(fov)*len);
end
P(1,1) = P(1,1)*scale;
P(2,2) = P(2,2)*scale;

% viewport transform
T = P*V;
cube2 = T*cube;
cube3 = cube2;
cube3(1,:) = cube3(1,:) ./ cube2(4,:);
cube3(2,:) = cube3(2,:) ./ cube2(4,:);
width = max(cube3(1,:)) - min(cube3(1,:));
height = max(cube3(2,:)) - min(cube3(2,:));
pxoffset = bottomleft(1);
% flip y in the parent reference frame
parent = get(ax,'Parent');
oldunits = get(parent,'Units');
set(parent,'Units','pixels')
parentpos = get(parent,'Position');
set(parent,'Units',oldunits)
pyoffset = parentpos(4) - bottomleft(2);
ov = [pxoffset+pxscale/2 pyoffset-pyscale/2 0];
if ~warp
    if cvaAuto
        if xConstraint
            scale = pxscale;
        else
            scale = pyscale;
        end
    else
        scale = min(pxscale, pyscale);
    end
    sv = [scale -scale];
else
    xscale = pxscale/width;
    yscale = pyscale/height;
    sv = [xscale -yscale];
end
SC = diag([sv 1 1]);
W = makehgtform('translate',ov) * SC;

NORM = diag([1./ascale 1]);

T = W * P * V * NORM;
roffset = roffset.';


function tno_sparse_grid_write ( header, l, m, n, x, w )

%*****************************************************************************80
%
%% TNO_SPARSE_GRID_WRITE writes a Truncated Normal Odd Sparse Grid to X and W files.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 July 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string HEADER, a header for the file names.
%
%    Input, integer L, the level of the rule.
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of points.
%
%    Input, real X(M,N), the abscissas.
%
%    Input, real W(N), the weights.
%
  w_filename = sprintf ( '%s_d%d_level%d_w.txt', header, m, l );
  x_filename = sprintf ( '%s_d%d_level%d_x.txt', header, m, l );

  r8vec_write ( w_filename, n, w );
  r8mat_write ( x_filename, m, n, x );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  W data written to "%s".\n', w_filename );
  fprintf ( 1, '  X data written to "%s",\n', x_filename );

  return
end


function d=mutate1(pop,mutprop)%uniform mutation.
global  bound rng
[pops,numvar]=size(pop);
mut=round(mutprop*pops*numvar);
for i=1:mut
    x=ceil(rand*size(pop,1));
    y=ceil(rand*numvar);
    pop(x,y)=bound(y,1)+rand*rng(y);
    offs(i,:)=pop(x,:);
    pop(x,:)=[];
end
d=[offs;pop];


classdef TukeyLoss < dagnn.Loss

  properties
    sigma = 1.
  end

  methods
    function outputs = forward(obj, inputs, params)
      outputs{1} = vl_nntukeyloss(inputs{1}, inputs{2}, ...
                       'instanceWeights', inputs{3}, ...
                       obj.opts{:}) ;
      n = obj.numAveraged ;
      m = n + size(inputs{1}, 4) ;
      obj.average = (n * obj.average + gather(outputs{1})) / m ;
      obj.numAveraged = m ;
    end

    function [derInputs, derParams] = backward(obj, inputs, params, derOutputs)
      derInputs{1} = vl_nntukeyloss(inputs{1}, inputs{2}, derOutputs{1}, ...
                                'instanceWeights', inputs{3}, ...
                                obj.opts{:}) ;
      derInputs{2} = [] ;
      derInputs{3} = [] ;
      derParams = {} ;
    end

    function obj = TukeyLoss(varargin)
      obj.load(varargin) ;
    end
  end
end


function [inF, n_r] = extend_dcm(inF,hA,hB,hC,hD,dim,sources)

%%- get dimensions
n=dim.n;
n_u=dim.n_u;
n_r = get_dims(hA,hB,hC,hD) ;

sourcesDim = cellfun(@numel,{sources(2:end).out});
for i=1:sum(sourcesDim)
    respSource(i) = sum(i>cumsum(sourcesDim))+1 ;
end

%%
[inFtemp] = prepare_dcm(hA,hB,hC,hD,respSource);

%% = save extended structure
%- raw matrices
inF.hA = inFtemp.A ;
inF.hB = inFtemp.B ;
inF.hC = inFtemp.C ;
inF.hD = inFtemp.D ;

%- parameter indices
inF.indhA = inFtemp.indA + inF.indself ;

for i=1:n_u
    inF.indhB{i} = inFtemp.indB{i} + inF.indself ;
end
inF.indhC = inFtemp.indC + inF.indself ;
for i=1:n
    inF.indhD{i} = inFtemp.indD{i} + inF.indself ;
end
inFtemp.indself = inFtemp.indself ;
inF.indhself = inFtemp.indself + inF.indself ;

%- indicators
inF.dhA = inFtemp.dA;
inF.dhB = inFtemp.dB;
inF.dhC = inFtemp.dC;
inF.dhD = inFtemp.dD;

%- save dimensions
% dim.n_r=n_r;

end

%% ========== subfunctions =============

%- compute number of predicted responses
function nr=get_dims(hA,hB,hC,hD)
nr=0;
nr=max(nr,size(hA,1));
for i=1:length(hB)
    nr=max(nr,size(hB{i},1));
end
nr=max(nr,size(hC,1));
for i=1:length(hD)
    nr=max(nr,size(hD{i},1));
end
end


function [FT,VT,CT]=dualLattice(varargin)

% function [FT,VT,CT]=dualLattice(E,V,shrinkFactor,cladOpt)
% ------------------------------------------------------------------------
%
%
%
% 2021/08/16: KMM Fixed error in cladding offset direction. Changed to "out
% of surface offset"
%
% ------------------------------------------------------------------------

%%

switch nargin
    case 2
        E=varargin{1};
        V=varargin{2};
        shrinkFactor=0.2;
        cladOpt=1;
    case 3
        E=varargin{1};
        V=varargin{2};
        shrinkFactor=varargin{3};
        cladOpt=1;
    case 4
        E=varargin{1};
        V=varargin{2};
        shrinkFactor=varargin{3};
        cladOpt=varargin{4};
end

%The default control parameters
cParDefault.shrinkFactor=0.25;
cParDefault.latticeSide=1;
cParDefault.numDigitKeep=5;
cParDefault.meshType='tri';
cParDefault.indBoundary=[];
cParDefault.hexSplit=0;
cParDefault.hexMethod=2;
cParDefault.elementType=[];

%Complement input structure with default
% [cPar]=structComplete(cPar,cParDefault,0);

%%

[Ec,Vc]=patchDetach(E,V,shrinkFactor);

[F]=element2patch(E);
[F_Ec]=element2patch(Ec);

[Fs,indSort]=sort(F,2);
[~,indUni1,indUni2,uniCount,IND_MAP]=unique_map(Fs,'rows');
IND_MAP=sort(IND_MAP,1,'descend');
IND_MAP=full(IND_MAP(1:2,:))';

I=(1:1:size(F,1))';
INDSort=sub2ind(size(F),I(:,ones(size(indSort,2),1)),indSort);

indSort2=indSort;
indSort2=indSort2(indUni1,:); %Unique
indSort2=indSort2(indUni2,:); %Expand

[~,indUnsort]=sort(indSort2,2);
I=(1:1:size(F,1))';
INDUnsort=sub2ind(size(F),I(:,ones(size(indUnsort,2),1)),indUnsort);

F_Ec=F_Ec(INDSort);
F_Ec=F_Ec(INDUnsort);

F=F(indUni1,:);
if numel(shrinkFactor)>1
    if numel(shrinkFactor)==size(E,1) %Shrink factor specified on faces
        [shrinkFactor_V]=faceToVertexMeasure(E,V,shrinkFactor); %Convert to nodal metric
    else
        shrinkFactor_V=shrinkFactor;
    end
    shrinkFactor_E=mean(shrinkFactor_V(F),2); %Convert to edge metric
else
    shrinkFactor_E=shrinkFactor;
end

Fq=NaN(size(IND_MAP,1),6);
Fq(uniCount==2,:)=[F_Ec(IND_MAP(uniCount==2,1),:) F_Ec(IND_MAP(uniCount==2,2),:)];

if any(uniCount==1)
    [Fcc,Vcc]=patchDetach(F,V,shrinkFactor_E);
    Fcc=Fcc(indUni2,:); %Expand
    Vq=[Vc;Vcc];
    F1=fliplr(F_Ec(IND_MAP(uniCount==1,1),:));
    F2=fliplr(Fcc(IND_MAP(uniCount==1,1),:))+size(Vc,1);
    Fq(uniCount==1,:)=[F1 F2];
else
    Vq=Vc;
end

Fq=[Fq(:,[1 2]) Fq(:,[5 4]);...
    Fq(:,[2 3]) Fq(:,[6 5]);...
    Fq(:,[3 1]) Fq(:,[4 6])];

Fq=fliplr(Fq);

if cladOpt==1
    F=F(indUni2,:); %Expand
    [Fq2,Vq2,Fc,~]=dualClad(F(IND_MAP(uniCount==1,1),:),V,shrinkFactor,1);
    [~,~,Nq2]=patchNormal(Fc,Vq2); %Vertex normals

    faceEdgeLengthSet=mean([ sqrt(sum((Vq2(Fc(:,1),:)-Vq2(Fc(:,2),:)).^2,2)) ...
                             sqrt(sum((Vq2(Fc(:,2),:)-Vq2(Fc(:,3),:)).^2,2)) ...
                             sqrt(sum((Vq2(Fc(:,3),:)-Vq2(Fc(:,1),:)).^2,2))],2);

    vertexEdgeLengthSet=faceToVertexMeasure(Fc,Vq2,faceEdgeLengthSet);

    [~,indUni_F2,~]=unique(F2(:)); %Indices of unique nodes

    %Creating offset nodes
    Vq2i=Vq2;
    Vq2i(Fc(indUni_F2),:)=Vq2i(Fc(indUni_F2),:)+Nq2(Fc(indUni_F2),:).*mean(vertexEdgeLengthSet(indUni_F2,:),2);%mean(edgeLengthSet(indPush,:),2); %Vq(F2(indUni_F2),:);

    Vq3=[Vq2;Vq2i]; %Joining node sets

    Fq2_p=fliplr(Fq2)+size(Vq2,1);
    Fq2_2=[Fq2(:,[3 2]) Fq2(:,[2 3])+size(Vq2,1)];%
    Fq2_3=[Fq2(:,[1 4]) Fq2(:,[4 1])+size(Vq2,1)];%
    Fq2_sides=[Fq2_p; Fq2_2; Fq2_3];
    Fq3=[Fq2_sides; Fq2;];
    Cq3=[2*ones(size(Fq2_sides,1),1); 3*ones(size(Fq2,1),1);];

    Cc=4*ones(size(Fc,1),1);

    FT=[Fq(:,[1 2 3]); Fq(:,[3 4 1]); Fq3(:,[1 2 3])+size(Vq,1); Fq3(:,[3 4 1])+size(Vq,1); Fc+size(Vq,1)+size(Vq2,1);]; %Faces
    VT=[Vq; Vq3]; %Vertices
    CT=[ones(size(Fq,1)*2,1); repmat(Cq3,2,1); Cc; ]; %Color data

    [FT,VT]=mergeVertices(FT,VT); %Merge vertices

else
   FT=[Fq(:,[1 2 3]); Fq(:,[3 4 1]); fliplr(F2)]; %Faces
   VT=Vq; %Vertices
   CT=[ones(size(Fq,1)*2,1);2*ones(size(F2,1),1)]; %Color data
end

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function volCoords = meshCursor2Volume(volView, msh)
%
%    volCoords = meshCursor2Volume(volView, [msh=viewGet(volView,'currentmesh')]);
%
%  Gets the current mrMesh cursor location and transforms it to mrVista
%  volume coordiantes.
%
% There are currently 3 ways to map the cursor to the VOLUME:
%
% 1. map the 3d cursor location directly to volume coords. This is the
% least accurate, since even smoothing the mesh will throw it off. If the
% mesh is not a real surface (eg. it's a cut plane or a DTI fiber), then
% this method will be used, since we have no other choice.
%
% 2. get the vertex index of the cursor and then find the 3d location of
% that vertex in the volume space. This avoids the smoothing problem in
% method 1, but is not consistent with all other mapping code that uses the
% vertexToGray map. This method will only be used if the vertex-to-gray
% transform field of the msh struct doesn't exist or is empty.
%
% 3. get the vertex index of the cursor and use the vertex-to-gray
% transform associated with the mesh to map these to layer-1 gray nodes.
% Then, we can just take the 3d coordinates of those layer 1 nodes. This is
% the most accurate method and is consistent with all our other mapping
% code. This method will be used by default, if it can.
%
% HISTORY:
%  2006.06.01 RFD: wrote it.
%  2006.10.24 RAS: doesn't crash, but warns, if cursor is outside volume
%  range.
%  2008.12.16 RFD & DY:
if(~exist('msh','var')||isempty(msh))
    msh = viewGet(volView,'currentmesh');
end

if(isfield(msh,'vertexGrayMap') && ~isempty(msh.vertexGrayMap))
    vertInd = mrmGet(msh,'cursorVertex');
    if vertInd < 1      % cursor not pointing to a volume vertex
        volCoords = [];
        warning('[%s]: Mesh Cursor is outside volume range.', mfilename); %#ok<*WNTAG>
        return
    end
    layerOneVolumeIndex = msh.vertexGrayMap(1,vertInd);

    % If the cursor position falls within the zone where functional data
    % was collected, translate this to a volume coordinate by grabbing the
    % position of the layer 1 node
    if(layerOneVolumeIndex>0 && layerOneVolumeIndex<=size(volView.coords,2) && ~isnan(layerOneVolumeIndex))
        volCoords = volView.coords(:,layerOneVolumeIndex)';
    % If not (and we have not computed layer 1 nodes for that position),
    % just get the cursor position at the gray/white boundary (which is
    % about 1mm off from where the layer 1 node position would be)
    else
        volCoords = round(msh.initVertices([2 1 3],vertInd)');
    end
else
    volCoords = round(mrmGet(msh,'cursor'));
end

return;


function D = driving_function_mono_wfs(x0,xs,src,f,conf)
%DRIVING_FUNCTION_MONO_WFS driving signal for WFS
%
%   Usage: D = driving_function_mono_wfs(x0,xs,src,f,conf)
%
%   Input parameters:
%       x0          - position and direction of the secondary source / m [nx7]
%       xs          - position of virtual source or direction of plane
%                     wave / m [1x3] or [1x6]
%       src         - source type of the virtual source
%                         'pw' - plane wave (xs is the direction of the
%                                plane wave in this case)
%                         'ps' - point source
%                         'ls' - line source
%                         'fs' - focused source
%       f           - frequency of the monochromatic source / Hz
%       conf        - configuration struct (see SFS_config)
%
%   Output parameters:
%       D           - driving function signal [nx1]
%
%   See also: plot_sound_field, sound_field_mono_wfs_25d,
%             driving_function_imp_wfs_25d

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

%% ===== Checking of input  parameters ==================================
nargmin = 5;
nargmax = 5;
narginchk(nargmin,nargmax);
isargsecondarysource(x0);
isargxs(xs);
isargpositivescalar(f);
isargchar(src);
isargstruct(conf);

%% ===== Computation ====================================================
% Calculate the driving function in time-frequency domain

% Secondary source positions and directions
nx0 = x0(:,4:6);
x0 = x0(:,1:3);

% Source position/direction/orientation
xs = repmat(xs,[size(x0,1) 1]);

% Get driving signals
if strcmp('pw',src)
    % === Plane wave ===
    % Direction of plane wave
    nk = bsxfun(@rdivide,xs,vector_norm(xs(:,1:3),2));
    % Driving signal
    D = driving_function_mono_wfs_pw(x0,nx0,nk,f,conf);

elseif strcmp('ps',src)
    % === Point source ===
    D = driving_function_mono_wfs_ps(x0,nx0,xs(:,1:3),f,conf);

elseif strcmp('ls',src)
    % === Line source ===
    D = driving_function_mono_wfs_ls(x0,nx0,xs,f,conf);

elseif strcmp('fs',src)
    % === Focused source ===
    D = driving_function_mono_wfs_fs(x0,nx0,xs(:,1:3),f,conf);

else
    error('%s: %s is not a known source type.',upper(mfilename),src);
end


function [c] = power(a,b,varargin)
% C = A.^B
%   [C]=POWER(A,B) Compute A.^B for a TT-tensor A and natural B.
%
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------

if isa(a,'tt_tensor') && numel(b) == 1
    c=a;
    b=b-1;
    while(b>0)
      c=c.*a;
      b=b-1;
    end

else
    error('Incorrect usage for TT-power');
end


classdef TestEstimator
    %TestEstimator

    properties (Constant)
        fields = {'aspect', 'focal', 'ppx', 'ppy', 'R', 't', 'K'};
    end

    methods (Static)
        function test_1
            img1 = imread(fullfile(mexopencv.root(),'test','tsukuba_l.png'));
            img2 = imread(fullfile(mexopencv.root(),'test','tsukuba_r.png'));

            finder = cv.FeaturesFinder('OrbFeaturesFinder');
            features = {finder.find(img1), finder.find(img2)};

            matcher = cv.FeaturesMatcher('BestOf2NearestMatcher');
            %matcher = cv.FeaturesMatcher('AffineBestOf2NearestMatcher');
            m = matcher.match_pairwise(features);

            obj = cv.Estimator('HomographyBasedEstimator');
            %obj = cv.Estimator('AffineBasedEstimator');
            typename = obj.typeid();
            validateattributes(typename, {'char'}, {'row', 'nonempty'});

            [cameras,success] = obj.estimate(features, m);
            validateattributes(success, {'logical'}, {'scalar'});
            if success
                validateattributes(cameras, {'struct'}, {'vector'});
                assert(all(ismember(TestEstimator.fields, fieldnames(cameras))));
                for i=1:numel(cameras)
                    validateattributes(cameras(i).aspect, {'numeric'}, {'scalar'});
                    validateattributes(cameras(i).focal, {'numeric'}, {'scalar'});
                    validateattributes(cameras(i).ppx, {'numeric'}, {'scalar'});
                    validateattributes(cameras(i).ppy, {'numeric'}, {'scalar'});
                    validateattributes(cameras(i).R, {'numeric'}, {'size',[3 3]});
                    validateattributes(cameras(i).t, {'numeric'}, {'vector','numel',3});
                    validateattributes(cameras(i).K, {'numeric'}, {'size',[3 3]});
                end
            end
        end
    end

end


function min_max = bounds2(someVector)
    % bounds2 is get min and max limits of vector.
    % like bounds, but returns a nx2 instead of two nx1 vectors.
    % min_max = bounds2([1 2 3 -1]); % returns [-1 3];
    %
    % see also bounds
    [min_max(1), min_max(2)] = bounds(someVector);
end


function rr = PRC(data, up)
%PRC fuses RR estimates using the pole ranking criterion.

%% Find times at which all three RRs were estimated
mutual_times = intersect(intersect(data.bw.t, data.fm.t), data.am.t);
quit_log = 0;
for mod = {'bw', 'am', 'fm'}
    eval(['temp_data = data.' mod{1,1} ';']);
    [rel_times,rel_els,~] = intersect(temp_data.t,mutual_times);
    if ~sum(strcmp(fieldnames(temp_data), 'pole_mag'))
        quit_log = 1;
    else
        eval([mod{1,1} '.est = data.' mod{1,1} '.v(rel_els);']);
        eval([mod{1,1} '.mag = data.' mod{1,1} '.pole_mag(rel_els);']);
        eval([mod{1,1} '.ang = data.' mod{1,1} '.pole_angle(rel_els);']);
    end
end

rr.v = nan(length(rel_times),1);
rr.t = rel_times;

% quit if one of the modulations wasn't present
if quit_log
    return
end

%% Find PRC
for win_no = 1 : length(mutual_times)
    [rel_ests, rel_mags, rel_angs] = deal([]);
    rel_mods = cell(0);
    for mod = {'bw', 'am', 'fm'}
        eval(['rel_ests = [rel_ests, ' mod{1,1} '.est(win_no)];']);
        eval(['rel_mags = [rel_mags, ' mod{1,1} '.mag(win_no)];']);
        eval(['rel_angs = [rel_angs, ' mod{1,1} '.ang(win_no)];']);
        rel_mods{length(rel_mods)+1,1} = mod{1,1};
    end
    %% Find RR
    comps = {'bwam', 'bwfm', 'amfm'};
    [prc.v, prc.rr] = deal(nan(length(comps),1));
    for comp_no = 1:length(comps)
        comp = comps(comp_no);
        % identify corresponding els
        temp1 = strfind(rel_mods, comp{1,1}(1:2));
        temp2 = strfind(rel_mods, comp{1,1}(3:4));
        rel_els = sort([find(~cellfun(@isempty,temp1)), find(~cellfun(@isempty,temp2))]);
        % find PRC for this pair
        prc.v(comp_no) = mean(rel_mags(rel_els))/abs(diff(rel_angs(rel_els)));
        prc.rr(comp_no) = mean(rel_ests(rel_els));
    end

    % identify relevant RR est
    [~,rel_comp_no] = max(prc.v);
    rr.v(win_no) = prc.rr(rel_comp_no);
end

end


function train_spatial_diff_part_neighbour(expidx,cidx)

fprintf('train_spatial_diff_part_app_dx_dy()\n');

if (ischar(expidx))
    expidx = str2num(expidx);
end

if (ischar(cidx))
    cidx = str2num(cidx);
end

p = exp_params(expidx);

pairwiseDir = p.pairwiseDir;
if (~exist(pairwiseDir,'dir')),mkdir(pairwiseDir);end

if isfield(p, 'bidirect') && p.bidirect
    next_joint = neighbour_joint_list();
    if cidx > length(next_joint)
        return;
    end
    start_joint = cidx;
    end_joint = next_joint(cidx);
    cidxs = sort([start_joint end_joint]);
end

if isfield(p, 'allpairs') && p.allpairs
    pwIdxsAllrel = build_joint_pairs(p);
    cidxs = pwIdxsAllrel{cidx};
end

modelFname = [pairwiseDir '/spatial_model_cidx_' num2str(cidxs(1)) '_' num2str(cidxs(2))];
try
    %assert(false);
    load(modelFname,'spatial_model');
    fprintf('cidx: %d - %d\n',cidxs);
    fprintf('spatial model file loaded. quitting.\n');
catch
    [X_pos, keys_pos, boxes_pos, X_neg, keys_neg, boxes_neg] = get_spatial_features_neighbour(expidx,cidx);

    opts.X_pos_mean = mean(X_pos);
    opts.X_pos_std = std(X_pos);
    %idxs1 = X_pos(:,1) >= opts.X_pos_mean(:,1) - 3*opts.X_pos_std(:,1) & X_pos(:,1) <= opts.X_pos_mean(:,1) + 3*opts.X_pos_std(:,1);
    %idxs2 = X_pos(:,2) >= opts.X_pos_mean(:,2) - 3*opts.X_pos_std(:,2) & X_pos(:,2) <= opts.X_pos_mean(:,2) + 3*opts.X_pos_std(:,2);
    %idxs3 = X_pos(:,3) >= opts.X_pos_mean(:,3) - 3*opts.X_pos_std(:,3) & X_pos(:,3) <= opts.X_pos_mean(:,3) + 3*opts.X_pos_std(:,3);
    %X_pos = X_pos(idxs1 & idxs2 & idxs3,:);
    if isfield(p, 'neighbor_locref') && p.neighbor_locref
        X_pos = get_augm_spatial_features_diff_neighbour_locref(X_pos); %opts.X_pos_mean, p
        X_neg = get_augm_spatial_features_diff_neighbour_locref(X_neg); %opts.X_pos_mean, p
    else
        X_pos = get_augm_spatial_features_diff_neighbour(X_pos, p); %opts.X_pos_mean, p
        X_neg = get_augm_spatial_features_diff_neighbour(X_neg, p); %opts.X_pos_mean, p
    end

    if (size(X_neg,1) > size(X_pos,1))
        nFeat = size(X_pos,1);
        idxs_rnd = randperm(size(X_neg,1));
        idxsSamp = idxs_rnd(1:nFeat);
        X_neg = X_neg(idxsSamp,:);
    end

    [X_norm, opts.X_min, opts.X_max] = getFeatNorm([X_pos;X_neg]);
    X_pos_norm = X_norm(1:size(X_pos,1),:);
    X_neg_norm = X_norm(size(X_pos,1)+1:end,:);
    %X_pos_norm = X_pos;
    %X_neg_norm = X_neg;

    reg_type = 0; % L2
    C = 1e-3;

    nPos = size(X_pos_norm,1);
    nNeg = size(X_neg_norm,1);
    lab_pos = ones(nPos,1);
    lab_neg = zeros(nNeg,1);
    lab = [lab_pos; lab_neg];
    ex = sparse(double([X_pos_norm; X_neg_norm]));
    model = train(lab, ex, ['-s ' num2str(reg_type) ' -B 1 -c ' num2str(C)]);

    spatial_model.training_opts = opts;
    spatial_model.log_reg = model;
    save(modelFname, 'spatial_model');

    if (1)
        visDir = [pairwiseDir '/vis/'];
        if (~exist(visDir,'dir')),mkdir(visDir);end
        [~,acc,pred] = predict(lab, ex, model, '-b 1');
        scrsz = get(0,'ScreenSize');
        figure('Position',[1 scrsz(4) scrsz(3)/2 scrsz(4)]);
        vis_logreg(pred,acc,1:nPos,1+nPos:nPos+nNeg);
        print(gcf,'-dpng',[visDir '/logreg_cidx_' num2str(cidxs(1)) '_' num2str(cidxs(2)) '.png']);
        close all;
    end
end
% ------------------------------------------------------------------------


%% DEMO_febio_0059_face_mask_loading
% Below is a demonstration for:
%
% * Building triangulated surface geometry for a face
% * Meshing the face using pentahedral elements
% * Building model of a tube
% * Defining the boundary conditions
% * Coding the febio structure
% * Running the model
% * Importing and visualizing results

%% Keywords
%
% * febio_spec version 3.0
% * febio, FEBio
% * face
% * contact, sliding, friction
% * pentahedral elements, penta6
% * static, solid
% * hyperelastic, Ogden
% * displacement logfile

%%

clear; close all; clc;

%%
% Plot settings
fontSize=15;
faceAlpha1=1;
faceAlpha2=0.3;
markerSize1=15;
markerSize2=10;
lineWidth=2;
cMap=spectral(250);

%% Control parameters

% Path names
defaultFolder = fileparts(fileparts(mfilename('fullpath')));
savePath=fullfile(defaultFolder,'data','temp');

% Defining file names
febioFebFileNamePart='tempModel';
febioFebFileName=fullfile(savePath,[febioFebFileNamePart,'.feb']); %FEB file name
febioLogFileName=fullfile(savePath,[febioFebFileNamePart,'.txt']); %FEBio log file name
febioLogFileName_disp=[febioFebFileNamePart,'_disp_out.txt']; %Log file name for exporting displacement
febioLogFileName_force=[febioFebFileNamePart,'_force_out.txt']; %Log file name for exporting force
febioLogFileName_strainEnergy=[febioFebFileNamePart,'_energy_out.txt']; %Log file name for exporting strain energy density

% Geometry parameters
pointSpacingTissue=6;
faceTissueThickness=6;
pointSpacingMask=pointSpacingTissue/2;
maskRimWidth=5;
maskRimFilletRadius=6;
maskDiscRadius1=25;
maskDiscRadius2=maskDiscRadius1+4;
maskDiscOffset=25;
bezierTangency=0.1;

distInclude=40; %Distance from mask to include face in FEA

%Ray tracing parameters
optionStructRayTrace.tolEps        = 1e-6;
optionStructRayTrace.triSide       = 0;
optionStructRayTrace.rayType       = 'ray';
optionStructRayTrace.exclusionType = 'inclusive';
optionStructRayTrace.paired        = 0;

%Material parameters
c1_tissue=1e-3; %Shear-modulus-like parameter
m1_tissue=2; %Material parameter setting degree of non-linearity
k_tissue=c1_tissue*100; %Bulk modulus

c1_rim=c1_tissue*5; %Shear-modulus-like parameter
m1_rim=2; %Material parameter setting degree of non-linearity
k_rim=c1_rim*10; %Bulk modulus

% FEA control settings
numTimeSteps=15; %Number of time steps desired
max_refs=35; %Max reforms
max_ups=0; %Set to zero to use full-Newton iterations
opt_iter=10; %Optimum number of iterations
max_retries=5; %Maximum number of retires
dtmin=(1/numTimeSteps)/100; %Minimum time step size
dtmax=(1/numTimeSteps); %Maximum time step size
symmetric_stiffness=0;
min_residual=1e-20;
runMode='external';

%Boundary condition parameters
initialOffset=0;
displacementMagnitude_z=-2-initialOffset; %Displacement applied

%Contact parameters
contactPenalty=10;
laugon=0;
minaug=1;
maxaug=10;
fric_coeff=0.5;

%% Load face geometry

testCase=1;
switch testCase
    case 1
        %Load surface model
        [Ff,Vf]=graphicsModels(9);

        %Surface markers
        V_markers=[65.51,49.94,217.14;... %Tip of the nose
            66.44,54.79,259.81;... %Nose between eyes
            53.81,115,263.39;... %Right eye outer corner
            126.5,46.62,269.75;... %Left eye outer corner
            85.67,69.44,194.1;... %Middle of mounth
            98.39,80.38,158]; %Bottom of chin
    case 2
        %Load surface model
        [Ff,Vf]=graphicsModels(13);

        %Surface markers
        V_markers=[3.50162,-181.107,-8.09110;...  %Tip of the nose
                   1.51627,-159.171,30.4679;... %Nose between eyes
                   -43.2473,-139.003,24.6407;... %Right eye outer corner
                   48.4245,-136.490,25.3913;... %Left eye outer corner
                   4.11905,-167.594,-36.9600;... %Middle of mounth
                   2.85290,-161.802,-73.4644]; %Bottom of chin
end

distEyes=sqrt(sum((V_markers(3,:)-V_markers(4,:)).^2,2));

%% Remeshing surface
optionStruct_remesh.pointSpacing=pointSpacingTissue; %Set desired point spacing
optionStruct_remesh.disp_on=0; % Turn off command window text display
[Ff,Vf]=ggremesh(Ff,Vf,optionStruct_remesh);

%%

ny=vecnormalize(V_markers(2,:)-V_markers(6,:));
nx=vecnormalize(V_markers(4,:)-V_markers(3,:));
nz=vecnormalize(cross(nx,ny));
nx=vecnormalize(cross(ny,nz));

Q=[nx;ny;nz]';

%%

% cFigure; hold on;
% gpatch(Ff,Vf,'w','none',1);
% % plotV(V_markers,'r.','MarkerSize',35);
% % text(V_markers(:,1)+4,V_markers(:,2),V_markers(:,3),{'1','2','3','4','5','6'},'FontSize',25);
% % quiverTriad(V_markers(1,:),Q,100);
% axisGeom; camlight headlight;
% colormap(spectral(250))
% gdrawnow;

%%

cFigure; hold on;
gpatch(Ff,Vf,'w','none',0.5);
plotV(V_markers,'r.','MarkerSize',35);
text(V_markers(:,1)+4,V_markers(:,2),V_markers(:,3),{'1','2','3','4','5','6'},'FontSize',25);
quiverTriad(V_markers(1,:),Q,100);
axisGeom; camlight headlight;
colormap(spectral(250))
gdrawnow;

%% Centre on nose and rotate to face face looking down Z-axis

Vf=Vf-V_markers(1,:);
Vf=Vf*Q;
V_markers=V_markers-V_markers(1,:);
V_markers=V_markers*Q;
nz=[0 0 1];%nz*Q;

%%

cFigure; hold on;
gpatch(Ff,Vf,'w','none',0.5);
plotV(V_markers,'r.','MarkerSize',35);
text(V_markers(:,1)+6,V_markers(:,2),V_markers(:,3)+15,{'1','2','3','4','5','6'},'FontSize',25);
axisGeom; camlight headlight;
colormap(spectral(250))
gdrawnow;

%% Construct mask rim curve

V1=V_markers(1,[1 2]);
V2=V_markers(2,[1 2]);
V3=V_markers(3,[1 2]);
V4=V_markers(4,[1 2]);
V5=V_markers(5,[1 2]);
V6=V_markers(6,[1 2]);

pp1=0.4*V1+0.6*V2;
pp2=0.6*V1+0.4*V3;
pp3=V3-[0 V3(2)]+[0 0.5*V5(2)+0.5*V1(2)];
pp4=[0.3*V2(1)+0.8*V3(1) 0.5*V5(2)+0.5*V6(2)];
pp5=V6;
pp6=[0.3*V2(1)+0.8*V4(1) 0.5*V5(2)+0.5*V6(2)];
pp7=V4-[0 V4(2)]+[0 0.5*V5(2)+0.5*V1(2)];
pp8=0.6*V1+0.4*V4;

V_rim_points=[pp1;pp2;pp3;pp4;pp5;pp6;pp7;pp8];
V_rim_points(:,3)=0;

[V_rim_points,indFaceIntersect]=traceToSurf(V_rim_points,-nz,Ff,Vf,optionStructRayTrace);
numRimControlPoints=size(V_rim_points,1);

V_rim_curve=evenlySpaceCurve(V_rim_points,pointSpacingMask,'pchip',1);
V_rim_curve=traceToSurf(V_rim_curve,-nz,Ff,Vf,optionStructRayTrace);
numPointsRimCurve=size(V_rim_curve,1);

Ne1=vecnormalize([V_rim_points(2:end,:); V_rim_points(1,:)]-V_rim_points(1:end,:));
Ne2=vecnormalize(V_rim_points - [V_rim_points(end,:); V_rim_points(1:end-1,:)]);
Ne=vecnormalize(0.5*Ne1+0.5*Ne2);

Nf=patchNormal(Ff,Vf); %Normal directions
Nff=Nf(indFaceIntersect(:,2),:);
N_rim_points=vecnormalize(cross(Nff,Ne));

V_rim_points1=V_rim_points-N_rim_points.*maskRimWidth/2;
V_rim_points2=V_rim_points+N_rim_points.*maskRimWidth/2;

%%

cFigure; hold on;
gpatch(Ff,Vf,'w','none',0.5);
plotV(V_markers,'r.','MarkerSize',25);
text(V_markers(:,1)+4,V_markers(:,2),V_markers(:,3),{'1','2','3','4','5','6'},'FontSize',25);

plotV(V_rim_points,'k.','MarkerSize',25);
plotV(V_rim_points1,'b.','MarkerSize',25);
plotV(V_rim_points2,'g.','MarkerSize',25);

quiverVec(V_rim_points,N_rim_points,maskRimWidth/2,'y');
quiverVec(V_rim_points,-N_rim_points,maskRimWidth/2,'y');
axisGeom; camlight headlight;
view(2);
gdrawnow;

%%

[~,indClose]=minDist(V_markers,Vf);
d=meshDistMarch(Ff,Vf,indClose([1 5]));

[~,indClose]=minDist(V_rim_curve,Vf);
d_rim_curve=meshDistMarch(Ff,Vf,indClose);
d_markers_max=max(d(indClose));

logicCloseVertices= d<=d_markers_max | d_rim_curve<distInclude;

logicCloseFaces= any(logicCloseVertices(Ff),2);
logicCloseFaces=triSurfLogicSharpFix(Ff,logicCloseFaces);

[Fs,Vs]=patchCleanUnused(Ff(logicCloseFaces,:),Vf);

ns=vecnormalize(mean(patchNormal(Fs,Vs)));

[Q]=pointSetPrincipalDir(Vs);
nz=Q(:,3)';
if dot(nz,ns)<1
    nz=-nz;
end

ny=vecnormalize(V_markers(2,:)-V_markers(1,:));
nx=cross(ny,nz);
ny=cross(nz,nx);
Q=[nx;ny;nz]';

Ffc=Ff(~logicCloseFaces,:);

%%

cFigure;  hold on;
gpatch(Ff,Vf,d,'k',0.5);
gpatch(Ff(logicCloseFaces,:),Vf,'none','b',1,2);
plotV(V_markers,'r.','MarkerSize',25);
plotV(V_rim_points,'k.','MarkerSize',15);
plotV(V_rim_curve,'k-','LineWidth',3);

axisGeom; camlight headlight;
colormap(spectral(250))
gdrawnow;

%%

cFigure;  hold on;
gpatch(Ff,Vf,'w','none',0.5);
% gpatch(Ff(logicCloseFaces,:),Vf,'none','b',1,2);
plotV(V_markers,'r.','MarkerSize',25);
plotV(V_rim_points,'k.','MarkerSize',15);
plotV(V_rim_curve,'k-','LineWidth',3);

axisGeom; camlight headlight;
colormap(spectral(250))
gdrawnow;

%%

cFigure;  hold on;
gpatch(Ff,Vf,'w','none');
gpatch(Fs,Vs,'w','b');
% patchNormPlot(Fs,Vs);
plotV(V_markers,'r.','MarkerSize',25);
% quiverTriad(V_markers(1,:),Q,50);
axisGeom;
camlight headlight;
% colormap(viridis(2)); icolorbar;
gdrawnow;

%%

Ebs=patchBoundary(Fs);
indBoundaryCurve=edgeListToCurve(Ebs);
indBoundaryCurve=indBoundaryCurve(1:end-1)';

[~,~,Ns]=patchNormal(Fs,Vs);

Vs2=Vs-Ns*faceTissueThickness;
Fs2=Fs;

Vsc=Vs2(indBoundaryCurve,:);%-Ns(indBoundaryCurve,:)*layerThickness;
[Fs2t,Vs2t]=regionTriMesh3D({Vsc},pointSpacingMask,0,'natural');
Vs2t_ori=Vs2t;
indBoundary=unique(patchBoundary(Fs2t));

[~,indMap]=minDist(Vsc,Vs2t(indBoundary,:));
indBoundaryCurve_2t=indBoundary(indMap);
indBoundaryCurve_2t=indBoundaryCurve_2t(:);

Vs2t=traceToSurf(Vs2t,-nz,Fs2,Vs2,optionStructRayTrace);

cParSmooth.n=3;
cParSmooth.Method='HC';
cParSmooth.RigidConstraints=indBoundaryCurve_2t;
Vs2t=patchSmooth(Fs2t,Vs2t,[],cParSmooth);

numNodesThickness=ceil(faceTissueThickness./pointSpacingTissue);
if numNodesThickness<2
    numNodesThickness=2;
end

cParLoft.numSteps=numNodesThickness;
cParLoft.closeLoopOpt=1;
cParLoft.patchType='tri';
[Fss,Vss,ind1,ind2]=polyLoftLinear(Vs(indBoundaryCurve,:),Vs2t(indBoundaryCurve_2t,:),cParLoft);

[Fb,Vb,Cb]=joinElementSets({Fs,Fs2t,Fss},{Vs,Vs2t,Vss});
[Fb,Vb]=mergeVertices(Fb,Vb);

%%

cFigure; hold on;
gpatch(Ffc,Vf,'w','none',0.5);
gpatch(Fb,Vb,Cb,'k',0.5);

% gpatch(Fs,Vs,'bw','k',1);
% % gpatch(Fs2t,Vs2t_ori,'gw','g',0.5);
% gpatch(Fs2t,Vs2t,'rw','k',1);
% gpatch(Fss,Vss,'gw','k',1);

% patchNormPlot(Fb,Vb);
axisGeom; camlight headlight;
colormap(spectral); icolorbar;
gdrawnow;

%%
% Get inner mesh point between top and bottom at nose
Pn=triSurfRayTrace(V_markers(1,:),-nz,Fb,Vb,optionStructRayTrace);
Pn=mean(Pn,1);

%%

cFigure; hold on;
%gpatch(Ffc,Vf,'w','none',0.5);
gpatch(Fb,Vb,Cb,'k',1);
plotV(Pn,'r.','MarkerSize',25);

% patchNormPlot(Fb,Vb);
axisGeom; camlight headlight;
colormap(spectral); icolorbar;
gdrawnow;

%%

%Create tetgen input structure
inputStruct.stringOpt='-pq1.2AaY'; %Options for tetgen
inputStruct.Faces=Fb; %Boundary faces
inputStruct.Nodes=Vb; %Nodes of boundary
inputStruct.faceBoundaryMarker=Cb;
inputStruct.regionPoints=Pn; %Interior points for regions
inputStruct.holePoints=[]; %Interior points for holes
inputStruct.regionA=tetVolMeanEst(Fb,Vb); %Desired tetrahedral volume for each region

% Mesh model using tetrahedral elements using tetGen
[meshOutput]=runTetGen(inputStruct); %Run tetGen

%%
% Access mesh output structure

E_face=meshOutput.elements; %The elements
V=meshOutput.nodes; %The vertices or nodes
F=meshOutput.faces; %Element faces (all)
CE=meshOutput.elementMaterialID; %Element material or region id
Fb=meshOutput.facesBoundary; %The boundary faces
Cb=meshOutput.boundaryMarker; %The boundary markers

%%
% Visualization

hf=cFigure;
subplot(1,2,1); hold on;
title('Input boundaries','FontSize',fontSize);
hp(1)=gpatch(Fb,V,Cb,'k',faceAlpha1);
hp(2)=plotV(Pn,'r.','MarkerSize',markerSize1);
legend(hp,{'Input mesh','Interior point(s)'},'Location','NorthWestOutside');
axisGeom(gca,fontSize); camlight headlight;
colormap(cMap); icolorbar;

hs=subplot(1,2,2); hold on;
title('Tetrahedral mesh','FontSize',fontSize);

% Visualizing using |meshView|
optionStruct.hFig=[hf,hs];

meshView(meshOutput,optionStruct);
hold on; plotV(Pn,'r.','MarkerSize',25);
axisGeom(gca,fontSize);
gdrawnow;

%%

pp9=V_markers(5,:);% (0.1*V_markers(1,:)+0.9*V_markers(5,:));

t=linspace(0.5*pi,2.5*pi,numPointsRimCurve+1)';
t=t(1:end-1);
Vcd1=maskDiscRadius1*[cos(t) sin(t) zeros(size(t))];
Vcd1=Vcd1+pp9;
Vcd1(:,3)=maskDiscOffset;

Vcd2=maskDiscRadius2*[cos(t) sin(t) zeros(size(t))];
Vcd2=Vcd2+pp9;
Vcd2(:,3)=maskDiscOffset;

%
cFigure; hold on;
gpatch(Fb,V,'w','none',0.5);
plotV(V_markers,'r.','MarkerSize',50);
text(V_markers(:,1)+4,V_markers(:,2),V_markers(:,3)+10,{'1','2','3','4','5','6'},'FontSize',25);

plotV(V_rim_points,'k.','MarkerSize',35);
plotV(V_rim_curve,'k-','LineWidth',6);
plotV(Vcd1,'b-','LineWidth',6);
plotV(Vcd2,'b-','LineWidth',6);
axisGeom;
camlight headlight;
view(2);
drawnow;

%%

V_rim_curve1=evenlySampleCurve(V_rim_points1,numPointsRimCurve,'pchip',1);
V_rim_curve2=evenlySampleCurve(V_rim_points2,numPointsRimCurve,'pchip',1);

V_rim_curve1=traceToSurf(V_rim_curve1,[0 0 -1],Fb(Cb==1,:),V,optionStructRayTrace);
V_rim_curve2=traceToSurf(V_rim_curve2,[0 0 -1],Fb(Cb==1,:),V,optionStructRayTrace);

%%
cFigure; hold on;
gpatch(Fb(Cb==1,:),Vb,'kw','none',0.5);

% patchNormPlot(Fm,Vm);
plotV(V_markers,'r.','MarkerSize',50);
text(V_markers(:,1)+2,V_markers(:,2),V_markers(:,3),{'1','2','3','4','5','6'},'FontSize',25);
plotV(V_rim_points,'k.','MarkerSize',35,'LineWidth',3);
plotV(V_rim_curve,'k-','LineWidth',3);

plotV(V_rim_points1,'g.','MarkerSize',35,'LineWidth',3);
plotV(V_rim_curve1,'g-','LineWidth',3);

plotV(V_rim_points2,'b.','MarkerSize',35,'LineWidth',3);
plotV(V_rim_curve2,'b-','LineWidth',3);

axisGeom; camlight headlight;
view(2);
drawnow;

%%

pointSpacingNow=mean(diff(pathLength(V_rim_curve1)));
numNodStrip=ceil(maskRimWidth./pointSpacingNow);
if numNodStrip<2
    numNodStrip=2;
end
if iseven(numNodStrip)
    numNodStrip=numNodStrip+1;
end

cParLoft.numSteps=numNodStrip;
cParLoft.closeLoopOpt=1;
cParLoft.patchType='tri';
[Fm,Vm,indStart_Vm,indEnd_Vm]=polyLoftLinear(V_rim_curve1,V_rim_curve2,cParLoft);
Fm=fliplr(Fm);
indStart_Vm=fliplr(indStart_Vm);
indEnd_Vm=fliplr(indEnd_Vm);

[~,~,Nm]=patchNormal(Fm,Vm);
Vm=traceToSurf(Vm,Nm,Fb(Cb==1,:),V,optionStructRayTrace);

%%
cFigure; hold on;
gpatch(Fb(Cb==1,:),Vb,'kw','none',0.5);
gpatch(Fm,Vm,'rw','k',1,1);
% patchNormPlot(Fm,Vm);
% plotV(V_markers,'r.','MarkerSize',50);
% text(V_markers(:,1)+4,V_markers(:,2),V_markers(:,3),{'1','2','3','4','5','6'},'FontSize',25);
plotV(V_rim_points,'k.','MarkerSize',35,'LineWidth',3);
plotV(Vm(indStart_Vm,:),'g-','LineWidth',3);
plotV(Vm(indEnd_Vm,:),'b-','LineWidth',3);
axisGeom; camlight headlight;
view(2);
drawnow;

%%

[~,~,Nm]=patchNormal(Fm,Vm);

pointSpacingNow=mean(diff(pathLength(Vm(indStart_Vm,:))));
nRim=ceil((pi/2*maskRimFilletRadius)/pointSpacingNow)+1;
if nRim<4
    nRim=4;
end

[Fr1,Vr1]=roundMesh(indStart_Vm,Vm,Nm,nRim,maskRimFilletRadius);
[Fr2,Vr2]=roundMesh(indEnd_Vm,Vm,Nm,nRim,maskRimFilletRadius);
indEnd_Vr1=size(Vr1)-numPointsRimCurve+1:1:size(Vr1);
indEnd_Vr2=fliplr(indEnd_Vr1);
[Fr1,Vr1]=quad2tri(Fr1,Vr1,'a');
[Fr2,Vr2]=quad2tri(Fr2,Vr2,'a');

%%

cFigure; hold on;
gpatch(Fb,V,'kw','none',0.5);
gpatch(Fm,Vm,'rw','k',1,1);
gpatch(Fr1,Vr1,'gw','k',1,1);
gpatch(Fr2,Vr2,'bw','k',1,1);
plotV(Vr1(indEnd_Vr1,:),'r-','LineWidth',3);
plotV(Vr2(indEnd_Vr2,:),'b-','LineWidth',3);
axisGeom; camlight headlight;
view(2);
drawnow;

%%

pointSpacingNow=mean(diff(pathLength(Vr1(indEnd_Vr1,:))));
numNodStripTop=ceil((maskRimWidth+2*maskRimFilletRadius)./pointSpacingNow);
if numNodStripTop<2
    numNodStripTop=2;
end
if iseven(numNodStripTop)
    numNodStripTop=numNodStripTop+1;
end

cParLoft.numSteps=numNodStripTop;
cParLoft.closeLoopOpt=1;
cParLoft.patchType='tri';
[Ft,Vt,indCurve1_Vt,indCurve2_Vt]=polyLoftLinear(Vr1(indEnd_Vr1,:),Vr2(indEnd_Vr2,:),cParLoft);
Ft=fliplr(Ft);
indCurve1_Vt=flipud(indCurve1_Vt(:));
% indCurve2_Vt=flipud(indCurve2_Vt(:));
[~,~,Nt]=patchNormal(Ft,Vt);

%%

cFigure; hold on;
gpatch(Fb,V,'w','none',1);
gpatch(Fm,Vm,'y','k',0,1);
gpatch(Ft,Vt,'gw','k',1,1);
gpatch(Fr1,Vr1,'rw','k',1,1);
gpatch(Fr2,Vr2,'bw','k',1,1);
plotV(Vr1(indEnd_Vr1,:),'r-','LineWidth',3);
plotV(Vr2(indEnd_Vr1,:),'b-','LineWidth',3);
axisGeom; camlight headlight;
view(2);
drawnow;

%% Join and merge rim surfaces

[F_rim,V_rim,C_rim]=joinElementSets({Fm,Fr1,Fr2,Ft},{Vm,Vr1,Vr2,Vt});
[F_rim,V_rim]=mergeVertices(F_rim,V_rim);
[F_rim,V_rim]=patchCleanUnused(F_rim,V_rim);

%%

cFigure; hold on;
gpatch(Fb(Cb==1,:),V,'w','none',1);
gpatch(F_rim,V_rim,C_rim,'none',1);
% patchNormPlot(F_rim,V_rim);
axisGeom; camlight headlight;
colormap spectral; icolorbar;
drawnow;

%%

V_loft1=Vt(indCurve1_Vt,:);
V_loft2=Vcd2;

V_loft3=Vt(indCurve2_Vt,:);
V_loft4=Vcd1;

[~,indMax]=max(V_loft4(:,1));
if indMax<numPointsRimCurve/2
    V_loft4=flipud(V_loft4);
end

[~,indMax]=max(V_loft2(:,1));
if indMax<numPointsRimCurve/2
    V_loft2=flipud(V_loft2);
end

N1=Nt(indCurve1_Vt,:);
N3=Nt(indCurve2_Vt,:);
N4=ones(size(V_loft4,1),1)*[0 0 1];

N2e=vecnormalize([V_loft2(2:end,:); V_loft2(1,:)]-V_loft2(1:end,:));
N2=vecnormalize(cross(N4,N2e));

[Fd1,Vd1,X,Y,Z]=bezierLoft(V_loft1,V_loft2,N1,N2,pointSpacingMask,bezierTangency);
[Fd1,Vd1]=quad2tri(Fd1,Vd1);
[Fd2,Vd2,X,Y,Z]=bezierLoft(V_loft3,V_loft4,N3,N4,pointSpacingMask,bezierTangency);
[Fd2,Vd2]=quad2tri(Fd2,Vd2);

pointSpacingNow=mean(diff(pathLength(V_loft2)));
n=ceil((maskDiscRadius2-maskDiscRadius1)./pointSpacingNow);
if n<2
    n=2;
end
if iseven(n)
    n=n+1;
end

cParLoft.numSteps=n;
cParLoft.closeLoopOpt=1;
cParLoft.patchType='tri';
[Fdt,Vdt]=polyLoftLinear(V_loft2,V_loft4,cParLoft);

[Fc,Vc]=regionTriMesh2D({V_loft4(:,[1 2])},pointSpacingNow,0,0);
Vc(:,3)=maskDiscOffset;

%%

cFigure; hold on;
gpatch(Fb(Cb==1,:),V,'kw','none',0.5);
gpatch(F_rim,V_rim,'kw','none',1);

gpatch(Fd1,Vd1,'rw','none',0.5);
gpatch(Fd2,Vd2,'bw','none',0.5);
gpatch(Fdt,Vdt,'gw','none',0.5);
gpatch(Fc,Vc,'yw','none',0.5);

plotV(V_loft1,'r-','LineWidth',3);
quiverVec(V_loft1,N1,5,'k');

plotV(V_loft4,'b-','LineWidth',2);
quiverVec(V_loft4,N4,5,'k');

plotV(V_loft3,'b-','LineWidth',3);
quiverVec(V_loft3,N3,5,'k');

plotV(V_loft2,'r-','LineWidth',2);

quiverVec(V_loft2,N2,5,'k');

axisGeom; camlight headlight;
drawnow;

%% Join and merge mask body components

[F_mask,V_mask,C_mask]=joinElementSets({Ft,Fd1,Fd2,Fdt,Fc},{Vt,Vd1,Vd2,Vdt,Vc});
[F_mask,V_mask]=mergeVertices(F_mask,V_mask);
[F_mask,V_mask]=patchCleanUnused(F_mask,V_mask);

%%
cFigure; hold on;
gpatch(Ff,Vf,'w','none',1);
% gpatch(Fb,V,'w','none',1);
gpatch(F_mask,V_mask,'bw','none',1);
gpatch(F_rim,V_rim,'kw','none',1);
axisGeom; camlight headlight;
view(2);
% colormap spectral; icolorbar;
drawnow;

%%
cFigure; hold on;
gpatch(Ff,Vf,'w','none',1);
% gpatch(Fb,V,'w','none',1);
% gpatch(F_mask,V_mask,'gw','none',0.5);
gpatch(F_rim,V_rim,'kw','none',1);
axisGeom; camlight headlight;
drawnow;

%%

cFigure; hold on;
% gpatch(Fp1,V,'w','none',1);
gpatch(F_mask,V_mask,'bw','none',1);
gpatch(F_rim,V_rim,'gw','none',1);
axisGeom;
camlight headlight;
view(2);
drawnow;

%%

cFigure; hold on;
% gpatch(Fb,V,'w','none',0.9);
gpatch(F_rim,V_rim,C_rim,'k',1);
% patchNormPlot(F_rim,V_rim);
axisGeom;
camlight headlight;
view(2);
colormap gjet; icolorbar;
drawnow;

%%

[V_regions]=getInnerPoint(F_rim,V_rim); % Define region points

%%

cFigure; hold on;
gpatch(F_rim,V_rim,C_rim,'k',1);
% plotV(V_regions,'r.','MarkerSize',markerSize1)
axisGeom;
camlight headlight; view(2);
drawnow;

%%

[regionA]=tetVolMeanEst(F_rim,V_rim); %Volume for regular tets

inputStruct.stringOpt='-pq1.2AaY';
inputStruct.Faces=F_rim;
inputStruct.Nodes=V_rim;
inputStruct.holePoints=[];
inputStruct.faceBoundaryMarker=C_rim; %Face boundary markers
inputStruct.regionPoints=V_regions; %region points
inputStruct.regionA=regionA;

% Mesh model using tetrahedral elements using tetGen
[meshOutput]=runTetGen(inputStruct); %Run tetGen

%%
% Access model element and patch data
Fb_rim=meshOutput.facesBoundary;
Cb_rim=meshOutput.boundaryMarker;
V_rim=meshOutput.nodes;
E_rim=meshOutput.elements;

% Visualizing mesh using |meshView|, see also |anim8|
meshView(meshOutput);

%% Join node sets
V_rim(:,3)=V_rim(:,3)+initialOffset;
Fb_rim=Fb_rim+size(V,1);
E_rim=E_rim+size(V,1);
V=[V;V_rim];

%%
cFigure;
gpatch(F,V,'w','k',1);
gpatch(Fb_rim,V,Cb_rim,'k',1);
axisGeom;
colormap gjet; icolorbar;
camlight headlight;
drawnow;

%% Define contact surfaces

% The rigid primary surface of the sphere
F_contact_primary=fliplr(Fb_rim(Cb_rim~=4,:));

% The deformable secondary surface of the slab
Fb_contact=Fb(Cb==1,:);
V_Fb_centre=patchCentre(Fb_contact,V);
D=minDist(V_Fb_centre,V_rim);
logicSecondary=D<=(2*pointSpacingTissue);
logicSecondary=triSurfLogicSharpFix(Fb_contact,logicSecondary,3);
F_contact_secondary=fliplr(Fb_contact(logicSecondary,:));

%%
% Visualize contact surfaces

cFigure; hold on;
title('Contact sets and normal directions','FontSize',fontSize);

gpatch(Fb,V,'kw','none',0.5);

hl(1)=gpatch(F_contact_primary,V,'gw','k',1);
patchNormPlot(F_contact_primary,V);
hl(2)=gpatch(F_contact_secondary,V,'rw','k',1);
patchNormPlot(F_contact_secondary,V);

legend(hl,{'Primary','secondary'});

axisGeom(gca,fontSize);
camlight headlight;
drawnow;

%% Define boundary conditions

%Supported nodes
bcSupportList=unique(Fb(Cb==2,:));

%Prescribed displacement nodes
bcPrescribeList=unique(Fb_rim(Cb_rim==4,:));

%%
% Visualize BC's

hf=cFigure; hold on;
% title('Boundary conditions model','FontSize',fontSize);
gpatch(Fb,V,'kw','none',faceAlpha2);
gpatch(Fb_rim(Cb_rim~=4,:),V,'kw','none',faceAlpha2);
gpatch(Fb_rim(Cb_rim==4,:),V,'rw','none',1);
gpatch(Fb(Cb==2,:),V,'kw','none',1);
hl2(1)=plotV(V(bcPrescribeList,:),'r.','MarkerSize',markerSize2);
hl2(2)=plotV(V(bcSupportList,:),'k.','MarkerSize',markerSize2);
legend(hl2,{'BC prescribe','BC support'});
axisGeom(gca,fontSize); camlight headlight;
drawnow;

%% Defining the FEBio input structure
% See also |febioStructTemplate| and |febioStruct2xml| and the FEBio user
% manual.

%Get a template with default settings
[febio_spec]=febioStructTemplate;

%febio_spec version
febio_spec.ATTR.version='3.0';

%Module section
febio_spec.Module.ATTR.type='solid';

%Control section
febio_spec.Control.analysis='STATIC';
febio_spec.Control.time_steps=numTimeSteps;
febio_spec.Control.step_size=1/numTimeSteps;
febio_spec.Control.solver.max_refs=max_refs;
febio_spec.Control.solver.max_ups=max_ups;
febio_spec.Control.solver.symmetric_stiffness=symmetric_stiffness;
febio_spec.Control.time_stepper.dtmin=dtmin;
febio_spec.Control.time_stepper.dtmax=dtmax;
febio_spec.Control.time_stepper.max_retries=max_retries;
febio_spec.Control.time_stepper.opt_iter=opt_iter;

%Material section
materialName1='Material1';
febio_spec.Material.material{1}.ATTR.name=materialName1;
febio_spec.Material.material{1}.ATTR.type='Ogden unconstrained';
febio_spec.Material.material{1}.ATTR.id=1;
febio_spec.Material.material{1}.c1=c1_tissue;
febio_spec.Material.material{1}.m1=m1_tissue;
febio_spec.Material.material{1}.c2=c1_tissue;
febio_spec.Material.material{1}.m2=-m1_tissue;
febio_spec.Material.material{1}.cp=k_tissue;

materialName2='Material2';
febio_spec.Material.material{2}.ATTR.name=materialName2;
febio_spec.Material.material{2}.ATTR.type='Ogden unconstrained';
febio_spec.Material.material{2}.ATTR.id=2;
febio_spec.Material.material{2}.c1=c1_rim;
febio_spec.Material.material{2}.m1=m1_rim;
febio_spec.Material.material{2}.c2=c1_rim;
febio_spec.Material.material{2}.m2=-m1_rim;
febio_spec.Material.material{2}.cp=k_rim;

% Mesh section
% -> Nodes
febio_spec.Mesh.Nodes{1}.ATTR.name='All'; %The node set name
febio_spec.Mesh.Nodes{1}.node.ATTR.id=(1:size(V,1))'; %The node id's
febio_spec.Mesh.Nodes{1}.node.VAL=V; %The nodel coordinates

% -> Elements
partName1='Part1';
febio_spec.Mesh.Elements{1}.ATTR.name=partName1; %Name of this part
febio_spec.Mesh.Elements{1}.ATTR.type='tet4'; %Element type
febio_spec.Mesh.Elements{1}.elem.ATTR.id=(1:1:size(E_face,1))'; %Element id's
febio_spec.Mesh.Elements{1}.elem.VAL=E_face; %The element matrix

partName2='Part2';
febio_spec.Mesh.Elements{2}.ATTR.name=partName2; %Name of this part
febio_spec.Mesh.Elements{2}.ATTR.type='tet4'; %Element type
febio_spec.Mesh.Elements{2}.elem.ATTR.id=size(E_face,1)+(1:1:size(E_rim,1))'; %Element id's
febio_spec.Mesh.Elements{2}.elem.VAL=E_rim; %The element matrix

% -> NodeSets
nodeSetName1='bcSupportList';
febio_spec.Mesh.NodeSet{1}.ATTR.name=nodeSetName1;
febio_spec.Mesh.NodeSet{1}.node.ATTR.id=bcSupportList(:);

nodeSetName2='bcPrescribeList';
febio_spec.Mesh.NodeSet{2}.ATTR.name=nodeSetName2;
febio_spec.Mesh.NodeSet{2}.node.ATTR.id=bcPrescribeList(:);

%MeshDomains section
febio_spec.MeshDomains.SolidDomain{1}.ATTR.name=partName1;
febio_spec.MeshDomains.SolidDomain{1}.ATTR.mat=materialName1;

febio_spec.MeshDomains.SolidDomain{2}.ATTR.name=partName2;
febio_spec.MeshDomains.SolidDomain{2}.ATTR.mat=materialName2;

% -> Surfaces
surfaceName1='contactSurface1';
febio_spec.Mesh.Surface{1}.ATTR.name=surfaceName1;
febio_spec.Mesh.Surface{1}.tri3.ATTR.id=(1:1:size(F_contact_primary,1))';
febio_spec.Mesh.Surface{1}.tri3.VAL=F_contact_primary;

surfaceName2='contactSurface2';
febio_spec.Mesh.Surface{2}.ATTR.name=surfaceName2;
febio_spec.Mesh.Surface{2}.tri3.ATTR.id=(1:1:size(F_contact_secondary,1))';
febio_spec.Mesh.Surface{2}.tri3.VAL=F_contact_secondary;

% -> Surface pairs
contactPairName='Contact1';
febio_spec.Mesh.SurfacePair{1}.ATTR.name=contactPairName;
febio_spec.Mesh.SurfacePair{1}.primary=surfaceName1;
febio_spec.Mesh.SurfacePair{1}.secondary=surfaceName2;

%Boundary condition section
% -> Fix boundary conditions
febio_spec.Boundary.bc{1}.ATTR.type='fix';
febio_spec.Boundary.bc{1}.ATTR.node_set=nodeSetName1;
febio_spec.Boundary.bc{1}.dofs='x,y,z';

febio_spec.Boundary.bc{2}.ATTR.type='fix';
febio_spec.Boundary.bc{2}.ATTR.node_set=nodeSetName2;
febio_spec.Boundary.bc{2}.dofs='x,y';

febio_spec.Boundary.bc{3}.ATTR.type='prescribe';
febio_spec.Boundary.bc{3}.ATTR.node_set=nodeSetName2;
febio_spec.Boundary.bc{3}.dof='z';
febio_spec.Boundary.bc{3}.scale.ATTR.lc=1;
febio_spec.Boundary.bc{3}.scale.VAL=displacementMagnitude_z;
febio_spec.Boundary.bc{3}.relative=0;

%Contact section
febio_spec.Contact.contact{1}.ATTR.type='sliding-elastic';
febio_spec.Contact.contact{1}.ATTR.surface_pair=contactPairName;
febio_spec.Contact.contact{1}.two_pass=0;
febio_spec.Contact.contact{1}.laugon=laugon;
febio_spec.Contact.contact{1}.tolerance=0.2;
febio_spec.Contact.contact{1}.gaptol=0;
febio_spec.Contact.contact{1}.minaug=minaug;
febio_spec.Contact.contact{1}.maxaug=maxaug;
febio_spec.Contact.contact{1}.search_tol=0.01;
febio_spec.Contact.contact{1}.search_radius=0.1*sqrt(sum((max(V,[],1)-min(V,[],1)).^2,2));
febio_spec.Contact.contact{1}.symmetric_stiffness=0;
febio_spec.Contact.contact{1}.auto_penalty=1;
febio_spec.Contact.contact{1}.penalty=contactPenalty;
febio_spec.Contact.contact{1}.fric_coeff=fric_coeff;

%LoadData section
% -> load_controller
febio_spec.LoadData.load_controller{1}.ATTR.id=1;
febio_spec.LoadData.load_controller{1}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{1}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{1}.points.point.VAL=[0 0; 1 1];

%Output section
% -> log file
febio_spec.Output.logfile.ATTR.file=febioLogFileName;
febio_spec.Output.logfile.node_data{1}.ATTR.file=febioLogFileName_disp;
febio_spec.Output.logfile.node_data{1}.ATTR.data='ux;uy;uz';
febio_spec.Output.logfile.node_data{1}.ATTR.delim=',';
febio_spec.Output.logfile.node_data{1}.VAL=1:size(V,1);

febio_spec.Output.logfile.node_data{2}.ATTR.file=febioLogFileName_force;
febio_spec.Output.logfile.node_data{2}.ATTR.data='Rx;Ry;Rz';
febio_spec.Output.logfile.node_data{2}.ATTR.delim=',';
febio_spec.Output.logfile.node_data{2}.VAL=1:size(V,1);

febio_spec.Output.logfile.element_data{1}.ATTR.file=febioLogFileName_strainEnergy;
febio_spec.Output.logfile.element_data{1}.ATTR.data='sed';
febio_spec.Output.logfile.element_data{1}.ATTR.delim=',';
febio_spec.Output.logfile.element_data{1}.VAL=1:size(E_face,1);

%% Quick viewing of the FEBio input file structure
% The |febView| function can be used to view the xml structure in a MATLAB
% figure window.

%%
% |febView(febio_spec); %Viewing the febio file|

%% Exporting the FEBio input file
% Exporting the febio_spec structure to an FEBio input file is done using
% the |febioStruct2xml| function.

febioStruct2xml(febio_spec,febioFebFileName); %Exporting to file and domNode

%% Running the FEBio analysis
% To run the analysis defined by the created FEBio input file the
% |runMonitorFEBio| function is used. The input for this function is a
% structure defining job settings e.g. the FEBio input file name. The
% optional output runFlag informs the user if the analysis was run
% succesfully.

febioAnalysis.run_filename=febioFebFileName; %The input file name
febioAnalysis.run_logname=febioLogFileName; %The name for the log file
febioAnalysis.disp_on=1; %Display information on the command window
febioAnalysis.disp_log_on=1; %Display convergence information in the command window
febioAnalysis.runMode=runMode;%'internal';
febioAnalysis.t_check=0.25; %Time for checking log file (dont set too small)
febioAnalysis.maxtpi=1e99; %Max analysis time
febioAnalysis.maxLogCheckTime=10; %Max log file checking time

[runFlag]=runMonitorFEBio(febioAnalysis);%START FEBio NOW!!!!!!!!

%% Import FEBio results

if runFlag==1 %i.e. a succesful run

    % Importing nodal displacements from a log file
    [time_mat, N_disp_mat,~]=importFEBio_logfile(fullfile(savePath,febioLogFileName_disp)); %Nodal displacements
    time_mat=[0; time_mat(:)]; %Time

    N_disp_mat=N_disp_mat(:,2:end,:);
    sizImport=size(N_disp_mat);
    sizImport(3)=sizImport(3)+1;
    N_disp_mat_n=zeros(sizImport);
    N_disp_mat_n(:,:,2:end)=N_disp_mat;
    N_disp_mat=N_disp_mat_n;
    DN=N_disp_mat(:,:,end);

    V_def=V+DN;
    V_DEF=N_disp_mat+repmat(V,[1 1 size(N_disp_mat,3)]);
    X_DEF=V_DEF(:,1,:);
    Y_DEF=V_DEF(:,2,:);
    Z_DEF=V_DEF(:,3,:);

    C=sqrt(sum(DN(:,3).^2,2));

    %%
    % Importing element strain energies from a log file
    [~,E_energy,~]=importFEBio_logfile(fullfile(savePath,febioLogFileName_strainEnergy)); %Element strain energy

    %Remove nodal index column
    E_energy=E_energy(:,2:end,:);

    %Add initial state i.e. zero energy
    sizImport=size(E_energy);
    sizImport(3)=sizImport(3)+1;
    E_energy_mat_n=zeros(sizImport);
    E_energy_mat_n(:,:,2:end)=E_energy;
    E_energy=E_energy_mat_n;

    [FE_face,C_energy_face]=element2patch(E_face,E_energy(1:size(E_face,1),:,end),'tet4');
    [CV]=faceToVertexMeasure(FE_face,V,C_energy_face);

    %%
    % Plotting the simulated results using |anim8| to visualize and animate
    % deformations
    cMap_c=gjet(250);
    cMap=[linspacen([1 1 1],cMap_c(1,:),50)'; cMap_c];

%     cMap=cMap(4:end,:);

    % Create basic view and store graphics handle to initiate animation
    hf=cFigure; %Open figure
    gtitle([febioFebFileNamePart,': Press play to animate']);

    gpatch(Ffc,Vf,cMap(1,:),'none',1)
    hp1=gpatch(Fb(Cb==1,:),V_def,CV,'none',1); %Add graphics object to animate
    hp1.FaceColor='Interp';
    hp2=gpatch(Fb_rim,V_def,'w','none',0.25); %Add graphics object to animate
    hp3=gpatch(F_mask,V_mask,'w','none',0.25);

    axisGeom(gca,fontSize); camlight headlight;
    colormap(cMap); colorbar;
    caxis([0 max(C_energy_face)/10]);
    axis([min(X_DEF(:)) max(X_DEF(:)) min(Y_DEF(:)) max(Y_DEF(:)) min(Z_DEF(:)) max(Z_DEF(:))]);
    axis tight;

    % Set up animation features
    animStruct.Time=time_mat; %The time vector
    for qt=1:1:size(N_disp_mat,3) %Loop over time increments
        DN=N_disp_mat(:,:,qt); %Current displacement
        V_def=V+DN; %Current nodal coordinates

        %         C=sqrt(sum(DN(:,3).^2,2)); %New color
        [FE_face,C_energy_face]=element2patch(E_face,E_energy(1:size(E_face,1),:,qt),'tet4');
        [CV]=faceToVertexMeasure(FE_face,V,C_energy_face);

        u=mean(DN(bcPrescribeList,:),1);
        V_mask_def=V_mask+u(ones(size(V_mask,1),1),:);

        %Set entries in animation structure
        animStruct.Handles{qt}=[hp1 hp1 hp2 hp3]; %Handles of objects to animate
        animStruct.Props{qt}={'Vertices','CData','Vertices','Vertices'}; %Properties of objects to animate
        animStruct.Set{qt}={V_def,CV,V_def,V_mask_def}; %Property values for to set in order to animate
    end
    anim8(hf,animStruct); %Initiate animation feature
    drawnow;

end

%%
function [varargout]=traceToSurf(V1,N1,F2,V2,optionStructRayTrace)

if size(N1,1)==1
    N1=N1(ones(size(V1,1),1),:);
end

numPoints=size(V1,1);
indFacesIntersect=nan(size(V1,1),2);
for q=1:1:numPoints
    [P,indFaceIntersect,~,~]=triSurfRayTrace(V1(q,:),N1(q,:),F2,V2,optionStructRayTrace);
    if size(P,1)>1
        [~,indMin]=minDist(V1(q,:),P);
        %         [~,indMin]=min(d);
        P=P(indMin,:);
        indFaceIntersect=indFaceIntersect(indMin,:);
    end
    if ~isempty(P)
        V1(q,:)=P;
        indFacesIntersect(q,:)=indFaceIntersect;
    end
end

varargout{1}=V1;
varargout{2}=indFacesIntersect;

end

function [Fr,Vr]=roundMesh(indCurve,Vm,Nm,nc,stripRadius)

E=[indCurve(1:end)' [indCurve(2:end) indCurve(1)]'];
ind1=indCurve(1:end)';
ind2=[indCurve(2:end) indCurve(1)]';
ind3=[indCurve(end) indCurve(1:end-1)]';

N1f=Vm(ind2,:)-Vm(ind1,:);
N1b=Vm(ind1,:)-Vm(ind3,:);
Ne=vecnormalize((N1f+N1b)/2);

% Ne=vecnormalize(edgeVec(E,Vm));
Nf=-Nm(E(:,1),:);% -vecnormalize((Nm(E(:,1),:)+Nm(E(:,2),:))/2);
Ne2=vecnormalize(cross(Nf,Ne));

X=repmat(Vm(E(:,1),1),1,nc);
Y=repmat(Vm(E(:,1),2),1,nc);
Z=repmat(Vm(E(:,1),3),1,nc);

t=repmat(linspace(0,pi/2,nc),size(Z,1),1);

X=X+stripRadius.*sin(t).*repmat(Ne2(:,1),1,nc)-stripRadius.*cos(t).*repmat(Nf(:,1),1,nc)+stripRadius.*repmat(Nf(:,1),1,nc);
Y=Y+stripRadius.*sin(t).*repmat(Ne2(:,2),1,nc)-stripRadius.*cos(t).*repmat(Nf(:,2),1,nc)+stripRadius.*repmat(Nf(:,2),1,nc);
Z=Z+stripRadius.*sin(t).*repmat(Ne2(:,3),1,nc)-stripRadius.*cos(t).*repmat(Nf(:,3),1,nc)+stripRadius.*repmat(Nf(:,3),1,nc);

for q=2:1:size(X,2)
   v=evenlySampleCurve([X(:,q) Y(:,q) Z(:,q)],size(X,1),'pchip',1);
   X(:,q)=v(:,1);
   Y(:,q)=v(:,2);
   Z(:,q)=v(:,3);
end

[Fr,Vr]=grid2patch(X,Y,Z,[],[1 0 0]);

end
%%

function [F,V,X,Y,Z]=bezierLoft(P1,P4,N1,N4,pointSpacing,f)

D12=sqrt(sum((P1-P4).^2,2));
numPoints=ceil(max(D12)./pointSpacing);
if numPoints<2
    numPoints=2;
end

P2=P1+D12.*f.*N1;
P3=P4-D12.*f.*N4;

X=zeros(numPoints,size(P1,1));
Y=zeros(numPoints,size(P1,1));
Z=zeros(numPoints,size(P1,1));
for q=1:1:size(P1,1)
    p=[P1(q,:); P2(q,:); P3(q,:); P4(q,:)]; %Control points
    V_bezier=bezierCurve(p,numPoints*2); %Compute bezier curve
    V_bezier=evenlySampleCurve(V_bezier,numPoints,'pchip'); %resample evenly
    X(:,q)=V_bezier(:,1);
    Y(:,q)=V_bezier(:,2);
    Z(:,q)=V_bezier(:,3);
end

%Create quad patch data
[F,V] = surf2patch(X,Y,Z);
I=[(1:size(Z,1)-1)' (1:size(Z,1)-1)' (2:size(Z,1))' (2:size(Z,1))' ];
J=[size(Z,2).*ones(size(Z,1)-1,1) ones(size(Z,1)-1,1) ones(size(Z,1)-1,1) size(Z,2).*ones(size(Z,1)-1,1)];
F_sub=sub2ind(size(Z),I,J);
F=[F;F_sub];
F=fliplr(F);

end

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function y = ipermute( x, order )

%   Disciplined convex/geometric programming information for IPERMUTE:
%      IPERMUTE(A,ORDER) imposes no convexity restrictions on A. ORDER
%      must be constant.

%
% Determine the permutation
%

s = x.size_;
ndxs = 1 : prod( s );
ndx2 = ipermute( reshape( ndxs, s ), order );

%
% Permute the data
%

b = x.basis_;
try
    b = b( :, ndx2 );
catch
    ndxs( ndx2( : ).' ) = ndxs;
    [ r, c, v ] = find( b );
    b = sparse( r, ndxs( c ), v, size( b, 1 ), size( b, 2 ) );
    clear r c v
end
y = cvx( size( ndx2 ), b );

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


function f = floor(f)
%FLOOR   Pointwise floor function of a CLASSICFUN.
%   G = FLOOR(F) returns the CLASSICFUN G such that G(X) = FLOOR(F(x)) for each x in
%   F.domain.
%
%   If F is complex, then the G = FLOOR(REAL(F)) + 1i*FLOOR(IMAG(F)).
%
%   Note that FLOOR() assumes the output G(X) is a constant. If it is not, then
%   garbage is returned with no warning.
%
% See also CEIL, ROUND, FIX.

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

% FLOOR() the ONEFUN:
f.onefun = floor(f.onefun);

end


function T = ctranspose(T,varargin)
% conjugate of a tensor
%
% Input
%  T - @tensor
%
% Output
%  T - @tensor
%

switch T.rank

  case 1

  case 2

    T.M = conj(permute(T.M,[2 1 3:ndims(T.M)]));

  case 3

  case 4

    % check for symmetry

    % convert to a matrix
    M = tensor42(T.M,T.doubleConvention);

    % invert the matrix
    M = conj(permute(M,[2 1 3:ndims(T.M)]));

    % make some corrections
    % TODO: why this is needed?
    if ~check_option(varargin,'skipCorrection')
      w = 1./(1+((1:6)>3));
      w = w.' * w;
      M = M .* w;
    end

    % convert back to a 4 rank tensor
    T.M = tensor24(M,T.doubleConvention);
end


% Copyright (C) 2017-2018 Titus Cieslewski, RPG, University of Zurich,
%   Switzerland
%   You can contact the author at <titus at ifi dot uzh dot ch>
% Copyright (C) 2017-2018 Siddharth Choudhary, College of Computing,
%   Georgia Institute of Technology, Atlanta, GA, USA
% Copyright (C) 2017-2018 Davide Scaramuzza, RPG, University of Zurich,
%   Switzerland
%
% This file is part of dslam_open.
%
% dslam_open is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% dslam_open is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with dslam_open. If not, see <http://www.gnu.org/licenses/>.

% Visualize trajectory outputs
import gtsam.*
close all;
nr_robots = 10;
dataDir = 'data';
cmap = hsv(nr_robots);

    figure(1);
    cla; hold on;
for robot_i = 1:nr_robots
    robot_i
    initial_file = sprintf('%s/%d_optimized.g2o', dataDir, robot_i-1)
    [graph, initial] = gtsam.readG2o(initial_file, true);

    plot3DSubGraph(graph, initial, '-',  cmap(robot_i,:), '');
    hold on;
end

axis equal;